Merge pull request #31 from sparkfun/DMP

Adding (limited) DMP functionality

Author: PaulZC

examples/Arduino/Example5_DMP_Quat9_Orientation/Example5_DMP_Quat9_Orientation.ino

@@ -0,0 +1,285 @@
+/****************************************************************
+ * Example5_DMP_Quat9_Orientation.ino
+ * ICM 20948 Arduino Library Demo
+ * Initialize the DMP based on the TDK InvenSense ICM20948_eMD_nucleo_1.0 example-icm20948
+ * Paul Clark, February 15th 2021
+ * Based on original code by:
+ * Owen Lyke @ SparkFun Electronics
+ * Original Creation Date: April 17 2019
+ * 
+ * ** This example is based on InvenSense's _confidential_ Application Note "Programming Sequence for DMP Hardware Functions".
+ * ** We are grateful to InvenSense for sharing this with us.
+ * 
+ * ** Important note: by default the DMP functionality is disabled in the library
+ * ** as the DMP firmware takes up 14290 Bytes of program memory.
+ * ** To use the DMP, you will need to:
+ * ** Edit ICM_20948_C.h
+ * ** Uncomment line 29: #define ICM_20948_USE_DMP
+ * ** Save changes
+ * ** If you are using Windows, you can find ICM_20948_C.h in:
+ * ** Documents\Arduino\libraries\SparkFun_ICM-20948_ArduinoLibrary\src\util
+ *
+ * Please see License.md for the license information.
+ *
+ * Distributed as-is; no warranty is given.
+ ***************************************************************/
+
+#include "ICM_20948.h"  // Click here to get the library: http://librarymanager/All#SparkFun_ICM_20948_IMU
+
+//#define USE_SPI       // Uncomment this to use SPI
+
+#define SERIAL_PORT Serial
+
+#define SPI_PORT SPI    // Your desired SPI port.       Used only when "USE_SPI" is defined
+#define CS_PIN 2        // Which pin you connect CS to. Used only when "USE_SPI" is defined
+
+#define WIRE_PORT Wire  // Your desired Wire port.      Used when "USE_SPI" is not defined
+#define AD0_VAL   1     // The value of the last bit of the I2C address.
+                        // On the SparkFun 9DoF IMU breakout the default is 1, and when
+                        // the ADR jumper is closed the value becomes 0
+
+#ifdef USE_SPI
+  ICM_20948_SPI myICM;  // If using SPI create an ICM_20948_SPI object
+#else
+  ICM_20948_I2C myICM;  // Otherwise create an ICM_20948_I2C object
+#endif
+
+
+void setup() {
+
+  SERIAL_PORT.begin(115200); // Start the serial console
+  SERIAL_PORT.println(F("ICM-20948 Example"));
+
+  delay(100);
+
+  while (SERIAL_PORT.available()) // Make sure the serial RX buffer is empty
+    SERIAL_PORT.read();
+
+  SERIAL_PORT.println(F("Press any key to continue..."));
+
+  while (!SERIAL_PORT.available()) // Wait for the user to press a key (send any serial character)
+    ;
+
+#ifdef USE_SPI
+    SPI_PORT.begin();
+#else
+    WIRE_PORT.begin();
+    WIRE_PORT.setClock(400000);
+#endif
+
+  //myICM.enableDebugging(); // Uncomment this line to enable helpful debug messages on Serial
+
+  bool initialized = false;
+  while( !initialized ){
+
+    // Initialize the ICM-20948
+    // If the DMP is enabled, .begin performs a minimal startup. We need to configure the sample mode etc. manually.
+#ifdef USE_SPI
+    myICM.begin( CS_PIN, SPI_PORT );
+#else
+    myICM.begin( WIRE_PORT, AD0_VAL );
+#endif
+
+    SERIAL_PORT.print( F("Initialization of the sensor returned: ") );
+    SERIAL_PORT.println( myICM.statusString() );
+    if( myICM.status != ICM_20948_Stat_Ok ){
+      SERIAL_PORT.println( "Trying again..." );
+      delay(500);
+    }else{
+      initialized = true;
+    }
+  }
+
+  SERIAL_PORT.println("Device connected!");
+
+  // The ICM-20948 is awake and ready but hasn't been configured. Let's step through the configuration
+  // sequence from InvenSense's _confidential_ Application Note "Programming Sequence for DMP Hardware Functions".
+
+  bool success = true; // Use success to show if the configuration was successful
+
+  // Configure clock source through PWR_MGMT_1
+  // ICM_20948_Clock_Auto selects the best available clock source – PLL if ready, else use the Internal oscillator
+  success &= (myICM.setClockSource(ICM_20948_Clock_Auto) == ICM_20948_Stat_Ok); // This is shorthand: success will be set to false if setClockSource fails
+
+  // Enable accel and gyro sensors through PWR_MGMT_2
+  // Enable Accelerometer (all axes) and Gyroscope (all axes) by writing zero to PWR_MGMT_2
+  uint8_t zero = 0;
+  success &= (myICM.write(AGB0_REG_PWR_MGMT_2, &zero, 1) == ICM_20948_Stat_Ok); // Write one byte to the PWR_MGMT_2 register
+
+  // Configure Gyro/Accel in Low Power Mode (cycled) with LP_CONFIG
+  success &= (myICM.setSampleMode( (ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), ICM_20948_Sample_Mode_Cycled ) == ICM_20948_Stat_Ok);
+
+  // Set Gyro FSR (Full scale range) to 2000dps through GYRO_CONFIG_1
+  // Set Accel FSR (Full scale range) to 4g through ACCEL_CONFIG
+  ICM_20948_fss_t myFSS;  // This uses a "Full Scale Settings" structure that can contain values for all configurable sensors
+  myFSS.a = gpm4;         // (ICM_20948_ACCEL_CONFIG_FS_SEL_e)
+                          // gpm2
+                          // gpm4
+                          // gpm8
+                          // gpm16
+  myFSS.g = dps2000;       // (ICM_20948_GYRO_CONFIG_1_FS_SEL_e)
+                          // dps250
+                          // dps500
+                          // dps1000
+                          // dps2000
+  success &= (myICM.setFullScale( (ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), myFSS ) == ICM_20948_Stat_Ok);
+
+  // Enable interrupt for FIFO overflow from FIFOs through INT_ENABLE_2
+  // If we see this interrupt, we'll need to reset the FIFO
+  //success &= (myICM.intEnableOverflowFIFO( 0x1F ) == ICM_20948_Stat_Ok); // Enable the interrupt on all FIFOs
+
+  // Turn off what goes into the FIFO through FIFO_EN_1, FIFO_EN_2
+  // Stop the peripheral data from being written to the FIFO by writing zero to FIFO_EN_1
+  success &= (myICM.write(AGB0_REG_FIFO_EN_1, &zero, 1) == ICM_20948_Stat_Ok);
+  // Stop the accelerometer, gyro and temperature data from being written to the FIFO by writing zero to FIFO_EN_2
+  success &= (myICM.write(AGB0_REG_FIFO_EN_2, &zero, 1) == ICM_20948_Stat_Ok);
+
+  // Turn off data ready interrupt through INT_ENABLE_1
+  success &= (myICM.intEnableRawDataReady(false) == ICM_20948_Stat_Ok);
+
+  // Reset FIFO through FIFO_RST
+  success &= (myICM.resetFIFO() == ICM_20948_Stat_Ok);
+
+  // Set gyro sample rate divider with GYRO_SMPLRT_DIV
+  // Set accel sample rate divider with ACCEL_SMPLRT_DIV_2
+  ICM_20948_smplrt_t mySmplrt;
+  mySmplrt.g = 43; // ODR is computed as follows: 1.1 kHz/(1+GYRO_SMPLRT_DIV[7:0]). 43 = 25Hz
+  mySmplrt.a = 44; // ODR is computed as follows: 1.125 kHz/(1+ACCEL_SMPLRT_DIV[11:0]). 44 = 25Hz
+  //myICM.setSampleRate( (ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), mySmplrt ); // ** Note: comment this line to leave the sample rates at the maximum **
+  
+  // Setup DMP start address through PRGM_STRT_ADDRH/PRGM_STRT_ADDRL
+  success &= (myICM.setDMPstartAddress() == ICM_20948_Stat_Ok); // Defaults to DMP_START_ADDRESS
+
+  // Now load the DMP firmware
+  success &= (myICM.loadDMPFirmware() == ICM_20948_Stat_Ok);
+
+  // Write the 2 byte Firmware Start Value to ICM PRGM_STRT_ADDRH/PRGM_STRT_ADDRL
+  success &= (myICM.setDMPstartAddress() == ICM_20948_Stat_Ok); // Defaults to DMP_START_ADDRESS
+
+  // Configure Accel scaling to DMP
+  // The DMP scales accel raw data internally to align 1g as 2^25
+  // In order to align internal accel raw data 2^25 = 1g write 0x4000000 when FSR is 4g
+  const unsigned char accScale[4] = {0x40, 0x00, 0x00, 0x00};
+  success &= (myICM.writeDMPmems(ACC_SCALE, 4, &accScale[0]) == ICM_20948_Stat_Ok); // Write 0x4000000 to ACC_SCALE DMP register
+  // In order to output hardware unit data as configured FSR write 0x40000 when FSR is 4g
+  const unsigned char accScale2[4] = {0x00, 0x04, 0x00, 0x00};
+  success &= (myICM.writeDMPmems(ACC_SCALE2, 4, &accScale2[0]) == ICM_20948_Stat_Ok); // Write 0x40000 to ACC_SCALE2 DMP register
+
+  // Configure Compass mount matrix and scale to DMP
+  // The mount matrix write to DMP register is used to align the compass axes with accel/gyro.
+  // This mechanism is also used to convert hardware unit to uT. The value is expressed as 1uT = 2^30.
+  // Each compass axis will be converted as below:
+  // X = raw_x * CPASS_MTX_00 + raw_y * CPASS_MTX_01 + raw_z * CPASS_MTX_02
+  // Y = raw_x * CPASS_MTX_10 + raw_y * CPASS_MTX_11 + raw_z * CPASS_MTX_12
+  // Z = raw_x * CPASS_MTX_20 + raw_y * CPASS_MTX_21 + raw_z * CPASS_MTX_22
+  // Magnetometer full scale is +/- 4900uT so _I think_ we need to multiply by 2^30 / 4900 = 0x000357FA
+  // The magnetometer Y and Z axes are reversed compared to the accelerometer so we'll invert those
+  const unsigned char mountMultiplierZero[4] = {0x00, 0x00, 0x00, 0x00};
+  const unsigned char mountMultiplierPlus[4] = {0x00, 0x03, 0x57, 0xFA};
+  const unsigned char mountMultiplierMinus[4] = {0xFF, 0xFC, 0xA8, 0x05};
+  success &= (myICM.writeDMPmems(CPASS_MTX_00, 4, &mountMultiplierPlus[0]) == ICM_20948_Stat_Ok);
+  success &= (myICM.writeDMPmems(CPASS_MTX_01, 4, &mountMultiplierZero[0]) == ICM_20948_Stat_Ok);
+  success &= (myICM.writeDMPmems(CPASS_MTX_02, 4, &mountMultiplierZero[0]) == ICM_20948_Stat_Ok);
+  success &= (myICM.writeDMPmems(CPASS_MTX_10, 4, &mountMultiplierZero[0]) == ICM_20948_Stat_Ok);
+  success &= (myICM.writeDMPmems(CPASS_MTX_11, 4, &mountMultiplierMinus[0]) == ICM_20948_Stat_Ok);
+  success &= (myICM.writeDMPmems(CPASS_MTX_12, 4, &mountMultiplierZero[0]) == ICM_20948_Stat_Ok);
+  success &= (myICM.writeDMPmems(CPASS_MTX_20, 4, &mountMultiplierZero[0]) == ICM_20948_Stat_Ok);
+  success &= (myICM.writeDMPmems(CPASS_MTX_21, 4, &mountMultiplierZero[0]) == ICM_20948_Stat_Ok);
+  success &= (myICM.writeDMPmems(CPASS_MTX_22, 4, &mountMultiplierMinus[0]) == ICM_20948_Stat_Ok);
+
+  // Enable DMP interrupt
+  // This would be the most efficient way of getting the DMP data, instead of polling the FIFO
+  //success &= (myICM.intEnableDMP(true) == ICM_20948_Stat_Ok);
+
+  // DMP sensor options are defined in ICM_20948_DMP.h
+  //    INV_ICM20948_SENSOR_ACCELEROMETER
+  //    INV_ICM20948_SENSOR_GYROSCOPE
+  //    INV_ICM20948_SENSOR_RAW_ACCELEROMETER
+  //    INV_ICM20948_SENSOR_RAW_GYROSCOPE
+  //    INV_ICM20948_SENSOR_MAGNETIC_FIELD_UNCALIBRATED
+  //    INV_ICM20948_SENSOR_GYROSCOPE_UNCALIBRATED
+  //    INV_ICM20948_SENSOR_ACTIVITY_CLASSIFICATON
+  //    INV_ICM20948_SENSOR_STEP_DETECTOR
+  //    INV_ICM20948_SENSOR_STEP_COUNTER
+  //    INV_ICM20948_SENSOR_GAME_ROTATION_VECTOR
+  //    INV_ICM20948_SENSOR_ROTATION_VECTOR
+  //    INV_ICM20948_SENSOR_GEOMAGNETIC_ROTATION_VECTOR
+  //    INV_ICM20948_SENSOR_GEOMAGNETIC_FIELD
+  //    INV_ICM20948_SENSOR_WAKEUP_SIGNIFICANT_MOTION
+  //    INV_ICM20948_SENSOR_FLIP_PICKUP
+  //    INV_ICM20948_SENSOR_WAKEUP_TILT_DETECTOR
+  //    INV_ICM20948_SENSOR_GRAVITY
+  //    INV_ICM20948_SENSOR_LINEAR_ACCELERATION
+  //    INV_ICM20948_SENSOR_ORIENTATION
+
+  // Enable the DMP orientation sensor
+  success &= (myICM.enableDMPSensor(INV_ICM20948_SENSOR_ORIENTATION) == ICM_20948_Stat_Ok);
+
+  // Configuring DMP to output data at multiple ODRs:
+  // DMP is capable of outputting multiple sensor data at different rates to FIFO.
+  // Set the DMP Output Data Rate for Quat9 to 12Hz.
+  success &= (myICM.setDMPODRrate(DMP_ODR_Reg_Quat9, 12) == ICM_20948_Stat_Ok);
+
+  // Enable the FIFO
+  success &= (myICM.enableFIFO() == ICM_20948_Stat_Ok);
+
+  // Enable the DMP
+  success &= (myICM.enableDMP() == ICM_20948_Stat_Ok);
+
+  // Reset DMP
+  success &= (myICM.resetDMP() == ICM_20948_Stat_Ok);
+
+  // Reset FIFO
+  success &= (myICM.resetFIFO() == ICM_20948_Stat_Ok);
+
+  // Check success
+  if( success )
+    SERIAL_PORT.println("DMP enabled!");
+  else
+  {
+    SERIAL_PORT.println("Enable DMP failed!");
+  }
+  
+}
+
+void loop()
+{
+  // Read any DMP data waiting in the FIFO
+  // Note:
+  //    readDMPdataFromFIFO will return ICM_20948_Stat_FIFONoDataAvail if no data or incomplete data is available.
+  //    If data is available, readDMPdataFromFIFO will attempt to read _one_ frame of DMP data.
+  //    readDMPdataFromFIFO will return ICM_20948_Stat_Ok if a valid frame was read.
+  //    readDMPdataFromFIFO will return ICM_20948_Stat_FIFOMoreDataAvail if a valid frame was read _and_ the FIFO contains more (unread) data.
+  icm_20948_DMP_data_t data;
+  myICM.readDMPdataFromFIFO(&data);
+  
+  if(( myICM.status == ICM_20948_Stat_Ok ) || ( myICM.status == ICM_20948_Stat_FIFOMoreDataAvail )) // Was valid data available?
+  {
+    //SERIAL_PORT.print(F("Received data! Header: 0x")); // Print the header in HEX so we can see what data is arriving in the FIFO
+    //if ( data.header < 0x1000) SERIAL_PORT.print( "0" ); // Pad the zeros
+    //if ( data.header < 0x100) SERIAL_PORT.print( "0" );
+    //if ( data.header < 0x10) SERIAL_PORT.print( "0" );
+    //SERIAL_PORT.println( data.header, HEX );
+    
+    if ( (data.header & DMP_header_bitmap_Quat9) > 0 ) // We have asked for orientation data so we should receive Quat9
+    {
+      // Q0 value is computed from this equation: Q0^2 + Q1^2 + Q2^2 + Q3^2 = 1.
+      // In case of drift, the sum will not add to 1, therefore, quaternion data need to be corrected with right bias values.
+      // The quaternion data is scaled by 2^30.
+
+      //SERIAL_PORT.printf("Quat9 data is: Q1:%ld Q2:%ld Q3:%ld Accuracy:%d\r\n", data.Quat9.Data.Q1, data.Quat9.Data.Q2, data.Quat9.Data.Q3, data.Quat9.Data.Accuracy);
+
+      // Scale to +/- 1
+      float q1 = ((float)data.Quat9.Data.Q1) / 1073741824.0; // Convert to float. Divide by 2^30
+      float q2 = ((float)data.Quat9.Data.Q2) / 1073741824.0; // Convert to float. Divide by 2^30
+      float q3 = ((float)data.Quat9.Data.Q3) / 1073741824.0; // Convert to float. Divide by 2^30
+      
+      SERIAL_PORT.printf("Q1:%.3f Q2:%.3f Q3:%.3f\r\n", q1, q2, q3);
+    }
+  }
+
+  if ( myICM.status != ICM_20948_Stat_FIFOMoreDataAvail ) // If more data is available then we should read it right away - and not delay
+  {
+    delay(10);
+  }
+}