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

IMU improvements

This commit is contained in:
Benjamin Vedder 2021-08-22 11:51:03 +02:00
parent 86cbc92b8d
commit 91a19fa8b3
10 changed files with 39 additions and 115 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
appconf/appconf_default.h
View File

@ -560,17 +560,5 @@
#ifndef APPCONF_IMU_G_OFFSET_2
#define APPCONF_IMU_G_OFFSET_2 0.0
#endif
#ifndef APPCONF_IMU_G_OFFSET_COMP_FACT_0
#define APPCONF_IMU_G_OFFSET_COMP_FACT_0 0.0
#endif
#ifndef APPCONF_IMU_G_OFFSET_COMP_FACT_1
#define APPCONF_IMU_G_OFFSET_COMP_FACT_1 0.0
#endif
#ifndef APPCONF_IMU_G_OFFSET_COMP_FACT_2
#define APPCONF_IMU_G_OFFSET_COMP_FACT_2 0.0
#endif
#ifndef APPCONF_IMU_G_OFFSET_COMP_CLAMP
#define APPCONF_IMU_G_OFFSET_COMP_CLAMP 5.0
#endif
#endif /* APPCONF_DEFAULT_H_ */

2
conf_general.h
View File

@ -24,7 +24,7 @@
#define FW_VERSION_MAJOR 5
#define FW_VERSION_MINOR 03
// Set to 0 for building a release and iterate during beta test builds
#define FW_TEST_VERSION_NUMBER 49
#define FW_TEST_VERSION_NUMBER 50
#include "datatypes.h"

12
confgenerator.c
View File

@ -362,10 +362,6 @@ int32_t confgenerator_serialize_appconf(uint8_t *buffer, const app_configuration
buffer_append_float32_auto(buffer, conf->imu_conf.gyro_offsets[0], &ind);
buffer_append_float32_auto(buffer, conf->imu_conf.gyro_offsets[1], &ind);
buffer_append_float32_auto(buffer, conf->imu_conf.gyro_offsets[2], &ind);
buffer_append_float32_auto(buffer, conf->imu_conf.gyro_offset_comp_fact[0], &ind);
buffer_append_float32_auto(buffer, conf->imu_conf.gyro_offset_comp_fact[1], &ind);
buffer_append_float32_auto(buffer, conf->imu_conf.gyro_offset_comp_fact[2], &ind);
buffer_append_float32_auto(buffer, conf->imu_conf.gyro_offset_comp_clamp, &ind);
return ind;
}
@ -734,10 +730,6 @@ bool confgenerator_deserialize_appconf(const uint8_t *buffer, app_configuration
conf->imu_conf.gyro_offsets[0] = buffer_get_float32_auto(buffer, &ind);
conf->imu_conf.gyro_offsets[1] = buffer_get_float32_auto(buffer, &ind);
conf->imu_conf.gyro_offsets[2] = buffer_get_float32_auto(buffer, &ind);
conf->imu_conf.gyro_offset_comp_fact[0] = buffer_get_float32_auto(buffer, &ind);
conf->imu_conf.gyro_offset_comp_fact[1] = buffer_get_float32_auto(buffer, &ind);
conf->imu_conf.gyro_offset_comp_fact[2] = buffer_get_float32_auto(buffer, &ind);
conf->imu_conf.gyro_offset_comp_clamp = buffer_get_float32_auto(buffer, &ind);
return true;
}
@ -1090,8 +1082,4 @@ void confgenerator_set_defaults_appconf(app_configuration *conf) {
conf->imu_conf.gyro_offsets[0] = APPCONF_IMU_G_OFFSET_0;
conf->imu_conf.gyro_offsets[1] = APPCONF_IMU_G_OFFSET_1;
conf->imu_conf.gyro_offsets[2] = APPCONF_IMU_G_OFFSET_2;
conf->imu_conf.gyro_offset_comp_fact[0] = APPCONF_IMU_G_OFFSET_COMP_FACT_0;
conf->imu_conf.gyro_offset_comp_fact[1] = APPCONF_IMU_G_OFFSET_COMP_FACT_1;
conf->imu_conf.gyro_offset_comp_fact[2] = APPCONF_IMU_G_OFFSET_COMP_FACT_2;
conf->imu_conf.gyro_offset_comp_clamp = APPCONF_IMU_G_OFFSET_COMP_CLAMP;
}

2
confgenerator.h
View File

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

2
datatypes.h
View File

