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

Наведён порядок в коде

Browse Source
This commit is contained in:
Radzhab Bisultanov
2026-02-18 14:23:01 +03:00
parent eecf8f2cc2
commit f9b7277a33
20 changed files with 647 additions and 928 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -5,13 +5,13 @@
#include "radio_receiver.h"
#include "pid.h"
#define CF_ALPHA 0.99f
#define CF_ALPHA 0.99f
typedef struct
{
float roll; // deg
float pitch; // deg
float yaw_rate; // deg/s
float roll; // deg
float pitch; // deg
float yaw_rate; // deg/s
} attitude_t;
void complementary_filter_update(attitude_t* att, const imu_scaled_t* imu);

12
Source/Control/Inc/biquad_filter.h
View File

@@ -3,15 +3,15 @@
#include <math.h>
#define PI 3.14159265359f
#define PI 3.14159265359f
typedef struct
{
float b0, b1, b2;
float a1, a2;
float x1, x2;
float y1, y2;
float b0, b1, b2;
float a1, a2;
float x1, x2;
float y1, y2;
} biquad_t;
void biquad_init_lpf(biquad_t* f, float cutoff, float sample_rate);

18
Source/Control/Inc/pid.h
View File

@@ -5,19 +5,19 @@
typedef struct
{
float kp;
float ki;
float kd;
float integral;
float prev_error;
float kp;
float ki;
float kd;
float integral;
float prev_error;
} pid_t;
typedef struct
{
float roll;
float pitch;
float yaw;
float roll;
float pitch;
float yaw;
} control_channels_t;
float pid_update(pid_t* pid, float error, float gyro_rate, float dt);

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

@@ -28,34 +28,34 @@ float error_yaw = 0.0f;
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;
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 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);
}
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)
{
attitude->yaw_rate = imu->gz;
attitude->yaw_rate = imu->gz;
}
void attitude_pid_update(control_channels_t* control,
@@ -63,67 +63,67 @@ void attitude_pid_update(control_channels_t* control,
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);
}
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);
}
}
void TIM6_DAC_IRQHandler()
{
if (TIM6->SR & TIM_SR_UIF)
{
TIM6->SR &= ~TIM_SR_UIF;
imu_update_flag = 1;
pid_update_flag = 1;
}
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
return atan2f(imu->ax, sqrtf(imu->ay * imu->ay + imu->az * imu->az)) * 180.0f / PI;
// 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;
// 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;
// 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;
// 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;
// forward-backward
*yaw += imu->gz * IMU_DT;
}

68
Source/Control/Src/biquad_filter.c
View File

@@ -2,42 +2,42 @@
void biquad_init_lpf(biquad_t* f, float cutoff, float sample_rate)
{
float omega = 2.0f * PI * cutoff / sample_rate;
float sin_omega = sinf(omega);
float cos_omega = cosf(omega);
float alpha = sin_omega / sqrtf(2.0f);
float b0 = (1 - cos_omega) / 2;
float b1 = 1 - cos_omega;
float b2 = (1 - cos_omega) / 2;
float a0 = 1 + alpha;
float a1 = -2 * cos_omega;
float a2 = 1 - alpha;
f->b0 = b0 / a0;
f->b1 = b1 / a0;
f->b2 = b2 / a0;
f->a1 = a1 / a0;
f->a2 = a2 / a0;
f->x1 = f->x2 = 0;
f->y1 = f->y2 = 0;
float omega = 2.0f * PI * cutoff / sample_rate;
float sin_omega = sinf(omega);
float cos_omega = cosf(omega);
float alpha = sin_omega / sqrtf(2.0f);
float b0 = (1 - cos_omega) / 2;
float b1 = 1 - cos_omega;
float b2 = (1 - cos_omega) / 2;
float a0 = 1 + alpha;
float a1 = -2 * cos_omega;
float a2 = 1 - alpha;
f->b0 = b0 / a0;
f->b1 = b1 / a0;
f->b2 = b2 / a0;
f->a1 = a1 / a0;
f->a2 = a2 / a0;
f->x1 = f->x2 = 0;
f->y1 = f->y2 = 0;
}
float biquad_apply(biquad_t* f, float input)
{
float output = f->b0 * input
+ f->b1 * f->x1
+ f->b2 * f->x2
- f->a1 * f->y1
- f->a2 * f->y2;
f->x2 = f->x1;
f->x1 = input;
f->y2 = f->y1;
f->y1 = output;
return output;
float output = f->b0 * input
+ f->b1 * f->x1
+ f->b2 * f->x2
- f->a1 * f->y1
- f->a2 * f->y2;
f->x2 = f->x1;
f->x1 = input;
f->y2 = f->y1;
f->y1 = output;
return output;
}

36
Source/Control/Src/pid.c
View File

@@ -2,22 +2,22 @@
float pid_update(pid_t* pid, float error, float gyro_rate, float dt)
{
float p = pid->kp * error;
pid->integral += error * dt;
if (pid->integral > 100) pid->integral = 100;
if (pid->integral < -100) pid->integral = -100;
float i = pid->ki * pid->integral;
float d = - pid->kd * gyro_rate;
pid->prev_error = error;
float spid = p + i + d;
if (spid > 300) spid = 300;
if (spid < -300) spid = -300;
return spid;
float p = pid->kp * error;
pid->integral += error * dt;
if (pid->integral > 100) pid->integral = 100;
if (pid->integral < -100) pid->integral = -100;
float i = pid->ki * pid->integral;
float d = - pid->kd * gyro_rate;
pid->prev_error = error;
float spid = p + i + d;
if (spid > 300) spid = 300;
if (spid < -300) spid = -300;
return spid;
}