Add support for the ICM 20948 IMU

sunray/config_example.h

@@ -68,8 +68,12 @@ Also, you may choose the serial port below for serial monitor output (CONSOLE).
 //#define MPU9150
 #define MPU9250   // also choose this for MPU9255
 //#define BNO055
+//#define ICM20948
 #define MPU_ADDR 0x69  // I2C address (0x68 if AD0=LOW, 0x69 if AD0=HIGH)
 
+// imu fifo rate (Hz)
+#define IMU_FIFO_RATE 5
+
 // should the mower turn off if IMU is tilt over? (yes: uncomment line, no: comment line)
 #define ENABLE_TILT_DETECTION  1
 

sunray/robot.cpp

@@ -30,6 +30,7 @@
 #include "src/driver/SerialRobotDriver.h"
 #include "src/driver/MpuDriver.h"
 #include "src/driver/BnoDriver.h"
+#include "src/driver/IcmDriver.h"
 #include "battery.h"
 #include "gps.h"
 #include "src/ublox/ublox.h"
@@ -58,7 +59,9 @@ const signed char orientationMatrix[9] = {
 #ifdef DRV_SIM_ROBOT
   SimImuDriver imuDriver(robotDriver);
 #elif defined(BNO055)
-  BnoDriver imuDriver;  
+  BnoDriver imuDriver;
+#elif defined(ICM20948)
+  IcmDriver imuDriver;
 #else
   MpuDriver imuDriver;
 #endif
@@ -908,7 +911,8 @@ void run(){
 
   // IMU
   if (millis() > nextImuTime){
-    nextImuTime = millis() + 150;        
+    int ims = 750 / IMU_FIFO_RATE;
+    nextImuTime = millis() + ims;        
     //imu.resetFifo();    
     if (imuIsCalibrating) {
       activeOp->onImuCalibration();             

sunray/robot.h

@@ -16,6 +16,7 @@
 #include "src/driver/SimRobotDriver.h"
 #include "src/driver/MpuDriver.h"
 #include "src/driver/BnoDriver.h"
+#include "src/driver/IcmDriver.h"
 #include "battery.h"
 #include "ble.h"
 #include "pinman.h"
@@ -137,6 +138,8 @@ extern int motorErrorCounter;
   extern SimImuDriver imuDriver;
 #elif defined(BNO055)
   extern BnoDriver imuDriver;  
+#elif defined(ICM20948)
+  extern IcmDriver imuDriver;  
 #else
   extern MpuDriver imuDriver;
 #endif

sunray/src/driver/IcmDriver.cpp

@@ -0,0 +1,106 @@
+
+#include "IcmDriver.h"
+#include "../../config.h"
+#include "../../i2c.h"
+
+
+
+IcmDriver::IcmDriver(){    
+}
+
+void IcmDriver::detect(){
+  Wire.begin();
+  Wire.setClock(400000);
+
+  int tries = 10;
+  while (tries > 0)
+  {
+    icm.begin(Wire, 1);
+    if (icm.status != ICM_20948_Stat_Ok)
+    {
+      tries--;
+      delay(500);
+    }
+    else
+    {
+      imuFound = true;
+      CONSOLE.println(" ");
+      CONSOLE.println("ICM 20948 found");
+      return;
+    }
+  }
+  imuFound = false;
+  CONSOLE.println(F("ICM 20948 not found"));        
+}
+
+
+bool IcmDriver::begin(){ 
+  bool success = true;
+  success &= (icm.initializeDMP() == ICM_20948_Stat_Ok);
+
+  success &= (icm.enableDMPSensor(INV_ICM20948_SENSOR_GAME_ROTATION_VECTOR) == ICM_20948_Stat_Ok);
+  int odrate = 55.0 / IMU_FIFO_RATE - 0.5;
+  success &= (icm.setDMPODRrate(DMP_ODR_Reg_Quat6, odrate) == ICM_20948_Stat_Ok);
+  //success &= (icm.enableDMPSensor(INV_ICM20948_SENSOR_ORIENTATION) == ICM_20948_Stat_Ok);
+  //success &= (icm.setDMPODRrate(DMP_ODR_Reg_Quat9, 2) == ICM_20948_Stat_Ok);
+
+  success &= (icm.enableFIFO() == ICM_20948_Stat_Ok);
+  success &= (icm.enableDMP() == ICM_20948_Stat_Ok);
+  success &= (icm.resetDMP() == ICM_20948_Stat_Ok);
+  success &= (icm.resetFIFO() == ICM_20948_Stat_Ok);
+  success &= (icm.lowPower(false) == ICM_20948_Stat_Ok);
+  //TODO: Add bias settings here
+  CONSOLE.println("using imu driver: IcmDriver");
+  return success;
+}
+
+
+void IcmDriver::run(){
+}
+
+
+bool IcmDriver::isDataAvail(){
+
+    icm_20948_DMP_data_t data;
+    icm.readDMPdataFromFIFO(&data);
+
+    if ((icm.status == ICM_20948_Stat_Ok) || (icm.status == ICM_20948_Stat_FIFOMoreDataAvail))
+    {
+        if ((data.header & DMP_header_bitmap_Quat6) > 0)
+        //if ((data.header & DMP_header_bitmap_Quat9) > 0)
+        {
+            double q1 = ((double)data.Quat6.Data.Q1) / 1073741824.0;
+            double q2 = ((double)data.Quat6.Data.Q2) / 1073741824.0;
+            double q3 = ((double)data.Quat6.Data.Q3) / 1073741824.0;
+
+            double q0 = sqrt(1.0 - min((q1 * q1) + (q2 * q2) + (q3 * q3), 1.0));
+
+            double q2sqr = q2 * q2;
+
+            // roll (x-axis rotation)
+            double t0 = +2.0 * (q0 * q1 + q2 * q3);
+            double t1 = +1.0 - 2.0 * (q1 * q1 + q2sqr);
+            roll = atan2(t0, t1);
+
+            // pitch (y-axis rotation)
+            double t2 = +2.0 * (q0 * q2 - q3 * q1);
+            t2 = t2 > 1.0 ? 1.0 : t2;
+            t2 = t2 < -1.0 ? -1.0 : t2;
+            pitch = asin(t2);
+
+            // yaw (z-axis rotation)
+            double t3 = +2.0 * (q0 * q3 + q1 * q2);
+            double t4 = +1.0 - 2.0 * (q2sqr + q3 * q3);
+            yaw = atan2(t3, t4);
+        }
+        return true;
+    }
+    return false;
+}         
+
+void IcmDriver::resetData(){
+    icm.resetFIFO();
+}
+
+
+

sunray/src/driver/IcmDriver.h

@@ -0,0 +1,25 @@
+
+// ICM driver 
+
+#ifndef ICM_DRIVER_H
+#define ICM_DRIVER_H
+
+#include <Arduino.h>
+#include "RobotDriver.h"
+#include "../icm/ICM_20948.h"
+
+
+class IcmDriver: public ImuDriver {    
+  public:    
+    IcmDriver();    
+    void detect() override;
+    bool begin() override;    
+    void run() override;
+    bool isDataAvail() override;         
+    void resetData() override;        
+  protected:
+    ICM_20948_I2C icm;
+};
+
+
+#endif

sunray/src/driver/MpuDriver.cpp

@@ -60,7 +60,7 @@ bool MpuDriver::begin(){
     mpu.dmpBegin(DMP_FEATURE_6X_LP_QUAT  // Enable 6-axis quat
                |  DMP_FEATURE_GYRO_CAL // Use gyro calibration
              //  | DMP_FEATURE_SEND_RAW_ACCEL
-              , 5); // Set DMP FIFO rate to 5 Hz
+              , IMU_FIFO_RATE); // Set DMP FIFO rate
     // DMP_FEATURE_LP_QUAT can also be used. It uses the 
     // accelerometer in low-power mode to estimate quat's.
     // DMP_FEATURE_LP_QUAT and 6X_LP_QUAT are mutually exclusive