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

Выполняется корректное чтение данных иму. Оси и наклоны соответствуют рабочим прошивке и дрону

Browse Source
This commit is contained in:
Раджаб Бисултанов
2026-04-06 16:13:16 +03:00
parent 699577d82b
commit b713794a26
8 changed files with 100 additions and 84 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
Source/BSP/Inc/imu.h
View File

@@ -5,18 +5,19 @@
#include "stm32g431xx.h" #include "stm32g431xx.h"
#define ICM_ADDR 0x68 #define ICM_ADDR 0x68
#define REG_PWR_MGMT_1 0x06 #define REG_PWR_MGMT_1 0x06
#define REG_BANK_SEL 0x7F #define REG_BANK_SEL 0x7F
#define REG_GYRO_CONFIG_1 0x01 #define REG_GYRO_CONFIG_1 0x01
#define REG_ACCEL_CONFIG 0x14 #define REG_ACCEL_CONFIG 0x14
#define IMU_DT 0.002f // 2 ms #define IMU_DT 0.002f // 2 ms
#define GYRO_FS_SEL_2000 3 << 1 #define GYRO_FS_SEL_2000 3 << 1
#define GYRO_DLPFCFG_73 3 << 3 #define GYRO_DLPFCFG_73 3 << 3
#define GYRO_FCHOICE_ON 1 #define GYRO_FCHOICE_ON 1
#define GYRO_FCHOICE_OFF 0 #define GYRO_FCHOICE_OFF 0
#define ACCEL_FS_SEL_4 1 << 1
#define ACCEL_FS_SEL_8 2 << 1 #define ACCEL_FS_SEL_8 2 << 1
#define ACCEL_DLPFCFG_69 3 << 3 #define ACCEL_DLPFCFG_69 3 << 3
#define ACCEL_FCHOICE_ON 1 #define ACCEL_FCHOICE_ON 1
@@ -24,8 +25,8 @@
typedef struct typedef struct
{ {
float ax, ay, az; // lsb int16_t ax, ay, az; // lsb
float gx, gy, gz; // lsb int16_t gx, gy, gz; // lsb
} imu_raw_t; } imu_raw_t;
void imu_pow_init(); void imu_pow_init();

7
Source/BSP/Inc/imu_processing.h
View File

