mirror of https://github.com/rusefi/bldc.git
commit
b03a848049
|
@ -563,6 +563,9 @@
|
||||||
#ifndef APPCONF_IMU_ACCEL_LOWPASS_FILTER_Z
|
#ifndef APPCONF_IMU_ACCEL_LOWPASS_FILTER_Z
|
||||||
#define APPCONF_IMU_ACCEL_LOWPASS_FILTER_Z 0.0
|
#define APPCONF_IMU_ACCEL_LOWPASS_FILTER_Z 0.0
|
||||||
#endif
|
#endif
|
||||||
|
#ifndef APPCONF_IMU_GYRO_LOWPASS_FILTER
|
||||||
|
#define APPCONF_IMU_GYRO_LOWPASS_FILTER 0.0
|
||||||
|
#endif
|
||||||
#ifndef APPCONF_IMU_SAMPLE_RATE_HZ
|
#ifndef APPCONF_IMU_SAMPLE_RATE_HZ
|
||||||
#define APPCONF_IMU_SAMPLE_RATE_HZ 200
|
#define APPCONF_IMU_SAMPLE_RATE_HZ 200
|
||||||
#endif
|
#endif
|
||||||
|
|
|
@ -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_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_y, 1, &ind);
|
||||||
buffer_append_float16(buffer, conf->imu_conf.accel_lowpass_filter_z, 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_append_uint16(buffer, conf->imu_conf.sample_rate_hz, &ind);
|
||||||
buffer[ind++] = conf->imu_conf.use_magnetometer;
|
buffer[ind++] = conf->imu_conf.use_magnetometer;
|
||||||
buffer_append_float32_auto(buffer, conf->imu_conf.accel_confidence_decay, &ind);
|
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_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_y = buffer_get_float16(buffer, 1, &ind);
|
||||||
conf->imu_conf.accel_lowpass_filter_z = 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.sample_rate_hz = buffer_get_uint16(buffer, &ind);
|
||||||
conf->imu_conf.use_magnetometer = buffer[ind++];
|
conf->imu_conf.use_magnetometer = buffer[ind++];
|
||||||
conf->imu_conf.accel_confidence_decay = buffer_get_float32_auto(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_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_y = APPCONF_IMU_ACCEL_LOWPASS_FILTER_Y;
|
||||||
conf->imu_conf.accel_lowpass_filter_z = APPCONF_IMU_ACCEL_LOWPASS_FILTER_Z;
|
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.sample_rate_hz = APPCONF_IMU_SAMPLE_RATE_HZ;
|
||||||
conf->imu_conf.use_magnetometer = APPCONF_IMU_USE_MAGNETOMETER;
|
conf->imu_conf.use_magnetometer = APPCONF_IMU_USE_MAGNETOMETER;
|
||||||
conf->imu_conf.accel_confidence_decay = APPCONF_IMU_ACCEL_CONFIDENCE_DECAY;
|
conf->imu_conf.accel_confidence_decay = APPCONF_IMU_ACCEL_CONFIDENCE_DECAY;
|
||||||
|
|
|
@ -9,7 +9,7 @@
|
||||||
|
|
||||||
// Constants
|
// Constants
|
||||||
#define MCCONF_SIGNATURE 347304876
|
#define MCCONF_SIGNATURE 347304876
|
||||||
#define APPCONF_SIGNATURE 3392562538
|
#define APPCONF_SIGNATURE 486554156
|
||||||
|
|
||||||
// Functions
|
// Functions
|
||||||
int32_t confgenerator_serialize_mcconf(uint8_t *buffer, const mc_configuration *conf);
|
int32_t confgenerator_serialize_mcconf(uint8_t *buffer, const mc_configuration *conf);
|
||||||
|
|
|
@ -873,6 +873,7 @@ typedef struct {
|
||||||
float accel_lowpass_filter_x;
|
float accel_lowpass_filter_x;
|
||||||
float accel_lowpass_filter_y;
|
float accel_lowpass_filter_y;
|
||||||
float accel_lowpass_filter_z;
|
float accel_lowpass_filter_z;
|
||||||
|
float gyro_lowpass_filter;
|
||||||
int sample_rate_hz;
|
int sample_rate_hz;
|
||||||
bool use_magnetometer;
|
bool use_magnetometer;
|
||||||
float accel_confidence_decay;
|
float accel_confidence_decay;
|
||||||
|
|
38
imu/imu.c
38
imu/imu.c
|
@ -46,7 +46,7 @@ static BMI_STATE m_bmi_state;
|
||||||
static imu_config m_settings;
|
static imu_config m_settings;
|
||||||
static systime_t init_time;
|
static systime_t init_time;
|
||||||
static bool imu_ready;
|
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";
|
static char *m_imu_type_internal = "Unknown";
|
||||||
|
|
||||||
// Private functions
|
// Private functions
|
||||||
|
@ -63,7 +63,7 @@ void imu_init(imu_config *set) {
|
||||||
|
|
||||||
m_settings = *set;
|
m_settings = *set;
|
||||||
|
|
||||||
// Acc biquad filter
|
//Biquad filters
|
||||||
float fc;
|
float fc;
|
||||||
if(m_settings.accel_lowpass_filter_x > 0){
|
if(m_settings.accel_lowpass_filter_x > 0){
|
||||||
fc = m_settings.accel_lowpass_filter_x / m_settings.sample_rate_hz;
|
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;
|
fc = m_settings.accel_lowpass_filter_z / m_settings.sample_rate_hz;
|
||||||
biquad_config(&acc_z_biquad, BQ_LOWPASS, fc);
|
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;
|
imu_ready = false;
|
||||||
|
|
||||||
|
@ -557,22 +564,27 @@ static void imu_read_callback(float *accel, float *gyro, float *mag) {
|
||||||
m_gyro[i] -= m_settings.gyro_offsets[i];
|
m_gyro[i] -= m_settings.gyro_offsets[i];
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// Apply filters
|
||||||
|
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){
|
||||||
|
m_accel[1] = biquad_process(&acc_y_biquad, m_accel[1]);
|
||||||
|
}
|
||||||
|
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];
|
float gyro_rad[3];
|
||||||
gyro_rad[0] = DEG2RAD_f(m_gyro[0]);
|
gyro_rad[0] = DEG2RAD_f(m_gyro[0]);
|
||||||
gyro_rad[1] = DEG2RAD_f(m_gyro[1]);
|
gyro_rad[1] = DEG2RAD_f(m_gyro[1]);
|
||||||
gyro_rad[2] = DEG2RAD_f(m_gyro[2]);
|
gyro_rad[2] = DEG2RAD_f(m_gyro[2]);
|
||||||
|
|
||||||
// Apply filters
|
|
||||||
if(m_settings.accel_lowpass_filter_x > 0){ // Acc biquad filter
|
|
||||||
m_accel[0] = biquad_process(&acc_x_biquad, m_accel[0]);
|
|
||||||
}
|
|
||||||
if(m_settings.accel_lowpass_filter_y > 0){ // Acc biquad filter
|
|
||||||
m_accel[1] = biquad_process(&acc_y_biquad, m_accel[1]);
|
|
||||||
}
|
|
||||||
if(m_settings.accel_lowpass_filter_z > 0){ // Acc biquad filter
|
|
||||||
m_accel[2] = biquad_process(&acc_z_biquad, m_accel[2]);
|
|
||||||
}
|
|
||||||
|
|
||||||
switch (m_settings.mode) {
|
switch (m_settings.mode) {
|
||||||
case AHRS_MODE_MADGWICK:
|
case AHRS_MODE_MADGWICK:
|
||||||
ahrs_update_madgwick_imu(gyro_rad, m_accel, dt, (ATTITUDE_INFO *)&m_att);
|
ahrs_update_madgwick_imu(gyro_rad, m_accel, dt, (ATTITUDE_INFO *)&m_att);
|
||||||
|
|
Loading…
Reference in New Issue