diff --git a/README.md b/README.md index 14b6804..0ac6bf1 100644 --- a/README.md +++ b/README.md @@ -5,6 +5,47 @@ This software is a sample program for the [RT-USB-9axisIMU3](https://www.rt-shop The file RT-9DOF-IMU-V3-Quat.ino is forked from the [SparkFun_ICM-20948_ArduinoLibrary examples](https://github.com/sparkfun/SparkFun_ICM-20948_ArduinoLibrary/tree/main/examples). +## Data format +Baud rate: 2000000 bps +Transmission frequency: 1 kHz + +|Byte | Content | Description | +|-- | -- | -- | +|0 | 0xff | Header | +|1 | 0xff | Header | +|2 | 0x52 | ASCII code "R" | +|3 | 0x54 | ASCII code "T" | +|4 | 0x40 | Lower Bits of Product Identifier | +|5 | 0x41 | Upper Bits of Product Identifier | +|6 | 0x01 | FW version | +|7 | 0x00~0xff | Timestamp - incremented with each data transmission, wraps to 0x00 after 0xff | +|8 | ACC_X_L | ax=(ACC_X_L\|ACC_X_H<<8)/2048*9.8 [m/s] | +|9 | ACC_X_H | | +|10 | ACC_Y_L | | +|11 | ACC_Y_H | | +|12 | ACC_Z_L | | +|13 | ACC_Z_H | | +|14 | GYRO_X_L | gx=(GYRO_X_L\|GYRO_X_H<<8)/(32767/2000)*PI/180 [rad/s] | +|15 | GYRO_X_H | | +|16 | GYRO_Y_L | | +|17 | GYRO_Y_H | | +|18 | GYRO_Z_L | | +|19 | GYRO_Z_H | | +|20 | Q1_1 | Q1 multiplied by (2^30). q1=(Q1_1 \| Q1_2<<8 \| Q1_3<<16 \| Q1_4<<24 )/(2^30) | +|21 | Q1_2 | | +|22 | Q1_3 | | +|23 | Q1_4 | | +|24 | Q2_1 | | +|25 | Q2_2 | | +|26 | Q2_3 | | +|27 | Q2_4 | | +|28 | Q3_1 | | +|29 | Q3_2 | | +|30 | Q3_3 | | +|31 | Q3_4 | | +|32 | Checksum | Lower 8 bits of the sum of all bytes | + + ## Flashing from the Arduino IDE ### Setting Up Dependencies diff --git a/STM32duino_sample/RT-9DOF-IMU-V3-Quat/RT-9DOF-IMU-V3-Quat.ino b/STM32duino_sample/RT-9DOF-IMU-V3-Quat/RT-9DOF-IMU-V3-Quat.ino index 42785eb..9e8d11e 100644 --- a/STM32duino_sample/RT-9DOF-IMU-V3-Quat/RT-9DOF-IMU-V3-Quat.ino +++ b/STM32duino_sample/RT-9DOF-IMU-V3-Quat/RT-9DOF-IMU-V3-Quat.ino @@ -8,10 +8,10 @@ * 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 14301 Bytes of program memory. * ** To use the DMP, you will need to: @@ -26,16 +26,15 @@ * Distributed as-is; no warranty is given. ***************************************************************/ - #include "ICM_20948.h" // Click here to get the library: http://librarymanager/All#SparkFun_ICM_20948_IMU // should install v1.2.5 #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 SPI_PORT SPI // Your desired SPI port. Used only when "USE_SPI" is defined #define SPI_FREQ 5000000 // You can override the default SPI frequency -#define CS_PIN D4 // Which pin you connect CS to. Used only when "USE_SPI" is defined +#define CS_PIN D4 // 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 // The value of the last bit of the I2C address. @@ -51,6 +50,16 @@ ICM_20948_I2C myICM; // Otherwise create an ICM_20948_I2C object #define LED_BUILTIN PA15 HardwareTimer *Timer6 = new HardwareTimer(TIM6); +#define FW_VERSION 0x01 + +uint8_t cal_checksum(const uint8_t *data, size_t size) { + uint8_t sum = 0; + for (size_t i = 0; i < size; i++) { + sum += data[i]; + } + return sum; +} + void timerInterrupt(void) { // Read any DMP data waiting in the FIFO @@ -62,45 +71,86 @@ void timerInterrupt(void) { // 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); + uint8_t raw_data[33]; + static uint8_t time_stamp = 0; if ((myICM.status == ICM_20948_Stat_Ok) || (myICM.status == ICM_20948_Stat_FIFOMoreDataAvail)) // Was valid data available? { - if ((data.header & DMP_header_bitmap_Quat6) > 0)// && (data.header & DMP_header_bitmap_Gyro) > 0) + if ((data.header & DMP_header_bitmap_Quat6) > 0) // && (data.header & DMP_header_bitmap_Gyro) > 0) { - double q1 = ((double)data.Quat6.Data.Q1) / 1073741824.0; // Convert to double. Divide by 2^30 - double q2 = ((double)data.Quat6.Data.Q2) / 1073741824.0; // Convert to double. Divide by 2^30 - double q3 = ((double)data.Quat6.Data.Q3) / 1073741824.0; // Convert to double. Divide by 2^30 - double q0 = sqrt(1.0 - ((q1 * q1) + (q2 * q2) + (q3 * q3))); - - double gx = ((double)data.Raw_Gyro.Data.X) / 16.3835 * PI / 180.0; // [rad/s] 16.3835=32767/2000 deg2rad - double gy = ((double)data.Raw_Gyro.Data.Y) / 16.3835 * PI / 180.0; - double gz = ((double)data.Raw_Gyro.Data.Z) / 16.3835 * PI / 180.0; - - uint32_t time_sec = micros()/1000000; - uint32_t time_subsecs = micros() - time_sec*1000000; - - SERIAL_PORT.print(time_sec); - SERIAL_PORT.print(F(".")); - SERIAL_PORT.print(time_subsecs); - SERIAL_PORT.print(F(",")); - SERIAL_PORT.print(q1, 4); - SERIAL_PORT.print(F(",")); - SERIAL_PORT.print(q2, 4); - SERIAL_PORT.print(F(",")); - SERIAL_PORT.print(q3, 4); - SERIAL_PORT.print(F(",")); - SERIAL_PORT.print(q0, 4); - SERIAL_PORT.print(F(",")); - SERIAL_PORT.print(gx, 4); - SERIAL_PORT.print(F(",")); - SERIAL_PORT.print(gy, 4); - SERIAL_PORT.print(F(",")); - SERIAL_PORT.print(gz, 4); - SERIAL_PORT.println(F("")); - + // header + raw_data[0] = 0xff; // header1 + raw_data[1] = 0xff; // header2 + raw_data[2] = 0x52; // ASCII 'R' + raw_data[3] = 0x54; // ASCII 'T' + raw_data[4] = 0x40; // device ID_L + raw_data[5] = 0x41; // device ID_H + raw_data[6] = FW_VERSION; // FW version + + // data + raw_data[7] = time_stamp++; + raw_data[8] = (data.Raw_Accel.Data.X >> 0) & 0xFF; + raw_data[9] = (data.Raw_Accel.Data.X >> 8) & 0xFF; + raw_data[10] = (data.Raw_Accel.Data.Y >> 0) & 0xFF; + raw_data[11] = (data.Raw_Accel.Data.Y >> 8) & 0xFF; + raw_data[12] = (data.Raw_Accel.Data.Z >> 0) & 0xFF; + raw_data[13] = (data.Raw_Accel.Data.Z >> 8) & 0xFF; + + raw_data[14] = (data.Raw_Gyro.Data.X >> 0) & 0xFF; + raw_data[15] = (data.Raw_Gyro.Data.X >> 8) & 0xFF; + raw_data[16] = (data.Raw_Gyro.Data.Y >> 0) & 0xFF; + raw_data[17] = (data.Raw_Gyro.Data.Y >> 8) & 0xFF; + raw_data[18] = (data.Raw_Gyro.Data.Z >> 0) & 0xFF; + raw_data[19] = (data.Raw_Gyro.Data.Z >> 8) & 0xFF; + + raw_data[20] = (data.Quat6.Data.Q1 >> 0) & 0xFF; + raw_data[21] = (data.Quat6.Data.Q1 >> 8) & 0xFF; + raw_data[22] = (data.Quat6.Data.Q1 >> 16) & 0xFF; + raw_data[23] = (data.Quat6.Data.Q1 >> 24) & 0xFF; + raw_data[24] = (data.Quat6.Data.Q2 >> 0) & 0xFF; + raw_data[25] = (data.Quat6.Data.Q2 >> 8) & 0xFF; + raw_data[26] = (data.Quat6.Data.Q2 >> 16) & 0xFF; + raw_data[27] = (data.Quat6.Data.Q2 >> 24) & 0xFF; + raw_data[28] = (data.Quat6.Data.Q3 >> 0) & 0xFF; + raw_data[29] = (data.Quat6.Data.Q3 >> 8) & 0xFF; + raw_data[30] = (data.Quat6.Data.Q3 >> 16) & 0xFF; + raw_data[31] = (data.Quat6.Data.Q3 >> 24) & 0xFF; + + raw_data[32] = cal_checksum(raw_data, 32); + + SERIAL_PORT.write(raw_data, 33); + + // double q1 = ((double)data.Quat6.Data.Q1) / 1073741824.0; // Convert to double. Divide by 2^30 + // double q2 = ((double)data.Quat6.Data.Q2) / 1073741824.0; // Convert to double. Divide by 2^30 + // double q3 = ((double)data.Quat6.Data.Q3) / 1073741824.0; // Convert to double. Divide by 2^30 + // double q0 = sqrt(1.0 - ((q1 * q1) + (q2 * q2) + (q3 * q3))); + + // double gx = ((double)data.Raw_Gyro.Data.X) / 16.3835 * PI / 180.0; // [rad/s] 16.3835=32767/2000 deg2rad + // double gy = ((double)data.Raw_Gyro.Data.Y) / 16.3835 * PI / 180.0; + // double gz = ((double)data.Raw_Gyro.Data.Z) / 16.3835 * PI / 180.0; + + // uint32_t time_sec = micros() / 1000000; + // uint32_t time_subsecs = micros() - time_sec * 1000000; + + // SERIAL_PORT.print(time_sec); + // SERIAL_PORT.print(F(".")); + // SERIAL_PORT.print(time_subsecs); + // SERIAL_PORT.print(F(",")); + // SERIAL_PORT.print(q1, 4); + // SERIAL_PORT.print(F(",")); + // SERIAL_PORT.print(q2, 4); + // SERIAL_PORT.print(F(",")); + // SERIAL_PORT.print(q3, 4); + // SERIAL_PORT.print(F(",")); + // SERIAL_PORT.print(q0, 4); + // SERIAL_PORT.print(F(",")); + // SERIAL_PORT.print(gx, 4); + // SERIAL_PORT.print(F(",")); + // SERIAL_PORT.print(gy, 4); + // SERIAL_PORT.print(F(",")); + // SERIAL_PORT.print(gz, 4); + // SERIAL_PORT.println(F("")); } - - } if (myICM.status != ICM_20948_Stat_FIFOMoreDataAvail) // If more data is available then we should read it right away - and not delay @@ -139,7 +189,6 @@ void setup() { } } - bool success = true; // Use success to show if the DMP configuration was successful // Initialize the DMP. initializeDMP is a weak function. You can overwrite it if you want to e.g. to change the sample rate @@ -168,8 +217,8 @@ void setup() { // Enable any additional sensors / features // success &= (myICM.enableDMPSensor(INV_ICM20948_SENSOR_GYROSCOPE) == ICM_20948_Stat_Ok); success &= (myICM.enableDMPSensor(INV_ICM20948_SENSOR_RAW_GYROSCOPE) == ICM_20948_Stat_Ok); - //success &= (myICM.enableDMPSensor(INV_ICM20948_SENSOR_RAW_ACCELEROMETER) == ICM_20948_Stat_Ok); - //success &= (myICM.enableDMPSensor(INV_ICM20948_SENSOR_MAGNETIC_FIELD_UNCALIBRATED) == ICM_20948_Stat_Ok); + success &= (myICM.enableDMPSensor(INV_ICM20948_SENSOR_RAW_ACCELEROMETER) == ICM_20948_Stat_Ok); + // success &= (myICM.enableDMPSensor(INV_ICM20948_SENSOR_MAGNETIC_FIELD_UNCALIBRATED) == 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. @@ -178,11 +227,11 @@ void setup() { // E.g. For a 5Hz ODR rate when DMP is running at 55Hz, value = (55/5) - 1 = 10. // success &= (myICM.setDMPODRrate(DMP_ODR_Reg_Quat9, 0) == ICM_20948_Stat_Ok); // Set to the maximum success &= (myICM.setDMPODRrate(DMP_ODR_Reg_Quat6, 0) == ICM_20948_Stat_Ok); // Set to the maximum - //success &= (myICM.setDMPODRrate(DMP_ODR_Reg_Accel, 0) == ICM_20948_Stat_Ok); // Set to the maximum - success &= (myICM.setDMPODRrate(DMP_ODR_Reg_Gyro, 0) == ICM_20948_Stat_Ok); // Set to the maximum - //success &= (myICM.setDMPODRrate(DMP_ODR_Reg_Gyro_Calibr, 0) == ICM_20948_Stat_Ok); // Set to the maximum - //success &= (myICM.setDMPODRrate(DMP_ODR_Reg_Cpass, 0) == ICM_20948_Stat_Ok); // Set to the maximum - //success &= (myICM.setDMPODRrate(DMP_ODR_Reg_Cpass_Calibr, 0) == ICM_20948_Stat_Ok); // Set to the maximum + // success &= (myICM.setDMPODRrate(DMP_ODR_Reg_Accel, 0) == ICM_20948_Stat_Ok); // Set to the maximum + success &= (myICM.setDMPODRrate(DMP_ODR_Reg_Gyro, 0) == ICM_20948_Stat_Ok); // Set to the maximum + // success &= (myICM.setDMPODRrate(DMP_ODR_Reg_Gyro_Calibr, 0) == ICM_20948_Stat_Ok); // Set to the maximum + // success &= (myICM.setDMPODRrate(DMP_ODR_Reg_Cpass, 0) == ICM_20948_Stat_Ok); // Set to the maximum + // success &= (myICM.setDMPODRrate(DMP_ODR_Reg_Cpass_Calibr, 0) == ICM_20948_Stat_Ok); // Set to the maximum // Enable the FIFO success &= (myICM.enableFIFO() == ICM_20948_Stat_Ok); @@ -211,7 +260,7 @@ void setup() { Timer6->setOverflow(1000, MICROSEC_FORMAT); Timer6->attachInterrupt(timerInterrupt); Timer6->refresh(); - Timer6->resume(); //Timter Start + Timer6->resume(); // Timter Start } void loop() { @@ -225,21 +274,19 @@ void loop() { } // initializeDMP is a weak function. Let's overwrite it so we can increase the sample rate -ICM_20948_Status_e ICM_20948::initializeDMP(void) -{ +ICM_20948_Status_e ICM_20948::initializeDMP(void) { // First, let's check if the DMP is available - if (_device._dmp_firmware_available != true) - { + if (_device._dmp_firmware_available != true) { debugPrint(F("ICM_20948::startupDMP: DMP is not available. Please check that you have uncommented line 29 (#define ICM_20948_USE_DMP) in ICM_20948_C.h...")); return ICM_20948_Stat_DMPNotSupported; } - ICM_20948_Status_e worstResult = ICM_20948_Stat_Ok; + ICM_20948_Status_e worstResult = ICM_20948_Stat_Ok; // 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". - ICM_20948_Status_e result = ICM_20948_Stat_Ok; // Use result and worstResult to show if the configuration was successful + ICM_20948_Status_e result = ICM_20948_Stat_Ok; // Use result and worstResult to show if the configuration was successful // Normally, when the DMP is not enabled, startupMagnetometer (called by startupDefault, which is called by begin) configures the AK09916 magnetometer // to run at 100Hz by setting the CNTL2 register (0x31) to 0x08. Then the ICM20948's I2C_SLV0 is configured to read @@ -262,7 +309,9 @@ ICM_20948_Status_e ICM_20948::initializeDMP(void) // false: clear the I2C_SLV0_CTRL I2C_SLV0_REG_DIS (we want to write the register value) // true: set the I2C_SLV0_CTRL I2C_SLV0_GRP bit to show the register pairing starts at byte 1+2 (copied from inv_icm20948_resume_akm) // true: set the I2C_SLV0_CTRL I2C_SLV0_BYTE_SW to byte-swap the data from the mag (copied from inv_icm20948_resume_akm) - result = i2cControllerConfigurePeripheral(0, MAG_AK09916_I2C_ADDR, AK09916_REG_RSV2, 10, true, true, false, true, true); if (result > worstResult) worstResult = result; + result = i2cControllerConfigurePeripheral(0, MAG_AK09916_I2C_ADDR, AK09916_REG_RSV2, 10, true, true, false, true, true); + if (result > worstResult) + worstResult = result; // // We also need to set up I2C_SLV1 to do the Single Measurement triggering: // 1: use I2C_SLV1 @@ -275,7 +324,9 @@ ICM_20948_Status_e ICM_20948::initializeDMP(void) // false: clear the I2C_SLV0_CTRL I2C_SLV0_GRP bit // false: clear the I2C_SLV0_CTRL I2C_SLV0_BYTE_SW bit // AK09916_mode_single: tell I2C_SLV1 to write the Single Measurement command each sample - result = i2cControllerConfigurePeripheral(1, MAG_AK09916_I2C_ADDR, AK09916_REG_CNTL2, 1, false, true, false, false, false, AK09916_mode_single); if (result > worstResult) worstResult = result; + result = i2cControllerConfigurePeripheral(1, MAG_AK09916_I2C_ADDR, AK09916_REG_CNTL2, 1, false, true, false, false, false, AK09916_mode_single); + if (result > worstResult) + worstResult = result; // Set the I2C Master ODR configuration // It is not clear why we need to do this... But it appears to be essential! From the datasheet: @@ -285,106 +336,156 @@ ICM_20948_Status_e ICM_20948::initializeDMP(void) // If gyroscope is disabled, then all sensors (including I2C_MASTER) use the accelerometer ODR." // Since both gyro and accel are running, setting this register should have no effect. But it does. Maybe because the Gyro and Accel are placed in Low Power Mode (cycled)? // You can see by monitoring the Aux I2C pins that the next three lines reduce the bus traffic (magnetometer reads) from 1125Hz to the chosen rate: 68.75Hz in this case. - result = setBank(3); if (result > worstResult) worstResult = result; // Select Bank 3 - uint8_t mstODRconfig = 0x04; // Set the ODR configuration to 1100/2^4 = 68.75Hz - result = write(AGB3_REG_I2C_MST_ODR_CONFIG, &mstODRconfig, 1); if (result > worstResult) worstResult = result; // Write one byte to the I2C_MST_ODR_CONFIG register + result = setBank(3); + if (result > worstResult) + worstResult = result; // Select Bank 3 + uint8_t mstODRconfig = 0x04; // Set the ODR configuration to 1100/2^4 = 68.75Hz + result = write(AGB3_REG_I2C_MST_ODR_CONFIG, &mstODRconfig, 1); + if (result > worstResult) + worstResult = result; // Write one byte to the I2C_MST_ODR_CONFIG register // 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 - result = setClockSource(ICM_20948_Clock_Auto); if (result > worstResult) worstResult = result; // This is shorthand: success will be set to false if setClockSource fails + result = setClockSource(ICM_20948_Clock_Auto); + if (result > worstResult) + worstResult = result; // 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 - result = setBank(0); if (result > worstResult) worstResult = result; // Select Bank 0 - uint8_t pwrMgmt2 = 0x40; // Set the reserved bit 6 (pressure sensor disable?) - result = write(AGB0_REG_PWR_MGMT_2, &pwrMgmt2, 1); if (result > worstResult) worstResult = result; // Write one byte to the PWR_MGMT_2 register + result = setBank(0); + if (result > worstResult) + worstResult = result; // Select Bank 0 + uint8_t pwrMgmt2 = 0x40; // Set the reserved bit 6 (pressure sensor disable?) + result = write(AGB0_REG_PWR_MGMT_2, &pwrMgmt2, 1); + if (result > worstResult) + worstResult = result; // Write one byte to the PWR_MGMT_2 register // Place _only_ I2C_Master in Low Power Mode (cycled) via LP_CONFIG // The InvenSense Nucleo example initially puts the accel and gyro into low power mode too, but then later updates LP_CONFIG so only the I2C_Master is in Low Power Mode - result = setSampleMode(ICM_20948_Internal_Mst, ICM_20948_Sample_Mode_Cycled); if (result > worstResult) worstResult = result; + result = setSampleMode(ICM_20948_Internal_Mst, ICM_20948_Sample_Mode_Cycled); + if (result > worstResult) + worstResult = result; // Disable the FIFO - result = enableFIFO(false); if (result > worstResult) worstResult = result; + result = enableFIFO(false); + if (result > worstResult) + worstResult = result; // Disable the DMP - result = enableDMP(false); if (result > worstResult) worstResult = result; + result = enableDMP(false); + if (result > worstResult) + worstResult = result; // 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 - result = setFullScale((ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), myFSS); if (result > worstResult) worstResult = result; + // Set Accel FSR (Full scale range) to 16g 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 = gpm16; // (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 + result = setFullScale((ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), myFSS); + if (result > worstResult) + worstResult = result; // The InvenSense Nucleo code also enables the gyro DLPF (but leaves GYRO_DLPFCFG set to zero = 196.6Hz (3dB)) // We found this by going through the SPI data generated by ZaneL's Teensy-ICM-20948 library byte by byte... // The gyro DLPF is enabled by default (GYRO_CONFIG_1 = 0x01) so the following line should have no effect, but we'll include it anyway - result = enableDLPF(ICM_20948_Internal_Gyr, true); if (result > worstResult) worstResult = result; + result = enableDLPF(ICM_20948_Internal_Gyr, true); + if (result > worstResult) + worstResult = result; // Enable interrupt for FIFO overflow from FIFOs through INT_ENABLE_2 // If we see this interrupt, we'll need to reset the FIFO - //result = intEnableOverflowFIFO( 0x1F ); if (result > worstResult) worstResult = result; // Enable the interrupt on all FIFOs + // result = intEnableOverflowFIFO( 0x1F ); if (result > worstResult) worstResult = result; // 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 - result = setBank(0); if (result > worstResult) worstResult = result; // Select Bank 0 + result = setBank(0); + if (result > worstResult) + worstResult = result; // Select Bank 0 uint8_t zero = 0; - result = write(AGB0_REG_FIFO_EN_1, &zero, 1); if (result > worstResult) worstResult = result; + result = write(AGB0_REG_FIFO_EN_1, &zero, 1); + if (result > worstResult) + worstResult = result; // Stop the accelerometer, gyro and temperature data from being written to the FIFO by writing zero to FIFO_EN_2 - result = write(AGB0_REG_FIFO_EN_2, &zero, 1); if (result > worstResult) worstResult = result; + result = write(AGB0_REG_FIFO_EN_2, &zero, 1); + if (result > worstResult) + worstResult = result; // Turn off data ready interrupt through INT_ENABLE_1 - result = intEnableRawDataReady(false); if (result > worstResult) worstResult = result; + result = intEnableRawDataReady(false); + if (result > worstResult) + worstResult = result; // Reset FIFO through FIFO_RST - result = resetFIFO(); if (result > worstResult) worstResult = result; + result = resetFIFO(); + if (result > worstResult) + worstResult = result; // 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 = 19; // ODR is computed as follows: 1.1 kHz/(1+GYRO_SMPLRT_DIV[7:0]). 19 = 55Hz. InvenSense Nucleo example uses 19 (0x13). - //mySmplrt.a = 19; // ODR is computed as follows: 1.125 kHz/(1+ACCEL_SMPLRT_DIV[11:0]). 19 = 56.25Hz. InvenSense Nucleo example uses 19 (0x13). - mySmplrt.g = 4; // 225Hz - mySmplrt.a = 4; // 225Hz - //mySmplrt.g = 8; // 112Hz - //mySmplrt.a = 8; // 112Hz - result = setSampleRate((ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), mySmplrt); if (result > worstResult) worstResult = result; + // mySmplrt.g = 19; // ODR is computed as follows: 1.1 kHz/(1+GYRO_SMPLRT_DIV[7:0]). 19 = 55Hz. InvenSense Nucleo example uses 19 (0x13). + // mySmplrt.a = 19; // ODR is computed as follows: 1.125 kHz/(1+ACCEL_SMPLRT_DIV[11:0]). 19 = 56.25Hz. InvenSense Nucleo example uses 19 (0x13). + mySmplrt.g = 4; // 225Hz + mySmplrt.a = 4; // 225Hz + // mySmplrt.g = 8; // 112Hz + // mySmplrt.a = 8; // 112Hz + result = setSampleRate((ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), mySmplrt); + if (result > worstResult) + worstResult = result; // Setup DMP start address through PRGM_STRT_ADDRH/PRGM_STRT_ADDRL - result = setDMPstartAddress(); if (result > worstResult) worstResult = result; // Defaults to DMP_START_ADDRESS + result = setDMPstartAddress(); + if (result > worstResult) + worstResult = result; // Defaults to DMP_START_ADDRESS // Now load the DMP firmware - result = loadDMPFirmware(); if (result > worstResult) worstResult = result; + result = loadDMPFirmware(); + if (result > worstResult) + worstResult = result; // Write the 2 byte Firmware Start Value to ICM PRGM_STRT_ADDRH/PRGM_STRT_ADDRL - result = setDMPstartAddress(); if (result > worstResult) worstResult = result; // Defaults to DMP_START_ADDRESS + result = setDMPstartAddress(); + if (result > worstResult) + worstResult = result; // Defaults to DMP_START_ADDRESS // Set the Hardware Fix Disable register to 0x48 - result = setBank(0); if (result > worstResult) worstResult = result; // Select Bank 0 + result = setBank(0); + if (result > worstResult) + worstResult = result; // Select Bank 0 uint8_t fix = 0x48; - result = write(AGB0_REG_HW_FIX_DISABLE, &fix, 1); if (result > worstResult) worstResult = result; + result = write(AGB0_REG_HW_FIX_DISABLE, &fix, 1); + if (result > worstResult) + worstResult = result; // Set the Single FIFO Priority Select register to 0xE4 - result = setBank(0); if (result > worstResult) worstResult = result; // Select Bank 0 + result = setBank(0); + if (result > worstResult) + worstResult = result; // Select Bank 0 uint8_t fifoPrio = 0xE4; - result = write(AGB0_REG_SINGLE_FIFO_PRIORITY_SEL, &fifoPrio, 1); if (result > worstResult) worstResult = result; + result = write(AGB0_REG_SINGLE_FIFO_PRIORITY_SEL, &fifoPrio, 1); + if (result > worstResult) + worstResult = result; // 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 0x04000000 when FSR is 4g - const unsigned char accScale[4] = {0x04, 0x00, 0x00, 0x00}; - result = writeDMPmems(ACC_SCALE, 4, &accScale[0]); if (result > worstResult) worstResult = result; // Write accScale to ACC_SCALE DMP register + const unsigned char accScale[4] = { 0x10, 0x00, 0x00, 0x00 }; + result = writeDMPmems(ACC_SCALE, 4, &accScale[0]); + if (result > worstResult) + worstResult = result; // Write accScale to ACC_SCALE DMP register // In order to output hardware unit data as configured FSR write 0x00040000 when FSR is 4g - const unsigned char accScale2[4] = {0x00, 0x04, 0x00, 0x00}; - result = writeDMPmems(ACC_SCALE2, 4, &accScale2[0]); if (result > worstResult) worstResult = result; // Write accScale2 to ACC_SCALE2 DMP register + const unsigned char accScale2[4] = { 0x00, 0x01, 0x00, 0x00 }; + result = writeDMPmems(ACC_SCALE2, 4, &accScale2[0]); + if (result > worstResult) + worstResult = result; // Write accScale2 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. @@ -395,78 +496,127 @@ ICM_20948_Status_e ICM_20948::initializeDMP(void) // Z = raw_x * CPASS_MTX_20 + raw_y * CPASS_MTX_21 + raw_z * CPASS_MTX_22 // The AK09916 produces a 16-bit signed output in the range +/-32752 corresponding to +/-4912uT. 1uT = 6.66 ADU. // 2^30 / 6.66666 = 161061273 = 0x9999999 - const unsigned char mountMultiplierZero[4] = {0x00, 0x00, 0x00, 0x00}; - const unsigned char mountMultiplierPlus[4] = {0x09, 0x99, 0x99, 0x99}; // Value taken from InvenSense Nucleo example - const unsigned char mountMultiplierMinus[4] = {0xF6, 0x66, 0x66, 0x67}; // Value taken from InvenSense Nucleo example - result = writeDMPmems(CPASS_MTX_00, 4, &mountMultiplierPlus[0]); if (result > worstResult) worstResult = result; - result = writeDMPmems(CPASS_MTX_01, 4, &mountMultiplierZero[0]); if (result > worstResult) worstResult = result; - result = writeDMPmems(CPASS_MTX_02, 4, &mountMultiplierZero[0]); if (result > worstResult) worstResult = result; - result = writeDMPmems(CPASS_MTX_10, 4, &mountMultiplierZero[0]); if (result > worstResult) worstResult = result; - result = writeDMPmems(CPASS_MTX_11, 4, &mountMultiplierMinus[0]); if (result > worstResult) worstResult = result; - result = writeDMPmems(CPASS_MTX_12, 4, &mountMultiplierZero[0]); if (result > worstResult) worstResult = result; - result = writeDMPmems(CPASS_MTX_20, 4, &mountMultiplierZero[0]); if (result > worstResult) worstResult = result; - result = writeDMPmems(CPASS_MTX_21, 4, &mountMultiplierZero[0]); if (result > worstResult) worstResult = result; - result = writeDMPmems(CPASS_MTX_22, 4, &mountMultiplierMinus[0]); if (result > worstResult) worstResult = result; + const unsigned char mountMultiplierZero[4] = { 0x00, 0x00, 0x00, 0x00 }; + const unsigned char mountMultiplierPlus[4] = { 0x09, 0x99, 0x99, 0x99 }; // Value taken from InvenSense Nucleo example + const unsigned char mountMultiplierMinus[4] = { 0xF6, 0x66, 0x66, 0x67 }; // Value taken from InvenSense Nucleo example + result = writeDMPmems(CPASS_MTX_00, 4, &mountMultiplierPlus[0]); + if (result > worstResult) + worstResult = result; + result = writeDMPmems(CPASS_MTX_01, 4, &mountMultiplierZero[0]); + if (result > worstResult) + worstResult = result; + result = writeDMPmems(CPASS_MTX_02, 4, &mountMultiplierZero[0]); + if (result > worstResult) + worstResult = result; + result = writeDMPmems(CPASS_MTX_10, 4, &mountMultiplierZero[0]); + if (result > worstResult) + worstResult = result; + result = writeDMPmems(CPASS_MTX_11, 4, &mountMultiplierMinus[0]); + if (result > worstResult) + worstResult = result; + result = writeDMPmems(CPASS_MTX_12, 4, &mountMultiplierZero[0]); + if (result > worstResult) + worstResult = result; + result = writeDMPmems(CPASS_MTX_20, 4, &mountMultiplierZero[0]); + if (result > worstResult) + worstResult = result; + result = writeDMPmems(CPASS_MTX_21, 4, &mountMultiplierZero[0]); + if (result > worstResult) + worstResult = result; + result = writeDMPmems(CPASS_MTX_22, 4, &mountMultiplierMinus[0]); + if (result > worstResult) + worstResult = result; // Configure the B2S Mounting Matrix - const unsigned char b2sMountMultiplierZero[4] = {0x00, 0x00, 0x00, 0x00}; - const unsigned char b2sMountMultiplierPlus[4] = {0x40, 0x00, 0x00, 0x00}; // Value taken from InvenSense Nucleo example - result = writeDMPmems(B2S_MTX_00, 4, &b2sMountMultiplierPlus[0]); if (result > worstResult) worstResult = result; - result = writeDMPmems(B2S_MTX_01, 4, &b2sMountMultiplierZero[0]); if (result > worstResult) worstResult = result; - result = writeDMPmems(B2S_MTX_02, 4, &b2sMountMultiplierZero[0]); if (result > worstResult) worstResult = result; - result = writeDMPmems(B2S_MTX_10, 4, &b2sMountMultiplierZero[0]); if (result > worstResult) worstResult = result; - result = writeDMPmems(B2S_MTX_11, 4, &b2sMountMultiplierPlus[0]); if (result > worstResult) worstResult = result; - result = writeDMPmems(B2S_MTX_12, 4, &b2sMountMultiplierZero[0]); if (result > worstResult) worstResult = result; - result = writeDMPmems(B2S_MTX_20, 4, &b2sMountMultiplierZero[0]); if (result > worstResult) worstResult = result; - result = writeDMPmems(B2S_MTX_21, 4, &b2sMountMultiplierZero[0]); if (result > worstResult) worstResult = result; - result = writeDMPmems(B2S_MTX_22, 4, &b2sMountMultiplierPlus[0]); if (result > worstResult) worstResult = result; + const unsigned char b2sMountMultiplierZero[4] = { 0x00, 0x00, 0x00, 0x00 }; + const unsigned char b2sMountMultiplierPlus[4] = { 0x40, 0x00, 0x00, 0x00 }; // Value taken from InvenSense Nucleo example + result = writeDMPmems(B2S_MTX_00, 4, &b2sMountMultiplierPlus[0]); + if (result > worstResult) + worstResult = result; + result = writeDMPmems(B2S_MTX_01, 4, &b2sMountMultiplierZero[0]); + if (result > worstResult) + worstResult = result; + result = writeDMPmems(B2S_MTX_02, 4, &b2sMountMultiplierZero[0]); + if (result > worstResult) + worstResult = result; + result = writeDMPmems(B2S_MTX_10, 4, &b2sMountMultiplierZero[0]); + if (result > worstResult) + worstResult = result; + result = writeDMPmems(B2S_MTX_11, 4, &b2sMountMultiplierPlus[0]); + if (result > worstResult) + worstResult = result; + result = writeDMPmems(B2S_MTX_12, 4, &b2sMountMultiplierZero[0]); + if (result > worstResult) + worstResult = result; + result = writeDMPmems(B2S_MTX_20, 4, &b2sMountMultiplierZero[0]); + if (result > worstResult) + worstResult = result; + result = writeDMPmems(B2S_MTX_21, 4, &b2sMountMultiplierZero[0]); + if (result > worstResult) + worstResult = result; + result = writeDMPmems(B2S_MTX_22, 4, &b2sMountMultiplierPlus[0]); + if (result > worstResult) + worstResult = result; // Configure the DMP Gyro Scaling Factor // @param[in] gyro_div Value written to GYRO_SMPLRT_DIV register, where // 0=1125Hz sample rate, 1=562.5Hz sample rate, ... 4=225Hz sample rate, ... // 10=102.2727Hz sample rate, ... etc. // @param[in] gyro_level 0=250 dps, 1=500 dps, 2=1000 dps, 3=2000 dps - result = setGyroSF(4, 3); if (result > worstResult) worstResult = result; // 19 = 55Hz (see above), 3 = 2000dps (see above) + result = setGyroSF(4, 3); + if (result > worstResult) + worstResult = result; // 19 = 55Hz (see above), 3 = 2000dps (see above) // Configure the Gyro full scale // 2000dps : 2^28 // 1000dps : 2^27 // 500dps : 2^26 // 250dps : 2^25 - const unsigned char gyroFullScale[4] = {0x10, 0x00, 0x00, 0x00}; // 2000dps : 2^28 - result = writeDMPmems(GYRO_FULLSCALE, 4, &gyroFullScale[0]); if (result > worstResult) worstResult = result; + const unsigned char gyroFullScale[4] = { 0x10, 0x00, 0x00, 0x00 }; // 2000dps : 2^28 + result = writeDMPmems(GYRO_FULLSCALE, 4, &gyroFullScale[0]); + if (result > worstResult) + worstResult = result; // Configure the Accel Only Gain: 15252014 (225Hz) 30504029 (112Hz) 61117001 (56Hz) - //const unsigned char accelOnlyGain[4] = {0x03, 0xA4, 0x92, 0x49}; // 56Hz - const unsigned char accelOnlyGain[4] = {0x00, 0xE8, 0xBA, 0x2E}; // 225Hz - //const unsigned char accelOnlyGain[4] = {0x01, 0xD1, 0x74, 0x5D}; // 112Hz - result = writeDMPmems(ACCEL_ONLY_GAIN, 4, &accelOnlyGain[0]); if (result > worstResult) worstResult = result; + // const unsigned char accelOnlyGain[4] = {0x03, 0xA4, 0x92, 0x49}; // 56Hz + const unsigned char accelOnlyGain[4] = { 0x00, 0xE8, 0xBA, 0x2E }; // 225Hz + // const unsigned char accelOnlyGain[4] = {0x01, 0xD1, 0x74, 0x5D}; // 112Hz + result = writeDMPmems(ACCEL_ONLY_GAIN, 4, &accelOnlyGain[0]); + if (result > worstResult) + worstResult = result; // Configure the Accel Alpha Var: 1026019965 (225Hz) 977872018 (112Hz) 882002213 (56Hz) - //const unsigned char accelAlphaVar[4] = {0x34, 0x92, 0x49, 0x25}; // 56Hz - const unsigned char accelAlphaVar[4] = {0x3D, 0x27, 0xD2, 0x7D}; // 225Hz - //const unsigned char accelAlphaVar[4] = {0x3A, 0x49, 0x24, 0x92}; // 112Hz - result = writeDMPmems(ACCEL_ALPHA_VAR, 4, &accelAlphaVar[0]); if (result > worstResult) worstResult = result; + // const unsigned char accelAlphaVar[4] = {0x34, 0x92, 0x49, 0x25}; // 56Hz + const unsigned char accelAlphaVar[4] = { 0x3D, 0x27, 0xD2, 0x7D }; // 225Hz + // const unsigned char accelAlphaVar[4] = {0x3A, 0x49, 0x24, 0x92}; // 112Hz + result = writeDMPmems(ACCEL_ALPHA_VAR, 4, &accelAlphaVar[0]); + if (result > worstResult) + worstResult = result; // Configure the Accel A Var: 47721859 (225Hz) 95869806 (112Hz) 191739611 (56Hz) - //const unsigned char accelAVar[4] = {0x0B, 0x6D, 0xB6, 0xDB}; // 56Hz - const unsigned char accelAVar[4] = {0x02, 0xD8, 0x2D, 0x83}; // 225Hz - //const unsigned char accelAVar[4] = {0x05, 0xB6, 0xDB, 0x6E}; // 112Hz - result = writeDMPmems(ACCEL_A_VAR, 4, &accelAVar[0]); if (result > worstResult) worstResult = result; + // const unsigned char accelAVar[4] = {0x0B, 0x6D, 0xB6, 0xDB}; // 56Hz + const unsigned char accelAVar[4] = { 0x02, 0xD8, 0x2D, 0x83 }; // 225Hz + // const unsigned char accelAVar[4] = {0x05, 0xB6, 0xDB, 0x6E}; // 112Hz + result = writeDMPmems(ACCEL_A_VAR, 4, &accelAVar[0]); + if (result > worstResult) + worstResult = result; // Configure the Accel Cal Rate - const unsigned char accelCalRate[4] = {0x00, 0x00}; // Value taken from InvenSense Nucleo example - result = writeDMPmems(ACCEL_CAL_RATE, 2, &accelCalRate[0]); if (result > worstResult) worstResult = result; + const unsigned char accelCalRate[4] = { 0x00, 0x00 }; // Value taken from InvenSense Nucleo example + result = writeDMPmems(ACCEL_CAL_RATE, 2, &accelCalRate[0]); + if (result > worstResult) + worstResult = result; // Configure the Compass Time Buffer. The I2C Master ODR Configuration (see above) sets the magnetometer read rate to 68.75Hz. // Let's set the Compass Time Buffer to 69 (Hz). - const unsigned char compassRate[2] = {0x00, 0x45}; // 69Hz - result = writeDMPmems(CPASS_TIME_BUFFER, 2, &compassRate[0]); if (result > worstResult) worstResult = result; + const unsigned char compassRate[2] = { 0x00, 0x45 }; // 69Hz + result = writeDMPmems(CPASS_TIME_BUFFER, 2, &compassRate[0]); + if (result > worstResult) + worstResult = result; // Enable DMP interrupt // This would be the most efficient way of getting the DMP data, instead of polling the FIFO - //result = intEnableDMP(true); if (result > worstResult) worstResult = result; - + // result = intEnableDMP(true); if (result > worstResult) worstResult = result; return worstResult; }