@@ -4,10 +4,10 @@
#include "imu.h" #include "imu.h"
#define ACCEL_SENS_SCALE_FACTOR 8192.0f #define ACCEL_SENS_SCALE_FACTOR 4096.0f
#define GYRO_SENS_SCALE_FACTOR 16.4f #define GYRO_SENS_SCALE_FACTOR 16.4f
#define PI 3.14159265359f #define PI 3.14159265359f
#define DEG2RAD PI / 180.0f #define DEG2RAD PI / 180.0f
typedef struct typedef struct
{ {
@@ -19,6 +19,7 @@ void imu_processing_init();
void imu_read_scaled(imu_scaled_t* out, const uint8_t* allowed_calib); void imu_read_scaled(imu_scaled_t* out, const uint8_t* allowed_calib);
void imu_calib(imu_raw_t* imu, long gyrLim, long accZero, long accMax); void imu_calib(imu_raw_t* imu, long gyrLim, long accZero, long accMax);
uint16_t Rev16(uint16_t v);
void imu_calibrate(); void imu_calibrate();

13
Source/BSP/Src/imu.c
View File

@@ -23,6 +23,9 @@ void i2c_gpio_init()
GPIOB->AFR[1] &= ~((0xF << ((8 - 8) * 4)) | (0xF << ((9 - 8) * 4))); GPIOB->AFR[1] &= ~((0xF << ((8 - 8) * 4)) | (0xF << ((9 - 8) * 4)));
GPIOB->AFR[1] |= ((4 << ((8 - 8) * 4)) | (4 << ((9 - 8) * 4))); GPIOB->AFR[1] |= ((4 << ((8 - 8) * 4)) | (4 << ((9 - 8) * 4)));
// high speed
GPIOB->OSPEEDR |= (2 << (8 * 2)) | (2 << (9 * 2));
// enable open-drain // enable open-drain
GPIOB->OTYPER |= (1 << 8) | (1 << 9); GPIOB->OTYPER |= (1 << 8) | (1 << 9);
@@ -36,8 +39,10 @@ void i2c1_init()
// enable I2C1 // enable I2C1
RCC->APB1ENR1 |= RCC_APB1ENR1_I2C1EN; RCC->APB1ENR1 |= RCC_APB1ENR1_I2C1EN;
// 0x00303D5B - 100 kHz
// 400 kHz @ 16 MHz // 400 kHz @ 16 MHz
I2C1->TIMINGR = 0x00303D5B; I2C1->TIMINGR = 0x10802D9BUL;
I2C1->CR1 |= I2C_CR1_PE; I2C1->CR1 |= I2C_CR1_PE;
} }
@@ -56,7 +61,7 @@ void imu_init()
// gyro ~2000 dps, FS_SEL = 3 // gyro ~2000 dps, FS_SEL = 3
i2c_write(ICM_ADDR, REG_GYRO_CONFIG_1, GYRO_FS_SEL_2000 | GYRO_DLPFCFG_73 | GYRO_FCHOICE_ON); i2c_write(ICM_ADDR, REG_GYRO_CONFIG_1, GYRO_FS_SEL_2000 | GYRO_DLPFCFG_73 | GYRO_FCHOICE_ON);
// accel ~4g, FS_SEL = 1 // accel 8g, FS_SEL = 2
i2c_write(ICM_ADDR, REG_ACCEL_CONFIG, ACCEL_FS_SEL_8 | ACCEL_DLPFCFG_69 | ACCEL_FCHOICE_ON); i2c_write(ICM_ADDR, REG_ACCEL_CONFIG, ACCEL_FS_SEL_8 | ACCEL_DLPFCFG_69 | ACCEL_FCHOICE_ON);
// back to bank 0 // back to bank 0
@@ -127,8 +132,8 @@ void imu_read_raw(imu_raw_t* data)
data->ay = (buf[2] << 8) | buf[3]; data->ay = (buf[2] << 8) | buf[3];
data->az = (buf[4] << 8) | buf[5]; data->az = (buf[4] << 8) | buf[5];
data->gx = (buf[6] << 8) | buf[7]; data->gx = (buf[6] << 8) | buf[7];
data->gy = (buf[8] << 8) | buf[9]; data->gy = (buf[8] << 8) | buf[9];
data->gz = (buf[10] << 8) | buf[11]; data->gz = (buf[10] << 8) | buf[11];
} }

91
Source/BSP/Src/imu_processing.c
View File

