From ddd15c41ab2687beb847e49e0eac582454c875e0 Mon Sep 17 00:00:00 2001 From: higherstateofawkwardness Date: Fri, 14 May 2021 22:42:55 +0100 Subject: [PATCH 1/3] Create Example11_DMP_SENSOR_LINEAR Updated the example code to use the SENSOR_LINEAR_ACCELERATION output (Quat6 and acceleration), streamed at around 40 Hz. Code rotates the accelerometer data using the Quat6. During initialisation, the gravity vector is calculated. The accelerometer data is then further rotated so that values for Z acceleration are always pointing down the Z axis etc. i.e. the sensor retains the spacial orientation. --- .../Example11_DMP_SENSOR_LINEAR | 604 ++++++++++++++++++ 1 file changed, 604 insertions(+) create mode 100644 examples/Arduino/Example11_DMP_SENSOR_LINEAR/Example11_DMP_SENSOR_LINEAR diff --git a/examples/Arduino/Example11_DMP_SENSOR_LINEAR/Example11_DMP_SENSOR_LINEAR b/examples/Arduino/Example11_DMP_SENSOR_LINEAR/Example11_DMP_SENSOR_LINEAR new file mode 100644 index 0000000..790b7b4 --- /dev/null +++ b/examples/Arduino/Example11_DMP_SENSOR_LINEAR/Example11_DMP_SENSOR_LINEAR @@ -0,0 +1,604 @@ +/**************************************************************** + * This is an example for OpenLog Artemis - based on a DMP example from the ICM-20948 Library: + * + * Example11_DMP_SENSORS_LINEAR.ino + * Jon Noble 14-May-21 + * Updated the example code to use the SENSOR_LINEAR_ACCELERATION output (Quat6 and acceleration) and output at around 40 Hz to the serial port. + * Code rotates the accelerometer data using the Quat6. During initialisation, the gravity vector is calculated. The accelerometer data is then further rotated so that down is always pointing down the Z axis. + * + * + * 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 14301 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. +***************************************************************/ + + + +// OLA Specifics: +const byte PIN_IMU_POWER = 27; // The Red SparkFun version of the OLA (V10) uses pin 27 +//const byte PIN_IMU_POWER = 22; // The Black SparkX version of the OLA (X04) uses pin 22 +const byte PIN_IMU_INT = 37; +const byte PIN_IMU_CHIP_SELECT = 44; + +#include "ICM_20948.h" // Click here to get the library: http://librarymanager/All#SparkFun_ICM_20948_IMU + +#define SERIAL_PORT Serial + +#define SPI_PORT SPI // Your desired SPI port. OLA uses SPI. +#define CS_PIN PIN_IMU_CHIP_SELECT // Which pin you connect CS to. OLA uses pin 44. + +ICM_20948_SPI myICM; // Create an ICM_20948_SPI object + +//Global variables +double quatx, quaty,quatz; +double gravityX = 0; +double gravityY = 0; +double gravityZ = 0; + +void setup() { + + SPI_PORT.begin(); + + enableCIPOpullUp(); // Enable CIPO pull-up on the OLA + + pinMode(PIN_IMU_CHIP_SELECT, OUTPUT); + digitalWrite(PIN_IMU_CHIP_SELECT, HIGH); //Be sure IMU is deselected + pinMode(PIN_IMU_INT, INPUT_PULLUP); + + //Reset ICM by power cycling it + imuPowerOff(); + + delay(10); + + imuPowerOn(); // Enable power for the OLA IMU + + delay(100); // Wait for the IMU to power up + + SERIAL_PORT.begin(115200); // Start the serial console + SERIAL_PORT.println(F("OpenLog Artemis 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) + ; + + //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. + myICM.begin( CS_PIN, SPI_PORT ); + + SERIAL_PORT.print( F("Initialization of the sensor returned: ") ); + SERIAL_PORT.println( myICM.statusString() ); + if( myICM.status != ICM_20948_Stat_Ok ){ + SERIAL_PORT.println( F("Trying again...") ); + delay(500); + }else{ + initialized = true; + } + } + + SERIAL_PORT.println(F("Device connected!")); + + bool success = true; // Use success to show if the DMP configuration was successful + + // Initialize the DMP. initializeDMP is a weak function. In this example we overwrite it to change the sample rate (see below) + success &= (myICM.initializeDMP() == ICM_20948_Stat_Ok); + + // DMP sensor options are defined in ICM_20948_DMP.h + // INV_ICM20948_SENSOR_ACCELEROMETER (16-bit accel) + // INV_ICM20948_SENSOR_GYROSCOPE (16-bit gyro + 32-bit calibrated gyro) + // INV_ICM20948_SENSOR_RAW_ACCELEROMETER (16-bit accel) + // INV_ICM20948_SENSOR_RAW_GYROSCOPE (16-bit gyro + 32-bit calibrated gyro) + // INV_ICM20948_SENSOR_MAGNETIC_FIELD_UNCALIBRATED (16-bit compass) + // INV_ICM20948_SENSOR_GYROSCOPE_UNCALIBRATED (16-bit gyro) + // INV_ICM20948_SENSOR_STEP_DETECTOR (Pedometer Step Detector) + // INV_ICM20948_SENSOR_STEP_COUNTER (Pedometer Step Detector) + // INV_ICM20948_SENSOR_GAME_ROTATION_VECTOR (32-bit 6-axis quaternion) + // INV_ICM20948_SENSOR_ROTATION_VECTOR (32-bit 9-axis quaternion + heading accuracy) + // INV_ICM20948_SENSOR_GEOMAGNETIC_ROTATION_VECTOR (32-bit Geomag RV + heading accuracy) + // INV_ICM20948_SENSOR_GEOMAGNETIC_FIELD (32-bit calibrated compass) + // INV_ICM20948_SENSOR_GRAVITY (32-bit 6-axis quaternion) + // INV_ICM20948_SENSOR_LINEAR_ACCELERATION (16-bit accel + 32-bit 6-axis quaternion) + // INV_ICM20948_SENSOR_ORIENTATION (32-bit 9-axis quaternion + heading accuracy) + + // Enable the DMP Game Rotation Vector sensor (Quat6) + //success &= (myICM.enableDMPSensor(INV_ICM20948_SENSOR_ROTATION_VECTOR) == ICM_20948_Stat_Ok); + success &= (myICM.enableDMPSensor(INV_ICM20948_SENSOR_LINEAR_ACCELERATION) == ICM_20948_Stat_Ok); + + + // Enable additional sensors / features + //success &= (myICM.enableDMPSensor(INV_ICM20948_SENSOR_RAW_ACCELEROMETER) == ICM_20948_Stat_Ok); + //success &= (myICM.enableDMPSensor(INV_ICM20948_SENSOR_RAW_GYROSCOPE) == 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. + // Setting value can be calculated as follows: + // Value = (DMP running rate / ODR ) - 1 + // E.g. For a 225Hz ODR rate when DMP is running at 225Hz, value = (225/225) - 1 = 0. + //success &= (myICM.setDMPODRrate(DMP_ODR_Reg_Quat6, 0) == ICM_20948_Stat_Ok); // Set to 225Hz + //success &= (myICM.setDMPODRrate(DMP_ODR_Reg_Quat9, 0) == ICM_20948_Stat_Ok); // Set to 225Hz + //success &= (myICM.setDMPODRrate(DMP_ODR_Reg_Accel, 0) == ICM_20948_Stat_Ok); // Set to 225Hz + //success &= (myICM.setDMPODRrate(DMP_ODR_Reg_Gyro, 0) == ICM_20948_Stat_Ok); // Set to 225Hz + //success &= (myICM.setDMPODRrate(DMP_ODR_Reg_Cpass, 0) == ICM_20948_Stat_Ok); // Set to 225Hz + + // 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(F("DMP enabled!")); + } + else + { + SERIAL_PORT.println(F("Enable DMP failed!")); + SERIAL_PORT.println(F("Please check that you have uncommented line 29 (#define ICM_20948_USE_DMP) in ICM_20948_C.h...")); + while (1) + ; // Do nothing more + } + + + orientIMUSensor(); +} + +void loop() +{ + int moreData = readIMUDMP(); + if (moreData >=0) //function returns -1 if no data was read. + { + //double sensorUp = ; + //double sensorEW = ; + // double sensorNS = ; + + double accelMag = sqrt(quatx*quatx+quaty*quaty+quatz*quatz); + + if ((accelMag <=1075)||(accelMag >=925)) //slowly update the gravity data whenever the sensor is still. + { + float ema = 0.01; + gravityX = ema*quatx+(1-ema)*gravityX; + gravityY = ema*quaty+(1-ema)*gravityY; + gravityZ = ema*quatz+(1-ema)*gravityZ; + } + + // Re-orient the sensor so that gravity is down. See: https://stackoverflow.com/questions/1171849/finding-quaternion-representing-the-rotation-from-one-vector-to-another + // Basically, we want to rotate the whole system so that the gravity vector (x,y,z) maps onto the unit vector (0,0,-1) + // The cross-product of these two vectors is (-y,x,0) and the dot-product is (-z). + // The rotation quaternion is therefore given by (q0,q1,q2,q3) = (sqrt(x*x+y*y+z*z)-z , -y , x , 0); although this needs normalising before application. + double q0 = sqrt(gravityX*gravityX+gravityY*gravityY+gravityZ*gravityZ)-gravityZ; + double q1 = -gravityY; + double q2 = gravityX; + double q3 = 0; + double quatMag = sqrt(q0*q0+q1*q1+q2*q2+q3*q3); + q0 /= quatMag; + q1 /= quatMag; + q2 /= quatMag; + q3 /= quatMag; + + //Perform Quaternion rotation through matrix multiplication + double newx = quatx*(1-2*q2*q2-2*q3*q3) + quaty*2*(q1*q2-q0*q3) + quatz*2*(q1*q3+q0*q2); + double newy = quatx*2*(q1*q2+q0*q3) + quaty*(1-2*q1*q1-2*q3*q3) + quatz*2*(q2*q3-q0*q1); + double newz = quatx*2*(q1*q3-q0*q2) + quaty*2*(q2*q3+q0*q1) + quatz*(1-2*q1*q1-2*q2*q2); + + //..and subtract gravity from the Z axis: + newz += sqrt(gravityX*gravityX+gravityY*gravityY+gravityZ*gravityZ); + + //String outString = ","+String(quatx-gravityX,4)+","+String(quaty-gravityY,4)+","+String(quatz-gravityZ,4); + String outString = ","+String(newx,4)+","+String(newy,4)+","+String(newz,4); + Serial.println(outString); + } + + if(moreData == 0){ + delay(10); + } + + + //The original calculated gravity vector is sound, to transform our values into the gravity frame, we need to rotate 90° around a vector perpendicular to the gravity vector, such that it points downwards... +} + +void orientIMUSensor() +{ + Serial.println("Calibrating gravity vector - keep the sensor still or this might give inaccurate results"); + //Do exponential moving average over a certain time period and wait until vector settles at around 1g. Takes around 8 seconds. + double totalAcc = sqrt(gravityX*gravityX + gravityY*gravityY + gravityZ*gravityZ); + float ema = 0.01; + + while (totalAcc < 1000) + { + bool badReading = true; //Confirm if we have a valid result + while (badReading) + { + int moreData = readIMUDMP(); + if (moreData >=0) //function returns -1 if no data was read. + { + gravityX = ema*quatx+(1-ema)*gravityX; + gravityY = ema*quaty+(1-ema)*gravityY; + gravityZ = ema*quatz+(1-ema)*gravityZ; + badReading = false; + totalAcc = sqrt(gravityX*gravityX + gravityY*gravityY + gravityZ*gravityZ); + //Serial.println(String(gravityX)+","+String(gravityY)+","+String(gravityZ)+","+String(totalAcc)); + } + if(moreData ==0){ + delay(10); + } + } + } + Serial.println("Gravity vector initialised (will slowly update with changing conditions"); + //String outString = String(q0,4)+","+String(q1,4)+","+String(q2,4)+","+String(q3,4)+","+String(acc_x,0)+","+String(acc_y,0)+","+String(acc_z,0)+","; +} + + + +// OLA Specifics +int readIMUDMP() +{ + // Read any DMP data waiting in the FIFO + // Note: + // readDMPdataFromFIFO will return ICM_20948_Stat_FIFONoDataAvail if no data is available. + // If data is available, readDMPdataFromFIFO will attempt to read _one_ frame of DMP data. + // readDMPdataFromFIFO will return ICM_20948_Stat_FIFOIncompleteData if a frame was present but was incomplete + // 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_FIFONoDataAvail) + { + return -1; + } + + 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_Quat6) > 0)|| ((data.header & DMP_header_bitmap_Accel) > 0)) // Check for orientation data (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.printf("Quat6 data is: Q1:%ld Q2:%ld Q3:%ld\r\n", data.Quat6.Data.Q1, data.Quat6.Data.Q2, data.Quat6.Data.Q3); + + + // Scale to +/- 1 + 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))); + + //Serial.print(F("Q0:")); + //Serial.print(q0, 3); + // Serial.print(F("Q1:")); + //Serial.print(q1, 3); + //Serial.print(F(" Q2:")); + // Serial.print(q2, 3); + // Serial.print(F(" Q3:")); + // Serial.println(q3, 3); + + //say that the quaternion represents the sensor orientation, but that the accelerometer also has a quaternion for the direction of force. + // there exists a matrix operation such that gravity is always down. + float acc_x = (float)data.Raw_Accel.Data.X; // Extract the raw accelerometer data + float acc_y = (float)data.Raw_Accel.Data.Y; + float acc_z = (float)data.Raw_Accel.Data.Z; + + //Serial.print(F("Accel: X:")); + // Serial.print(acc_x,0); + // Serial.print(F(" Y:")); + // Serial.print(acc_y,0); + // Serial.print(F(" Z:")); + // Serial.println(acc_z,0); + + // Perform Quat6 rotation of raw acceleration values + + quatx = acc_x*(1-2*q2*q2-2*q3*q3)+acc_y*2*(q1*q2-q0*q3)+acc_z*2*(q1*q3+q0*q2); + quaty = acc_x*2*(q1*q2+q0*q3)+acc_y*(1-2*q1*q1-2*q3*q3)+acc_z*2*(q2*q3-q0*q1); + quatz = acc_x*2*(q1*q3-q0*q2)+acc_y*2*(q2*q3+q0*q1)+acc_z*(1-2*q1*q1-2*q2*q2); + + } + } + if (myICM.status != ICM_20948_Stat_FIFOMoreDataAvail) // If more data is available then we should read it right away - and not delay + { + return 0; // No more data + } + else + { + return 1; // More data + } + +} + + + + + +void imuPowerOn() +{ + pinMode(PIN_IMU_POWER, OUTPUT); + digitalWrite(PIN_IMU_POWER, HIGH); +} +void imuPowerOff() +{ + pinMode(PIN_IMU_POWER, OUTPUT); + digitalWrite(PIN_IMU_POWER, LOW); +} + +bool enableCIPOpullUp() +{ + //Add CIPO pull-up + ap3_err_t retval = AP3_OK; + am_hal_gpio_pincfg_t cipoPinCfg = AP3_GPIO_DEFAULT_PINCFG; + cipoPinCfg.uFuncSel = AM_HAL_PIN_6_M0MISO; + cipoPinCfg.eDriveStrength = AM_HAL_GPIO_PIN_DRIVESTRENGTH_12MA; + cipoPinCfg.eGPOutcfg = AM_HAL_GPIO_PIN_OUTCFG_PUSHPULL; + cipoPinCfg.uIOMnum = AP3_SPI_IOM; + cipoPinCfg.ePullup = AM_HAL_GPIO_PIN_PULLUP_1_5K; + padMode(MISO, cipoPinCfg, &retval); + return (retval == AP3_OK); +} + +// initializeDMP is a weak function. Let's overwrite it so we can increase the sample rate +ICM_20948_Status_e ICM_20948::initializeDMP(void) +{ + // 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 worstResult = ICM_20948_Stat_Ok; + + // 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 + // nine bytes from the mag every sample, starting from the STATUS1 register (0x10). ST1 includes the DRDY (Data Ready) bit. + // Next are the six magnetometer readings (little endian). After a dummy byte, the STATUS2 register (0x18) contains the HOFL (Overflow) bit. + // + // But looking very closely at the InvenSense example code, we can see in inv_icm20948_resume_akm (in Icm20948AuxCompassAkm.c) that, + // when the DMP is running, the magnetometer is set to Single Measurement (SM) mode and that ten bytes are read, starting from the reserved + // RSV2 register (0x03). The datasheet does not define what registers 0x04 to 0x0C contain. There is definitely some secret sauce in here... + // The magnetometer data appears to be big endian (not little endian like the HX/Y/Z registers) and starts at register 0x04. + // We had to examine the I2C traffic between the master and the AK09916 on the AUX_DA and AUX_CL pins to discover this... + // + // So, we need to set up I2C_SLV0 to do the ten byte reading. The parameters passed to i2cControllerConfigurePeripheral are: + // 0: use I2C_SLV0 + // MAG_AK09916_I2C_ADDR: the I2C address of the AK09916 magnetometer (0x0C unshifted) + // AK09916_REG_RSV2: we start reading here (0x03). Secret sauce... + // 10: we read 10 bytes each cycle + // true: set the I2C_SLV0_RNW ReadNotWrite bit so we read the 10 bytes (not write them) + // true: set the I2C_SLV0_CTRL I2C_SLV0_EN bit to enable reading from the peripheral at the sample rate + // 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; + // + // We also need to set up I2C_SLV1 to do the Single Measurement triggering: + // 1: use I2C_SLV1 + // MAG_AK09916_I2C_ADDR: the I2C address of the AK09916 magnetometer (0x0C unshifted) + // AK09916_REG_CNTL2: we start writing here (0x31) + // 1: not sure why, but the write does not happen if this is set to zero + // false: clear the I2C_SLV0_RNW ReadNotWrite bit so we write the dataOut byte + // true: set the I2C_SLV0_CTRL I2C_SLV0_EN bit. Not sure why, but the write does not happen if this is clear + // false: clear the I2C_SLV0_CTRL I2C_SLV0_REG_DIS (we want to write the register value) + // 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; + + // 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: + // "I2C_MST_ODR_CONFIG[3:0]: ODR configuration for external sensor when gyroscope and accelerometer are disabled. + // ODR is computed as follows: 1.1 kHz/(2^((odr_config[3:0])) ) + // When gyroscope is enabled, all sensors (including I2C_MASTER) use the gyroscope ODR. + // 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 + + // 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 + + // 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 + result = write(AGB0_REG_PWR_MGMT_2, &pwrMgmt2, 1); if (result > worstResult) worstResult = result; // Write one byte to the PWR_MGMT_2 register + + // Configure I2C_Master/Gyro/Accel in Low Power Mode (cycled) with LP_CONFIG + result = setSampleMode((ICM_20948_Internal_Mst | ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), ICM_20948_Sample_Mode_Cycled); if (result > worstResult) worstResult = result; + + // Disable the FIFO + result = enableFIFO(false); if (result > worstResult) worstResult = result; + + // Disable the DMP + 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; + + // 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 + + // 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 + uint8_t zero = 0; + 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; + + // Turn off data ready interrupt through INT_ENABLE_1 + result = intEnableRawDataReady(false); if (result > worstResult) worstResult = result; + + // Reset FIFO through FIFO_RST + 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 = 4; // ODR is computed as follows: 1.1 kHz/(1+GYRO_SMPLRT_DIV[7:0]). 4 = 220Hz + mySmplrt.a = 4; // ODR is computed as follows: 1.125 kHz/(1+ACCEL_SMPLRT_DIV[11:0]). 4 = 225Hz + 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 + + // Now load the DMP firmware + 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 + + // Set the Hardware Fix Disable register to 0x48 + 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; + + // Set the Single FIFO Priority Select register to 0xE4 + 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; + + // 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 + // 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 + + // 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 + // 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; + + // 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; + + // 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; // 0 = 225Hz (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; + + // 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}; // InvenSense Nucleo example uses 225Hz + 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 + 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 + 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; + + // 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; + + // 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; + + return worstResult; +} From 2704be79958558e1c4040403239147f03654f5ba Mon Sep 17 00:00:00 2001 From: higherstateofawkwardness Date: Fri, 14 May 2021 22:51:24 +0100 Subject: [PATCH 2/3] Update Example11_DMP_SENSOR_LINEAR --- .../Example11_DMP_SENSOR_LINEAR | 12 ------------ 1 file changed, 12 deletions(-) diff --git a/examples/Arduino/Example11_DMP_SENSOR_LINEAR/Example11_DMP_SENSOR_LINEAR b/examples/Arduino/Example11_DMP_SENSOR_LINEAR/Example11_DMP_SENSOR_LINEAR index 790b7b4..c8246ce 100644 --- a/examples/Arduino/Example11_DMP_SENSOR_LINEAR/Example11_DMP_SENSOR_LINEAR +++ b/examples/Arduino/Example11_DMP_SENSOR_LINEAR/Example11_DMP_SENSOR_LINEAR @@ -174,8 +174,6 @@ void setup() { while (1) ; // Do nothing more } - - orientIMUSensor(); } @@ -184,9 +182,6 @@ void loop() int moreData = readIMUDMP(); if (moreData >=0) //function returns -1 if no data was read. { - //double sensorUp = ; - //double sensorEW = ; - // double sensorNS = ; double accelMag = sqrt(quatx*quatx+quaty*quaty+quatz*quatz); @@ -228,9 +223,6 @@ void loop() if(moreData == 0){ delay(10); } - - - //The original calculated gravity vector is sound, to transform our values into the gravity frame, we need to rotate 90° around a vector perpendicular to the gravity vector, such that it points downwards... } void orientIMUSensor() @@ -349,10 +341,6 @@ int readIMUDMP() } - - - - void imuPowerOn() { pinMode(PIN_IMU_POWER, OUTPUT); From 9e7d96649db4e835246d20030ab6ebc53bab66c2 Mon Sep 17 00:00:00 2001 From: higherstateofawkwardness Date: Wed, 19 May 2021 13:20:25 +0100 Subject: [PATCH 3/3] Create Example11_DMP_SENSOR_LINEAR This uses the onboard DMP rotation sensor to adjust the XYZ readings from the accelerometer into a constant orientation in the real world. Gravity is always down. Due to drift in the Quat6 sensor, the gravity vector is periodically adjusted when the sensor is stationary. If we can get the Quat 9 reading to be more stable, then this step might be omitted. --- .../Example11_DMP_SENSOR_LINEAR | 433 +++++++++--------- 1 file changed, 218 insertions(+), 215 deletions(-) diff --git a/examples/Arduino/Example11_DMP_SENSOR_LINEAR/Example11_DMP_SENSOR_LINEAR b/examples/Arduino/Example11_DMP_SENSOR_LINEAR/Example11_DMP_SENSOR_LINEAR index c8246ce..8ce6953 100644 --- a/examples/Arduino/Example11_DMP_SENSOR_LINEAR/Example11_DMP_SENSOR_LINEAR +++ b/examples/Arduino/Example11_DMP_SENSOR_LINEAR/Example11_DMP_SENSOR_LINEAR @@ -1,15 +1,14 @@ /**************************************************************** - * This is an example for OpenLog Artemis - based on a DMP example from the ICM-20948 Library: + * Example11_DMP_SENSOR_LINEAR.ino + * ICM 20948 Arduino Library Demo + * Initialize the DMP based on the TDK InvenSense ICM20948_eMD_nucleo_1.0 example-icm20948 * - * Example11_DMP_SENSORS_LINEAR.ino - * Jon Noble 14-May-21 - * Updated the example code to use the SENSOR_LINEAR_ACCELERATION output (Quat6 and acceleration) and output at around 40 Hz to the serial port. - * Code rotates the accelerometer data using the Quat6. During initialisation, the gravity vector is calculated. The accelerometer data is then further rotated so that down is always pointing down the Z axis. + * Jon Noble 19-May-21 + * The following example uses QUAT6 for sensor orientation. An initialisation routine determines the gravity vector, and the accelerometer readings are then rotated onto a vector where down is always down (North-East-Up orientation) * + * Based on DMP example 10 by: + * Paul Clark, April 25th, 2021 * - * 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 @@ -29,52 +28,39 @@ * 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 - -// OLA Specifics: -const byte PIN_IMU_POWER = 27; // The Red SparkFun version of the OLA (V10) uses pin 27 -//const byte PIN_IMU_POWER = 22; // The Black SparkX version of the OLA (X04) uses pin 22 -const byte PIN_IMU_INT = 37; -const byte PIN_IMU_CHIP_SELECT = 44; - -#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. OLA uses SPI. -#define CS_PIN PIN_IMU_CHIP_SELECT // Which pin you connect CS to. OLA uses pin 44. +#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 -ICM_20948_SPI myICM; // Create an ICM_20948_SPI object +#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 -//Global variables -double quatx, quaty,quatz; -double gravityX = 0; -double gravityY = 0; -double gravityZ = 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() { - SPI_PORT.begin(); +double quat_x, quat_y,quat_z; // Quaternion corrected acceleration values +double g_x = 0, g_y = 0, g_z = -1000; // Vector used to store the gravity values and rotate the subsequent acceleration onto. +double abs_x, abs_y, abs_z; // Absolute orientation vector - enableCIPOpullUp(); // Enable CIPO pull-up on the OLA - pinMode(PIN_IMU_CHIP_SELECT, OUTPUT); - digitalWrite(PIN_IMU_CHIP_SELECT, HIGH); //Be sure IMU is deselected - pinMode(PIN_IMU_INT, INPUT_PULLUP); - - //Reset ICM by power cycling it - imuPowerOff(); - - delay(10); - - imuPowerOn(); // Enable power for the OLA IMU - - delay(100); // Wait for the IMU to power up +void setup() +{ SERIAL_PORT.begin(115200); // Start the serial console - SERIAL_PORT.println(F("OpenLog Artemis ICM-20948 Example")); + SERIAL_PORT.println(F("ICM-20948 Example")); delay(100); @@ -86,21 +72,36 @@ void setup() { 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 ){ + 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. - myICM.begin( CS_PIN, SPI_PORT ); - - SERIAL_PORT.print( F("Initialization of the sensor returned: ") ); - SERIAL_PORT.println( myICM.statusString() ); - if( myICM.status != ICM_20948_Stat_Ok ){ - SERIAL_PORT.println( F("Trying again...") ); +#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(F("Trying again...")); delay(500); - }else{ + } + else + { initialized = true; } } @@ -109,7 +110,7 @@ void setup() { bool success = true; // Use success to show if the DMP configuration was successful - // Initialize the DMP. initializeDMP is a weak function. In this example we overwrite it to change the sample rate (see below) + // Initialize the DMP. initializeDMP is a weak function. In this example we overwrite it to change the sample rate (see below) success &= (myICM.initializeDMP() == ICM_20948_Stat_Ok); // DMP sensor options are defined in ICM_20948_DMP.h @@ -129,14 +130,12 @@ void setup() { // INV_ICM20948_SENSOR_LINEAR_ACCELERATION (16-bit accel + 32-bit 6-axis quaternion) // INV_ICM20948_SENSOR_ORIENTATION (32-bit 9-axis quaternion + heading accuracy) - // Enable the DMP Game Rotation Vector sensor (Quat6) - //success &= (myICM.enableDMPSensor(INV_ICM20948_SENSOR_ROTATION_VECTOR) == ICM_20948_Stat_Ok); + // Enable the DMP Linear Acceleration sensor (Quat6 + accel) success &= (myICM.enableDMPSensor(INV_ICM20948_SENSOR_LINEAR_ACCELERATION) == ICM_20948_Stat_Ok); - // Enable additional sensors / features - //success &= (myICM.enableDMPSensor(INV_ICM20948_SENSOR_RAW_ACCELEROMETER) == 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); // Configuring DMP to output data at multiple ODRs: @@ -144,13 +143,16 @@ void setup() { // Setting value can be calculated as follows: // Value = (DMP running rate / ODR ) - 1 // E.g. For a 225Hz ODR rate when DMP is running at 225Hz, value = (225/225) - 1 = 0. - //success &= (myICM.setDMPODRrate(DMP_ODR_Reg_Quat6, 0) == ICM_20948_Stat_Ok); // Set to 225Hz - //success &= (myICM.setDMPODRrate(DMP_ODR_Reg_Quat9, 0) == ICM_20948_Stat_Ok); // Set to 225Hz - //success &= (myICM.setDMPODRrate(DMP_ODR_Reg_Accel, 0) == ICM_20948_Stat_Ok); // Set to 225Hz //success &= (myICM.setDMPODRrate(DMP_ODR_Reg_Gyro, 0) == ICM_20948_Stat_Ok); // Set to 225Hz //success &= (myICM.setDMPODRrate(DMP_ODR_Reg_Cpass, 0) == ICM_20948_Stat_Ok); // Set to 225Hz - // Enable the FIFO + // For correct sensor fusion of the quaternion and the acceleration in this example, we need them to be running at the same rate. + // Winding the data rate up too far results in instability in the DMP. + int fusionrate = 4; + success &= (myICM.setDMPODRrate(DMP_ODR_Reg_Quat6, fusionrate) == ICM_20948_Stat_Ok); // Set to 225Hz + success &= (myICM.setDMPODRrate(DMP_ODR_Reg_Accel, fusionrate) == ICM_20948_Stat_Ok); // Set to 225Hz + + // Enable the FIFO success &= (myICM.enableFIFO() == ICM_20948_Stat_Ok); // Enable the DMP @@ -174,163 +176,154 @@ void setup() { while (1) ; // Do nothing more } - orientIMUSensor(); + + findGravity(); + } void loop() { - int moreData = readIMUDMP(); + int moreData = readIMUDMP(); + if (moreData >=0) //function returns -1 if no data was read. { - double accelMag = sqrt(quatx*quatx+quaty*quaty+quatz*quatz); - - if ((accelMag <=1075)||(accelMag >=925)) //slowly update the gravity data whenever the sensor is still. - { - float ema = 0.01; - gravityX = ema*quatx+(1-ema)*gravityX; - gravityY = ema*quaty+(1-ema)*gravityY; - gravityZ = ema*quatz+(1-ema)*gravityZ; - } - - // Re-orient the sensor so that gravity is down. See: https://stackoverflow.com/questions/1171849/finding-quaternion-representing-the-rotation-from-one-vector-to-another - // Basically, we want to rotate the whole system so that the gravity vector (x,y,z) maps onto the unit vector (0,0,-1) - // The cross-product of these two vectors is (-y,x,0) and the dot-product is (-z). - // The rotation quaternion is therefore given by (q0,q1,q2,q3) = (sqrt(x*x+y*y+z*z)-z , -y , x , 0); although this needs normalising before application. - double q0 = sqrt(gravityX*gravityX+gravityY*gravityY+gravityZ*gravityZ)-gravityZ; - double q1 = -gravityY; - double q2 = gravityX; - double q3 = 0; - double quatMag = sqrt(q0*q0+q1*q1+q2*q2+q3*q3); - q0 /= quatMag; - q1 /= quatMag; - q2 /= quatMag; - q3 /= quatMag; - - //Perform Quaternion rotation through matrix multiplication - double newx = quatx*(1-2*q2*q2-2*q3*q3) + quaty*2*(q1*q2-q0*q3) + quatz*2*(q1*q3+q0*q2); - double newy = quatx*2*(q1*q2+q0*q3) + quaty*(1-2*q1*q1-2*q3*q3) + quatz*2*(q2*q3-q0*q1); - double newz = quatx*2*(q1*q3-q0*q2) + quaty*2*(q2*q3+q0*q1) + quatz*(1-2*q1*q1-2*q2*q2); - - //..and subtract gravity from the Z axis: - newz += sqrt(gravityX*gravityX+gravityY*gravityY+gravityZ*gravityZ); - - //String outString = ","+String(quatx-gravityX,4)+","+String(quaty-gravityY,4)+","+String(quatz-gravityZ,4); - String outString = ","+String(newx,4)+","+String(newy,4)+","+String(newz,4); - Serial.println(outString); - } - - if(moreData == 0){ - delay(10); - } -} - -void orientIMUSensor() -{ - Serial.println("Calibrating gravity vector - keep the sensor still or this might give inaccurate results"); - //Do exponential moving average over a certain time period and wait until vector settles at around 1g. Takes around 8 seconds. - double totalAcc = sqrt(gravityX*gravityX + gravityY*gravityY + gravityZ*gravityZ); - float ema = 0.01; - - while (totalAcc < 1000) - { - bool badReading = true; //Confirm if we have a valid result - while (badReading) + //Quat 6 sensor drifts over time, so slowly update gravity with the latest readings + if (sqrt(quat_x*quat_x+quat_y*quat_y+quat_z*quat_z) < 1080) // We can only update the gravity vector when the sensor is relatively stationary. { - int moreData = readIMUDMP(); - if (moreData >=0) //function returns -1 if no data was read. - { - gravityX = ema*quatx+(1-ema)*gravityX; - gravityY = ema*quaty+(1-ema)*gravityY; - gravityZ = ema*quatz+(1-ema)*gravityZ; - badReading = false; - totalAcc = sqrt(gravityX*gravityX + gravityY*gravityY + gravityZ*gravityZ); - //Serial.println(String(gravityX)+","+String(gravityY)+","+String(gravityZ)+","+String(totalAcc)); - } - if(moreData ==0){ - delay(10); - } + float ema = 0.01; // Use exponential moving average. + g_x = g_x*(1-ema)+ quat_x*ema; + g_y = g_y*(1-ema)+ quat_y*ema; + g_z = g_z*(1-ema)+ quat_z*ema; + } + + bool fulloutput = false; //True outputs full descriptors, false outputs comma-separated values for plotting. + if (fulloutput) + { + SERIAL_PORT.print(F(" Accel: North:")); + SERIAL_PORT.print(abs_x); + SERIAL_PORT.print(F(" East:")); + SERIAL_PORT.print(abs_y); + SERIAL_PORT.print(F(" Down:")); + SERIAL_PORT.println(abs_z); + } else { + SERIAL_PORT.print(abs_x); + SERIAL_PORT.print(F(" ,")); + SERIAL_PORT.print(abs_y); + SERIAL_PORT.print(F(" ,")); + SERIAL_PORT.println(abs_z); } } - Serial.println("Gravity vector initialised (will slowly update with changing conditions"); - //String outString = String(q0,4)+","+String(q1,4)+","+String(q2,4)+","+String(q3,4)+","+String(acc_x,0)+","+String(acc_y,0)+","+String(acc_z,0)+","; + + if(moreData <= 0){ + //Wait if there is no more data available + delay(1); + } } -// OLA Specifics -int readIMUDMP() +int readIMUDMP() // return a value depending on whether there is valid data or not. { - // Read any DMP data waiting in the FIFO - // Note: - // readDMPdataFromFIFO will return ICM_20948_Stat_FIFONoDataAvail if no data is available. - // If data is available, readDMPdataFromFIFO will attempt to read _one_ frame of DMP data. - // readDMPdataFromFIFO will return ICM_20948_Stat_FIFOIncompleteData if a frame was present but was incomplete - // 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_FIFONoDataAvail) + // Read any DMP data waiting in the FIFO + // Note: + // readDMPdataFromFIFO will return ICM_20948_Stat_FIFONoDataAvail if no data is available. + // If data is available, readDMPdataFromFIFO will attempt to read _one_ frame of DMP data. + // readDMPdataFromFIFO will return ICM_20948_Stat_FIFOIncompleteData if a frame was present but was incomplete + // 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_FIFONoDataAvail) { return -1; } - - if ((myICM.status == ICM_20948_Stat_Ok) || (myICM.status == ICM_20948_Stat_FIFOMoreDataAvail)) // Was valid data available? + + 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_Quat6) > 0)||((data.header & DMP_header_bitmap_Accel) > 0)) // Check for simulataneous quaternion and accel data { - //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_Quat6) > 0)|| ((data.header & DMP_header_bitmap_Accel) > 0)) // Check for orientation data (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.printf("Quat6 data is: Q1:%ld Q2:%ld Q3:%ld\r\n", data.Quat6.Data.Q1, data.Quat6.Data.Q2, data.Quat6.Data.Q3); - - - // Scale to +/- 1 - 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))); - - //Serial.print(F("Q0:")); - //Serial.print(q0, 3); - // Serial.print(F("Q1:")); - //Serial.print(q1, 3); - //Serial.print(F(" Q2:")); - // Serial.print(q2, 3); - // Serial.print(F(" Q3:")); - // Serial.println(q3, 3); - - //say that the quaternion represents the sensor orientation, but that the accelerometer also has a quaternion for the direction of force. - // there exists a matrix operation such that gravity is always down. - float acc_x = (float)data.Raw_Accel.Data.X; // Extract the raw accelerometer data - float acc_y = (float)data.Raw_Accel.Data.Y; - float acc_z = (float)data.Raw_Accel.Data.Z; - - //Serial.print(F("Accel: X:")); - // Serial.print(acc_x,0); - // Serial.print(F(" Y:")); - // Serial.print(acc_y,0); - // Serial.print(F(" Z:")); - // Serial.println(acc_z,0); - - // Perform Quat6 rotation of raw acceleration values + // 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("Quat6 data is: Q1:%ld Q2:%ld Q3:%ld Accuracy:%d\r\n", data.Quat6.Data.Q1, data.Quat6.Data.Q2, data.Quat6.Data.Q3); + + // Scale to +/- 1 + 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))); + + float acc_x = (float)data.Raw_Accel.Data.X; // Extract the raw accelerometer data + float acc_y = (float)data.Raw_Accel.Data.Y; + float acc_z = (float)data.Raw_Accel.Data.Z; + + + /* Uncomment the following for raw data output from the Quaternion and the Accelerometer + SERIAL_PORT.print(F("Q0:")); + SERIAL_PORT.print(q0, 3); + 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(" Accel: 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); + */ + + // Quaternion is initialised as q1=q2=q3=0. Perform Quaternion rotation of raw acceleration values into the initial sensor rotation frame: + + quat_x = acc_x*(1-2*q2*q2-2*q3*q3)+acc_y*2*(q1*q2-q0*q3)+acc_z*2*(q1*q3+q0*q2); + quat_y = acc_x*2*(q1*q2+q0*q3)+acc_y*(1-2*q1*q1-2*q3*q3)+acc_z*2*(q2*q3-q0*q1); + quat_z = acc_x*2*(q1*q3-q0*q2)+acc_y*2*(q2*q3+q0*q1)+acc_z*(1-2*q1*q1-2*q2*q2); + + // Re-orient the sensor so that gravity is down. See: https://stackoverflow.com/questions/1171849/finding-quaternion-representing-the-rotation-from-one-vector-to-another + // Basically, we want the quaternion rotation to rotate the whole system so that the gravity vector (x,y,z) maps onto the unit vector (0,0,-1) + // The cross-product of the gravity and the "down" vector is (-y,x,0) and the dot-product is (-z). + // The rotation quaternion is therefore given by (q0,q1,q2,q3) = (sqrt(g_x*g_x+g_y*g_y+g_z*g_z)-g_z , -g_y , g_x , 0); although this needs normalising before application. + + // Re-use the previous variables from Quat rotation - this might not be best practice + q0 = sqrt(g_x*g_x+g_y*g_y+g_z*g_z)-g_z; + q1 = -g_y; + q2 = g_x; + q3 = 0; + double quatMag = sqrt(q0*q0+q1*q1+q2*q2+q3*q3); - quatx = acc_x*(1-2*q2*q2-2*q3*q3)+acc_y*2*(q1*q2-q0*q3)+acc_z*2*(q1*q3+q0*q2); - quaty = acc_x*2*(q1*q2+q0*q3)+acc_y*(1-2*q1*q1-2*q3*q3)+acc_z*2*(q2*q3-q0*q1); - quatz = acc_x*2*(q1*q3-q0*q2)+acc_y*2*(q2*q3+q0*q1)+acc_z*(1-2*q1*q1-2*q2*q2); - + q0 /= quatMag; + q1 /= quatMag; + q2 /= quatMag; + q3 /= quatMag; + + abs_x = quat_x*(1-2*q2*q2-2*q3*q3) + quat_y*2*(q1*q2-q0*q3) + quat_z*2*(q1*q3+q0*q2); + abs_y = quat_x*2*(q1*q2+q0*q3) + quat_y*(1-2*q1*q1-2*q3*q3) + quat_z*2*(q2*q3-q0*q1); + abs_z = quat_x*2*(q1*q3-q0*q2) + quat_y*2*(q2*q3+q0*q1) + quat_z*(1-2*q1*q1-2*q2*q2); + + + bool subtractG = true; // Do you want to subtract the gravity reading (true), or just orient the sensor (false) + if (subtractG) + { + abs_z += sqrt(g_x*g_x+g_y*g_y+g_z*g_z); } } - if (myICM.status != ICM_20948_Stat_FIFOMoreDataAvail) // If more data is available then we should read it right away - and not delay + + + } + + if (myICM.status != ICM_20948_Stat_FIFOMoreDataAvail) // If more data is available then we should read it right away - and not delay { return 0; // No more data } @@ -338,34 +331,44 @@ int readIMUDMP() { return 1; // More data } - } -void imuPowerOn() -{ - pinMode(PIN_IMU_POWER, OUTPUT); - digitalWrite(PIN_IMU_POWER, HIGH); -} -void imuPowerOff() -{ - pinMode(PIN_IMU_POWER, OUTPUT); - digitalWrite(PIN_IMU_POWER, LOW); -} -bool enableCIPOpullUp() +void findGravity() { - //Add CIPO pull-up - ap3_err_t retval = AP3_OK; - am_hal_gpio_pincfg_t cipoPinCfg = AP3_GPIO_DEFAULT_PINCFG; - cipoPinCfg.uFuncSel = AM_HAL_PIN_6_M0MISO; - cipoPinCfg.eDriveStrength = AM_HAL_GPIO_PIN_DRIVESTRENGTH_12MA; - cipoPinCfg.eGPOutcfg = AM_HAL_GPIO_PIN_OUTCFG_PUSHPULL; - cipoPinCfg.uIOMnum = AP3_SPI_IOM; - cipoPinCfg.ePullup = AM_HAL_GPIO_PIN_PULLUP_1_5K; - padMode(MISO, cipoPinCfg, &retval); - return (retval == AP3_OK); + Serial.println("Calibrating gravity vector - keep the sensor still or this might give inaccurate results"); + //Do exponential moving average over a certain time period and wait until vector settles at around 1g. Takes around 8 seconds. + double totalAcc = sqrt(g_x*g_x + g_y*g_y+ g_z*g_z); + float ema = 0.01; + // Average the first xxx readings. + int counter = 200; + int i = 0; + double grav_x = 0, grav_y = 0, grav_z = 0; + + while (i < counter) + { + int moreData = readIMUDMP(); + if (moreData >=0) //function returns -1 if no data was read. + { + grav_x += quat_x/counter; + grav_y += quat_y/counter; + grav_z += quat_z/counter; + i += 1; + } + + if(moreData <=0){ + delay(1); + } + + } + g_x = grav_x; + g_y = grav_y; + g_z = grav_z; + } + + // initializeDMP is a weak function. Let's overwrite it so we can increase the sample rate ICM_20948_Status_e ICM_20948::initializeDMP(void) {