From 8e69ab9cdcaf95466b73d3f46dd4971d1138f081 Mon Sep 17 00:00:00 2001 From: Radzhab Bisultanov Date: Mon, 23 Mar 2026 12:31:59 +0300 Subject: [PATCH] save --- Source/BSP/Src/imu_processing.c | 6 +++--- Source/Control/Src/attitude.c | 26 +++++++++++++------------- 2 files changed, 16 insertions(+), 16 deletions(-) diff --git a/Source/BSP/Src/imu_processing.c b/Source/BSP/Src/imu_processing.c index 65094bc..876673c 100644 --- a/Source/BSP/Src/imu_processing.c +++ b/Source/BSP/Src/imu_processing.c @@ -24,9 +24,9 @@ void imu_processing_init() biquad_init_lpf(&accel_y_lpf, 25.0f, 500.0f); biquad_init_lpf(&accel_z_lpf, 25.0f, 500.0f); - biquad_init_lpf(&gyro_x_lpf, 100.0f, 500.0f); - biquad_init_lpf(&gyro_y_lpf, 100.0f, 500.0f); - biquad_init_lpf(&gyro_z_lpf, 100.0f, 500.0f); + biquad_init_lpf(&gyro_x_lpf, 25.0f, 500.0f); + biquad_init_lpf(&gyro_y_lpf, 25.0f, 500.0f); + biquad_init_lpf(&gyro_z_lpf, 25.0f, 500.0f); } void imu_read_scaled(imu_scaled_t* out) diff --git a/Source/Control/Src/attitude.c b/Source/Control/Src/attitude.c index e2df6c9..d81923b 100644 --- a/Source/Control/Src/attitude.c +++ b/Source/Control/Src/attitude.c @@ -10,13 +10,13 @@ static Vector3 gyro_bias = {}; volatile uint8_t imu_update_flag = 0; volatile uint8_t pid_update_flag = 0; -float angle_kp_roll = 2.5f; float angle_kp_pitch = 2.5f; +float angle_kp_roll = 2.5f; float angle_kp_yaw = 2.0f; -pid_t pid_roll = {.kp = 0.6f, .ki = 0.0f, .kd = 0.025f}; pid_t pid_pitch = {.kp = 0.6f, .ki = 0.0f, .kd = 0.025f}; -pid_t pid_yaw = {.kp = 1.8f, .ki = 0.6f, .kd = 0.0f}; +pid_t pid_roll = {.kp = 0.6f, .ki = 0.0f, .kd = 0.025f}; +pid_t pid_yaw = {.kp = 0.6f, .ki = 0.0f, .kd = 0.0f}; /*int16_t desired_roll = 0; int16_t desired_pitch = 0; @@ -51,7 +51,7 @@ void attitude_estimator_update(attitude_t* att, const imu_scaled_t* imu) float norm = sqrtf(accel.x*accel.x + accel.y*accel.y + accel.z*accel.z); - if (norm > 0.0001f) + if (norm > 0.00000000001f) { accel.x /= norm; accel.y /= norm; @@ -104,8 +104,8 @@ void attitude_controller_update(control_channels_t* control, const rc_channels* Vector3 desired_rate = { - angle_error.x * angle_kp_roll, - angle_error.y * angle_kp_pitch, + angle_error.x * angle_kp_pitch, + angle_error.y * angle_kp_roll, angle_error.z * angle_kp_yaw }; @@ -113,24 +113,24 @@ void attitude_controller_update(control_channels_t* control, const rc_channels* desired_rate.y = constrain(desired_rate.y, -RATE_LIM, RATE_LIM); desired_rate.z = constrain(desired_rate.z, -RATE_LIM, RATE_LIM); - float roll_error = desired_rate.x - att->gyro.x; - float pitch_error = desired_rate.y - att->gyro.y; + float pitch_error = desired_rate.x - att->gyro.x; + float roll_error = desired_rate.y - att->gyro.y; float yaw_error = desired_rate.z - att->gyro.z; - control->roll = pid_update(&pid_roll, roll_error, att->gyro.x, IMU_DT); - control->pitch = pid_update(&pid_pitch, pitch_error, att->gyro.y, IMU_DT); + control->pitch = pid_update(&pid_pitch, pitch_error, att->gyro.x, IMU_DT); + control->roll = pid_update(&pid_roll, roll_error, att->gyro.y, IMU_DT); control->yaw = pid_update(&pid_yaw, yaw_error, att->gyro.z, IMU_DT); } Quaternion rx_to_quaternion(const rc_channels* rx) { - float roll = int_mapping(rx->rc_roll, -500, 500, -45, 45) * DEG2RAD; float pitch = int_mapping(rx->rc_pitch, -500, 500, -45, 45) * DEG2RAD; + float roll = int_mapping(rx->rc_roll, -500, 500, -45, 45) * DEG2RAD; float yaw = 0; - Vector3 rpy = {roll, pitch, yaw}; + Vector3 pry = {pitch, roll, yaw}; - return QuatCreateFromEuler(&rpy); + return QuatCreateFromEuler(&pry); } Vector3 gravity_from_quat(const Quaternion* q)