@@ -4,13 +4,13 @@
static float gyro_bias_x = 0.0f; /*static float gyro_bias_x = 0.0f;
static float gyro_bias_y = 0.0f; static float gyro_bias_y = 0.0f;
static float gyro_bias_z = 0.0f; static float gyro_bias_z = 0.0f;
static float accel_bias_x = 0.0f; static float accel_bias_x = 0.0f;
static float accel_bias_y = 0.0f; static float accel_bias_y = 0.0f;
static float accel_bias_z = 0.0f; static float accel_bias_z = 0.0f;*/
static biquad_t accel_x_lpf; static biquad_t accel_x_lpf;
static biquad_t accel_y_lpf; static biquad_t accel_y_lpf;
@@ -20,8 +20,8 @@ static biquad_t gyro_x_lpf;
static biquad_t gyro_y_lpf; static biquad_t gyro_y_lpf;
static biquad_t gyro_z_lpf; static biquad_t gyro_z_lpf;
const float accel_min = -8192.0f; const float accel_min = -4096.0f;
const float accel_max = 8192.0f; const float accel_max = 4096.0f;
static int16_t x_calib; static int16_t x_calib;
static int16_t y_calib; static int16_t y_calib;
@@ -46,25 +46,15 @@ void imu_read_scaled(imu_scaled_t* out, const uint8_t* allowed_calib)
imu_read_raw(&raw); imu_read_raw(&raw);
raw.ax = biquad_apply(&accel_x_lpf, raw.ax); raw.ax = (uint16_t) biquad_apply(&accel_x_lpf, raw.ax);
raw.ay = biquad_apply(&accel_y_lpf, raw.ay); raw.ay = (uint16_t) biquad_apply(&accel_y_lpf, raw.ay);
raw.az = biquad_apply(&accel_z_lpf, raw.az); raw.az = (uint16_t) biquad_apply(&accel_z_lpf, raw.az);
raw.gx = biquad_apply(&gyro_x_lpf, raw.gx); raw.gx = (uint16_t) biquad_apply(&gyro_x_lpf, raw.gx);
raw.gy = biquad_apply(&gyro_y_lpf, raw.gy); raw.gy = (uint16_t) biquad_apply(&gyro_y_lpf, raw.gy);
raw.gz = biquad_apply(&gyro_z_lpf, raw.gz); raw.gz = (uint16_t) biquad_apply(&gyro_z_lpf, raw.gz);
if (allowed_calib) imu_calib(&raw, 50, 8192, 1000); if (allowed_calib) imu_calib(&raw, 50, 4096, 500);
/*float alpha = 0.01;
out->ax = out->ax * (1.0f - alpha) + raw.ax * alpha;
out->ay = out->ay * (1.0f - alpha) + raw.ay * alpha;
out->az = out->az * (1.0f - alpha) + raw.az * alpha;
out->gx = out->gx * (1.0f - alpha) + raw.gx * alpha;
out->gy = out->gy * (1.0f - alpha) + raw.gy * alpha;
out->gz = out->gz * (1.0f - alpha) + raw.gz * alpha;*/
out->ax = normalize(raw.ax, 1.0f, accel_min, accel_max); out->ax = normalize(raw.ax, 1.0f, accel_min, accel_max);
out->ay = normalize(raw.ay, 1.0f, accel_min, accel_max); out->ay = normalize(raw.ay, 1.0f, accel_min, accel_max);
@@ -75,21 +65,38 @@ void imu_read_scaled(imu_scaled_t* out, const uint8_t* allowed_calib)
out->gz = (raw.gz - gyro_shift[2]) / GYRO_SENS_SCALE_FACTOR; out->gz = (raw.gz - gyro_shift[2]) / GYRO_SENS_SCALE_FACTOR;
//////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////
/*out->ax = raw.ax / ACCEL_SENS_SCALE_FACTOR - accel_bias_x;
out->ay = raw.ay / ACCEL_SENS_SCALE_FACTOR - accel_bias_y;
out->az = raw.az / ACCEL_SENS_SCALE_FACTOR - accel_bias_z;
out->ax = biquad_apply(&accel_x_lpf, out->ax); /*raw.ax = Rev16(raw.ax);
raw.ay = Rev16(raw.ay);
raw.az = Rev16(raw.az);
raw.gx = Rev16(raw.gx);
raw.gy = Rev16(raw.gy);
raw.gz = Rev16(raw.gz);
out->ax = raw.ax - accel_bias_x;
out->ay = raw.ay - accel_bias_y;
out->az = raw.az - accel_bias_z;
out->gx = raw.gx - gyro_bias_x;
out->gy = raw.gy - gyro_bias_y;
out->gz = raw.gz - gyro_bias_z;
out->ax = biquad_apply(&accel_x_lpf, out->ax);
out->ay = biquad_apply(&accel_y_lpf, out->ay); out->ay = biquad_apply(&accel_y_lpf, out->ay);
out->az = biquad_apply(&accel_z_lpf, out->az); out->az = biquad_apply(&accel_z_lpf, out->az);
out->gx = raw.gx / GYRO_SENS_SCALE_FACTOR - gyro_bias_x;
out->gy = raw.gy / GYRO_SENS_SCALE_FACTOR - gyro_bias_y;
out->gz = raw.gz / GYRO_SENS_SCALE_FACTOR - gyro_bias_z;
out->gx = biquad_apply(&gyro_x_lpf, out->gx); out->gx = biquad_apply(&gyro_x_lpf, out->gx);
out->gy = biquad_apply(&gyro_y_lpf, out->gy); out->gy = biquad_apply(&gyro_y_lpf, out->gy);
out->gz = biquad_apply(&gyro_z_lpf, out->gz);*/ out->gz = biquad_apply(&gyro_z_lpf, out->gz);
out->ax /= ACCEL_SENS_SCALE_FACTOR;
out->ay /= ACCEL_SENS_SCALE_FACTOR;
out->az /= ACCEL_SENS_SCALE_FACTOR;
out->gx /= GYRO_SENS_SCALE_FACTOR;
out->gy /= GYRO_SENS_SCALE_FACTOR;
out->gz /= GYRO_SENS_SCALE_FACTOR;*/
} }
void imu_calib(imu_raw_t* imu, long gyrLim, long accZero, long accMax) void imu_calib(imu_raw_t* imu, long gyrLim, long accZero, long accMax)
@@ -118,7 +125,13 @@ void imu_calib(imu_raw_t* imu, long gyrLim, long accZero, long accMax)
acc = acc * (1.0f - alpha) + len * alpha; acc = acc * (1.0f - alpha) + len * alpha;
} }
void imu_calibrate() uint16_t Rev16(uint16_t v)
{
asm("REV16 %1, %0" : "=r" (v) : "r" (v));
return v;
}
/*void imu_calibrate()
{ {
const int samples = 1000; const int samples = 1000;
@@ -136,25 +149,25 @@ void imu_calibrate()
{ {
imu_read_raw(&imu); imu_read_raw(&imu);
sum_ax += imu.ax / ACCEL_SENS_SCALE_FACTOR; sum_ax += imu.ax;
sum_ay += imu.ay / ACCEL_SENS_SCALE_FACTOR; sum_ay += imu.ay;
sum_az += imu.az / ACCEL_SENS_SCALE_FACTOR; sum_az += imu.az;
sum_gx += imu.gx / GYRO_SENS_SCALE_FACTOR; sum_gx += imu.gx;
sum_gy += imu.gy / GYRO_SENS_SCALE_FACTOR; sum_gy += imu.gy;
sum_gz += imu.gz / GYRO_SENS_SCALE_FACTOR; sum_gz += imu.gz;
for (volatile uint16_t d = 0; d < 5000; ++d); for (volatile uint16_t d = 0; d < 5000; ++d);
} }
accel_bias_x = sum_ax / samples; accel_bias_x = sum_ax / samples;
accel_bias_y = sum_ay / samples; accel_bias_y = sum_ay / samples;
accel_bias_z = (sum_az / samples) + 1.0f; accel_bias_z = sum_az / samples;
gyro_bias_x = sum_gx / samples; gyro_bias_x = sum_gx / samples;
gyro_bias_y = sum_gy / samples; gyro_bias_y = sum_gy / samples;
gyro_bias_z = sum_gz / samples; gyro_bias_z = sum_gz / samples;
} }*/
float normalize(float value, float scale, float min, float max) float normalize(float value, float scale, float min, float max)
{ {

12
Source/INS/IRS.c
View File

@@ -27,19 +27,17 @@ void IRS_update(IRS* irs, float dt)
irs->q = QuatSum(&irs->q, &g); irs->q = QuatSum(&irs->q, &g);
irs->q = QuatNormalize(&irs->q, 1.0f); irs->q = QuatNormalize(&irs->q, 1.0f);
// /gyro update // /gyro update
// accel update // accel update
restoreQuat(irs);
Vector3 accel = {irs->accel.x, irs->accel.y, irs->accel.z};
restoreQuat(irs, &accel);
// /accel update // /accel update
} }
void restoreQuat(IRS* irs, const Vector3* accel) void restoreQuat(IRS* irs)
{ {
float len = lengthV3(accel); float len = lengthV3(&irs->accel);
static float quat_acc_alpha = 0.03f; static float quat_acc_alpha = 0.03f;
static float quat_acc_max = 0.02f; static float quat_acc_max = 0.02f;
@@ -51,7 +49,7 @@ void restoreQuat(IRS* irs, const Vector3* accel)
if (gain < 0.0001f) return; if (gain < 0.0001f) return;
Vector3 acc = normalizeV3(accel, 1.0f); Vector3 acc = normalizeV3(&irs->accel, 1.0f);
Vector3 est = IRS_getGravity(&irs->q); Vector3 est = IRS_getGravity(&irs->q);

2
Source/INS/IRS.h
View File

@@ -21,7 +21,7 @@ typedef struct
void IRS_init(IRS* irs); void IRS_init(IRS* irs);
void IRS_update(IRS* irs, float dt); void IRS_update(IRS* irs, float dt);
void restoreQuat(IRS* irs, const Vector3* accel); void restoreQuat(IRS* irs);
void setAccelShift(IRS* irs, const float pitch, const float roll, const float yaw); void setAccelShift(IRS* irs, const float pitch, const float roll, const float yaw);

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

@@ -5,7 +5,7 @@
Quaternion QuatNormalize(const Quaternion* q, const float gain) Quaternion QuatNormalize(const Quaternion* q, const float gain)
{ {
Quaternion res = {}; Quaternion res = {0.0f, 0.0f, 0.0f, 0.0f};
float norm = sqrtf(q->x * q->x + q->y * q->y + q->z * q->z + q->w * q->w); float norm = sqrtf(q->x * q->x + q->y * q->y + q->z * q->z + q->w * q->w);
@@ -70,7 +70,7 @@ Quaternion QuatDiff(const Quaternion* q1, const Quaternion* q2)
Quaternion QuatConstProd(const Quaternion* q, const float value) Quaternion QuatConstProd(const Quaternion* q, const float value)
{ {
Quaternion res = {.x = q->x * value, .y = q->y * value, .z = q->z * value, .w = q->w * value}; Quaternion res = {q->x * value, q->y * value, q->z * value, q->w * value};
return res; return res;
} }
@@ -83,8 +83,6 @@ Quaternion QuatProd(const Quaternion* q1, const Quaternion* q2)
q1->w * q2->z + q1->x * q2->y + q1->y * q2->x - q1->z * q2->w, q1->w * q2->z + q1->x * q2->y + q1->y * q2->x - q1->z * q2->w,
q1->w * q2->w + q1->x * q2->x + q1->y * q2->y - q1->z * q2->z q1->w * q2->w + q1->x * q2->x + q1->y * q2->y - q1->z * q2->z
}; };
return res; return res;
} }

