Adding the more recent DMP firmware binary (14301 bytes). Adding missing DMP register locations.

Author: PaulZC

src/util/ICM_20948_C.h

@@ -25,8 +25,8 @@ extern "C"
 extern int memcmp(const void *, const void *, size_t); // Avoid compiler warnings
 
 // Define if the DMP will be supported
-// Note: you must have 14290 Bytes of program memory available to store the DMP firmware!
-//#define ICM_20948_USE_DMP // Uncomment this line to enable DMP support.
+// Note: you must have 14301 Bytes of program memory available to store the DMP firmware!
+#define ICM_20948_USE_DMP // Uncomment this line to enable DMP support.
 
 #define ICM_20948_I2C_ADDR_AD0 0x68 // Or 0x69 when AD0 is high
 #define ICM_20948_I2C_ADDR_AD1 0x69 //

src/util/ICM_20948_DMP.h

@@ -17,7 +17,9 @@ extern "C" {
 #define DMP_MEM_BANK_SIZE   256
 #define DMP_LOAD_START      0x90
 
-#define DMP_CODE_SIZE 14290
+#define DMP_CODE_SIZE 14301 /* dmp3a.20x48-0.4.1 */
+
+#define CFG_FIFO_SIZE                   (4222)
 
 // AGB0_REG_DMP_INT_STATUS bit definitions
 #define BIT_WAKE_ON_MOTION_INT          0x08
@@ -38,13 +40,21 @@ extern "C" {
 #define DATA_OUT_CTL2 (4 * 16 + 2) // 16-bit: Data output control 2 register : configure the BM, accel/gyro/compass accuracy and gesture such as Pick-up
 #define DATA_INTR_CTL (4 * 16 + 12) // 16-bit: Determines which sensors can generate interrupt according to bit map defined for DATA_OUT_CTL1
 #define FIFO_WATERMARK (31 * 16 + 14) // 16-bit: DMP will send FIFO interrupt if FIFO count > FIFO watermark. FIFO watermark is set to 80% of actual FIFO size by default
+
 // motion event control
 #define MOTION_EVENT_CTL (4 * 16 + 14) // 16-bit: configure DMP for Android L and Invensense specific features
+
+// indicates to DMP which sensors are available
+/*	1: gyro samples available
+	2: accel samples available
+	8: secondary samples available	*/
 #define DATA_RDY_STATUS (8 * 16 + 10) // 16-bit: indicates to DMP which sensors are available
+
 // batch mode
 #define BM_BATCH_CNTR (27 * 16) // 32-bit: Batch counter
 #define BM_BATCH_THLD (19 * 16 + 12) // 32-bit: Batch mode threshold
 #define BM_BATCH_MASK (21 * 16 + 14) // 16-bit
+
 // sensor output data rate: all 16-bit
 #define ODR_ACCEL (11 * 16 + 14) // ODR_ACCEL Register for accel ODR
 #define ODR_GYRO (11 * 16 + 10) // ODR_GYRO Register for gyro ODR
@@ -57,6 +67,7 @@ extern "C" {
 #define ODR_PRESSURE (11 * 16 + 12) // ODR_PRESSURE Register for pressure ODR
 #define ODR_GYRO_CALIBR (11 * 16 + 8) // ODR_GYRO_CALIBR Register for calibrated gyro ODR
 #define ODR_CPASS_CALIBR (11 * 16 + 4) // ODR_CPASS_CALIBR Register for calibrated compass ODR
+
 // sensor output data rate counter: all 16-bit
 #define ODR_CNTR_ACCEL (9 * 16 + 14) // ODR_CNTR_ACCEL Register for accel ODR counter
 #define ODR_CNTR_GYRO (9 * 16 + 10) // ODR_CNTR_GYRO Register for gyro ODR counter
@@ -69,6 +80,7 @@ extern "C" {
 #define ODR_CNTR_PRESSURE (9 * 16 + 12) // ODR_CNTR_PRESSURE Register for pressure ODR counter
 #define ODR_CNTR_GYRO_CALIBR (9 * 16 + 8) // ODR_CNTR_GYRO_CALIBR Register for calibrated gyro ODR counter
 #define ODR_CNTR_CPASS_CALIBR (9 * 16 + 4) // ODR_CNTR_CPASS_CALIBR Register for calibrated compass ODR counter
+
 // mounting matrix: all 32-bit
 #define CPASS_MTX_00 (23 * 16) // Compass mount matrix and scale
 #define CPASS_MTX_01 (23 * 16 + 4) // Compass mount matrix and scale
@@ -79,6 +91,7 @@ extern "C" {
 #define CPASS_MTX_20 (24 * 16 + 8) // Compass mount matrix and scale
 #define CPASS_MTX_21 (24 * 16 + 12) // Compass mount matrix and scale
 #define CPASS_MTX_22 (25 * 16) // Compass mount matrix and scale
+
 // bias calibration: all 32-bit
 // The biases are 32-bits in chip frame in hardware unit scaled by:
 // 2^12 (FSR 4g) for accel, 2^15 for gyro, in uT scaled by 2^16 for compass.
@@ -91,25 +104,191 @@ extern "C" {
 #define CPASS_BIAS_X (126 * 16 + 4)
 #define CPASS_BIAS_Y (126 * 16 + 8)
 #define CPASS_BIAS_Z (126 * 16 + 12)
+
+#define GYRO_ACCURACY			(138 * 16 +  2)
+#define GYRO_BIAS_SET			(138 * 16 +  6)
+#define GYRO_LAST_TEMPR			(134 * 16)
+#define GYRO_SLOPE_X			( 78 * 16 +  4)
+#define GYRO_SLOPE_Y			( 78 * 16 +  8)
+#define GYRO_SLOPE_Z			( 78 * 16 + 12)
+
+// parameters for accel calibration
+#define ACCEL_ACCURACY			(97 * 16)
+#define ACCEL_CAL_RESET			(77 * 16)
+#define ACCEL_VARIANCE_THRESH	(93 * 16)
+#define ACCEL_CAL_RATE			(94 * 16 + 4) // 16-bit
+#define ACCEL_PRE_SENSOR_DATA	(97 * 16 + 4)
+#define ACCEL_COVARIANCE		(101 * 16 + 8)
+#define ACCEL_ALPHA_VAR			(91 * 16) // 32-bit: 1026019965 (225Hz) 977872018 (112Hz) 882002213 (56Hz)
+#define ACCEL_A_VAR				(92 * 16) // 32-bit: 47721859 (225Hz) 95869806 (112Hz) 191739611 (56Hz)
+#define ACCEL_CAL_INIT			(94 * 16 + 2)
+#define ACCEL_CAL_SCALE_COVQ_IN_RANGE	(194 * 16)
+#define ACCEL_CAL_SCALE_COVQ_OUT_RANGE	(195 * 16)
+#define ACCEL_CAL_TEMPERATURE_SENSITIVITY	(194 * 16 + 4)
+#define ACCEL_CAL_TEMPERATURE_OFFSET_TRIM	(194 * 16 + 12)
+
+#define CPASS_ACCURACY			(37 * 16)
+#define CPASS_BIAS_SET			(34 * 16 + 14)
+#define MAR_MODE				(37 * 16 + 2)
+#define CPASS_COVARIANCE		(115 * 16)
+#define CPASS_COVARIANCE_CUR	(118 * 16 +  8)
+#define CPASS_REF_MAG_3D		(122 * 16)
+#define CPASS_CAL_INIT			(114 * 16)
+#define CPASS_EST_FIRST_BIAS	(113 * 16)
+#define MAG_DISTURB_STATE		(113 * 16 + 2)
+#define CPASS_VAR_COUNT			(112 * 16 + 6)
+#define CPASS_COUNT_7			( 87 * 16 + 2)
+#define CPASS_MAX_INNO			(124 * 16)
+#define CPASS_BIAS_OFFSET		(113 * 16 + 4)
+#define CPASS_CUR_BIAS_OFFSET	(114 * 16 + 4)
+#define CPASS_PRE_SENSOR_DATA	( 87 * 16 + 4)
+
+// Compass Cal params to be adjusted according to sampling rate
+#define CPASS_TIME_BUFFER		(112 * 16 + 14)
+#define CPASS_RADIUS_3D_THRESH_ANOMALY	(112 * 16 + 8)
+
+#define CPASS_STATUS_CHK		(25 * 16 + 12)
+
+// gains
+#define ACCEL_FB_GAIN (34 * 16)
+#define ACCEL_ONLY_GAIN (16 * 16 + 12) // 32-bit: 15252014 (225Hz) 30504029 (112Hz) 61117001 (56Hz)
+#define GYRO_SF (19 * 16) // 32-bit: gyro scaling factor
+
+// 9-axis
+#define MAGN_THR_9X				(80 * 16)
+#define MAGN_LPF_THR_9X			(80 * 16 + 8)
+#define QFB_THR_9X				(80 * 16 + 12)
+
+// DMP running counter
+#define DMPRATE_CNTR			(18 * 16 + 4)
+
+// pedometer
+#define PEDSTD_BP_B				(49 * 16 + 12)
+#define PEDSTD_BP_A4			(52 * 16)
+#define PEDSTD_BP_A3			(52 * 16 + 4)
+#define PEDSTD_BP_A2			(52 * 16 + 8)
+#define PEDSTD_BP_A1			(52 * 16 + 12)
+#define PEDSTD_SB				(50 * 16 + 8)
+#define PEDSTD_SB_TIME			(50 * 16 + 12)
+#define PEDSTD_PEAKTHRSH		(57 * 16 + 8)
+#define PEDSTD_TIML				(50 * 16 + 10)
+#define PEDSTD_TIMH				(50 * 16 + 14)
+#define PEDSTD_PEAK				(57 * 16 + 4)
+#define PEDSTD_STEPCTR			(54 * 16)
+#define PEDSTD_STEPCTR2			(58 * 16 + 8)
+#define PEDSTD_TIMECTR			(60 * 16 + 4)
+#define PEDSTD_DECI				(58 * 16)
+#define PEDSTD_SB2				(60 * 16 + 14)
+#define STPDET_TIMESTAMP		(18 * 16 + 8)
+#define PEDSTEP_IND				(19 * 16 + 4)
+#define PED_Y_RATIO				(17 * 16 + 0)
+
+// SMD
+#define SMD_VAR_TH              (141 * 16 + 12)
+#define SMD_VAR_TH_DRIVE        (143 * 16 + 12)
+#define SMD_DRIVE_TIMER_TH      (143 * 16 +  8)
+#define SMD_TILT_ANGLE_TH       (179 * 16 + 12)
+#define BAC_SMD_ST_TH           (179 * 16 +  8)
+#define BAC_ST_ALPHA4           (180 * 16 + 12)
+#define BAC_ST_ALPHA4A          (176 * 16 + 12)
+
+// Wake on Motion
+#define WOM_ENABLE              (64 * 16 + 14)
+#define WOM_STATUS              (64 * 16 + 6)
+#define WOM_THRESHOLD_DMP       (64 * 16) // Renamed by PaulZC to avoid duplication with the Bank 2 Reg 0x13
+#define WOM_CNTR_TH             (64 * 16 + 12)
+
+// Activity Recognition
+#define BAC_RATE                (48 * 16 + 10)
+#define BAC_STATE               (179 * 16 + 0)
+#define BAC_STATE_PREV          (179 * 16 + 4)
+#define BAC_ACT_ON              (182 * 16 + 0)
+#define BAC_ACT_OFF             (183 * 16 + 0)
+#define BAC_STILL_S_F           (177 * 16 + 0)
+#define BAC_RUN_S_F             (177 * 16 + 4)
+#define BAC_DRIVE_S_F           (178 * 16 + 0)
+#define BAC_WALK_S_F            (178 * 16 + 4)
+#define BAC_SMD_S_F             (178 * 16 + 8)
+#define BAC_BIKE_S_F            (178 * 16 + 12)
+#define BAC_E1_SHORT            (146 * 16 + 0)
+#define BAC_E2_SHORT            (146 * 16 + 4)
+#define BAC_E3_SHORT            (146 * 16 + 8)
+#define BAC_VAR_RUN             (148 * 16 + 12)
+#define BAC_TILT_INIT           (181 * 16 + 0)
+#define BAC_MAG_ON              (225 * 16 + 0)
+#define BAC_PS_ON               (74 * 16 + 0)
+#define BAC_BIKE_PREFERENCE     (173 * 16 + 8)
+#define BAC_MAG_I2C_ADDR        (229 * 16 + 8)
+#define BAC_PS_I2C_ADDR         (75 * 16 + 4)
+#define BAC_DRIVE_CONFIDENCE    (144 * 16 + 0)
+#define BAC_WALK_CONFIDENCE     (144 * 16 + 4)
+#define BAC_SMD_CONFIDENCE      (144 * 16 + 8)
+#define BAC_BIKE_CONFIDENCE     (144 * 16 + 12)
+#define BAC_STILL_CONFIDENCE    (145 * 16 + 0)
+#define BAC_RUN_CONFIDENCE      (145 * 16 + 4)
+#define BAC_MODE_CNTR           (150 * 16)
+#define BAC_STATE_T_PREV        (185 * 16 + 4)
+#define BAC_ACT_T_ON            (184 * 16 + 0)
+#define BAC_ACT_T_OFF           (184 * 16 + 4)
+#define BAC_STATE_WRDBS_PREV    (185 * 16 + 8)
+#define BAC_ACT_WRDBS_ON        (184 * 16 + 8)
+#define BAC_ACT_WRDBS_OFF       (184 * 16 + 12)
+#define BAC_ACT_ON_OFF          (190 * 16 + 2)
+#define PREV_BAC_ACT_ON_OFF     (188 * 16 + 2)
+#define BAC_CNTR                (48 * 16 + 2)
+
+// Flip/Pick-up
+#define FP_VAR_ALPHA            (245 * 16 + 8)
+#define FP_STILL_TH             (246 * 16 + 4)
+#define FP_MID_STILL_TH         (244 * 16 + 8)
+#define FP_NOT_STILL_TH         (246 * 16 + 8)
+#define FP_VIB_REJ_TH           (241 * 16 + 8)
+#define FP_MAX_PICKUP_T_TH      (244 * 16 + 12)
+#define FP_PICKUP_TIMEOUT_TH    (248 * 16 + 8)
+#define FP_STILL_CONST_TH       (246 * 16 + 12)
+#define FP_MOTION_CONST_TH      (240 * 16 + 8)
+#define FP_VIB_COUNT_TH         (242 * 16 + 8)
+#define FP_STEADY_TILT_TH       (247 * 16 + 8)
+#define FP_STEADY_TILT_UP_TH    (242 * 16 + 12)
+#define FP_Z_FLAT_TH_MINUS      (243 * 16 + 8)
+#define FP_Z_FLAT_TH_PLUS       (243 * 16 + 12)
+#define FP_DEV_IN_POCKET_TH     (76  * 16 + 12)
+#define FP_PICKUP_CNTR          (247 * 16 + 4)
+#define FP_RATE                 (240 * 16 + 12)
+
+// Gyro FSR
+#define GYRO_FULLSCALE          (72 * 16 + 12)
+
 // Accel FSR
 // The DMP scales accel raw data internally to align 1g as 2^25.
 // To do this and output hardware unit again as configured FSR, write 0x4000000 to ACC_SCALE DMP register, and write 0x40000 to ACC_SCALE2 DMP register.
 #define ACC_SCALE (30 * 16 + 0) // 32-bit: Write accel scaling value for internal use
 #define ACC_SCALE2 (79 * 16 + 4) // 32-bit: Write accel scaling down value
-// pedometer
-#define PEDSTD_STEPCTR (54 * 16) // 32-bit: step count (big endian)
-#define PEDSTD_TIMECTR (60 * 16 + 4) // 32-bit: walk time (big endian)
-// Activity Recognition (BAC)
-#define BAC_RATE (48 * 16 + 10) // 16-bit
-// parameters for accel calibration
-#define ACCEL_CAL_RATE (94 * 16 + 4) // 16-bit
-#define ACCEL_ALPHA_VAR (91 * 16) // 32-bit: 1026019965 (225Hz) 977872018 (112Hz) 882002213 (56Hz)
-#define ACCEL_A_VAR (92 * 16) // 32-bit: 47721859 (225Hz) 95869806 (112Hz) 191739611 (56Hz)
-// parameters for compass calibration
-#define CPASS_TIME_BUFFER (112 * 16 + 14) // 16-bit: running rate of compass. e.g. 70 (70Hz)
-// gains
-#define ACCEL_ONLY_GAIN (16 * 16 + 12) // 32-bit: 15252014 (225Hz) 30504029 (112Hz) 61117001 (56Hz)
-#define GYRO_SF (19 * 16) // 32-bit: gyro scaling factor
+
+// EIS authentication
+#define EIS_AUTH_INPUT			(160 * 16 + 4)
+#define EIS_AUTH_OUTPUT			(160 * 16 + 0)
+
+// B2S
+#define B2S_RATE                (48 * 16 + 8)
+
+// B2S mounting matrix
+#define B2S_MTX_00              (208 * 16)
+#define B2S_MTX_01              (208 * 16 + 4)
+#define B2S_MTX_02              (208 * 16 + 8)
+#define B2S_MTX_10              (208 * 16 + 12)
+#define B2S_MTX_11              (209 * 16)
+#define B2S_MTX_12              (209 * 16 + 4)
+#define B2S_MTX_20              (209 * 16 + 8)
+#define B2S_MTX_21              (209 * 16 + 12)
+#define B2S_MTX_22              (210 * 16)
+
+// Dmp3 orientation parameters (Q30) initialization
+#define Q0_QUAT6				(33 * 16 + 0)
+#define Q1_QUAT6				(33 * 16 + 4)
+#define Q2_QUAT6				(33 * 16 + 8)
+#define Q3_QUAT6				(33 * 16 + 12)
+
 
 enum DMP_ODR_Registers {
 	DMP_ODR_Reg_Accel = ODR_ACCEL, // ODR_ACCEL Register for accel ODR
@@ -385,23 +564,26 @@ typedef struct // DMP Secondary On/Off data
 #define icm_20948_DMP_Header2_Bytes 2
 #define icm_20948_DMP_Raw_Accel_Bytes 6
 #define icm_20948_DMP_Raw_Gyro_Bytes 6
+#define icm_20948_DMP_Gyro_Bias_Bytes 6
 #define icm_20948_DMP_Compass_Bytes 6
 #define icm_20948_DMP_ALS_Bytes 8
 #define icm_20948_DMP_Quat6_Bytes 12
 #define icm_20948_DMP_Quat9_Bytes 14
+// <-- lcm20948MPUFifoControl.c suggests icm_20948_DMP_Step_Detector_Bytes comes here <--
 #define icm_20948_DMP_PQuat6_Bytes 6
 #define icm_20948_DMP_Geomag_Bytes 14
 #define icm_20948_DMP_Pressure_Bytes 6
-#define icm_20948_DMP_Gyro_Calibr_Bytes 12
+#define icm_20948_DMP_Gyro_Calibr_Bytes 12 // lcm20948MPUFifoControl.c suggests icm_20948_DMP_Gyro_Calibr_Bytes does not exist?
 #define icm_20948_DMP_Compass_Calibr_Bytes 12
-#define icm_20948_DMP_Step_Detector_Bytes 4
+#define icm_20948_DMP_Step_Detector_Bytes 4 // See note above
 #define icm_20948_DMP_Accel_Accuracy_Bytes 2
 #define icm_20948_DMP_Gyro_Accuracy_Bytes 2
 #define icm_20948_DMP_Compass_Accuracy_Bytes 2
-#define icm_20948_DMP_Fsync_Detection_Bytes 2
+#define icm_20948_DMP_Fsync_Detection_Bytes 2 // lcm20948MPUFifoControl.c suggests icm_20948_DMP_Fsync_Detection_Bytes does not exist?
 #define icm_20948_DMP_Pickup_Bytes 2
 #define icm_20948_DMP_Activity_Recognition_Bytes 6
 #define icm_20948_DMP_Secondary_On_Off_Bytes 2
+#define icm_20948_DMP_Footer_Bytes 2
 
 // ICM-20948 data is big-endian. We need to make it little-endian when writing into icm_20948_DMP_data_t
 const int DMP_Quat9_Byte_Ordering[icm_20948_DMP_Quat9_Bytes] =
@@ -448,7 +630,7 @@ typedef struct
 			int16_t Y;
 			int16_t Z;
 		} Data;
-	} Raw_Gyro;
+	} Raw_Gyro; // Note: may also include 3*16 gyro bias?
 	union
 	{
 		uint8_t Bytes[icm_20948_DMP_Compass_Bytes];