bisultanov/RaDrone
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Обновлена архитектура обработки IMU

Browse Source
This commit is contained in:
Раджаб Бисултанов
2026-03-23 14:11:50 +03:00
parent 8e69ab9cdc
commit 49f45bd4fe
5 changed files with 187 additions and 309 deletions
Download Patch File Download Diff File Expand all files Collapse all files

54
Source/Control/Inc/attitude.h
View File

@@ -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);
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_controller_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);
const Quaternion* current_q,
const Vector3* gyro);
Quaternion rx_to_quaternion(const rc_channels* rx);
float constrain(float x, float min, float max);
void TIM6_DAC_IRQHandler();
#endif

222
Source/Control/Src/attitude.c
View File

@@ -1,99 +1,32 @@
#include "attitude.h"
#include "pid.h"
#include "imu.h"
#include "imu_processing.h"
#include <math.h>
#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;
#define RATE_LIM 300.0f
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)
{
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);
}
void attitude_controller_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)
{
Quaternion q_target = rx_to_quaternion(rx);
Quaternion q_error = QuatGetError(&att->orientation, &q_target, true);
Quaternion q_error = QuatGetError(current_q, &q_target, true);
Vector3 angle_error =
{
@@ -113,13 +46,9 @@ 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 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);
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()
@@ -153,111 +77,3 @@ void TIM6_DAC_IRQHandler()
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;
}

83
Source/INS/IRS.c
View File

@@ -1 +1,82 @@
#include "IRS.h"
#include "irs.h"
#include <math.h>
#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;
}

34
Source/INS/IRS.h
View File

@@ -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
Quaternion q; // ориентация
Vector3 gyro; // deg/s
Vector3 accel; // g
Vector3 gyro, accel; // последние значения датчиков
float roll, pitch, yaw;
// 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

37
Source/main.c
View File

@@ -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,16 +45,40 @@ 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 (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);
motors_set_throttle_mix(
rx_chs_normalized.rc_throttle,
&ctrl_chs,
rx_chs_normalized.rc_armed
);
}
else
{
@@ -58,6 +86,7 @@ int main(void)
}
}
}
}
void delay_ms(uint32_t ms)
{