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

src/ahr/ahr.cpp

@@ -44,20 +44,9 @@ 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) {
@@ -75,6 +64,10 @@ int Ahr::setup() {
       break;
     case Cfg::ahr_gizmo_enum::mf_IMU :
       gizmo = new AhrGizmoImu(this);
+      if (!config.pimu->hasSensorFusion()) {
+        Serial.printf("Cannot use mf_IMU for AHR with an IMU that does not support sensor fusion!\n");
+        return -1;
+      }
       break;
     default:
       gizmo = new AhrGizmoMahony(this, false);

src/imu/imu.h

@@ -87,6 +87,7 @@ class Imu : public ImuState {
     ImuGizmo *gizmo = nullptr;
 
     int setup(); // Use config to setup gizmo, returns 0 on success, or error code
+    int setup_interrupt(); // set up the low level interrupt handler
     bool update(); // Returns true if state was updated
     bool installed() {return (gizmo != nullptr); } //returns true if a gizmo is installed
 

src/imu/imu_cpp.h

@@ -118,39 +118,39 @@ 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 || config.has_mag > 0;
+        gizmo->has_mag = true || config.has_mag;
         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 || config.has_mag > 0;
+        gizmo->has_mag = false || config.has_mag;
         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 || config.has_mag > 0;
+        gizmo->has_mag = false || config.has_mag;
         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 || config.has_mag > 0;
+        gizmo->has_mag = false || config.has_mag;
         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 || config.has_mag > 0;
+        gizmo->has_mag = false || config.has_mag;
         break;
       }
       case Cfg::imu_gizmo_enum::mf_ICM42688 : {
         gizmo = ImuGizmoICM426XX::create(&config, (ImuState*)this);
-        gizmo->has_mag = gizmo->has_mag || config.has_mag > 0;
+        gizmo->has_mag = gizmo->has_mag || config.has_mag;
         break;
       }
       case Cfg::imu_gizmo_enum::mf_ICM20948 : {
@@ -227,6 +227,10 @@ int Imu::setup() {
   //exit if no interrupt pin specified
   if(config.pin_int < 0) return 0;
 
+  return 0;
+}
+
+int Imu::setup_interrupt() {
   //==============================================================
   //setup low-level interrupt stuff
   //==============================================================

src/madflight.h

@@ -206,19 +206,6 @@ void madflight_setup() {
   // ALT - Altitude Estimator
   alt.setup(bar.alt);
 
-  // AHR - setup low pass filters for AHRS filters
-  ahr.config.gizmo = (Cfg::ahr_gizmo_enum)cfg.ahr_gizmo; //the gizmo to use
-  ahr.config.gyrLpFreq = cfg.imu_gyr_lp; //gyro low pass filter freq [Hz]
-  ahr.config.accLpFreq = cfg.imu_acc_lp; //accelerometer low pass filter freq [Hz]
-  ahr.config.magLpFreq = cfg.mag_lp; //magnetometer low pass filter freq [Hz]
-  ahr.config.pimu = &imu; //pointer to Imu to use
-  ahr.config.pmag = &mag; //pointer to Mag to use
-  ahr.config.gyr_offset = &(cfg.imu_cal_gx); //gyro offset[3] [deg/sec]
-  ahr.config.acc_offset = &(cfg.imu_cal_ax); //acc offset[3] [G]
-  ahr.config.mag_offset = &(cfg.mag_cal_x); //mag offset[3] [adc_lsb]
-  ahr.config.mag_scale = &(cfg.mag_cal_sx); //mag scale[3] [uT/adc_lsb]
-  ahr.setup();
-
   // IMU - Intertial Measurement Unit (gyro/acc/mag)
   imu.config.sampleRate = cfg.imu_rate; //sample rate [Hz]
   imu.config.pin_int = cfg.pin_imu_int; //IMU data ready interrupt pin
@@ -242,6 +229,24 @@ void madflight_setup() {
   }
   // start IMU update handler
   if(imu.installed()) {
+    // AHR - setup low pass filters for AHRS filters after IMU is set up and installed
+    ahr.config.gizmo = (Cfg::ahr_gizmo_enum)cfg.ahr_gizmo; //the gizmo to use
+    ahr.config.gyrLpFreq = cfg.imu_gyr_lp; //gyro low pass filter freq [Hz]
+    ahr.config.accLpFreq = cfg.imu_acc_lp; //accelerometer low pass filter freq [Hz]
+    ahr.config.magLpFreq = cfg.mag_lp; //magnetometer low pass filter freq [Hz]
+    ahr.config.pimu = &imu; //pointer to Imu to use
+    ahr.config.pmag = &mag; //pointer to Mag to use
+    ahr.config.gyr_offset = &(cfg.imu_cal_gx); //gyro offset[3] [deg/sec]
+    ahr.config.acc_offset = &(cfg.imu_cal_ax); //acc offset[3] [G]
+    ahr.config.mag_offset = &(cfg.mag_cal_x); //mag offset[3] [adc_lsb]
+    ahr.config.mag_scale = &(cfg.mag_cal_sx); //mag scale[3] [uT/adc_lsb]
+    if (ahr.setup() < 0) {
+      madflight_die("AHR setup failed.");
+    }
+
+    // now set up the interrupt handler after IMU and AHR are set up
+    imu.setup_interrupt();
+
     ahr.setInitalOrientation(); //do this before IMU update handler is started
 
     if(!imu_loop) madflight_warn("'void imu_loop()' not defined.");