38
Source/main.c
View File

@@ -40,7 +40,7 @@ int main(void)
imu_init(); imu_init();
imu_processing_init(); imu_processing_init();
imu_calibrate(); //imu_calibrate();
imu_tim6_init(); imu_tim6_init();
@@ -59,7 +59,7 @@ int main(void)
if (irs_update_flag) if (irs_update_flag)
{ {
irs_update_flag = 0; irs_update_flag = 0;
imu_read_scaled(&imu, &allowed_calib); imu_read_scaled(&imu, &allowed_calib);
@@ -71,34 +71,34 @@ int main(void)
irs.accel.y = imu.ay; irs.accel.y = imu.ay;
irs.accel.z = imu.az; irs.accel.z = imu.az;
IRS_update(&irs, IMU_DT); IRS_update(&irs, IMU_DT);
} }
euler = QuatToEuler(&irs.q); euler = QuatToEuler(&irs.q);
if (control_update_flag) if (control_update_flag)
{ {
control_update_flag = 0; control_update_flag = 0;
attitude_controller_update( attitude_controller_update(
&ctrl_chs, &ctrl_chs,
&rx_chs_normalized, &rx_chs_normalized,
&irs.q, &irs.q,
&irs.gyro &irs.gyro
); );
} }
if (rx_chs_normalized.rc_armed) if (rx_chs_normalized.rc_armed)
{ {
allowed_calib = 1; allowed_calib = 1;
motors_set_throttle_mix( motors_set_throttle_mix(
rx_chs_normalized.rc_throttle, rx_chs_normalized.rc_throttle,
&ctrl_chs, &ctrl_chs,
rx_chs_normalized.rc_armed rx_chs_normalized.rc_armed
); );
} }
else else
{ {
motors_turn_off(); motors_turn_off();
allowed_calib = 0; allowed_calib = 0;