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

Осуществлён переход на кватернионы. Но нужно протестировать

Browse Source
This commit is contained in:
Раджаб Бисултанов
2026-03-13 16:28:06 +03:00
parent 469ec64d45
commit c3a7907e16
8 changed files with 217 additions and 56 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -2,17 +2,23 @@
#include "pid.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;
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