From b95a71641555220503793416162274c3b4b1f7f9 Mon Sep 17 00:00:00 2001 From: Radzhab Bisultanov Date: Thu, 26 Mar 2026 12:31:03 +0300 Subject: [PATCH] =?UTF-8?q?=D0=9D=D0=B5=D0=BA=D0=BE=D1=82=D0=BE=D1=80?= =?UTF-8?q?=D0=B0=D1=8F=20=D1=80=D0=B5=D0=B0=D0=BB=D0=B8=D0=B7=D0=B0=D1=86?= =?UTF-8?q?=D0=B8=D1=8F=20=D0=BB=D0=BE=D0=B3=D0=B8=D0=BA=D0=B8=20=D0=BF?= =?UTF-8?q?=D0=B5=D1=80=D0=B5=D0=BD=D0=B5=D1=81=D0=B5=D0=BD=D0=B0=20=D0=B2?= =?UTF-8?q?=20main?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Source/Control/Inc/attitude.h | 6 +- Source/Control/Src/attitude.c | 10 --- Source/INS/IRS.c | 137 ++++++++++++++++++------------- Source/INS/IRS.h | 2 +- Source/INS/geometry/quaternion.c | 21 ++++- Source/INS/geometry/quaternion.h | 1 + Source/main.c | 103 +++++++++++++---------- 7 files changed, 164 insertions(+), 116 deletions(-) diff --git a/Source/Control/Inc/attitude.h b/Source/Control/Inc/attitude.h index d40fe99..b6128cf 100644 --- a/Source/Control/Inc/attitude.h +++ b/Source/Control/Inc/attitude.h @@ -5,15 +5,14 @@ #include "vector.h" #include "pid.h" #include "radio_receiver.h" +#include "imu_processing.h" +#include "IRS.h" typedef struct { 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_controller_update(control_channels_t* control, @@ -23,6 +22,5 @@ void attitude_controller_update(control_channels_t* control, Quaternion rx_to_quaternion(const rc_channels* rx); float constrain(float x, float min, float max); -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 c257495..9501cb5 100644 --- a/Source/Control/Src/attitude.c +++ b/Source/Control/Src/attitude.c @@ -66,14 +66,4 @@ float constrain(float x, float min, float max) { if (x < min) x = min; else if (x > max) x = max; return x; -} - -void TIM6_DAC_IRQHandler() -{ - if (TIM6->SR & TIM_SR_UIF) - { - TIM6->SR &= ~TIM_SR_UIF; - imu_update_flag = 1; - pid_update_flag = 1; - } } \ No newline at end of file diff --git a/Source/INS/IRS.c b/Source/INS/IRS.c index 28dbed5..fe09c06 100644 --- a/Source/INS/IRS.c +++ b/Source/INS/IRS.c @@ -1,8 +1,11 @@ #include "irs.h" #include -#define MAHONY_KP 2.5f -#define MAHONY_KI 0.01f +#define MAHONY_KP 2.5f +#define MAHONY_KI 0.01f + +#define ACC_ALPHA 0.01f +#define ACC_MAX_ERROR 0.2f static Vector3 gyro_bias = {0}; @@ -14,14 +17,14 @@ void IRS_init(IRS* irs) irs->q.z = 0.0f; } -Vector3 IRS_getGravity(const IRS* irs) +Vector3 IRS_getGravity(const Quaternion* q) { - 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; + Vector3 g = + { + 2 * (q->x*q->z - q->w*q->y), + 2 * (q->w*q->x + q->y*q->z), + q->w*q->w - q->x*q->x - q->y*q->y + q->z*q->z + }; return g; } @@ -29,54 +32,72 @@ Vector3 IRS_getGravity(const IRS* irs) void IRS_update(IRS* irs, const Vector3* gyro_in, const Vector3* accel_in, float dt) { Vector3 gyro = *gyro_in; - Vector3 accel = *accel_in; + Vector3 accel = {accel_in->x, accel_in->y, -accel_in->z}; - // --- нормализация акселя - 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; + // gyro intergate + 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); + + // accel correction + float acc_len = sqrtf(accel.x*accel.x + accel.y*accel.y + accel.z*accel.z); + + if (acc_len > 1e-6f) + { + Vector3 acc_norm = + { + accel.x / acc_len, + accel.y / acc_len, + accel.z / acc_len + }; + + float dyn = fabsf(acc_len - 1.0f); + + float trust; + if (dyn > ACC_MAX_ERROR) + trust = 0.0f; + else + trust = 1.0f - (dyn / ACC_MAX_ERROR); + + float gain = ACC_ALPHA * trust; + + if (gain > 1e-5f) + { + Vector3 est = IRS_getGravity(&q); + + Vector3 error = + { + acc_norm.y*est.z - acc_norm.z*est.y, + acc_norm.z*est.x - acc_norm.x*est.z, + acc_norm.x*est.y - acc_norm.y*est.x + }; + + Quaternion corretrion = + { + error.x * gain * 0.5f, + error.y * gain * 0.5f, + error.z * gain * 0.5f, + 1.0f + }; + + q = QuatProd(&corretrion, &q); + q = QuatNormalize(&q, 1.0f); + } + } + + irs->q = q; + irs->gyro = gyro; + irs->accel = accel; } \ No newline at end of file diff --git a/Source/INS/IRS.h b/Source/INS/IRS.h index 41a970b..261e18a 100644 --- a/Source/INS/IRS.h +++ b/Source/INS/IRS.h @@ -19,6 +19,6 @@ void IRS_init(IRS* irs); void IRS_update(IRS* irs, const Vector3* gyro, const Vector3* accel, float dt); -Vector3 IRS_getGravity(const IRS* irs); +Vector3 IRS_getGravity(const Quaternion* q); #endif \ No newline at end of file diff --git a/Source/INS/geometry/quaternion.c b/Source/INS/geometry/quaternion.c index d392c3b..2fc2f25 100644 --- a/Source/INS/geometry/quaternion.c +++ b/Source/INS/geometry/quaternion.c @@ -1,6 +1,8 @@ #include "quaternion.h" #include +#define PI 3.14159265359f + Quaternion QuatNormalize(const Quaternion* q, const float gain) { Quaternion res = {}; @@ -134,7 +136,24 @@ Quaternion QuatGetError(const Quaternion* current, const Quaternion* target, boo return error; } - +Vector3 QuatToEuler(const Quaternion* q) +{ + Vector3 e; + + e.x = atan2f(2*(q->w*q->x + q->y*q->z), + 1 - 2*(q->x*q->x + q->y*q->y)); + + e.y = asinf(2*(q->w*q->y - q->z*q->x)); + + e.z = atan2f(2*(q->w*q->z + q->x*q->y), + 1 - 2*(q->y*q->y + q->z*q->z)); + + e.x *= 180.0f / PI; + e.y *= 180.0f / PI; + e.z *= 180.0f / PI; + + return e; +} diff --git a/Source/INS/geometry/quaternion.h b/Source/INS/geometry/quaternion.h index 45b7ae0..d3513b2 100644 --- a/Source/INS/geometry/quaternion.h +++ b/Source/INS/geometry/quaternion.h @@ -25,6 +25,7 @@ Vector3 QuatRotateAroundZ(const Quaternion* q, const Vector3* vec, bool CCW); Quaternion QuatCreateFromEuler(const Vector3* eulerAngels); Quaternion QuatGetError(const Quaternion* current, const Quaternion* target, bool fastWay); +Vector3 QuatToEuler(const Quaternion* q); #endif diff --git a/Source/main.c b/Source/main.c index 2620ac4..2a0408e 100644 --- a/Source/main.c +++ b/Source/main.c @@ -8,6 +8,10 @@ #include "pid.h" #include "lidar.h" +void TIM6_DAC_IRQHandler(); + +static uint8_t irs_update_flag = 0; +static uint8_t control_update_flag = 0; imu_scaled_t imu; IRS irs; @@ -17,6 +21,8 @@ rc_channels rx_chs_normalized; control_channels_t ctrl_chs; lidar_data lidar; +Vector3 euler; + void delay_ms(uint32_t ms); int main(void) @@ -31,11 +37,12 @@ int main(void) i2c_gpio_init(); i2c1_init(); imu_init(); - imu_tim6_init(); - imu_processing_init(); + imu_processing_init(); imu_calibrate(); + imu_tim6_init(); + IRS_init(&irs); attitude_init(&attitude); @@ -45,47 +52,59 @@ int main(void) motors_init(); while (1) -{ - 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(); - } - } + { + receiver_update(&rx_chs_raw); + rx_chs_normalized = normalize_channels(rx_chs_raw); + + if (irs_update_flag) + { + irs_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); + } + + euler = QuatToEuler(&irs.q); + + if (control_update_flag) + { + control_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 TIM6_DAC_IRQHandler() +{ + if (TIM6->SR & TIM_SR_UIF) + { + TIM6->SR &= ~TIM_SR_UIF; + irs_update_flag = 1; + control_update_flag = 1; + } } void delay_ms(uint32_t ms)