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

Полный переход на C++

Browse Source 
*Чтение IMU и обработка его данных выполняется в точности как в рабочей прошивке.
*Определение вращения работает корректно.
This commit is contained in:
Раджаб Бисултанов
2026-04-17 13:40:27 +03:00
parent a3d845df9e
commit 0faafbf089
21 changed files with 247 additions and 1210 deletions
Download Patch File Download Diff File Expand all files Collapse all files

73
Source/Control/Attitude.cpp Normal file
View File

@@ -0,0 +1,73 @@
#include "Attitude.h"
#include "PID.h"
#include <math.h>
#define FREQUENCY 100.0f
#define PERIOD 1.0f / FREQUENCY
#define PI 3.14159265359f
#define DEG2RAD PI / 180.0f
#define RATE_LIM 300.0f
constexpr float angle_kp_pitch = 2.5f;
constexpr float angle_kp_roll = 2.5f;
constexpr float angle_kp_yaw = 2.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->gyro = (Vector3){};
}
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 = current_q->GetError(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, PERIOD);
control->roll = pid_update(&pid_roll, desired_rate.Y - gyro->Y, gyro->Y, PERIOD);
control->yaw = pid_update(&pid_yaw, desired_rate.Z - gyro->Z, gyro->Z, PERIOD);
}
Quaternion rx_to_quaternion(const rc_channels* rx)
{
Quaternion q;
float pitch = int_mapping(rx->rc_pitch, -500, 500, -45, 45) * DEG2RAD;
float roll = int_mapping(rx->rc_roll, -500, 500, -45, 45) * DEG2RAD;
float yaw = 0;
Vector3 pry = {pitch, roll, yaw};
q.CreatePitchRollYaw(pry);
return q;
}
float constrain(float x, float min, float max)
{
if (x < min) x = min; else if (x > max) x = max;
return x;
}

17
Source/Control/Inc/attitude.h → Source/Control/Attitude.h
View File

@@ -1,17 +1,18 @@
#pragma once
#ifndef ATTITUDE_H
#define ATTITUDE_H
#include "quaternion.h"
#include "vector.h"
#include "pid.h"
#include "radio_receiver.h"
#include "imu_processing.h"
#include "Quaternion.h"
#include "Vector.h"
#include "PID.h"
#include "RadioReceiver.h"
#include "IRS.h"
typedef struct
struct attitude_t
{
Vector3 gyro;
} attitude_t;
Vector3 gyro;
};
void attitude_init(attitude_t* att);

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

@@ -1,20 +0,0 @@
#ifndef BIQUAD_FILTER_H
#define BIQUAD_FILTER_H
#include <math.h>
#define PI 3.14159265359f
typedef struct
{
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);
float biquad_apply(biquad_t* f, float input);
#endif

2
Source/Control/Src/pid.c → Source/Control/PID.cpp
View File

@@ -1,4 +1,4 @@
#include "pid.h"
#include "PID.h"
float pid_update(pid_t* pid, float error, float gyro_rate, float dt)
{

12
Source/Control/Inc/pid.h → Source/Control/PID.h
View File

@@ -1,9 +1,11 @@
#pragma once
#ifndef PID_H
#define PID_H
#include "stm32g431xx.h"
#include "stm32g4xx.h"
typedef struct
struct pid_t
{
float kp;
float ki;
@@ -11,14 +13,14 @@ typedef struct
float integral;
float prev_error;
} pid_t;
};
typedef struct
struct control_channels_t
{
float roll;
float pitch;
float yaw;
} control_channels_t;
};
float pid_update(pid_t* pid, float error, float gyro_rate, float dt);

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

@@ -1,69 +0,0 @@
#include "attitude.h"
#include "pid.h"
#include "imu.h"
#include "imu_processing.h"
#include <math.h>
#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, .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->gyro = (Vector3){0};
}
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(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)
{
float pitch = int_mapping(rx->rc_pitch, -500, 500, -45, 45) * DEG2RAD;
float roll = int_mapping(rx->rc_roll, -500, 500, -45, 45) * DEG2RAD;
float yaw = 0;
Vector3 pry = {pitch, roll, yaw};
return QuatCreateFromEuler(&pry);
}
float constrain(float x, float min, float max)
{
if (x < min) x = min; else if (x > max) x = max;
return x;
}

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

@@ -1,43 +0,0 @@
#include "biquad_filter.h"
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 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;
}