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

Включен low pass фильтр датчиков, уменьшен срез биквада

Browse Source
This commit is contained in:
Раджаб Бисултанов
2026-03-12 13:54:48 +03:00
parent c1fab9fcb3
commit 469ec64d45
3 changed files with 20 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -1,3 +1,5 @@
#pragma once
#ifndef IMU_H
#define IMU_H
@@ -10,6 +12,16 @@
#define REG_ACCEL_CONFIG 0x14
#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_8 2 << 1
#define ACCEL_DLPFCFG_69 3 << 3
#define ACCEL_FCHOICE_ON 1
#define ACCEL_FCHOICE_OFF 0
typedef struct
{
int16_t ax, ay, az; // lsb

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

@@ -54,10 +54,10 @@ void imu_init()
i2c_write(ICM_ADDR, REG_BANK_SEL, 2 << 4);
// gyro ~2000 dps, FS_SEL = 3
i2c_write(ICM_ADDR, REG_GYRO_CONFIG_1, (3 << 1));
i2c_write(ICM_ADDR, REG_GYRO_CONFIG_1, GYRO_FS_SEL_2000 | GYRO_DLPFCFG_73 | GYRO_FCHOICE_ON);
// accel ~4g, FS_SEL = 1
i2c_write(ICM_ADDR, REG_ACCEL_CONFIG, (1 << 1));
i2c_write(ICM_ADDR, REG_ACCEL_CONFIG, ACCEL_FS_SEL_8 | ACCEL_DLPFCFG_69 | ACCEL_FCHOICE_ON);
// back to bank 0
i2c_write(ICM_ADDR, REG_BANK_SEL, ~(3 << 4));

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

@@ -20,13 +20,13 @@ static biquad_t gyro_z_lpf;
void imu_processing_init()
{
biquad_init_lpf(&accel_x_lpf, 30.0f, 500.0f);
biquad_init_lpf(&accel_y_lpf, 30.0f, 500.0f);
biquad_init_lpf(&accel_z_lpf, 30.0f, 500.0f);
biquad_init_lpf(&accel_x_lpf, 25.0f, 500.0f);
biquad_init_lpf(&accel_y_lpf, 25.0f, 500.0f);
biquad_init_lpf(&accel_z_lpf, 25.0f, 500.0f);
biquad_init_lpf(&gyro_x_lpf, 120.0f, 500.0f);
biquad_init_lpf(&gyro_y_lpf, 120.0f, 500.0f);
biquad_init_lpf(&gyro_z_lpf, 120.0f, 500.0f);
biquad_init_lpf(&gyro_x_lpf, 25.0f, 500.0f);
biquad_init_lpf(&gyro_y_lpf, 25.0f, 500.0f);
biquad_init_lpf(&gyro_z_lpf, 25.0f, 500.0f);
}
void imu_read_scaled(imu_scaled_t* out)