diff --git a/Source/BSP/Inc/imu_processing.h b/Source/BSP/Inc/imu_processing.h index 2bb14b7..bd32da9 100644 --- a/Source/BSP/Inc/imu_processing.h +++ b/Source/BSP/Inc/imu_processing.h @@ -7,6 +7,7 @@ #define ACCEL_SENS_SCALE_FACTOR 8192.0f #define GYRO_SENS_SCALE_FACTOR 16.4f #define PI 3.14159265359f +#define DEG2RAD PI / 180.0f typedef struct { diff --git a/Source/BSP/Src/imu_processing.c b/Source/BSP/Src/imu_processing.c index 876673c..65094bc 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, 25.0f, 500.0f); - biquad_init_lpf(&gyro_y_lpf, 25.0f, 500.0f); - biquad_init_lpf(&gyro_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); } void imu_read_scaled(imu_scaled_t* out) diff --git a/Source/Control/Inc/attitude.h b/Source/Control/Inc/attitude.h index 77680d2..0526b6b 100644 --- a/Source/Control/Inc/attitude.h +++ b/Source/Control/Inc/attitude.h @@ -4,24 +4,49 @@ #include "imu_processing.h" #include "radio_receiver.h" #include "pid.h" +#include "quaternion.h" +#include "vector.h" #define CF_ALPHA 0.99f +#define GYRO_BIAS_LIM 0.5f +#define RATE_LIM 300.0f -typedef struct +/*typedef struct { float roll; // deg float pitch; // deg float yaw_rate; // deg/s +} attitude_t;*/ + +typedef struct +{ + Quaternion orientation; // current orientation + Vector3 gyro; // current angular velocity } attitude_t; -void complementary_filter_update(attitude_t* att, const imu_scaled_t* imu); + +void attitude_init(attitude_t* att); +void attitude_estimator_update(attitude_t* att, const imu_scaled_t* imu); +void attitude_controller_update(control_channels_t* control, const rc_channels* rx, const attitude_t* att); + +void TIM6_DAC_IRQHandler(); void attitude_update(attitude_t* attitude, imu_scaled_t* imu); +void attitude_pid_update(control_channels_t* control, const rc_channels* rx, const attitude_t* att); + +Vector3 gravity_from_quat(const Quaternion* q); +Quaternion rx_to_quaternion(const rc_channels* rx); + +float constrain(float x, float min, float max); + +/////////////////////////////////////////////////////////////////////////////// + +void complementary_filter_update(attitude_t* att, const imu_scaled_t* imu); void yaw_rate_update(attitude_t* attitude, imu_scaled_t* imu); -void attitude_pid_update(control_channels_t* control, +/*void attitude_pid_update(control_channels_t* control, const rc_channels* rx, const attitude_t* att, - const imu_scaled_t* imu); -void TIM6_DAC_IRQHandler(); + const imu_scaled_t* imu);*/ + float accel_roll_deg(const imu_scaled_t* imu); float accel_pitch_deg(const imu_scaled_t* imu); diff --git a/Source/Control/Src/attitude.c b/Source/Control/Src/attitude.c index aa8859e..e2df6c9 100644 --- a/Source/Control/Src/attitude.c +++ b/Source/Control/Src/attitude.c @@ -2,17 +2,23 @@ #include "pid.h" #include +#define MAHONY_KP 2.5f +#define MAHONY_KI 0.01f + +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_pitch = 2.5f; +float angle_kp_yaw = 2.0f; -pid_t pid_roll = {.kp = 0.6f, .ki = 0.0f, .kd = 0.025f}; +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_yaw = {.kp = 1.8f, .ki = 0.6f, .kd = 0.0f}; -int16_t desired_roll = 0; +/*int16_t desired_roll = 0; int16_t desired_pitch = 0; float desired_roll_rate = 0.0f; @@ -24,9 +30,159 @@ float yaw_rate_error = 0.0f; float error_roll = 0.0f; float error_pitch = 0.0f; -float error_yaw = 0.0f; +float error_yaw = 0.0f;*/ -void complementary_filter_update(attitude_t* att, const imu_scaled_t* imu) +void attitude_init(attitude_t* att) +{ + att->orientation.w = 1.0f; + att->orientation.x = 0.0f; + att->orientation.y = 0.0f; + att->orientation.z = 0.0f; + + att->gyro.x = 0.0f; + att->gyro.y = 0.0f; + att->gyro.z = 0.0f; +} + +void attitude_estimator_update(attitude_t* att, const imu_scaled_t* imu) +{ + Vector3 gyro = {imu->gx, imu->gy, imu->gz}; + Vector3 accel = {imu->ax, imu->ay, imu->az}; + + float norm = sqrtf(accel.x*accel.x + accel.y*accel.y + accel.z*accel.z); + + if (norm > 0.0001f) + { + accel.x /= norm; + accel.y /= norm; + accel.z /= norm; + + Vector3 g = gravity_from_quat(&att->orientation); + + Vector3 error = + { + accel.y*g.z - accel.z*g.y, + accel.z*g.x - accel.x*g.z, + accel.x*g.y - accel.y*g.x + }; + + gyro_bias.x += MAHONY_KI * error.x * IMU_DT; + gyro_bias.y += MAHONY_KI * error.y * IMU_DT; + gyro_bias.z += MAHONY_KI * error.z * IMU_DT; + + gyro_bias.x = constrain(gyro_bias.x, -GYRO_BIAS_LIM, GYRO_BIAS_LIM); + gyro_bias.y = constrain(gyro_bias.y, -GYRO_BIAS_LIM, GYRO_BIAS_LIM); + gyro_bias.z = constrain(gyro_bias.z, -GYRO_BIAS_LIM, GYRO_BIAS_LIM); + + gyro.x += MAHONY_KP * error.x + gyro_bias.x; + gyro.y += MAHONY_KP * error.y + gyro_bias.y; + gyro.z += MAHONY_KP * error.z + gyro_bias.z; + } + + att->gyro = gyro; + + Quaternion h = {gyro.x * DEG2RAD, gyro.y * DEG2RAD, gyro.z * DEG2RAD, 0}; + + Quaternion dq = QuatProd(&att->orientation, &h); + dq = QuatConstProd(&dq, 0.5f * IMU_DT); + att->orientation = QuatSum(&att->orientation, &dq); + + att->orientation = QuatNormalize(&att->orientation, 1.0f); +} + +void attitude_controller_update(control_channels_t* control, const rc_channels* rx, const attitude_t* att) +{ + Quaternion q_target = rx_to_quaternion(rx); + Quaternion q_error = QuatGetError(&att->orientation, &q_target, true); + + Vector3 angle_error = + { + 2.0f * q_error.x, + 2.0f * q_error.y, + 2.0f * q_error.z + }; + + Vector3 desired_rate = + { + angle_error.x * angle_kp_roll, + angle_error.y * angle_kp_pitch, + angle_error.z * angle_kp_yaw + }; + + desired_rate.x = constrain(desired_rate.x, -RATE_LIM, RATE_LIM); + 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 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->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 yaw = 0; + + Vector3 rpy = {roll, pitch, yaw}; + + return QuatCreateFromEuler(&rpy); +} + +Vector3 gravity_from_quat(const Quaternion* q) +{ + Vector3 g; + + g.x = 2 * (q->x*q->z - q->w*q->y); + g.y = 2 * (q->w*q->x + q->y*q->z); + g.z = q->w*q->w - q->x*q->x - q->y*q->y + q->z*q->z; + + return g; +} + +void TIM6_DAC_IRQHandler() +{ + if (TIM6->SR & TIM_SR_UIF) + { + TIM6->SR &= ~TIM_SR_UIF; + imu_update_flag = 1; + pid_update_flag = 1; + } +} + +void attitude_update(attitude_t* attitude, imu_scaled_t* imu) +{ + if (!imu_update_flag) + return; + + imu_update_flag = 0; + + imu_read_scaled(imu); + attitude_estimator_update(attitude, imu); + + // complementary_filter_update(attitude, imu); + // yaw_rate_update(attitude, imu); +} + +void attitude_pid_update(control_channels_t* control, const rc_channels* rx, const attitude_t* att) +{ + if (!pid_update_flag) + return; + pid_update_flag = 0; + attitude_controller_update(control, rx, att); +} + +float constrain(float x, float min, float max) +{ + if (x < min) x = min; else if (x > max) x = max; + return x; +} + +/*void complementary_filter_update(attitude_t* att, const imu_scaled_t* imu) { static float roll_acc; static float pitch_acc; @@ -39,21 +195,9 @@ void complementary_filter_update(attitude_t* att, const imu_scaled_t* imu) att->roll = CF_ALPHA * att->roll + (1 - CF_ALPHA) * roll_acc; att->pitch = CF_ALPHA * att->pitch + (1 - CF_ALPHA) * pitch_acc; -} +}*/ -void attitude_update(attitude_t* attitude, imu_scaled_t* imu) -{ - if (imu_update_flag) - { - imu_update_flag = 0; - - imu_read_scaled(imu); - complementary_filter_update(attitude, imu); - yaw_rate_update(attitude, imu); - } -} - -void yaw_rate_update(attitude_t* attitude, imu_scaled_t* imu) +/*void yaw_rate_update(attitude_t* attitude, imu_scaled_t* imu) { attitude->yaw_rate = imu->gz; } @@ -87,17 +231,7 @@ void attitude_pid_update(control_channels_t* control, control->pitch = pid_update(&pid_pitch, pitch_rate_error, imu->gx, IMU_DT); control->yaw = pid_update(&pid_yaw, yaw_rate_error, imu->gz, IMU_DT); } -} - -void TIM6_DAC_IRQHandler() -{ - if (TIM6->SR & TIM_SR_UIF) - { - TIM6->SR &= ~TIM_SR_UIF; - imu_update_flag = 1; - pid_update_flag = 1; - } -} +}*/ float accel_roll_deg(const imu_scaled_t* imu) { // right-left diff --git a/Source/INS/geometry/quaternion.c b/Source/INS/geometry/quaternion.c index bbca733..d392c3b 100644 --- a/Source/INS/geometry/quaternion.c +++ b/Source/INS/geometry/quaternion.c @@ -3,7 +3,7 @@ Quaternion QuatNormalize(const Quaternion* q, const float gain) { - Quaternion res; + Quaternion res = {}; float norm = sqrtf(q->x * q->x + q->y * q->y + q->z * q->z + q->w * q->w); @@ -74,11 +74,12 @@ Quaternion QuatConstProd(const Quaternion* q, const float value) Quaternion QuatProd(const Quaternion* q1, const Quaternion* q2) { - Quaternion res = { - .x = q1->w * q2->x + q1->x * q2->w + q1->y * q2->z - q1->z * q2->y, - .y = q1->w * q2->y + q1->x * q2->z + q1->y * q2->w - q1->z * q2->x, - .z = q1->w * q2->z + q1->x * q2->y + q1->y * q2->x - q1->z * q2->w, - .w = q1->w * q2->w + q1->x * q2->x + q1->y * q2->y - q1->z * q2->z + Quaternion res = + { + q1->w * q2->x + q1->x * q2->w + q1->y * q2->z - q1->z * q2->y, + q1->w * q2->y + q1->x * q2->z + q1->y * q2->w - q1->z * q2->x, + q1->w * q2->z + q1->x * q2->y + q1->y * q2->x - q1->z * q2->w, + q1->w * q2->w + q1->x * q2->x + q1->y * q2->y - q1->z * q2->z }; @@ -99,13 +100,13 @@ Vector3 QuatRotateAroundZ(const Quaternion* q, const Vector3* vec, bool CCW) return res; } -Quaternion QuatCreateRollPitchYaw(const Vector3* RollPitchYawRad) +Quaternion QuatCreateFromEuler(const Vector3* eulerAngels) { Quaternion res; - float h_r = 0.5f * RollPitchYawRad->y; - float h_p = 0.5f * RollPitchYawRad->x; - float h_y = 0.5f * RollPitchYawRad->z; + float h_r = 0.5f * eulerAngels->y; + float h_p = 0.5f * eulerAngels->x; + float h_y = 0.5f * eulerAngels->z; float c_r = cosf(h_r), s_r = sinf(h_r); float c_p = cosf(h_p), s_p = sinf(h_p); diff --git a/Source/INS/geometry/quaternion.h b/Source/INS/geometry/quaternion.h index e32d96d..45b7ae0 100644 --- a/Source/INS/geometry/quaternion.h +++ b/Source/INS/geometry/quaternion.h @@ -22,7 +22,7 @@ Quaternion QuatConstProd(const Quaternion* q, const float value); Quaternion QuatProd(const Quaternion* q1, const Quaternion* q2); Vector3 QuatRotateAroundZ(const Quaternion* q, const Vector3* vec, bool CCW); -Quaternion QuatCreateRollPitchYaw(const Vector3* RollPitchYawRad); +Quaternion QuatCreateFromEuler(const Vector3* eulerAngels); Quaternion QuatGetError(const Quaternion* current, const Quaternion* target, bool fastWay); diff --git a/Source/main.c b/Source/main.c index d353e0f..94f791c 100644 --- a/Source/main.c +++ b/Source/main.c @@ -34,20 +34,19 @@ int main(void) imu_calibrate(); + attitude_init(&attitude); + receiver_init(); motors_init(); while (1) { - attitude_update(&attitude, &imu); - receiver_update(&rx_chs_raw); rx_chs_normalized = normalize_channels(rx_chs_raw); - attitude_pid_update(&ctrl_chs, &rx_chs_normalized, &attitude, &imu); - - lidar_update(&lidar); + attitude_update(&attitude, &imu); + attitude_pid_update(&ctrl_chs, &rx_chs_normalized, &attitude); if (rx_chs_normalized.rc_armed) { diff --git a/drone.ewp b/drone.ewp index 7a3938d..eff5930 100644 --- a/drone.ewp +++ b/drone.ewp @@ -363,6 +363,7 @@ $PROJ_DIR$\Source\BSP\Inc $PROJ_DIR$\Source\Control\Inc $PROJ_DIR$\Source\INS\geometry + $PROJ_DIR$\Source\INS