Support Sparkfun ICM20948 Breakout board (SPI only, I2C TBD)

src/ahr/ahr.cpp

@@ -25,6 +25,7 @@ SOFTWARE.
 #define MF_MOD "AHR"
 
 #include <Arduino.h> //Serial
+#include <math.h>
 #include "ahr.h"
 #include "AhrGizmoMahony.h"
 #include "AhrGizmoMadgwick.h"
@@ -43,9 +44,20 @@ int Ahr::setup() {
   cfg.printModule(MF_MOD);
 
   B_gyr = lowpass_to_beta(config.gyrLpFreq, config.pimu->getSampleRate());
+  if (isnan(B_gyr)) {
+    // imu might not be set up yet
+    B_gyr = 1.0;
+  }
   B_acc = lowpass_to_beta(config.accLpFreq, config.pimu->getSampleRate());
+  if (isnan(B_acc)) {
+    // imu might not be set up yet
+    B_acc = 1.0;
+  }
   B_mag = lowpass_to_beta(config.magLpFreq, config.pimu->getSampleRate());
-
+  if (isnan(B_mag)) {
+    // imu might not be set up yet
+    B_mag = 1.0;
+  }
   //create gizmo
   delete gizmo;
   switch(config.gizmo) {

src/cfg/cfg.h

@@ -211,6 +211,7 @@ SOFTWARE.
   MF_PARAM( imu_rate,       1000, float, 'f') /*IMU sample rate in Hz (default 1000) NOTE: not all IMU drivers support a different rate*/ \
   MF_PARAM( imu_acc_lp,       70, float, 'f') /*Accelerometer Low Pass Filter cutoff frequency in Hz */ \
   MF_PARAM( imu_gyr_lp,       60, float, 'f') /*Gyro Low Pass Filter cutoff frequency in Hz */ \
+  MF_PARAM( imu_has_mag,      -1, int32_t, 'i') /*whether the IMU has/should use its magnetometer, <= 0: no, 1: yes*/ \
 \
   /*LED*/ \
   MF_PARAM( led_on,            0, int32_t, 'e', mf_LOW_IS_ON,mf_HIGH_IS_ON) \

src/imu/ImuGizmoICM20948.cpp

@@ -30,10 +30,10 @@ uint16_t ImuGizmoICM20948::_convertGyroScale(uint16_t range) {
     return ret;
 }
 
-ImuGizmoICM20948::ImuGizmoICM20948(SPIClass *spi, const uint8_t csPin, const uint8_t intPin)
+ImuGizmoICM20948::ImuGizmoICM20948(SPIClass *spi, const uint8_t csPin, const uint8_t intPin, const bool has_mag)
     : _spi(spi), _csPin(csPin), _interrupt_pin(intPin) {
     _wrapped_imu = new ICM_20948_SPI();
-    has_mag = true;
+    this->has_mag = has_mag;
     uses_i2c = false;
     has_sensor_fusion = true;
     interrupt_has_rising_edge = false;

src/imu/ImuGizmoICM20948.h

@@ -50,7 +50,7 @@ class ImuGizmoICM20948 : public ImuGizmo {
     }
 
 public:
-    ImuGizmoICM20948(SPIClass *spi, const uint8_t csPin, const uint8_t intPin);
+    ImuGizmoICM20948(SPIClass *spi, const uint8_t csPin, const uint8_t intPin, const bool has_mag);
     ~ImuGizmoICM20948();
 
     int begin(int gyro_scale_dps, int acc_scale_g, int rate_hz);

src/imu/imu.h

@@ -38,6 +38,7 @@ struct ImuConfig {
     MF_I2C *i2c_bus = nullptr; //i2c bus (only used if spi_bus == nullptr)
     uint8_t i2c_adr = 0; //i2c address. 0=default address
     bool uses_i2c = false; //use I2C bus?
+    bool has_mag = false; // has magnetometer
 };
 
 //imu sample data (raw, uncorrected and unfiltered) 

src/imu/imu_cpp.h

@@ -118,42 +118,43 @@ int Imu::setup() {
         auto mpu_iface = new MPU_InterfaceSPI(config.spi_bus, config.spi_cs);
         gizmo = new MPUXXXX(MPUXXXX::MPU9250, mpu_iface);
         gizmo->uses_i2c = false;
-        gizmo->has_mag = true;
+        gizmo->has_mag = true || config.has_mag > 0;
         break;
       }
       case Cfg::imu_gizmo_enum::mf_MPU6500 : {
         auto mpu_iface = new MPU_InterfaceSPI(config.spi_bus, config.spi_cs);
         gizmo = new MPUXXXX(MPUXXXX::MPU6500, mpu_iface);
         gizmo->uses_i2c = false;
-        gizmo->has_mag = false;
+        gizmo->has_mag = false || config.has_mag > 0;
         break;
       }
       case Cfg::imu_gizmo_enum::mf_MPU6000 : {
         auto mpu_iface = new MPU_InterfaceSPI(config.spi_bus, config.spi_cs);
         gizmo = new MPUXXXX(MPUXXXX::MPU6000, mpu_iface);
         gizmo->uses_i2c = false;
-        gizmo->has_mag = false;
+        gizmo->has_mag = false || config.has_mag > 0;
         break;
       }
       case Cfg::imu_gizmo_enum::mf_BMI270 : {
         gizmo = new ImuGizmoBMI270(config.spi_bus, config.spi_cs);
         gizmo->uses_i2c = false;
-        gizmo->has_mag = false;
+        gizmo->has_mag = false || config.has_mag > 0;
         break;
       }
       case Cfg::imu_gizmo_enum::mf_ICM45686 : {
         auto icm_iface = new Invensensev3_InterfaceSPI(config.spi_bus, config.spi_cs);
         gizmo = new ImuGizmoICM45686( (uint8_t)config.pin_int, icm_iface );
         gizmo->uses_i2c = false;
-        gizmo->has_mag = false;
+        gizmo->has_mag = false || config.has_mag > 0;
         break;
       }
       case Cfg::imu_gizmo_enum::mf_ICM42688 : {
         gizmo = ImuGizmoICM426XX::create(&config, (ImuState*)this);
+        gizmo->has_mag = gizmo->has_mag || config.has_mag > 0;
         break;
       }
       case Cfg::imu_gizmo_enum::mf_ICM20948 : {
-        gizmo = new ImuGizmoICM20948(config.spi_bus, config.spi_cs, config.pin_int);
+        gizmo = new ImuGizmoICM20948(config.spi_bus, config.spi_cs, config.pin_int, config.has_mag);
         break;
       }
       default: {

src/madflight.h

@@ -228,7 +228,7 @@ void madflight_setup() {
   imu.config.i2c_bus = hal_get_i2c_bus(cfg.imu_i2c_bus); //I2C bus (only used if spi_bus==nullptr)
   imu.config.i2c_adr = cfg.imu_i2c_adr; //i2c address. 0=default address
   imu.config.uses_i2c = ((Cfg::imu_bus_type_enum)cfg.imu_bus_type == Cfg::imu_bus_type_enum::mf_I2C);
-
+  imu.config.has_mag = cfg.imu_has_mag > 0;
   // Some sensors need a couple of tries...
   int tries = 10;
   while(true) {