rusefi
/
bldc
mirror of https://github.com/rusefi/bldc.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Add Gyro filter

This commit is contained in:
Mitch Lustig 2022-11-15 01:47:52 -08:00
parent ea6cd46b94
commit 00c03fa5bb
5 changed files with 30 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
applications/appconf_default.h
View File

@ -563,6 +563,9 @@
#ifndef APPCONF_IMU_ACCEL_LOWPASS_FILTER_Z
#define APPCONF_IMU_ACCEL_LOWPASS_FILTER_Z 0.0
#endif
#ifndef APPCONF_IMU_GYRO_LOWPASS_FILTER
#define APPCONF_IMU_GYRO_LOWPASS_FILTER 0.0
#endif
#ifndef APPCONF_IMU_SAMPLE_RATE_HZ
#define APPCONF_IMU_SAMPLE_RATE_HZ 200
#endif

3
confgenerator.c
View File

@ -377,6 +377,7 @@ int32_t confgenerator_serialize_appconf(uint8_t *buffer, const app_configuration
buffer_append_float16(buffer, conf->imu_conf.accel_lowpass_filter_x, 1, &ind);
buffer_append_float16(buffer, conf->imu_conf.accel_lowpass_filter_y, 1, &ind);
buffer_append_float16(buffer, conf->imu_conf.accel_lowpass_filter_z, 1, &ind);
buffer_append_float16(buffer, conf->imu_conf.gyro_lowpass_filter, 1, &ind);
buffer_append_uint16(buffer, conf->imu_conf.sample_rate_hz, &ind);
buffer[ind++] = conf->imu_conf.use_magnetometer;
buffer_append_float32_auto(buffer, conf->imu_conf.accel_confidence_decay, &ind);
@ -774,6 +775,7 @@ bool confgenerator_deserialize_appconf(const uint8_t *buffer, app_configuration
conf->imu_conf.accel_lowpass_filter_x = buffer_get_float16(buffer, 1, &ind);
conf->imu_conf.accel_lowpass_filter_y = buffer_get_float16(buffer, 1, &ind);
conf->imu_conf.accel_lowpass_filter_z = buffer_get_float16(buffer, 1, &ind);
conf->imu_conf.gyro_lowpass_filter = buffer_get_float16(buffer, 1, &ind);
conf->imu_conf.sample_rate_hz = buffer_get_uint16(buffer, &ind);
conf->imu_conf.use_magnetometer = buffer[ind++];
conf->imu_conf.accel_confidence_decay = buffer_get_float32_auto(buffer, &ind);
@ -1155,6 +1157,7 @@ void confgenerator_set_defaults_appconf(app_configuration *conf) {
conf->imu_conf.accel_lowpass_filter_x = APPCONF_IMU_ACCEL_LOWPASS_FILTER_X;
conf->imu_conf.accel_lowpass_filter_y = APPCONF_IMU_ACCEL_LOWPASS_FILTER_Y;
conf->imu_conf.accel_lowpass_filter_z = APPCONF_IMU_ACCEL_LOWPASS_FILTER_Z;
conf->imu_conf.gyro_lowpass_filter = APPCONF_IMU_GYRO_LOWPASS_FILTER;
conf->imu_conf.sample_rate_hz = APPCONF_IMU_SAMPLE_RATE_HZ;
conf->imu_conf.use_magnetometer = APPCONF_IMU_USE_MAGNETOMETER;
conf->imu_conf.accel_confidence_decay = APPCONF_IMU_ACCEL_CONFIDENCE_DECAY;

2
confgenerator.h
View File

@ -9,7 +9,7 @@
// Constants
#define MCCONF_SIGNATURE 347304876
#define APPCONF_SIGNATURE 3392562538
#define APPCONF_SIGNATURE 486554156
// Functions
int32_t confgenerator_serialize_mcconf(uint8_t *buffer, const mc_configuration *conf);

1
datatypes.h
View File

@ -873,6 +873,7 @@ typedef struct {
float accel_lowpass_filter_x;
float accel_lowpass_filter_y;
float accel_lowpass_filter_z;
float gyro_lowpass_filter;
int sample_rate_hz;
bool use_magnetometer;
float accel_confidence_decay;

32
imu/imu.c
View File

@ -46,7 +46,7 @@ static BMI_STATE m_bmi_state;
static imu_config m_settings;
static systime_t init_time;
static bool imu_ready;
static Biquad acc_x_biquad, acc_y_biquad, acc_z_biquad;
static Biquad acc_x_biquad, acc_y_biquad, acc_z_biquad, gyro_x_biquad, gyro_y_biquad, gyro_z_biquad;
static char *m_imu_type_internal = "Unknown";
// Private functions
@ -63,7 +63,7 @@ void imu_init(imu_config *set) {
m_settings = *set;
// Acc biquad filter
//Biquad filters
float fc;
if(m_settings.accel_lowpass_filter_x > 0){
fc = m_settings.accel_lowpass_filter_x / m_settings.sample_rate_hz;
@ -77,6 +77,13 @@ void imu_init(imu_config *set) {
fc = m_settings.accel_lowpass_filter_z / m_settings.sample_rate_hz;
biquad_config(&acc_z_biquad, BQ_LOWPASS, fc);
}
if(m_settings.gyro_lowpass_filter > 0){
fc = m_settings.gyro_lowpass_filter / m_settings.sample_rate_hz;
biquad_config(&gyro_x_biquad, BQ_LOWPASS, fc);
biquad_config(&gyro_y_biquad, BQ_LOWPASS, fc);
biquad_config(&gyro_z_biquad, BQ_LOWPASS, fc);
}
imu_ready = false;
@ -557,21 +564,26 @@ static void imu_read_callback(float *accel, float *gyro, float *mag) {
m_gyro[i] -= m_settings.gyro_offsets[i];
}
float gyro_rad[3];
gyro_rad[0] = DEG2RAD_f(m_gyro[0]);
gyro_rad[1] = DEG2RAD_f(m_gyro[1]);
gyro_rad[2] = DEG2RAD_f(m_gyro[2]);
// Apply filters
if(m_settings.accel_lowpass_filter_x > 0){ // Acc biquad filter
if(m_settings.accel_lowpass_filter_x > 0){
m_accel[0] = biquad_process(&acc_x_biquad, m_accel[0]);
}
if(m_settings.accel_lowpass_filter_y > 0){ // Acc biquad filter
if(m_settings.accel_lowpass_filter_y > 0){
m_accel[1] = biquad_process(&acc_y_biquad, m_accel[1]);
}
if(m_settings.accel_lowpass_filter_z > 0){ // Acc biquad filter
if(m_settings.accel_lowpass_filter_z > 0){
m_accel[2] = biquad_process(&acc_z_biquad, m_accel[2]);
}
if(m_settings.gyro_lowpass_filter > 0){
m_gyro[0] = biquad_process(&gyro_x_biquad, m_gyro[0]);
m_gyro[1] = biquad_process(&gyro_y_biquad, m_gyro[1]);
m_gyro[2] = biquad_process(&gyro_z_biquad, m_gyro[2]);
}
float gyro_rad[3];
gyro_rad[0] = DEG2RAD_f(m_gyro[0]);
gyro_rad[1] = DEG2RAD_f(m_gyro[1]);
gyro_rad[2] = DEG2RAD_f(m_gyro[2]);
switch (m_settings.mode) {
case AHRS_MODE_MADGWICK: