diff --git a/applications/appconf_default.h b/applications/appconf_default.h index 7368d729..728464ee 100755 --- a/applications/appconf_default.h +++ b/applications/appconf_default.h @@ -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 diff --git a/confgenerator.c b/confgenerator.c index 57b381d0..aecb9c3f 100644 --- a/confgenerator.c +++ b/confgenerator.c @@ -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; diff --git a/confgenerator.h b/confgenerator.h index 68242a04..49153bbb 100644 --- a/confgenerator.h +++ b/confgenerator.h @@ -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); diff --git a/datatypes.h b/datatypes.h index a677138b..8d363a4c 100644 --- a/datatypes.h +++ b/datatypes.h @@ -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; diff --git a/imu/imu.c b/imu/imu.c index ddfdd6bd..78c9bf6c 100644 --- a/imu/imu.c +++ b/imu/imu.c @@ -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: