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"
#define ICM_ADDR 0x68
#define ICM_ADDR 0x68
#define REG_PWR_MGMT_1 0x06
#define REG_BANK_SEL 0x7F
#define REG_BANK_SEL 0x7F
#define REG_GYRO_CONFIG_1 0x01
#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_DLPFCFG_73 3 << 3
#define GYRO_FCHOICE_ON 1
#define GYRO_FCHOICE_OFF 0
#define ACCEL_FS_SEL_4 1 << 1
#define ACCEL_FS_SEL_8 2 << 1
#define ACCEL_DLPFCFG_69 3 << 3
#define ACCEL_FCHOICE_ON 1
@@ -24,8 +25,8 @@
typedef struct
{
float ax, ay, az; // lsb
float gx, gy, gz; // lsb
int16_t ax, ay, az; // lsb
int16_t gx, gy, gz; // lsb
} imu_raw_t;
void imu_pow_init();

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

@@ -4,10 +4,10 @@
#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 PI 3.14159265359f
#define DEG2RAD PI / 180.0f
#define PI 3.14159265359f
#define DEG2RAD PI / 180.0f
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_calib(imu_raw_t* imu, long gyrLim, long accZero, long accMax);
uint16_t Rev16(uint16_t v);
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] |= ((4 << ((8 - 8) * 4)) | (4 << ((9 - 8) * 4)));
// high speed
GPIOB->OSPEEDR |= (2 << (8 * 2)) | (2 << (9 * 2));
// enable open-drain
GPIOB->OTYPER |= (1 << 8) | (1 << 9);
@@ -36,8 +39,10 @@ void i2c1_init()
// enable I2C1
RCC->APB1ENR1 |= RCC_APB1ENR1_I2C1EN;
// 0x00303D5B - 100 kHz
// 400 kHz @ 16 MHz
I2C1->TIMINGR = 0x00303D5B;
I2C1->TIMINGR = 0x10802D9BUL;
I2C1->CR1 |= I2C_CR1_PE;
}
@@ -56,7 +61,7 @@ void imu_init()
// gyro ~2000 dps, FS_SEL = 3
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);
// back to bank 0
@@ -127,8 +132,8 @@ void imu_read_raw(imu_raw_t* data)
data->ay = (buf[2] << 8) | buf[3];
data->az = (buf[4] << 8) | buf[5];
data->gx = (buf[6] << 8) | buf[7];
data->gy = (buf[8] << 8) | buf[9];
data->gx = (buf[6] << 8) | buf[7];
data->gy = (buf[8] << 8) | buf[9];
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_z = 0.0f;
static float accel_bias_x = 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_y_lpf;
@@ -20,8 +20,8 @@ static biquad_t gyro_x_lpf;
static biquad_t gyro_y_lpf;
static biquad_t gyro_z_lpf;
const float accel_min = -8192.0f;
const float accel_max = 8192.0f;
const float accel_min = -4096.0f;
const float accel_max = 4096.0f;
static int16_t x_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);
raw.ax = biquad_apply(&accel_x_lpf, raw.ax);
raw.ay = biquad_apply(&accel_y_lpf, raw.ay);
raw.az = biquad_apply(&accel_z_lpf, raw.az);
raw.ax = (uint16_t) biquad_apply(&accel_x_lpf, raw.ax);
raw.ay = (uint16_t) biquad_apply(&accel_y_lpf, raw.ay);
raw.az = (uint16_t) biquad_apply(&accel_z_lpf, raw.az);
raw.gx = biquad_apply(&gyro_x_lpf, raw.gx);
raw.gy = biquad_apply(&gyro_y_lpf, raw.gy);
raw.gz = biquad_apply(&gyro_z_lpf, raw.gz);
raw.gx = (uint16_t) biquad_apply(&gyro_x_lpf, raw.gx);
raw.gy = (uint16_t) biquad_apply(&gyro_y_lpf, raw.gy);
raw.gz = (uint16_t) biquad_apply(&gyro_z_lpf, raw.gz);
if (allowed_calib) imu_calib(&raw, 50, 8192, 1000);
/*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;*/
if (allowed_calib) imu_calib(&raw, 50, 4096, 500);
out->ax = normalize(raw.ax, 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->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->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->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)
@@ -118,7 +125,13 @@ void imu_calib(imu_raw_t* imu, long gyrLim, long accZero, long accMax)
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;
@@ -136,25 +149,25 @@ void imu_calibrate()
{
imu_read_raw(&imu);
sum_ax += imu.ax / ACCEL_SENS_SCALE_FACTOR;
sum_ay += imu.ay / ACCEL_SENS_SCALE_FACTOR;
sum_az += imu.az / ACCEL_SENS_SCALE_FACTOR;
sum_ax += imu.ax;
sum_ay += imu.ay;
sum_az += imu.az;
sum_gx += imu.gx / GYRO_SENS_SCALE_FACTOR;
sum_gy += imu.gy / GYRO_SENS_SCALE_FACTOR;
sum_gz += imu.gz / GYRO_SENS_SCALE_FACTOR;
sum_gx += imu.gx;
sum_gy += imu.gy;
sum_gz += imu.gz;
for (volatile uint16_t d = 0; d < 5000; ++d);
}
accel_bias_x = sum_ax / 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_y = sum_gy / samples;
gyro_bias_z = sum_gz / samples;
}
}*/
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 = QuatNormalize(&irs->q, 1.0f);
// /gyro update
// /gyro update
// accel update
Vector3 accel = {irs->accel.x, irs->accel.y, irs->accel.z};
restoreQuat(irs, &accel);
restoreQuat(irs);
// /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_max = 0.02f;
@@ -51,7 +49,7 @@ void restoreQuat(IRS* irs, const Vector3* accel)
if (gain < 0.0001f) return;
Vector3 acc = normalizeV3(accel, 1.0f);
Vector3 acc = normalizeV3(&irs->accel, 1.0f);
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_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);

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

@@ -5,7 +5,7 @@
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);
@@ -70,7 +70,7 @@ Quaternion QuatDiff(const Quaternion* q1, const Quaternion* q2)
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;
}
@@ -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->w + q1->x * q2->x + q1->y * q2->y - q1->z * q2->z
};
return res;
}

38
Source/main.c
View File

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