IMU improvements

Author: vedderb

appconf/appconf_default.h

@@ -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_ */

conf_general.h

@@ -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"
 

confgenerator.c

@@ -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;
 }

confgenerator.h

@@ -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);

datatypes.h

@@ -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 {

imu/Fusion/FusionAhrs.c

@@ -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;
 }
 
 /**

imu/Fusion/FusionAhrs.h

@@ -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);

imu/Fusion/FusionTypes.h

@@ -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);
 }

imu/imu.c

@@ -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,18 +399,18 @@ 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);
 }
 
 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);

timer.c

@@ -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);