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

Некоторая реализация логики перенесена в main

Browse Source
This commit is contained in:
Раджаб Бисултанов
2026-03-26 12:31:03 +03:00
parent 49f45bd4fe
commit b95a716415
7 changed files with 164 additions and 116 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -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

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

@@ -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;
}
}

137
Source/INS/IRS.c
View File

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

2
Source/INS/IRS.h
View File

@@ -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

21
Source/INS/geometry/quaternion.c
View File

@@ -1,6 +1,8 @@
#include "quaternion.h"
#include <math.h>
#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;
}

1
Source/INS/geometry/quaternion.h
View File

@@ -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

103
Source/main.c
View File

@@ -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)