diff --git a/Source/Control/Inc/attitude.h b/Source/Control/Inc/attitude.h index 0526b6b..d40fe99 100644 --- a/Source/Control/Inc/attitude.h +++ b/Source/Control/Inc/attitude.h @@ -1,60 +1,28 @@ #ifndef ATTITUDE_H #define ATTITUDE_H -#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 -{ - float roll; // deg - float pitch; // deg - float yaw_rate; // deg/s -} attitude_t;*/ +#include "pid.h" +#include "radio_receiver.h" typedef struct { - Quaternion orientation; // current orientation - Vector3 gyro; // current angular velocity + Vector3 gyro; } attitude_t; +static uint8_t imu_update_flag = 0; +static uint8_t pid_update_flag = 0; 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); +void attitude_controller_update(control_channels_t* control, + const rc_channels* rx, + const Quaternion* current_q, + const Vector3* gyro); -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, - const rc_channels* rx, - const attitude_t* att, - const imu_scaled_t* imu);*/ - - -float accel_roll_deg(const imu_scaled_t* imu); -float accel_pitch_deg(const imu_scaled_t* imu); - -void integrate_gyro_roll_deg(float* roll, const imu_scaled_t* imu); -void integrate_gyro_pitch_deg(float* pitch, const imu_scaled_t* imu); -void integrate_gyro_yaw_deg(float* yaw, const imu_scaled_t* imu); - - +void TIM6_DAC_IRQHandler(); #endif \ No newline at end of file diff --git a/Source/Control/Src/attitude.c b/Source/Control/Src/attitude.c index d81923b..c257495 100644 --- a/Source/Control/Src/attitude.c +++ b/Source/Control/Src/attitude.c @@ -1,125 +1,54 @@ #include "attitude.h" #include "pid.h" +#include "imu.h" +#include "imu_processing.h" #include -#define MAHONY_KP 2.5f -#define MAHONY_KI 0.01f +#define RATE_LIM 300.0f -static Vector3 gyro_bias = {}; +float angle_kp_pitch = 2.5f; +float angle_kp_roll = 2.5f; +float angle_kp_yaw = 2.0f; -volatile uint8_t imu_update_flag = 0; -volatile uint8_t pid_update_flag = 0; - -float angle_kp_pitch = 2.5f; -float angle_kp_roll = 2.5f; -float angle_kp_yaw = 2.0f; - -pid_t pid_pitch = {.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_yaw = {.kp = 0.6f, .ki = 0.0f, .kd = 0.0f}; - -/*int16_t desired_roll = 0; -int16_t desired_pitch = 0; - -float desired_roll_rate = 0.0f; -float desired_pitch_rate = 0.0f; - -float roll_rate_error = 0.0f; -float pitch_rate_error = 0.0f; -float yaw_rate_error = 0.0f; - -float error_roll = 0.0f; -float error_pitch = 0.0f; -float error_yaw = 0.0f;*/ +pid_t pid_pitch = {.kp = 0.6f, .kd = 0.025f}; +pid_t pid_roll = {.kp = 0.6f, .kd = 0.025f}; +pid_t pid_yaw = {.kp = 0.6f}; 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; + att->gyro = (Vector3){0}; } -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 Quaternion* current_q, + const Vector3* gyro) { - 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.00000000001f) - { - 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); -} + Quaternion q_target = rx_to_quaternion(rx); -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_pitch, - angle_error.y * angle_kp_roll, - 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 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->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 q_error = QuatGetError(current_q, &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_pitch, + angle_error.y * angle_kp_roll, + 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); + + control->pitch = pid_update(&pid_pitch, desired_rate.x - gyro->x, gyro->x, IMU_DT); + control->roll = pid_update(&pid_roll, desired_rate.y - gyro->y, gyro->y, IMU_DT); + control->yaw = pid_update(&pid_yaw, desired_rate.z - gyro->z, gyro->z, IMU_DT); } Quaternion rx_to_quaternion(const rc_channels* rx) @@ -133,15 +62,10 @@ Quaternion rx_to_quaternion(const rc_channels* rx) return QuatCreateFromEuler(&pry); } -Vector3 gravity_from_quat(const Quaternion* q) +float constrain(float x, float min, float max) { - 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; + if (x < min) x = min; else if (x > max) x = max; + return x; } void TIM6_DAC_IRQHandler() @@ -152,112 +76,4 @@ void TIM6_DAC_IRQHandler() 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; - - roll_acc = accel_roll_deg(imu); - pitch_acc = accel_pitch_deg(imu); - - integrate_gyro_roll_deg(&att->roll, imu); - integrate_gyro_pitch_deg(&att->pitch, imu); - - att->roll = CF_ALPHA * att->roll + (1 - CF_ALPHA) * roll_acc; - att->pitch = CF_ALPHA * att->pitch + (1 - CF_ALPHA) * pitch_acc; -}*/ - -/*void yaw_rate_update(attitude_t* attitude, imu_scaled_t* imu) -{ - attitude->yaw_rate = imu->gz; -} - -void attitude_pid_update(control_channels_t* control, - const rc_channels* rx, - const attitude_t* att, - const imu_scaled_t* imu) -{ - if (pid_update_flag) - { - pid_update_flag = 0; - - desired_roll = int_mapping(rx->rc_roll, -500, 500, -45, 45); - desired_pitch = int_mapping(rx->rc_pitch, -500, 500, -45, 45); - - desired_roll_rate = angle_kp_roll * (desired_roll - att->roll); - desired_pitch_rate = angle_kp_pitch * (desired_pitch - att->pitch); - - if (desired_roll_rate > 200) desired_roll_rate = 200; - if (desired_roll_rate < -200) desired_roll_rate = -200; - - if (desired_pitch_rate > 200) desired_pitch_rate = 200; - if (desired_pitch_rate < -200) desired_pitch_rate = -200; - - roll_rate_error = desired_roll_rate - imu->gy; - pitch_rate_error = desired_pitch_rate - imu->gx; - yaw_rate_error = - imu->gz; - - control->roll = pid_update(&pid_roll, roll_rate_error, imu->gy, IMU_DT); - 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); - } -}*/ - -float accel_roll_deg(const imu_scaled_t* imu) { - // right-left - return atan2f(imu->ax, sqrtf(imu->ay * imu->ay + imu->az * imu->az)) * 180.0f / PI; -} - -float accel_pitch_deg(const imu_scaled_t* imu) -{ - // forward-backward - return atan2f(-imu->ay, sqrtf(imu->ax * imu->ax + imu->az * imu->az)) * 180.0f / PI; -} - -void integrate_gyro_roll_deg(float* roll, const imu_scaled_t* imu) -{ - // right-left - *roll += imu->gy * IMU_DT; -} - -void integrate_gyro_pitch_deg(float* pitch, const imu_scaled_t* imu) -{ - // forward-backward - *pitch += imu->gx * IMU_DT; -} - -void integrate_gyro_yaw_deg(float* yaw, const imu_scaled_t* imu) -{ - // forward-backward - *yaw += imu->gz * IMU_DT; } \ No newline at end of file diff --git a/Source/INS/IRS.c b/Source/INS/IRS.c index c449f88..28dbed5 100644 --- a/Source/INS/IRS.c +++ b/Source/INS/IRS.c @@ -1 +1,82 @@ -#include "IRS.h" +#include "irs.h" +#include + +#define MAHONY_KP 2.5f +#define MAHONY_KI 0.01f + +static Vector3 gyro_bias = {0}; + +void IRS_init(IRS* irs) +{ + irs->q.w = 1.0f; + irs->q.x = 0.0f; + irs->q.y = 0.0f; + irs->q.z = 0.0f; +} + +Vector3 IRS_getGravity(const IRS* irs) +{ + const Quaternion* q = &irs->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 IRS_update(IRS* irs, const Vector3* gyro_in, const Vector3* accel_in, float dt) +{ + Vector3 gyro = *gyro_in; + Vector3 accel = *accel_in; + + // --- нормализация акселя + float norm = sqrtf(accel.x*accel.x + accel.y*accel.y + accel.z*accel.z); + + if (norm > 1e-6f) + { + accel.x /= norm; + accel.y /= norm; + accel.z /= norm; + + Vector3 g = IRS_getGravity(irs); + + 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 * dt; + gyro_bias.y += MAHONY_KI * error.y * dt; + gyro_bias.z += MAHONY_KI * error.z * dt; + + 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; + } + + irs->gyro = gyro; + irs->accel = accel; + + // --- интеграция кватерниона + Quaternion q = irs->q; + + Quaternion omega = + { + gyro.x * DEG2RAD, + gyro.y * DEG2RAD, + gyro.z * DEG2RAD, + 0 + }; + + Quaternion dq = QuatProd(&q, &omega); + dq = QuatConstProd(&dq, 0.5f * dt); + + q = QuatSum(&q, &dq); + q = QuatNormalize(&q, 1.0f); + + irs->q = q; +} \ No newline at end of file diff --git a/Source/INS/IRS.h b/Source/INS/IRS.h index 18ba789..41a970b 100644 --- a/Source/INS/IRS.h +++ b/Source/INS/IRS.h @@ -1,40 +1,24 @@ -#pragma once - #ifndef IRS_H #define IRS_H #include "quaternion.h" #include "vector.h" +#define PI 3.14159265359f +#define DEG2RAD PI / 180.0f + typedef struct { - Quaternion orientationQ; // главный кватерион ориентации - // Vector3 tilts; // локально ориентированные наклоны sinX sinY cosZ - - Vector3 gyro, accel; // последние значения датчиков - - float roll, pitch, yaw; + Quaternion q; // ориентация + Vector3 gyro; // deg/s + Vector3 accel; // g - // Vector3 accel, spd, pos; // инерциальные значения движения } IRS; -void updateIRS(IRS* irs, const Vector3* gyro, const Vector3* accel, float dt); -void IRSgetEuler(const IRS* irs); +void IRS_init(IRS* irs); -void updateGyro(Vector3* gyro, const Vector3* newGyro); -void updateAccel(Vector3* accel, const Vector3* newAccel); +void IRS_update(IRS* irs, const Vector3* gyro, const Vector3* accel, float dt); -// void setShiftAlpha(const Vector3* pos, const Vector3* spd, const float* qua); // коэффициент восстановления позиции и скорости в секунду - -// void updatePosSpeed(); -void updateQuat(); - -// void restoreAllShift(); - -// void setAccelShift(); - -// void getSinXYCosZ(); -// void getRollPitchYaw(); -// void getInertial(); +Vector3 IRS_getGravity(const IRS* irs); #endif \ No newline at end of file diff --git a/Source/main.c b/Source/main.c index 94f791c..2620ac4 100644 --- a/Source/main.c +++ b/Source/main.c @@ -1,6 +1,7 @@ #include "stm32g431xx.h" #include "imu.h" #include "imu_processing.h" +#include "IRS.h" #include "attitude.h" #include "radio_receiver.h" #include "motors.h" @@ -9,6 +10,7 @@ imu_scaled_t imu; +IRS irs; attitude_t attitude; rc_channels rx_chs_raw; rc_channels rx_chs_normalized; @@ -34,6 +36,8 @@ int main(void) imu_calibrate(); + IRS_init(&irs); + attitude_init(&attitude); receiver_init(); @@ -41,22 +45,47 @@ int main(void) motors_init(); while (1) - { - receiver_update(&rx_chs_raw); - rx_chs_normalized = normalize_channels(rx_chs_raw); - - attitude_update(&attitude, &imu); - attitude_pid_update(&ctrl_chs, &rx_chs_normalized, &attitude); - - if (rx_chs_normalized.rc_armed) - { - motors_set_throttle_mix(rx_chs_normalized.rc_throttle, &ctrl_chs, rx_chs_normalized.rc_armed); - } - else - { - motors_turn_off(); - } - } +{ + receiver_update(&rx_chs_raw); + rx_chs_normalized = normalize_channels(rx_chs_raw); + + if (imu_update_flag) + { + imu_update_flag = 0; + + imu_read_scaled(&imu); + + Vector3 gyro = {imu.gx, imu.gy, imu.gz}; + Vector3 accel = {imu.ax, imu.ay, imu.az}; + + IRS_update(&irs, &gyro, &accel, IMU_DT); + } + + if (pid_update_flag) + { + pid_update_flag = 0; + + attitude_controller_update( + &ctrl_chs, + &rx_chs_normalized, + &irs.q, + &irs.gyro + ); + + if (rx_chs_normalized.rc_armed) + { + motors_set_throttle_mix( + rx_chs_normalized.rc_throttle, + &ctrl_chs, + rx_chs_normalized.rc_armed + ); + } + else + { + motors_turn_off(); + } + } +} } void delay_ms(uint32_t ms)