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

src/ahr/AhrGizmoImu.h

@@ -0,0 +1,42 @@
+#pragma once
+
+#include "ahr.h"
+
+class AhrGizmoImu : public AhrGizmo {
+private:
+  AhrConfig *config;
+  AhrState *state;
+
+  //NED reference frame
+  //gyro in rad/sec
+  //acc in g
+  //mag any unit of measurement
+
+  // quaternion of sensor frame relative to auxiliary frame
+  float q0 = 1.0f; //Initialize quaternion
+  float q1 = 0.0f;
+  float q2 = 0.0f;
+  float q3 = 0.0f;
+
+  public:
+  AhrGizmoImu(Ahr *ahr) {
+    this->config = &(ahr->config);
+    this->state = (AhrState*)ahr;
+  }
+
+  void setInitalOrientation(float *qnew) {
+    q0 = qnew[0];
+    q1 = qnew[1];
+    q2 = qnew[2];
+    q3 = qnew[3];
+  }
+
+  bool update() {
+    // just copy the quaternions from the IMU
+    state->q[0] = config->pimu->q[0];
+    state->q[1] = config->pimu->q[1];
+    state->q[2] = config->pimu->q[2];
+    state->q[3] = config->pimu->q[3];
+    return true;
+  }
+};

src/ahr/ahr.cpp

@@ -25,10 +25,12 @@ SOFTWARE.
 #define MF_MOD "AHR"
 
 #include <Arduino.h> //Serial
+#include <math.h>
 #include "ahr.h"
 #include "AhrGizmoMahony.h"
 #include "AhrGizmoMadgwick.h"
 #include "AhrGizmoVqf.h"
+#include "AhrGizmoImu.h"
 
 #include "../mag/mag.h"
 #include "../imu/imu.h"
@@ -60,6 +62,13 @@ int Ahr::setup() {
     case Cfg::ahr_gizmo_enum::mf_VQF :
       gizmo = new AhrGizmoVqf(this);
       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);
       break;
@@ -121,9 +130,28 @@ bool Ahr::update() {
   //call gizmo to update q
   gizmo->update();
 
+  // Apply q_bias to correct the IMU quaternion if using sensor fusion
+  if (config.pimu->hasSensorFusion()) {
+    // Apply q_bias to correct the IMU quaternion
+    float qb0 = cfg.imu_cal_q0_bias;
+    float qb1 = cfg.imu_cal_q1_bias;
+    float qb2 = cfg.imu_cal_q2_bias;
+    float qb3 = cfg.imu_cal_q3_bias;
+
+    float q0 = q[0];
+    float q1 = q[1];
+    float q2 = q[2];
+    float q3 = q[3];
+
+    // q = q_bias * q
+    q[0] = qb0*q0 - qb1*q1 - qb2*q2 - qb3*q3;
+    q[1] = qb0*q1 + qb1*q0 + qb2*q3 - qb3*q2;
+    q[2] = qb0*q2 - qb1*q3 + qb2*q0 + qb3*q1;
+    q[3] = qb0*q3 + qb1*q2 - qb2*q1 + qb3*q0;
+  }
+
   //update euler angles
   computeAngles();
-
   return true;
 }
 
