Merge pull request #39 from sparkfun/release_candidate

v1.2.1
sparkfun · Feb 28, 2021 · cb9951f · cb9951f
2 parents 64f2de9 + 46ce7b8
commit cb9951f
Show file tree

Hide file tree

Showing 8 changed files with 162 additions and 14 deletions.
diff --git a/DMP.md b/DMP.md
@@ -516,7 +516,7 @@ Brace yourself. Here it is:
  *  Set register 0x7C (Memory Start Address) to 0xB0
  *  Write 0x06666666 to memory (91 * 16 + 0 == Accel Alpha Var)
  *  Set register 0x7C (Memory Start Address) to 0xC0
- *  Write 0x39999A to memory (92 * 16 + 0 == Accel A Var)
+ *  Write 0x3999999A to memory (92 * 16 + 0 == Accel A Var)
  *  Set register 0x7C (Memory Start Address) to 0xE4
  *  Write 0x0000 to memory (94 * 16 + 4 == Accel Cal Rate)
  *  .....

diff --git a/examples/Arduino/Example6_DMP_Quat9_Orientation/Example6_DMP_Quat9_Orientation.ino b/examples/Arduino/Example6_DMP_Quat9_Orientation/Example6_DMP_Quat9_Orientation.ino
@@ -212,6 +212,18 @@ void setup() {
   const unsigned char gyroFullScale[4] = {0x10, 0x00, 0x00, 0x00}; // Value taken from InvenSense Nucleo example
   success &= (myICM.writeDMPmems(GYRO_FULLSCALE, 4, &gyroFullScale[0]) == ICM_20948_Stat_Ok);
 
+  // Configure the Accel Only Gain: 15252014 (225Hz) 30504029 (112Hz) 61117001 (56Hz)
+  const unsigned char accelOnlyGain[4] = {0x00, 0xE8, 0xBA, 0x2E}; // Value taken from InvenSense Nucleo example
+  success &= (myICM.writeDMPmems(ACCEL_ONLY_GAIN, 4, &accelOnlyGain[0]) == ICM_20948_Stat_Ok);
+
+  // Configure the Accel Alpha Var: 1026019965 (225Hz) 977872018 (112Hz) 882002213 (56Hz)
+  const unsigned char accelAlphaVar[4] = {0x06, 0x66, 0x66, 0x66}; // Value taken from InvenSense Nucleo example
+  success &= (myICM.writeDMPmems(ACCEL_ALPHA_VAR, 4, &accelAlphaVar[0]) == ICM_20948_Stat_Ok);
+
+  // Configure the Accel A Var: 47721859 (225Hz) 95869806 (112Hz) 191739611 (56Hz)
+  const unsigned char accelAVar[4] = {0x39, 0x99, 0x99, 0x9A}; // Value taken from InvenSense Nucleo example
+  success &= (myICM.writeDMPmems(ACCEL_A_VAR, 4, &accelAlphaVar[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);
@@ -241,6 +253,11 @@ void setup() {
   // Set the DMP Output Data Rate for Quat9 to 12Hz.
   success &= (myICM.setDMPODRrate(DMP_ODR_Reg_Quat9, 12) == ICM_20948_Stat_Ok);
 
+  // Set the DMP Data Ready Status register
+  const uint16_t dsrBits = DMP_Data_ready_Gyro | DMP_Data_ready_Accel | DMP_Data_ready_Secondary_Compass;
+  const unsigned char drsReg[2] = {(const unsigned char)(dsrBits >> 8), (const unsigned char)(dsrBits & 0xFF)};
+  success &= (myICM.writeDMPmems(DATA_RDY_STATUS, 2, &drsReg[0]) == ICM_20948_Stat_Ok);  
+
   // Enable the FIFO
   success &= (myICM.enableFIFO() == ICM_20948_Stat_Ok);
 
@@ -295,7 +312,14 @@ void loop()
       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);
+      SERIAL_PORT.print(F("Q1:"));
+      SERIAL_PORT.print(q1, 3);
+      SERIAL_PORT.print(F(" Q2:"));
+      SERIAL_PORT.print(q2, 3);
+      SERIAL_PORT.print(F(" Q3:"));
+      SERIAL_PORT.print(q3, 3);
+      SERIAL_PORT.print(F(" Accuracy:"));
+      SERIAL_PORT.println(data.Quat9.Data.Accuracy);
     }
   }
 

diff --git a/examples/Arduino/Example7_DMP_Quat6_EulerAngles/Example7_DMP_Quat6_EulerAngles.ino b/examples/Arduino/Example7_DMP_Quat6_EulerAngles/Example7_DMP_Quat6_EulerAngles.ino
@@ -212,6 +212,18 @@ void setup() {
   const unsigned char gyroFullScale[4] = {0x10, 0x00, 0x00, 0x00}; // Value taken from InvenSense Nucleo example
   success &= (myICM.writeDMPmems(GYRO_FULLSCALE, 4, &gyroFullScale[0]) == ICM_20948_Stat_Ok);
 
+  // Configure the Accel Only Gain: 15252014 (225Hz) 30504029 (112Hz) 61117001 (56Hz)
+  const unsigned char accelOnlyGain[4] = {0x00, 0xE8, 0xBA, 0x2E}; // Value taken from InvenSense Nucleo example
+  success &= (myICM.writeDMPmems(ACCEL_ONLY_GAIN, 4, &accelOnlyGain[0]) == ICM_20948_Stat_Ok);
+
+  // Configure the Accel Alpha Var: 1026019965 (225Hz) 977872018 (112Hz) 882002213 (56Hz)
+  const unsigned char accelAlphaVar[4] = {0x06, 0x66, 0x66, 0x66}; // Value taken from InvenSense Nucleo example
+  success &= (myICM.writeDMPmems(ACCEL_ALPHA_VAR, 4, &accelAlphaVar[0]) == ICM_20948_Stat_Ok);
+
+  // Configure the Accel A Var: 47721859 (225Hz) 95869806 (112Hz) 191739611 (56Hz)
+  const unsigned char accelAVar[4] = {0x39, 0x99, 0x99, 0x9A}; // Value taken from InvenSense Nucleo example
+  success &= (myICM.writeDMPmems(ACCEL_A_VAR, 4, &accelAlphaVar[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);
@@ -318,7 +330,12 @@ void loop()
       double t4 = +1.0 - 2.0 * (q2sqr + q3 * q3);
       double yaw = atan2(t3, t4) * 180.0 / PI;
 
-      SERIAL_PORT.printf("Roll:%.1f Pitch:%.1f Yaw:%.1f\r\n", roll, pitch, yaw);
+      SERIAL_PORT.print(F("Roll:"));
+      SERIAL_PORT.print(roll, 1);
+      SERIAL_PORT.print(F(" Pitch:"));
+      SERIAL_PORT.print(pitch, 1);
+      SERIAL_PORT.print(F(" Yaw:"));
+      SERIAL_PORT.println(yaw, 1);
     }
   }
 

diff --git a/examples/Arduino/Example8_DMP_RawAccel/Example8_DMP_RawAccel.ino b/examples/Arduino/Example8_DMP_RawAccel/Example8_DMP_RawAccel.ino
@@ -212,6 +212,18 @@ void setup() {
   const unsigned char gyroFullScale[4] = {0x10, 0x00, 0x00, 0x00}; // Value taken from InvenSense Nucleo example
   success &= (myICM.writeDMPmems(GYRO_FULLSCALE, 4, &gyroFullScale[0]) == ICM_20948_Stat_Ok);
 
+  // Configure the Accel Only Gain: 15252014 (225Hz) 30504029 (112Hz) 61117001 (56Hz)
+  const unsigned char accelOnlyGain[4] = {0x00, 0xE8, 0xBA, 0x2E}; // Value taken from InvenSense Nucleo example
+  success &= (myICM.writeDMPmems(ACCEL_ONLY_GAIN, 4, &accelOnlyGain[0]) == ICM_20948_Stat_Ok);
+
+  // Configure the Accel Alpha Var: 1026019965 (225Hz) 977872018 (112Hz) 882002213 (56Hz)
+  const unsigned char accelAlphaVar[4] = {0x06, 0x66, 0x66, 0x66}; // Value taken from InvenSense Nucleo example
+  success &= (myICM.writeDMPmems(ACCEL_ALPHA_VAR, 4, &accelAlphaVar[0]) == ICM_20948_Stat_Ok);
+
+  // Configure the Accel A Var: 47721859 (225Hz) 95869806 (112Hz) 191739611 (56Hz)
+  const unsigned char accelAVar[4] = {0x39, 0x99, 0x99, 0x9A}; // Value taken from InvenSense Nucleo example
+  success &= (myICM.writeDMPmems(ACCEL_A_VAR, 4, &accelAlphaVar[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);
@@ -241,11 +253,6 @@ void setup() {
   // Set the DMP Output Data Rates to 2Hz
   //success &= (myICM.setDMPODRrate(DMP_ODR_Reg_Accel, 2) == ICM_20948_Stat_Ok); // ** Note: comment this line to leave the data rate at the maximum **
 
-  // Set the DMP Data Ready Status register
-  const uint16_t dsrBits = DMP_Data_ready_Gyro | DMP_Data_ready_Accel; // Value taken from InvenSense Nucleo example
-  const unsigned char drsReg[2] = {(const unsigned char)(dsrBits >> 8), (const unsigned char)(dsrBits & 0xFF)};
-  success &= (myICM.writeDMPmems(DATA_RDY_STATUS, 2, &drsReg[0]) == ICM_20948_Stat_Ok);  
-
   // Enable the FIFO
   success &= (myICM.enableFIFO() == ICM_20948_Stat_Ok);
 
@@ -293,7 +300,12 @@ void loop()
       float acc_y = (float)data.Raw_Accel.Data.Y; 
       float acc_z = (float)data.Raw_Accel.Data.Z; 
 
-      SERIAL_PORT.printf("X:%f Y:%f Z:%f\r\n", acc_x, acc_y, acc_z);
+      SERIAL_PORT.print(F("X:"));
+      SERIAL_PORT.print(acc_x);
+      SERIAL_PORT.print(F(" Y:"));
+      SERIAL_PORT.print(acc_y);
+      SERIAL_PORT.print(F(" Z:"));
+      SERIAL_PORT.println(acc_z);
     }
   }
 

diff --git a/keywords.txt b/keywords.txt
@@ -184,3 +184,6 @@ DMP_header2_bitmap_Fsync	LITERAL1
 DMP_header2_bitmap_Compass_Accuracy	LITERAL1
 DMP_header2_bitmap_Gyro_Accuracy	LITERAL1
 DMP_header2_bitmap_Accel_Accuracy	LITERAL1
+DMP_Data_ready_Gyro	LITERAL1
+DMP_Data_ready_Accel	LITERAL1
+DMP_Data_ready_Secondary_Compass	LITERAL1
diff --git a/library.properties b/library.properties
@@ -1,8 +1,8 @@
 name=SparkFun 9DoF IMU Breakout - ICM 20948 - Arduino Library
-version=1.2.0
+version=1.2.1
 author=SparkFun Electronics <[email protected]>
 maintainer=SparkFun Electronics <sparkfun.com>
-sentence=Use the low-power high-resolution ICM 20948 9 DoF IMU from Invensense with I2C or SPI. Version 1.2 of the library includes support for the InvenSense Digital Motion Processor (DMP™). 
+sentence=Use the low-power high-resolution ICM 20948 9 DoF IMU from Invensense with I2C or SPI. Version 1.2 of the library includes support for the InvenSense Digital Motion Processor (DMP™).
 paragraph=The <a href="https://www.sparkfun.com/products/15335">SparkFun 9DoF IMU Breakout</a> uses the Invensense <a href="https://www.invensense.com/products/motion-tracking/9-axis/icm-20948">ICM-20948</a> -- a system-in-package featuring acceleration full-scales of ±2 / ±4 / ±8 / ±16 (g), rotational full-scales of ±250 / ±500 / ±1000 / ±2000 (°/sec) and a magnetic field full scale of ±4800 µT. The ICM-20948 can be accessed via either I2C (400 kHz) or SPI (7 MHz)
 category=Sensors
 url=https://github.com/sparkfun/SparkFun_ICM-20948_ArduinoLibrary

diff --git a/src/util/ICM_20948_C.c b/src/util/ICM_20948_C.c
@@ -1484,6 +1484,9 @@ ICM_20948_Status_e inv_icm20948_enable_dmp_sensor(ICM_20948_Device_t *pdev, enum
 
 		ICM_20948_Status_e result = ICM_20948_Stat_Ok;
 
+		uint16_t inv_event_control = 0;
+		uint16_t data_rdy_status = 0;
+
 		if (pdev->_dmp_firmware_available == false)
 				return ICM_20948_Stat_DMPNotSupported;
 
@@ -1540,6 +1543,81 @@ ICM_20948_Status_e inv_icm20948_enable_dmp_sensor(ICM_20948_Device_t *pdev, enum
 		data_output_control_reg[0] = (unsigned char)(delta2 >> 8);
     data_output_control_reg[1] = (unsigned char)(delta2 & 0xff);
 		result = inv_icm20948_write_mems(pdev, DATA_OUT_CTL2, 2, (const unsigned char *)&data_output_control_reg);
+		if (result != ICM_20948_Stat_Ok)
+		{
+				return result;
+		}
+
+		// Check which bits need to be set in the Data Ready Status and Motion Event Control registers
+		// Convert androidSensor into a bit mask and compare to INV_NEEDS_ACCEL_MASK, INV_NEEDS_GYRO_MASK and INV_NEEDS_COMPASS_MASK
+		unsigned long androidSensorAsBitMask;
+		if (androidSensor < 32)
+		{
+				androidSensorAsBitMask = 1L << androidSensor;
+				if ((androidSensorAsBitMask & INV_NEEDS_ACCEL_MASK) > 0)
+				{
+						data_rdy_status |= DMP_Data_ready_Accel;
+						inv_event_control |= DMP_Motion_Event_Control_Accel_Calibr;
+				}
+				if ((androidSensorAsBitMask & INV_NEEDS_GYRO_MASK) > 0)
+				{
+						data_rdy_status |= DMP_Data_ready_Gyro;
+						inv_event_control |= DMP_Motion_Event_Control_Gyro_Calibr;
+				}
+				if ((androidSensorAsBitMask & INV_NEEDS_COMPASS_MASK) > 0)
+				{
+						data_rdy_status |= DMP_Data_ready_Secondary_Compass;
+						inv_event_control |= DMP_Motion_Event_Control_Compass_Calibr;
+				}
+		}
+		else
+		{
+				androidSensorAsBitMask = 1L << (androidSensor - 32);
+				if ((androidSensorAsBitMask & INV_NEEDS_ACCEL_MASK1) > 0)
+				{
+						data_rdy_status |= DMP_Data_ready_Accel;
+						inv_event_control |= DMP_Motion_Event_Control_Accel_Calibr;
+				}
+				if ((androidSensorAsBitMask & INV_NEEDS_GYRO_MASK1) > 0)
+				{
+						data_rdy_status |= DMP_Data_ready_Gyro;
+						inv_event_control |= DMP_Motion_Event_Control_Gyro_Calibr;
+				}
+				if ((androidSensorAsBitMask & INV_NEEDS_COMPASS_MASK1) > 0)
+				{
+						data_rdy_status |= DMP_Data_ready_Secondary_Compass;
+						inv_event_control |= DMP_Motion_Event_Control_Compass_Calibr;
+				}
+		}
+		data_output_control_reg[0] = (unsigned char)(data_rdy_status >> 8);
+    data_output_control_reg[1] = (unsigned char)(data_rdy_status & 0xff);
+		result = inv_icm20948_write_mems(pdev, DATA_RDY_STATUS, 2, (const unsigned char *)&data_output_control_reg);
+		if (result != ICM_20948_Stat_Ok)
+		{
+				return result;
+		}
+
+		// Check which extra bits need to be set in the Motion Event Control register
+		if ((delta & DMP_Data_Output_Control_1_Quat9) > 0)
+		{
+				inv_event_control |= DMP_Motion_Event_Control_9axis;
+		}
+		if (((delta & DMP_Data_Output_Control_1_Step_Detector) > 0) || ((delta & DMP_Data_Output_Control_1_Step_Ind_0) > 0)
+				 || ((delta & DMP_Data_Output_Control_1_Step_Ind_1) > 0) || ((delta & DMP_Data_Output_Control_1_Step_Ind_2) > 0))
+		{
+				inv_event_control |= DMP_Motion_Event_Control_Pedometer_Interrupt;
+		}
+		if ((delta & DMP_Data_Output_Control_1_Geomag) > 0)
+		{
+				inv_event_control |= DMP_Motion_Event_Control_Geomag;
+		}
+		data_output_control_reg[0] = (unsigned char)(inv_event_control >> 8);
+    data_output_control_reg[1] = (unsigned char)(inv_event_control & 0xff);
+		result = inv_icm20948_write_mems(pdev, MOTION_EVENT_CTL, 2, (const unsigned char *)&data_output_control_reg);
+		if (result != ICM_20948_Stat_Ok)
+		{
+				return result;
+		}
 
 		// result = ICM_20948_low_power(pdev, true); // Put chip into low power state
 		// if (result != ICM_20948_Stat_Ok)

diff --git a/src/util/ICM_20948_DMP.h b/src/util/ICM_20948_DMP.h
@@ -47,7 +47,7 @@ extern "C" {
 // indicates to DMP which sensors are available
 /*	1: gyro samples available
 	2: accel samples available
-	8: secondary samples available	*/
+	8: secondary compass samples available	*/
 #define DATA_RDY_STATUS (8 * 16 + 10) // 16-bit: indicates to DMP which sensors are available
 
 // batch mode
@@ -388,11 +388,22 @@ enum ANDROID_SENSORS {
 	GENERAL_SENSORS_MAX // 51
 };
 
+// Determines which base sensor needs to be on based upon ANDROID_SENSORS 0-31
+#define INV_NEEDS_ACCEL_MASK   ((1L<<1)|        (1L<<3)|        (1L<<9)|(1L<<10)|(1L<<11)|         (1L<<15)|         (1L<<17)|(1L<<18)|(1L<<19)|(1L<<20)|(1<<23)|       (1<<25)|        (1<<29)|(1<<30)|(1<<31))
+#define INV_NEEDS_GYRO_MASK		 (                (1L<<3)|(1L<<4)|(1L<<9)|(1L<<10)|(1L<<11)|         (1L<<15)|(1L<<16)|                                                   (1<<25)|(1<<26)|(1<<29)|(1<<30)|(1<<31))
+#define INV_NEEDS_COMPASS_MASK (        (1L<<2)|(1L<<3)|                         (1L<<11)|(1L<<14)|                                             (1L<<20)|       (1<<24)|(1<<25)|                        (1<<31))
+#define INV_NEEDS_PRESSURE		 ((1L<<6)|(1<<28))
+
+// Determines which base sensor needs to be on based upon ANDROID_SENSORS 32-
+#define INV_NEEDS_ACCEL_MASK1	  (       (1<<3)|      (1<<5)|(1<<6)|(1<<7)|(1<<9)|(1<<10)) // I.e. 35, 37, 38, 39, 41, 42
+#define INV_NEEDS_GYRO_MASK1	  (       (1<<3)|(1<<4)                                   |(1<<11)) // I.e. 35, 36, 43
+#define INV_NEEDS_COMPASS_MASK1	((1<<2)|                           (1<<7)) // I.e. 34 and 39
+
 enum DMP_Data_Ready_Status_Register_Bits
 {
 	DMP_Data_ready_Gyro = 0x0001, // Gyro samples available
 	DMP_Data_ready_Accel = 0x0002, // Accel samples available
-	DMP_Data_ready_Secondary = 0x0008 // Secondary samples available
+	DMP_Data_ready_Secondary_Compass = 0x0008 // Secondary compass samples available
 };
 
 enum DMP_Data_Output_Control_1_Register_Bits
@@ -430,16 +441,19 @@ enum DMP_Data_Output_Control_2_Register_Bits
 enum DMP_Motion_Event_Control_Register_Bits
 {
 	DMP_Motion_Event_Control_Activity_Recog_Pedom_Accel = 0x0002, // Activity Recognition / Pedometer accel only
+	DMP_Motion_Event_Control_Bring_Look_To_See = 0x0004,
 	DMP_Motion_Event_Control_Geomag = 0x0008, // Geomag rv
 	DMP_Motion_Event_Control_Pickup = 0x0010,
+	DMP_Motion_Event_Control_BTS = 0x0020,
 	DMP_Motion_Event_Control_9axis = 0x0040,
 	DMP_Motion_Event_Control_Compass_Calibr = 0x0080,
 	DMP_Motion_Event_Control_Gyro_Calibr = 0x0100,
 	DMP_Motion_Event_Control_Accel_Calibr = 0x0200,
 	DMP_Motion_Event_Control_Significant_Motion_Det = 0x0800,
 	DMP_Motion_Event_Control_Tilt_Interrupt = 0x1000,
 	DMP_Motion_Event_Control_Pedometer_Interrupt = 0x2000,
-	DMP_Motion_Event_Control_Activity_Recog_Pedom = 0x4000
+	DMP_Motion_Event_Control_Activity_Recog_Pedom = 0x4000,
+	DMP_Motion_Event_Control_BAC_Wearable = 0x8000
 };
 
 enum DMP_Header_Bitmap