Adding setGyroSF

Author: PaulZC

keywords.txt

@@ -94,6 +94,7 @@ writeDMPmems	KEYWORD2
 readDMPmems	KEYWORD2
 setDMPODRrate	KEYWORD2
 readDMPdataFromFIFO	KEYWORD2
+setGyroSF	KEYWORD2
 begin	KEYWORD2
 
 #######################################
@@ -164,6 +165,7 @@ INV_ICM20948_SENSOR_GRAVITY	LITERAL1
 INV_ICM20948_SENSOR_LINEAR_ACCELERATION	LITERAL1
 INV_ICM20948_SENSOR_ORIENTATION	LITERAL1
 INV_ICM20948_SENSOR_B2S	LITERAL1
+INV_ICM20948_SENSOR_RAW_MAGNETOMETER	LITERAL1
 DMP_header_bitmap_Header2	LITERAL1
 DMP_header_bitmap_Step_Detector	LITERAL1
 DMP_header_bitmap_Compass_Calibr	LITERAL1

src/ICM_20948.cpp

@@ -1026,6 +1026,15 @@ ICM_20948_Status_e ICM_20948::enableDMPSensor(enum inv_icm20948_sensor sensor, b
   if (_device._dmp_firmware_available == true) // Should we attempt to enable the sensor?
   {
     status = inv_icm20948_enable_dmp_sensor(&_device, sensor, enable == true ? 1 : 0);
+    debugPrint(F("ICM_20948::enableDMPSensor:  _DATA_OUT_CTL1: "));
+    debugPrintf((int)_device._DATA_OUT_CTL1);
+    debugPrint(F("  _DATA_OUT_CTL2: "));
+    debugPrintf((int)_device._DATA_OUT_CTL2);
+    debugPrint(F("  _DATA_RDY_STATUS: "));
+    debugPrintf((int)_device._DATA_RDY_STATUS);
+    debugPrint(F("  _MOTION_EVENT_CTL: "));
+    debugPrintf((int)_device._MOTION_EVENT_CTL);
+    debugPrintln(F(""));
     return status;
   }
   return ICM_20948_Stat_DMPNotSupported;
@@ -1061,7 +1070,7 @@ ICM_20948_Status_e ICM_20948::readDMPmems(unsigned short reg, unsigned int lengt
   return ICM_20948_Stat_DMPNotSupported;
 }
 
-ICM_20948_Status_e ICM_20948::setDMPODRrate(enum DMP_ODR_Registers odr_reg, int rate)
+ICM_20948_Status_e ICM_20948::setDMPODRrate(enum DMP_ODR_Registers odr_reg, int interval)
 {
   if (_device._dmp_firmware_available == true) // Should we attempt to set the DMP ODR?
   {
@@ -1070,11 +1079,6 @@ ICM_20948_Status_e ICM_20948::setDMPODRrate(enum DMP_ODR_Registers odr_reg, int
   	// Value = (DMP running rate (225Hz) / ODR ) - 1
   	// E.g. For a 25Hz ODR rate, value= (225/25) - 1 = 8.
 
-    if (rate <= 0) // Check rate is valid
-        rate = 1;
-    if (rate > 225)
-        rate = 225;
-    uint16_t interval = (225 / rate) - 1;
     status = inv_icm20948_set_dmp_sensor_period(&_device, odr_reg, interval);
     return status;
   }
@@ -1091,6 +1095,21 @@ ICM_20948_Status_e ICM_20948::readDMPdataFromFIFO(icm_20948_DMP_data_t *data)
   return ICM_20948_Stat_DMPNotSupported;
 }
 
+ICM_20948_Status_e ICM_20948::setGyroSF(unsigned char div, int gyro_level)
+{
+  if (_device._dmp_firmware_available == true) // Should we attempt to set the Gyro SF?
+  {
+    status = inv_icm20948_set_gyro_sf(&_device, div, gyro_level);
+    debugPrint(F("ICM_20948::setGyroSF:  pll: "));
+    debugPrintf((int)_device._gyroSFpll);
+    debugPrint(F("  Gyro SF is: "));
+    debugPrintf((int)_device._gyroSF);
+    debugPrintln(F(""));
+    return status;
+  }
+  return ICM_20948_Stat_DMPNotSupported;
+}
+
 // I2C
 ICM_20948_I2C::ICM_20948_I2C()
 {
@@ -1140,6 +1159,12 @@ ICM_20948_Status_e ICM_20948_I2C::begin(TwoWire &wirePort, bool ad0val, uint8_t
     _device._firmware_loaded = false; // Initialize _firmware_loaded
     _device._last_bank = 255;  // Initialize _last_bank. Make it invalid. It will be set by the first call of ICM_20948_set_bank.
     _device._last_mems_bank = 255;  // Initialize _last_mems_bank. Make it invalid. It will be set by the first call of inv_icm20948_write_mems.
+    _device._gyroSF = 0; // Use this to record the GyroSF, calculated by inv_icm20948_set_gyro_sf
+		_device._gyroSFpll = 0;
+		_device._DATA_OUT_CTL1 = 0; // Keep a record of what sensors are enabled
+		_device._DATA_OUT_CTL2 = 0; // Keep a record of what header2 items are enabled
+		_device._DATA_RDY_STATUS = 0; // Keep a record of how Data Ready Status is configured
+		_device._MOTION_EVENT_CTL = 0; // Keep a record of how Motion Event Ctrl is configured
 
     // Perform default startup
     // Do a minimal startupDefault if using the DMP. User can always call startupDefault(false) manually if required.
@@ -1321,6 +1346,12 @@ ICM_20948_Status_e ICM_20948_SPI::begin(uint8_t csPin, SPIClass &spiPort, uint32
     _device._firmware_loaded = false; // Initialize _firmware_loaded
     _device._last_bank = 255;  // Initialize _last_bank. Make it invalid. It will be set by the first call of ICM_20948_set_bank.
     _device._last_mems_bank = 255;  // Initialize _last_mems_bank. Make it invalid. It will be set by the first call of inv_icm20948_write_mems.
+    _device._gyroSF = 0; // Use this to record the GyroSF, calculated by inv_icm20948_set_gyro_sf
+		_device._gyroSFpll = 0;
+		_device._DATA_OUT_CTL1 = 0; // Keep a record of what sensors are enabled
+		_device._DATA_OUT_CTL2 = 0; // Keep a record of what header2 items are enabled
+		_device._DATA_RDY_STATUS = 0; // Keep a record of how Data Ready Status is configured
+		_device._MOTION_EVENT_CTL = 0; // Keep a record of how Motion Event Ctrl is configured
 
     // Perform default startup
     // Do a minimal startupDefault if using the DMP. User can always call startupDefault(false) manually if required.

src/ICM_20948.h

@@ -206,8 +206,9 @@ class ICM_20948
     ICM_20948_Status_e enableDMPSensorInt(enum inv_icm20948_sensor sensor, bool enable = true);
     ICM_20948_Status_e writeDMPmems(unsigned short reg, unsigned int length, const unsigned char *data);
     ICM_20948_Status_e readDMPmems(unsigned short reg, unsigned int length, unsigned char *data);
-    ICM_20948_Status_e setDMPODRrate(enum DMP_ODR_Registers odr_reg, int rate);
+    ICM_20948_Status_e setDMPODRrate(enum DMP_ODR_Registers odr_reg, int interval);
     ICM_20948_Status_e readDMPdataFromFIFO(icm_20948_DMP_data_t *data);
+    ICM_20948_Status_e setGyroSF(unsigned char div, int gyro_level);
 
 };
 

src/util/ICM_20948_C.c

@@ -1300,7 +1300,7 @@ ICM_20948_Status_e inv_icm20948_write_mems(ICM_20948_Device_t *pdev, unsigned sh
 ICM_20948_Status_e inv_icm20948_read_mems(ICM_20948_Device_t *pdev, unsigned short reg, unsigned int length, unsigned char *data)
 {
 	ICM_20948_Status_e result = ICM_20948_Stat_Ok;
-	unsigned int bytesWritten = 0;
+	unsigned int bytesRead = 0;
 	unsigned int thisLen;
 	unsigned char lBankSelected;
 	unsigned char lStartAddrSelected;
@@ -1318,13 +1318,17 @@ ICM_20948_Status_e inv_icm20948_read_mems(ICM_20948_Device_t *pdev, unsigned sho
 
 	lBankSelected = (reg >> 8);
 
-	result = ICM_20948_execute_w(pdev, AGB0_REG_MEM_BANK_SEL, &lBankSelected, 1);
-	if (result != ICM_20948_Stat_Ok)
+	if (lBankSelected != pdev->_last_mems_bank)
 	{
-			return result;
+			pdev->_last_mems_bank = lBankSelected;
+			result = ICM_20948_execute_w(pdev, AGB0_REG_MEM_BANK_SEL, &lBankSelected, 1);
+			if (result != ICM_20948_Stat_Ok)
+			{
+					return result;
+			}
 	}
 
-	while (bytesWritten < length)
+	while (bytesRead < length)
 	{
 		lStartAddrSelected = (reg & 0xff);
 
@@ -1339,20 +1343,20 @@ ICM_20948_Status_e inv_icm20948_read_mems(ICM_20948_Device_t *pdev, unsigned sho
 				return result;
 		}
 
-		if (length-bytesWritten <= INV_MAX_SERIAL_READ)
-				thisLen = length-bytesWritten;
+		if (length-bytesRead <= INV_MAX_SERIAL_READ)
+				thisLen = length-bytesRead;
 		else
 				thisLen = INV_MAX_SERIAL_READ;
 
 		/* Read data */
 
-		result = ICM_20948_execute_r(pdev, AGB0_REG_MEM_R_W, &data[bytesWritten], thisLen);
+		result = ICM_20948_execute_r(pdev, AGB0_REG_MEM_R_W, &data[bytesRead], thisLen);
 		if (result != ICM_20948_Stat_Ok)
 		{
 				return result;
 		}
 
-		bytesWritten += thisLen;
+		bytesRead += thisLen;
 		reg += thisLen;
 	}
 
@@ -1361,7 +1365,7 @@ ICM_20948_Status_e inv_icm20948_read_mems(ICM_20948_Device_t *pdev, unsigned sho
 
 ICM_20948_Status_e inv_icm20948_set_dmp_sensor_period(ICM_20948_Device_t *pdev, enum DMP_ODR_Registers odr_reg, uint16_t interval)
 {
-	// Set the ODR registersand clear the ODR counter
+	// Set the ODR registers and clear the ODR counter
 
 	// In order to set an ODR for a given sensor data, write 2-byte value to DMP using key defined above for a particular sensor.
 	// Setting value can be calculated as follows:
@@ -1529,6 +1533,10 @@ ICM_20948_Status_e inv_icm20948_enable_dmp_sensor(ICM_20948_Device_t *pdev, enum
 		}
 		// TO DO: Add DMP_Data_Output_Control_2_Pickup etc. if required
 
+		// Keep a record of DATA_OUT_CTL1 so multiple sensors can be configured
+		delta |= pdev->_DATA_OUT_CTL1;
+		pdev->_DATA_OUT_CTL1 = delta;
+
 		// Write the sensor control bits into memory address DATA_OUT_CTL1
 		unsigned char data_output_control_reg[2];
     data_output_control_reg[0] = (unsigned char)(delta >> 8);
@@ -1539,6 +1547,10 @@ ICM_20948_Status_e inv_icm20948_enable_dmp_sensor(ICM_20948_Device_t *pdev, enum
 				return result;
 		}
 
+		// Keep a record of DATA_OUT_CTL2 so multiple sensors can be configured
+		delta2 |= pdev->_DATA_OUT_CTL2;
+		pdev->_DATA_OUT_CTL2 = delta2;
+
 		// Write the 'header2' sensor control bits into memory address DATA_OUT_CTL2
 		data_output_control_reg[0] = (unsigned char)(delta2 >> 8);
     data_output_control_reg[1] = (unsigned char)(delta2 & 0xff);
@@ -1589,6 +1601,11 @@ ICM_20948_Status_e inv_icm20948_enable_dmp_sensor(ICM_20948_Device_t *pdev, enum
 						inv_event_control |= DMP_Motion_Event_Control_Compass_Calibr;
 				}
 		}
+
+		// Keep a record of _DATA_RDY_STATUS so multiple sensors can be configured
+		data_rdy_status |= pdev->_DATA_RDY_STATUS;
+		pdev->_DATA_RDY_STATUS = data_rdy_status;
+
 		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);
@@ -1611,6 +1628,11 @@ ICM_20948_Status_e inv_icm20948_enable_dmp_sensor(ICM_20948_Device_t *pdev, enum
 		{
 				inv_event_control |= DMP_Motion_Event_Control_Geomag;
 		}
+
+		// Keep a record of _MOTION_EVENT_CTL so multiple sensors can be configured
+		inv_event_control |= pdev->_MOTION_EVENT_CTL;
+		pdev->_MOTION_EVENT_CTL = inv_event_control;
+
 		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);
@@ -2171,6 +2193,7 @@ static uint8_t sensor_type_2_android_sensor(enum inv_icm20948_sensor sensor)
 	case INV_ICM20948_SENSOR_LINEAR_ACCELERATION:           return ANDROID_SENSOR_LINEAR_ACCELERATION; // 10
 	case INV_ICM20948_SENSOR_ORIENTATION:                   return ANDROID_SENSOR_ORIENTATION; // 3
 	case INV_ICM20948_SENSOR_B2S:                           return ANDROID_SENSOR_B2S; // 45
+	case INV_ICM20948_SENSOR_RAW_MAGNETOMETER:              return ANDROID_SENSOR_WAKEUP_MAGNETIC_FIELD_UNCALIBRATED; // 34
 	default:                                                return ANDROID_SENSOR_NUM_MAX;
 	}
 }
@@ -2198,6 +2221,65 @@ enum inv_icm20948_sensor inv_icm20948_sensor_android_2_sensor_type(int sensor)
 	case ANDROID_SENSOR_LINEAR_ACCELERATION:              return INV_ICM20948_SENSOR_LINEAR_ACCELERATION;
 	case ANDROID_SENSOR_ORIENTATION:                      return INV_ICM20948_SENSOR_ORIENTATION;
 	case ANDROID_SENSOR_B2S:                              return INV_ICM20948_SENSOR_B2S;
+	case ANDROID_SENSOR_WAKEUP_MAGNETIC_FIELD_UNCALIBRATED: return INV_ICM20948_SENSOR_RAW_MAGNETOMETER;
 	default:                                              return INV_ICM20948_SENSOR_MAX;
 	}
 }
+
+ICM_20948_Status_e inv_icm20948_set_gyro_sf(ICM_20948_Device_t *pdev, unsigned char div, int gyro_level)
+{
+	ICM_20948_Status_e result = ICM_20948_Stat_Ok;
+
+	if (pdev->_dmp_firmware_available == false)
+			return ICM_20948_Stat_DMPNotSupported;
+
+	// gyro_level should be set to 4 regardless of fullscale, due to the addition of API dmp_icm20648_set_gyro_fsr()
+	gyro_level = 4;
+
+	// First read the TIMEBASE_CORRECTION_PLL register from Bank 1
+	int8_t pll; // Signed. Typical value is 0x18
+	result = ICM_20948_set_bank(pdev, 1);
+	result = ICM_20948_execute_r(pdev, AGB1_REG_TIMEBASE_CORRECTION_PLL, (uint8_t *)&pll, 1);
+	if (result != ICM_20948_Stat_Ok)
+	{
+			return result;
+	}
+
+	pdev->_gyroSFpll = pll; // Record the PLL value so we can debug print it
+
+	// Now calculate the Gyro SF using code taken from the InvenSense example (inv_icm20948_set_gyro_sf)
+
+	long gyro_sf;
+
+	unsigned long long const MagicConstant = 264446880937391LL;
+	unsigned long long const MagicConstantScale = 100000LL;
+	unsigned long long ResultLL;
+
+	if (pll & 0x80) {
+		ResultLL = (MagicConstant * (long long)(1ULL << gyro_level) * (1 + div) / (1270 - (pll & 0x7F)) / MagicConstantScale);
+	}
+	else {
+		ResultLL = (MagicConstant * (long long)(1ULL << gyro_level) * (1 + div) / (1270 + pll) / MagicConstantScale);
+	}
+	/*
+	    In above deprecated FP version, worst case arguments can produce a result that overflows a signed long.
+	    Here, for such cases, we emulate the FP behavior of setting the result to the maximum positive value, as
+	    the compiler's conversion of a u64 to an s32 is simple truncation of the u64's high half, sadly....
+	*/
+	if  (ResultLL > 0x7FFFFFFF)
+		gyro_sf = 0x7FFFFFFF;
+	else
+		gyro_sf = (long)ResultLL;
+
+	pdev->_gyroSF = gyro_sf; // Record value so we can debug print it
+
+	// Finally, write the value to the DMP GYRO_SF register
+	unsigned char gyro_sf_reg[4];
+	gyro_sf_reg[0] = (unsigned char)(gyro_sf >> 24);
+	gyro_sf_reg[1] = (unsigned char)(gyro_sf >> 16);
+	gyro_sf_reg[2] = (unsigned char)(gyro_sf >> 8);
+	gyro_sf_reg[3] = (unsigned char)(gyro_sf & 0xff);
+	result = inv_icm20948_write_mems(pdev, GYRO_SF, 4, (const unsigned char *)&gyro_sf_reg);
+
+	return result;
+}

src/util/ICM_20948_C.h

@@ -161,6 +161,12 @@ extern int memcmp(const void *, const void *, size_t); // Avoid compiler warning
 		bool _firmware_loaded; // Indicates if DMP has been loaded
 		uint8_t _last_bank; // Keep track of which bank was selected last - to avoid unnecessary writes
 		uint8_t _last_mems_bank; // Keep track of which bank was selected last - to avoid unnecessary writes
+		int32_t _gyroSF; // Use this to record the GyroSF, calculated by inv_icm20948_set_gyro_sf
+		int8_t _gyroSFpll;
+		uint16_t _DATA_OUT_CTL1; // Keep a record of what sensors are enabled
+		uint16_t _DATA_OUT_CTL2; // Keep a record of what header2 items are enabled
+		uint16_t _DATA_RDY_STATUS; // Keep a record of how Data Ready Status is configured
+		uint16_t _MOTION_EVENT_CTL; // Keep a record of how Motion Event Ctrl is configured
 	} ICM_20948_Device_t;				 // Definition of device struct type
 
 	/*
@@ -268,6 +274,7 @@ extern int memcmp(const void *, const void *, size_t); // Avoid compiler warning
 	enum inv_icm20948_sensor inv_icm20948_sensor_android_2_sensor_type(int sensor);
 
 	ICM_20948_Status_e inv_icm20948_read_dmp_data(ICM_20948_Device_t *pdev, icm_20948_DMP_data_t *data);
+	ICM_20948_Status_e inv_icm20948_set_gyro_sf(ICM_20948_Device_t *pdev, unsigned char div, int gyro_level);
 
 	// ToDo:
 

src/util/ICM_20948_DMP.h

@@ -116,7 +116,7 @@ extern "C" {
 #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_CAL_RATE			(94 * 16 + 4) // 16-bit: 0 (225Hz, 112Hz, 56Hz)
 #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)
@@ -327,6 +327,7 @@ enum inv_icm20948_sensor {
 	INV_ICM20948_SENSOR_LINEAR_ACCELERATION,
 	INV_ICM20948_SENSOR_ORIENTATION,
 	INV_ICM20948_SENSOR_B2S,
+	INV_ICM20948_SENSOR_RAW_MAGNETOMETER,
 	INV_ICM20948_SENSOR_MAX,
 };