@ -821,8 +821,6 @@ typedef struct {
float rot_yaw;
float accel_offsets[3];
float gyro_offsets[3];
float gyro_offset_comp_fact[3];
float gyro_offset_comp_clamp;
} imu_config;
typedef struct {

62
imu/Fusion/FusionAhrs.c
View File

@ -41,6 +41,7 @@
#include "FusionAhrs.h"
#include <float.h> // FLT_MAX
#include <math.h> // atan2f, cosf, sinf
#include "utils.h"
//------------------------------------------------------------------------------
// Definitions
@ -53,20 +54,35 @@
//------------------------------------------------------------------------------
// Functions
static float calculateAccConfidence(float acc_confidence_decay, float accMag, float *accMagP);
static float calculateAccConfidence(float acc_confidence_decay, float accMag, float *accMagP) {
// G.K. Egan (C) computes confidence in accelerometers when
// aircraft is being accelerated over and above that due to gravity
float confidence;
accMag = *accMagP * 0.9f + accMag * 0.1f;
*accMagP = accMag;
confidence = 1.0 - (acc_confidence_decay * sqrtf(fabsf(accMag - 1.0f)));
utils_truncate_number(&confidence, 0.0, 1.0);
return confidence;
}
/**
* @brief Initialises the AHRS algorithm structure.
* @param fusionAhrs AHRS algorithm structure.
* @param gain AHRS algorithm gain.
*/
void FusionAhrsInitialise(FusionAhrs * const fusionAhrs, const float gain, const float initialGain) {
void FusionAhrsInitialise(FusionAhrs * const fusionAhrs, const float gain, const float acc_conf_decay) {
fusionAhrs->gain = gain;
fusionAhrs->initialGain = initialGain;
fusionAhrs->acc_conf_decay = acc_conf_decay;
fusionAhrs->minimumMagneticFieldSquared = 0.0f;
fusionAhrs->maximumMagneticFieldSquared = FLT_MAX;
fusionAhrs->quaternion = FUSION_QUATERNION_IDENTITY;
fusionAhrs->linearAcceleration = FUSION_VECTOR3_ZERO;
fusionAhrs->rampedGain = fusionAhrs->initialGain;
fusionAhrs->zeroYawPending = false;
}
/**
@ -78,8 +94,8 @@ void FusionAhrsSetGain(FusionAhrs * const fusionAhrs, const float gain) {
fusionAhrs->gain = gain;
}
void FusionAhrsSetInitialGain(FusionAhrs * const fusionAhrs, const float initialGain) {
fusionAhrs->initialGain = initialGain;
void FusionAhrsSetAccConfDecay(FusionAhrs * const fusionAhrs, const float acc_conf_decay) {
fusionAhrs->acc_conf_decay = acc_conf_decay;
}
/**
@ -142,15 +158,11 @@ void FusionAhrsUpdate(FusionAhrs * const fusionAhrs, const FusionVector3 gyrosco
} while (false);
// Ramp down gain until initialisation complete
if (fusionAhrs->gain == 0) {
fusionAhrs->rampedGain = 0; // skip initialisation if gain is zero
}
float feedbackGain = fusionAhrs->gain;
if (fusionAhrs->rampedGain > fusionAhrs->gain) {
fusionAhrs->rampedGain -= (fusionAhrs->initialGain - fusionAhrs->gain) * samplePeriod / INITIALISATION_PERIOD;
feedbackGain = fusionAhrs->rampedGain;
}
float accMag = sqrtf(SQ(accelerometer.axis.x) + SQ(accelerometer.axis.y) + SQ(accelerometer.axis.z));
float accelConfidence = calculateAccConfidence(fusionAhrs->acc_conf_decay, accMag, &fusionAhrs->accMagP);
feedbackGain *= accelConfidence;
// Convert gyroscope to radians per second scaled by 0.5
FusionVector3 halfGyroscope = FusionVectorMultiplyScalar(gyroscope, 0.5f * FusionDegreesToRadians(1.0f));
@ -185,19 +197,7 @@ void FusionAhrsUpdate(FusionAhrs * const fusionAhrs, const FusionVector3 gyrosco
* between the current and previous gyroscope measurements.
*/
void FusionAhrsUpdateWithoutMagnetometer(FusionAhrs * const fusionAhrs, const FusionVector3 gyroscope, const FusionVector3 accelerometer, const float samplePeriod) {
// Update AHRS algorithm
FusionAhrsUpdate(fusionAhrs, gyroscope, accelerometer, FUSION_VECTOR3_ZERO, samplePeriod);
// Zero yaw once initialisation complete
if (FusionAhrsIsInitialising(fusionAhrs) == true) {
fusionAhrs->zeroYawPending = true;
} else {
if (fusionAhrs->zeroYawPending == true) {
FusionAhrsSetYaw(fusionAhrs, 0.0f);
fusionAhrs->zeroYawPending = false;
}
}
}
/**
@ -252,16 +252,6 @@ FusionVector3 FusionAhrsGetEarthAcceleration(const FusionAhrs * const fusionAhrs
void FusionAhrsReinitialise(FusionAhrs * const fusionAhrs) {
fusionAhrs->quaternion = FUSION_QUATERNION_IDENTITY;
fusionAhrs->linearAcceleration = FUSION_VECTOR3_ZERO;
fusionAhrs->rampedGain = fusionAhrs->initialGain;
}
/**
* @brief Returns true while the AHRS algorithm is initialising.
* @param fusionAhrs AHRS algorithm structure.
* @return True while the AHRS algorithm is initialising.
*/
bool FusionAhrsIsInitialising(const FusionAhrs * const fusionAhrs) {
return fusionAhrs->rampedGain > fusionAhrs->gain;
}
/**

9
imu/Fusion/FusionAhrs.h
View File

@ -53,21 +53,20 @@
*/
typedef struct {
float gain;
float initialGain;
float acc_conf_decay;
float minimumMagneticFieldSquared;
float maximumMagneticFieldSquared;
FusionQuaternion quaternion; // describes the Earth relative to the sensor
FusionVector3 linearAcceleration;
float rampedGain;
bool zeroYawPending;
float accMagP;
} FusionAhrs;
//------------------------------------------------------------------------------
// Function prototypes
void FusionAhrsInitialise(FusionAhrs * const fusionAhrs, const float gain, const float initialGain);
void FusionAhrsInitialise(FusionAhrs * const fusionAhrs, const float gain, const float acc_conf_decay);
void FusionAhrsSetGain(FusionAhrs * const fusionAhrs, const float gain);
void FusionAhrsSetInitialGain(FusionAhrs * const fusionAhrs, const float initialGain);
void FusionAhrsSetAccConfDecay(FusionAhrs * const fusionAhrs, const float acc_conf_decay);
void FusionAhrsSetMagneticField(FusionAhrs * const fusionAhrs, const float minimumMagneticField, const float maximumMagneticField);
void FusionAhrsUpdate(FusionAhrs * const fusionAhrs, const FusionVector3 gyroscope, const FusionVector3 accelerometer, const FusionVector3 magnetometer, const float samplePeriod);
void FusionAhrsUpdateWithoutMagnetometer(FusionAhrs * const fusionAhrs, const FusionVector3 gyroscope, const FusionVector3 accelerometer, const float samplePeriod);

2
imu/Fusion/FusionTypes.h
View File

@ -143,7 +143,7 @@ static inline __attribute__((always_inline)) float FusionRadiansToDegrees(const
*/
static inline __attribute__((always_inline)) float FusionFastInverseSqrt(const float x) {
if (x == 0.0) {
return 0.0;
return 1.0;
}
return 1.0 / sqrtf(x);
}

49
imu/imu.c
View File

@ -43,13 +43,11 @@ static spi_bb_state m_spi_bb;
static ICM20948_STATE m_icm20948_state;
static BMI_STATE m_bmi_state;
static imu_config m_settings;
static float m_gyro_offset[3] = {0.0};
static systime_t init_time;
static bool imu_ready;
// Private functions
static void imu_read_callback(float *accel, float *gyro, float *mag);
static void terminal_gyro_info(int argc, const char **argv);
static void rotate(float *input, float *rotation, float *output);
int8_t user_i2c_read(uint8_t dev_addr, uint8_t reg_addr, uint8_t *data, uint16_t len);
int8_t user_i2c_write(uint8_t dev_addr, uint8_t reg_addr, uint8_t *data, uint16_t len);
@ -58,7 +56,6 @@ int8_t user_spi_write(uint8_t dev_id, uint8_t reg_addr, uint8_t *data, uint16_t
void imu_init(imu_config *set) {
m_settings = *set;
memset(m_gyro_offset, 0, sizeof(m_gyro_offset));
imu_stop();
imu_reset_orientation();
@ -112,19 +109,13 @@ void imu_init(imu_config *set) {
imu_init_bmi160_i2c(HW_I2C_SDA_PORT, HW_I2C_SDA_PIN,
HW_I2C_SCL_PORT, HW_I2C_SCL_PIN);
}
terminal_register_command_callback(
"imu_gyro_info",
"Print gyro offsets",
0,
terminal_gyro_info);
}
void imu_reset_orientation(void) {
imu_ready = false;
init_time = chVTGetSystemTimeX();
ahrs_init_attitude_info(&m_att);
FusionAhrsInitialise(&m_fusionAhrs, 2.0, 20.0);
FusionAhrsInitialise(&m_fusionAhrs, 10.0, 1.0);
ahrs_update_all_parameters(1.0, 10.0, 0.0, 2.0);
}
@ -298,9 +289,6 @@ void imu_get_calibration(float yaw, float *imu_cal) {
float backup_gyro_offset_x = m_settings.gyro_offsets[0];
float backup_gyro_offset_y = m_settings.gyro_offsets[1];
float backup_gyro_offset_z = m_settings.gyro_offsets[2];
float backup_gyro_comp_x = m_settings.gyro_offset_comp_fact[0];
float backup_gyro_comp_y = m_settings.gyro_offset_comp_fact[1];
float backup_gyro_comp_z = m_settings.gyro_offset_comp_fact[2];
// Override settings
m_settings.sample_rate_hz = 1000;
@ -315,14 +303,6 @@ void imu_get_calibration(float yaw, float *imu_cal) {
m_settings.gyro_offsets[0] = 0;
m_settings.gyro_offsets[1] = 0;
m_settings.gyro_offsets[2] = 0;
m_settings.gyro_offset_comp_fact[0] = 0;
m_settings.gyro_offset_comp_fact[1] = 0;
m_settings.gyro_offset_comp_fact[2] = 0;
// Clear computed offsets
m_gyro_offset[0] = 0;
m_gyro_offset[1] = 0;
m_gyro_offset[2] = 0;
// Sample gyro for offsets
float original_gyro_offsets[3] = {0, 0, 0};
@ -419,9 +399,6 @@ void imu_get_calibration(float yaw, float *imu_cal) {
m_settings.gyro_offsets[0] = backup_gyro_offset_x;
m_settings.gyro_offsets[1] = backup_gyro_offset_y;
m_settings.gyro_offsets[2] = backup_gyro_offset_z;
m_settings.gyro_offset_comp_fact[0] = backup_gyro_comp_x;
m_settings.gyro_offset_comp_fact[1] = backup_gyro_comp_y;
m_settings.gyro_offset_comp_fact[2] = backup_gyro_comp_z;
ahrs_init_attitude_info(&m_att);
FusionAhrsReinitialise(&m_fusionAhrs);
@ -429,8 +406,11 @@ void imu_get_calibration(float yaw, float *imu_cal) {
static void imu_read_callback(float *accel, float *gyro, float *mag) {
static uint32_t last_time = 0;
chSysLock();
float dt = timer_seconds_elapsed_since(last_time);
last_time = timer_time_now();
chSysUnlock();
if (!imu_ready && ST2MS(chVTGetSystemTimeX() - init_time) > 1000) {
ahrs_update_all_parameters(
@ -440,7 +420,7 @@ static void imu_read_callback(float *accel, float *gyro, float *mag) {
m_settings.madgwick_beta);
FusionAhrsSetGain(&m_fusionAhrs, m_settings.madgwick_beta);
FusionAhrsSetInitialGain(&m_fusionAhrs, m_settings.madgwick_beta * 10.0);
FusionAhrsSetAccConfDecay(&m_fusionAhrs, m_settings.accel_confidence_decay);
imu_ready = true;
}
@ -492,15 +472,6 @@ static void imu_read_callback(float *accel, float *gyro, float *mag) {
for (int i = 0; i < 3; i++) {
m_accel[i] -= m_settings.accel_offsets[i];
m_gyro[i] -= m_settings.gyro_offsets[i];
if (m_settings.gyro_offset_comp_fact[i] > 0.0) {
utils_step_towards(&m_gyro_offset[i], m_gyro[i], m_settings.gyro_offset_comp_fact[i] * dt);
utils_truncate_number_abs(&m_gyro_offset[i], m_settings.gyro_offset_comp_clamp);
} else {
m_gyro_offset[i] = 0.0;
}
m_gyro[i] -= m_gyro_offset[i];
}
float gyro_rad[3];
@ -535,16 +506,6 @@ static void imu_read_callback(float *accel, float *gyro, float *mag) {
}
}
static void terminal_gyro_info(int argc, const char **argv) {
(void)argc;
(void)argv;
commands_printf("Gyro offsets: [%.3f %.3f %.3f]\n",
(double)(m_settings.gyro_offsets[0] + m_gyro_offset[0]),
(double)(m_settings.gyro_offsets[1] + m_gyro_offset[1]),
(double)(m_settings.gyro_offsets[2] + m_gyro_offset[2]));
}
void rotate(float *input, float *rotation, float *output) {
// Rotate imu offsets to match
float s1 = sinf(rotation[2] * M_PI / 180.0);

2
timer.c
View File

@ -23,7 +23,7 @@
#include "stm32f4xx_conf.h"
// Settings
#define TIMER_HZ 1e7
#define TIMER_HZ 1.4e7
void timer_init(void) {
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM5, ENABLE);