@@ -167,9 +195,10 @@ void Ahr::getQFromMag(float *q) {
 
 //compute euler angles from q
 void Ahr::computeAngles() {
-  roll = atan2(q[0]*q[1] + q[2]*q[3], 0.5f - q[1]*q[1] - q[2]*q[2]) * rad_to_deg; //degrees - roll right is positive
-  pitch = asin(constrain(-2.0f * (q[1]*q[3] - q[0]*q[2]), -1.0, 1.0)) * rad_to_deg; //degrees - pitch up is positive - use constrain() to prevent NaN due to rounding
-  yaw = atan2(q[1]*q[2] + q[0]*q[3], 0.5f - q[2]*q[2] - q[3]*q[3]) * rad_to_deg; //degrees - yaw right is positive
+  // standard ZYX (yaw-pitch-roll) from quaternion
+  roll  = atan2(2.0f * (q[0]*q[1] + q[2]*q[3]), 1.0f - 2.0f * (q[1]*q[1] + q[2]*q[2])) * rad_to_deg;
+  pitch = asin (constrain(2.0f * (q[0]*q[2] - q[3]*q[1]), -1.0f, 1.0f)) * rad_to_deg;
+  yaw = atan2(2.0f * (q[0]*q[3] + q[1]*q[2]), 1.0f - 2.0f * (q[2]*q[2] + q[3]*q[3])) * rad_to_deg;
 }
 
 //get acceleration in earth-frame up direction in [m/s^2]

src/cfg/cfg.h

@@ -180,7 +180,7 @@ SOFTWARE.
   MF_PARAM( pin_rcl_ppm,      -1, int32_t, 'p') \
 \
   /*AHR - AHRS*/ \
-  MF_PARAM( ahr_gizmo,         0, int32_t, 'e', mf_MAHONY,mf_MAHONY_BF,mf_MADGWICK,mf_VQF) \
+  MF_PARAM( ahr_gizmo,         0, int32_t, 'e', mf_MAHONY,mf_MAHONY_BF,mf_MADGWICK,mf_VQF,mf_IMU) \
 \
   /*BAR - Barometer*/ \
   MF_PARAM( bar_gizmo,         0, int32_t, 'e', mf_NONE,mf_BMP280,mf_BMP388,mf_BMP390,mf_MS5611,mf_HP203B) \
@@ -203,7 +203,7 @@ SOFTWARE.
   MF_PARAM( gps_baud,          0, int32_t, 'i') \
 \
   /*IMU - Inertial Measurement Unit (acc/gyro)*/ \
-  MF_PARAM( imu_gizmo,         0, int32_t, 'e', mf_NONE,mf_BMI270,mf_MPU6000,mf_MPU6050,mf_MPU6500,mf_MPU9150,mf_MPU9250,mf_ICM45686,mf_ICM42688) \
+  MF_PARAM( imu_gizmo,         0, int32_t, 'e', mf_NONE,mf_BMI270,mf_MPU6000,mf_MPU6050,mf_MPU6500,mf_MPU9150,mf_MPU9250,mf_ICM45686,mf_ICM42688,mf_ICM20948) \
   MF_PARAM( imu_spi_bus,      -1, int32_t, 'i') \
   MF_PARAM( imu_i2c_bus,      -1, int32_t, 'i') \
   MF_PARAM( imu_i2c_adr,       0, int32_t, 'i') \
@@ -237,6 +237,12 @@ SOFTWARE.
   MF_PARAM( pin_rdr_trig,     -1, int32_t, 'p') \
   MF_PARAM( pin_rdr_echo,     -1, int32_t, 'p') \
   MF_PARAM( imu_bus_type,      0, int32_t, 'e', mf_SPI,mf_I2C) \
+  /*IMU should use its magnetometer*/ \
+  MF_PARAM( imu_use_mag,       1, int32_t, 'i') /*whether the IMU has/should use its magnetometer, 0: no, 1: yes*/ \
+  MF_PARAM( imu_cal_q0_bias,   0, float, 'f') /*imu q0 bias*/ \
+  MF_PARAM( imu_cal_q1_bias,   0, float, 'f') /*imu q1 bias*/ \
+  MF_PARAM( imu_cal_q2_bias,   0, float, 'f') /*imu q2 bias*/ \
+  MF_PARAM( imu_cal_q3_bias,   0, float, 'f') /*imu q3 bias*/ \
 //end MF_PARAM_LIST
 
 

src/cli/cli.cpp

@@ -66,6 +66,14 @@ static void cli_print_imu_MagData() {
   Serial.printf("mx:%+.2f\tmy:%+.2f\tmz:%+.2f\t", ahr.mx, ahr.my, ahr.mz); 
 }
 
+static void cli_print_imu_Quaternion() {
+  if (imu.hasSensorFusion()) {
+    Serial.printf("w:%+.2f\tq1:%+.2f\tq2:%+.2f\tq3:%+.2f\t", imu.q[0], imu.q[1], imu.q[2], imu.q[3]);
+  } else {
+    Serial.printf("IMU does not have sensor fusion.");
+  }
+}
+
 static void cli_print_ahr_RollPitchYaw_verbose() {
     // from ahr.h:
   // roll in degrees: -180 to 180, roll right is +
@@ -81,6 +89,10 @@ static void cli_print_ahr_RollPitchYaw() {
   Serial.printf("roll:%+.1f\tpitch:%+.1f\tyaw:%+.1f\t", ahr.roll, ahr.pitch, ahr.yaw);
 }
 
+static void cli_print_ahr_Quaternion() {
+  Serial.printf("w:%+.2f\tq1:%+.2f\tq2:%+.2f\tq3:%+.2f", ahr.q[0], ahr.q[1], ahr.q[2], ahr.q[3]);
+}
+
 static void cli_print_control_PIDoutput() {
   Serial.printf("PID.roll:%+.3f\t",PIDroll.PID);
   Serial.printf("pitch:%+.3f\t",PIDpitch.PID);
@@ -175,7 +187,7 @@ struct cli_print_s {
   void (*function)(void);
 };
 
-#define CLI_PRINT_FLAG_COUNT 16
+#define CLI_PRINT_FLAG_COUNT 18
 bool cli_print_flag[CLI_PRINT_FLAG_COUNT] = {false};
 
 static const struct cli_print_s cli_print_options[CLI_PRINT_FLAG_COUNT] = {
@@ -186,8 +198,10 @@ static const struct cli_print_s cli_print_options[CLI_PRINT_FLAG_COUNT] = {
   {"pgyr",   "Filtered gyro (expected: -250 to 250, 0 at rest)", cli_print_imu_GyroData},
   {"pacc",   "Filtered accelerometer (expected: -2 to 2; when level: x=0,y=0,z=1)", cli_print_imu_AccData},
   {"pmag",   "Filtered magnetometer (expected: -300 to 300)", cli_print_imu_MagData},
+  {"pquat",  "IMU quaternion data", cli_print_imu_Quaternion},
   {"pahr",   "AHRS roll, pitch, and yaw in human friendly format (expected: degrees, 0 when level)", cli_print_ahr_RollPitchYaw_verbose},
   {"pah",    "AHRS roll, pitch, and yaw in less verbose format (expected: degrees, 0 when level)", cli_print_ahr_RollPitchYaw},
+  {"pahq",   "AHRS quaternion data", cli_print_ahr_Quaternion},
   {"ppid",   "PID output (expected: -1 to 1)", cli_print_control_PIDoutput},
   {"pout",   "Motor/servo output (expected: 0 to 1)", cli_print_out_Command},
   {"pbat",   "Battery voltage, current, Ah used and Wh used", cli_print_bat},
@@ -444,12 +458,12 @@ void Cli::print_i2cScan() {
 //========================================================================================================================//
 
 void Cli::calibrate_gyro() {
-  Serial.println("Calibrating gyro, don't move vehicle, this takes a couple of seconds...");
+  Serial.println("Calibrating gyro, don't move vehicle, this takes a few seconds...");
   calibrate_IMU2(true);
 }
 
 void Cli::calibrate_IMU() {
-  Serial.println("Calibrating IMU, don't move vehicle, this takes a couple of seconds...");
+  Serial.println("Calibrating IMU, don't move vehicle, this takes a few seconds...");
   calibrate_IMU2(false);
 }
 
@@ -518,7 +532,39 @@ void Cli::calibrate_IMU2(bool gyro_only) {
     cfg.imu_cal_ay = ayerr;
     cfg.imu_cal_az = azerr;
   }
-
+
+  if (imu.hasSensorFusion()) {
+    Serial.println("Calibrating IMU sensor fusion, don't move vehicle, this takes a few seconds...");
+
+    // Wait for a fresh sample to ensure imu.q[] is up-to-date
+    imu.waitNewSample(); // Ensure fresh sample from ISR before using imu.q[]
+
+    // Compute quaternion bias from IMU fusion quaternion
+    float q_dmp[4] = {0};
+    const int q_cnt = 3000;
+    for (int i = 0; i < q_cnt; i++) {
+      imu.waitNewSample();
+      for (int j = 0; j < 4; j++) {
+        q_dmp[j] += imu.q[j];
+      }
+    }
+
+    // Normalize the quaternion
+    float norm = sqrt(q_dmp[0]*q_dmp[0] + q_dmp[1]*q_dmp[1] + q_dmp[2]*q_dmp[2] + q_dmp[3]*q_dmp[3]);
+    for (int i = 0; i < 4; i++) q_dmp[i] /= norm;
+
+    // Store inverse as q_bias
+    cfg.imu_cal_q0_bias =  q_dmp[0];
+    cfg.imu_cal_q1_bias = -q_dmp[1];
+    cfg.imu_cal_q2_bias = -q_dmp[2];
+    cfg.imu_cal_q3_bias = -q_dmp[3];
+
+    Serial.printf("set imu_cal_q0_bias %+f\n", cfg.imu_cal_q0_bias);
+    Serial.printf("set imu_cal_q1_bias %+f\n", cfg.imu_cal_q1_bias);
+    Serial.printf("set imu_cal_q2_bias %+f\n", cfg.imu_cal_q2_bias);
+    Serial.printf("set imu_cal_q3_bias %+f\n", cfg.imu_cal_q3_bias);
+  }
+
   Serial.println("Use 'save' to save these values to flash");
 }
 

src/imu/ICM20948/AK09916_ENUMERATIONS.h

@@ -0,0 +1,15 @@
+#ifndef _AK09916_ENUMERATIONS_H_
+#define _AK09916_ENUMERATIONS_H_
+
+typedef enum
+{
+  AK09916_mode_power_down = 0x00,
+  AK09916_mode_single = (0x01 << 0),
+  AK09916_mode_cont_10hz = (0x01 << 1),
+  AK09916_mode_cont_20hz = (0x02 << 1),
+  AK09916_mode_cont_50hz = (0x03 << 1),
+  AK09916_mode_cont_100hz = (0x04 << 1),
+  AK09916_mode_self_test = (0x01 << 4),
+} AK09916_mode_e;
+
+#endif // _AK09916_ENUMERATIONS_H_

src/imu/ICM20948/AK09916_REGISTERS.h

@@ -0,0 +1,83 @@
+#ifndef _AK09916_REGISTERS_H_
+#define _AK09916_REGISTERS_H_
+
+#include <stdint.h>
+
+typedef enum
+{
+  AK09916_REG_WIA1 = 0x00,
+  AK09916_REG_WIA2,
+  AK09916_REG_RSV1,
+  AK09916_REG_RSV2, // Reserved register. We start reading here when using the DMP. Secret sauce...
+  // discontinuity - containing another nine reserved registers? Secret sauce...
+  AK09916_REG_ST1 = 0x10,
+  AK09916_REG_HXL,
+  AK09916_REG_HXH,
+  AK09916_REG_HYL,
+  AK09916_REG_HYH,
+  AK09916_REG_HZL,
+  AK09916_REG_HZH,
+  // discontinuity
+  AK09916_REG_ST2 = 0x18,
+  // discontinuity
+  AK09916_REG_CNTL2 = 0x31,
+  AK09916_REG_CNTL3,
+} AK09916_Reg_Addr_e;
+
+typedef struct
+{
+  uint8_t WIA1;
+} AK09916_WIA1_Reg_t;
+
+typedef struct
+{
+  uint8_t WIA2;
+} AK09916_WIA2_Reg_t;
+
+typedef struct
+{
+  uint8_t DRDY : 1;
+  uint8_t DOR : 1;
+  uint8_t reserved_0 : 6;
+} AK09916_ST1_Reg_t;
+
+// typedef struct{
+
+// }AK09916_HXL_Reg_t;
+
+// typedef struct{
+
+// }AK09916_HXH_Reg_t;
+// typedef struct{
+
+// }AK09916_HYL_Reg_t;
+// typedef struct{
+
+// }AK09916_HYH_Reg_t;
+// typedef struct{
+
+// }AK09916_HZL_Reg_t;
+// typedef struct{
+
+// }AK09916_HZH_Reg_t;
+
+typedef struct
+{
+  uint8_t reserved_0 : 3;
+  uint8_t HOFL : 1;
+  uint8_t reserved_1 : 4;
+} AK09916_ST2_Reg_t;
+
+typedef struct
+{
+  uint8_t MODE : 5;
+  uint8_t reserved_0 : 3;
+} AK09916_CNTL2_Reg_t;
+
+typedef struct
+{
+  uint8_t SRST : 1;
+  uint8_t reserved_0 : 7;
+} AK09916_CNTL3_Reg_t;
+
+#endif // _AK09916_REGISTERS_H_