diff --git a/examples/Arduino/Example10_DMP_FastMultipleSensors/Example10_DMP_FastMultipleSensors.ino b/examples/Arduino/Example10_DMP_FastMultipleSensors/Example10_DMP_FastMultipleSensors.ino new file mode 100644 index 0000000..0ad9f5a --- /dev/null +++ b/examples/Arduino/Example10_DMP_FastMultipleSensors/Example10_DMP_FastMultipleSensors.ino @@ -0,0 +1,415 @@ +/**************************************************************** + * Example10_DMP_FastMultipleSensors.ino + * 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. + ***************************************************************/ + +#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. Used only when "USE_SPI" is defined +#define CS_PIN 2 // 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 +#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 + +#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() { + + SERIAL_PORT.begin(115200); // Start the serial console + SERIAL_PORT.println(F("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) + ; + +#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 ){ + + // Initialize the ICM-20948 + // If the DMP is enabled, .begin performs a minimal startup. We need to configure the sample mode etc. manually. +#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{ + initialized = true; + } + } + + SERIAL_PORT.println(F("Device connected!")); + + // 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". + + bool success = true; // Use success to show if the configuration was successful + + // 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 + success &= (myICM.setClockSource(ICM_20948_Clock_Auto) == ICM_20948_Stat_Ok); // 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 + success &= (myICM.setBank(0) == ICM_20948_Stat_Ok); // Select Bank 0 + uint8_t pwrMgmt2 = 0x40; // Set the reserved bit 6 + success &= (myICM.write(AGB0_REG_PWR_MGMT_2, &pwrMgmt2, 1) == ICM_20948_Stat_Ok); // Write one byte to the PWR_MGMT_2 register + + // Configure I2C_Master/Gyro/Accel in Low Power Mode (cycled) with LP_CONFIG + success &= (myICM.setSampleMode( (ICM_20948_Internal_Mst | ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), ICM_20948_Sample_Mode_Cycled ) == ICM_20948_Stat_Ok); + + // Disable the FIFO + success &= (myICM.enableFIFO(false) == ICM_20948_Stat_Ok); + + // Disable the DMP + success &= (myICM.enableDMP(false) == ICM_20948_Stat_Ok); + + // 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 + success &= (myICM.setFullScale( (ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), myFSS ) == ICM_20948_Stat_Ok); + + // Enable interrupt for FIFO overflow from FIFOs through INT_ENABLE_2 + // If we see this interrupt, we'll need to reset the FIFO + //success &= (myICM.intEnableOverflowFIFO( 0x1F ) == ICM_20948_Stat_Ok); // 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 + success &= (myICM.setBank(0) == ICM_20948_Stat_Ok); // Select Bank 0 + uint8_t zero = 0; + success &= (myICM.write(AGB0_REG_FIFO_EN_1, &zero, 1) == ICM_20948_Stat_Ok); + // Stop the accelerometer, gyro and temperature data from being written to the FIFO by writing zero to FIFO_EN_2 + success &= (myICM.write(AGB0_REG_FIFO_EN_2, &zero, 1) == ICM_20948_Stat_Ok); + + // Turn off data ready interrupt through INT_ENABLE_1 + success &= (myICM.intEnableRawDataReady(false) == ICM_20948_Stat_Ok); + + // Reset FIFO through FIFO_RST + success &= (myICM.resetFIFO() == ICM_20948_Stat_Ok); + + // 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 + myICM.setSampleRate( (ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), mySmplrt ); + + // Setup DMP start address through PRGM_STRT_ADDRH/PRGM_STRT_ADDRL + success &= (myICM.setDMPstartAddress() == ICM_20948_Stat_Ok); // Defaults to DMP_START_ADDRESS + + // Now load the DMP firmware + success &= (myICM.loadDMPFirmware() == ICM_20948_Stat_Ok); + + // Write the 2 byte Firmware Start Value to ICM PRGM_STRT_ADDRH/PRGM_STRT_ADDRL + success &= (myICM.setDMPstartAddress() == ICM_20948_Stat_Ok); // Defaults to DMP_START_ADDRESS + + // Set the Hardware Fix Disable register to 0x48 + success &= (myICM.setBank(0) == ICM_20948_Stat_Ok); // Select Bank 0 + uint8_t fix = 0x48; + success &= (myICM.write(AGB0_REG_HW_FIX_DISABLE, &fix, 1) == ICM_20948_Stat_Ok); + + // Set the Single FIFO Priority Select register to 0xE4 + success &= (myICM.setBank(0) == ICM_20948_Stat_Ok); // Select Bank 0 + uint8_t fifoPrio = 0xE4; + success &= (myICM.write(AGB0_REG_SINGLE_FIFO_PRIORITY_SEL, &fifoPrio, 1) == ICM_20948_Stat_Ok); + + // 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}; + success &= (myICM.writeDMPmems(ACC_SCALE, 4, &accScale[0]) == ICM_20948_Stat_Ok); // 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}; + success &= (myICM.writeDMPmems(ACC_SCALE2, 4, &accScale2[0]) == ICM_20948_Stat_Ok); // 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 + success &= (myICM.writeDMPmems(CPASS_MTX_00, 4, &mountMultiplierPlus[0]) == ICM_20948_Stat_Ok); + success &= (myICM.writeDMPmems(CPASS_MTX_01, 4, &mountMultiplierZero[0]) == ICM_20948_Stat_Ok); + success &= (myICM.writeDMPmems(CPASS_MTX_02, 4, &mountMultiplierZero[0]) == ICM_20948_Stat_Ok); + success &= (myICM.writeDMPmems(CPASS_MTX_10, 4, &mountMultiplierZero[0]) == ICM_20948_Stat_Ok); + success &= (myICM.writeDMPmems(CPASS_MTX_11, 4, &mountMultiplierMinus[0]) == ICM_20948_Stat_Ok); + success &= (myICM.writeDMPmems(CPASS_MTX_12, 4, &mountMultiplierZero[0]) == ICM_20948_Stat_Ok); + success &= (myICM.writeDMPmems(CPASS_MTX_20, 4, &mountMultiplierZero[0]) == ICM_20948_Stat_Ok); + success &= (myICM.writeDMPmems(CPASS_MTX_21, 4, &mountMultiplierZero[0]) == ICM_20948_Stat_Ok); + success &= (myICM.writeDMPmems(CPASS_MTX_22, 4, &mountMultiplierMinus[0]) == ICM_20948_Stat_Ok); + + // 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 + success &= (myICM.writeDMPmems(B2S_MTX_00, 4, &b2sMountMultiplierPlus[0]) == ICM_20948_Stat_Ok); + success &= (myICM.writeDMPmems(B2S_MTX_01, 4, &b2sMountMultiplierZero[0]) == ICM_20948_Stat_Ok); + success &= (myICM.writeDMPmems(B2S_MTX_02, 4, &b2sMountMultiplierZero[0]) == ICM_20948_Stat_Ok); + success &= (myICM.writeDMPmems(B2S_MTX_10, 4, &b2sMountMultiplierZero[0]) == ICM_20948_Stat_Ok); + success &= (myICM.writeDMPmems(B2S_MTX_11, 4, &b2sMountMultiplierPlus[0]) == ICM_20948_Stat_Ok); + success &= (myICM.writeDMPmems(B2S_MTX_12, 4, &b2sMountMultiplierZero[0]) == ICM_20948_Stat_Ok); + success &= (myICM.writeDMPmems(B2S_MTX_20, 4, &b2sMountMultiplierZero[0]) == ICM_20948_Stat_Ok); + success &= (myICM.writeDMPmems(B2S_MTX_21, 4, &b2sMountMultiplierZero[0]) == ICM_20948_Stat_Ok); + success &= (myICM.writeDMPmems(B2S_MTX_22, 4, &b2sMountMultiplierPlus[0]) == ICM_20948_Stat_Ok); + + // 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 + success &= (myICM.setGyroSF(4, 3) == ICM_20948_Stat_Ok); // 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 + success &= (myICM.writeDMPmems(GYRO_FULLSCALE, 4, &gyroFullScale[0]) == ICM_20948_Stat_Ok); + + // Configure the Accel Only Gain: 15252014 (225Hz) 30504029 (112Hz) 61117001 (56Hz) + //const unsigned char accelOnlyGain[4] = {0x03, 0xA4, 0x92, 0x49}; // 56Hz + const unsigned char accelOnlyGain[4] = {0x00, 0xE8, 0xBA, 0x2E}; // InvenSense Nucleo example uses 225Hz + success &= (myICM.writeDMPmems(ACCEL_ONLY_GAIN, 4, &accelOnlyGain[0]) == ICM_20948_Stat_Ok); + + // Configure the Accel Alpha Var: 1026019965 (225Hz) 977872018 (112Hz) 882002213 (56Hz) + //const unsigned char accelAlphaVar[4] = {0x34, 0x92, 0x49, 0x25}; // 56Hz + const unsigned char accelAlphaVar[4] = {0x3D, 0x27, 0xD2, 0x7D}; // 225Hz + success &= (myICM.writeDMPmems(ACCEL_ALPHA_VAR, 4, &accelAlphaVar[0]) == ICM_20948_Stat_Ok); + + // Configure the Accel A Var: 47721859 (225Hz) 95869806 (112Hz) 191739611 (56Hz) + //const unsigned char accelAVar[4] = {0x0B, 0x6D, 0xB6, 0xDB}; // 56Hz + const unsigned char accelAVar[4] = {0x02, 0xD8, 0x2D, 0x83}; // 225Hz + success &= (myICM.writeDMPmems(ACCEL_A_VAR, 4, &accelAVar[0]) == ICM_20948_Stat_Ok); + + // Configure the Accel Cal Rate + const unsigned char accelCalRate[4] = {0x00, 0x00}; // Value taken from InvenSense Nucleo example + success &= (myICM.writeDMPmems(ACCEL_CAL_RATE, 2, &accelCalRate[0]) == ICM_20948_Stat_Ok); + + // Configure the Compass Time Buffer. The compass (magnetometer) is set to 100Hz (AK09916_mode_cont_100hz) + // in startupMagnetometer. We need to set CPASS_TIME_BUFFER to 100 too. + const unsigned char compassRate[2] = {0x00, 0x64}; // 100Hz + success &= (myICM.writeDMPmems(CPASS_TIME_BUFFER, 2, &compassRate[0]) == ICM_20948_Stat_Ok); + + // Enable DMP interrupt + // This would be the most efficient way of getting the DMP data, instead of polling the FIFO + //success &= (myICM.intEnableDMP(true) == ICM_20948_Stat_Ok); + + // 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_GAME_ROTATION_VECTOR) == ICM_20948_Stat_Ok); + + // Enable additional sensors / features + //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: + // 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_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 + } +} + +void loop() +{ + // 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_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 ) // Check for GRV data (Quat6) + { + // 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 + + 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.println(q3, 3); + } + + if ( (data.header & DMP_header_bitmap_Accel) > 0 ) // Check for Accel + { + 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_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); + } + +// if ( (data.header & DMP_header_bitmap_Gyro) > 0 ) // Check for Gyro +// { +// float x = (float)data.Raw_Gyro.Data.X; // Extract the raw gyro data +// float y = (float)data.Raw_Gyro.Data.Y; +// float z = (float)data.Raw_Gyro.Data.Z; +// +// SERIAL_PORT.print(F("Gyro: X:")); +// SERIAL_PORT.print(x); +// SERIAL_PORT.print(F(" Y:")); +// SERIAL_PORT.print(y); +// SERIAL_PORT.print(F(" Z:")); +// SERIAL_PORT.println(z); +// } +// +// if ( (data.header & DMP_header_bitmap_Compass) > 0 ) // Check for Compass +// { +// float x = (float)data.Compass.Data.X; // Extract the compass data +// float y = (float)data.Compass.Data.Y; +// float z = (float)data.Compass.Data.Z; +// +// SERIAL_PORT.print(F("Compass: X:")); +// SERIAL_PORT.print(x); +// SERIAL_PORT.print(F(" Y:")); +// SERIAL_PORT.print(y); +// SERIAL_PORT.print(F(" Z:")); +// SERIAL_PORT.println(z); +// } + + } + + if ( myICM.status != ICM_20948_Stat_FIFOMoreDataAvail ) // If more data is available then we should read it right away - and not delay + { + delay(1); // Keep this short! + } +} diff --git a/examples/Arduino/Example6_DMP_Quat9_Orientation/Example6_DMP_Quat9_Orientation.ino b/examples/Arduino/Example6_DMP_Quat9_Orientation/Example6_DMP_Quat9_Orientation.ino index f63ff78..10d8cb6 100644 --- a/examples/Arduino/Example6_DMP_Quat9_Orientation/Example6_DMP_Quat9_Orientation.ino +++ b/examples/Arduino/Example6_DMP_Quat9_Orientation/Example6_DMP_Quat9_Orientation.ino @@ -24,7 +24,7 @@ * Distributed as-is; no warranty is given. ***************************************************************/ -#define QUAT_ANIMATION // Uncomment this line to output data in the correct format for ZaneL's Node.js Quaternion animation tool: https://github.com/ZaneL/quaternion_sensor_3d_nodejs +//#define QUAT_ANIMATION // Uncomment this line to output data in the correct format for ZaneL's Node.js Quaternion animation tool: https://github.com/ZaneL/quaternion_sensor_3d_nodejs #include "ICM_20948.h" // Click here to get the library: http://librarymanager/All#SparkFun_ICM_20948_IMU @@ -223,19 +223,17 @@ void setup() { // 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 - success &= (myICM.writeDMPmems(B2S_MTX_00, 4, &mountMultiplierPlus[0]) == ICM_20948_Stat_Ok); - success &= (myICM.writeDMPmems(B2S_MTX_01, 4, &mountMultiplierZero[0]) == ICM_20948_Stat_Ok); - success &= (myICM.writeDMPmems(B2S_MTX_02, 4, &mountMultiplierZero[0]) == ICM_20948_Stat_Ok); - success &= (myICM.writeDMPmems(B2S_MTX_10, 4, &mountMultiplierZero[0]) == ICM_20948_Stat_Ok); - success &= (myICM.writeDMPmems(B2S_MTX_11, 4, &mountMultiplierPlus[0]) == ICM_20948_Stat_Ok); - success &= (myICM.writeDMPmems(B2S_MTX_12, 4, &mountMultiplierZero[0]) == ICM_20948_Stat_Ok); - success &= (myICM.writeDMPmems(B2S_MTX_20, 4, &mountMultiplierZero[0]) == ICM_20948_Stat_Ok); - success &= (myICM.writeDMPmems(B2S_MTX_21, 4, &mountMultiplierZero[0]) == ICM_20948_Stat_Ok); - success &= (myICM.writeDMPmems(B2S_MTX_22, 4, &mountMultiplierPlus[0]) == ICM_20948_Stat_Ok); + success &= (myICM.writeDMPmems(B2S_MTX_00, 4, &b2sMountMultiplierPlus[0]) == ICM_20948_Stat_Ok); + success &= (myICM.writeDMPmems(B2S_MTX_01, 4, &b2sMountMultiplierZero[0]) == ICM_20948_Stat_Ok); + success &= (myICM.writeDMPmems(B2S_MTX_02, 4, &b2sMountMultiplierZero[0]) == ICM_20948_Stat_Ok); + success &= (myICM.writeDMPmems(B2S_MTX_10, 4, &b2sMountMultiplierZero[0]) == ICM_20948_Stat_Ok); + success &= (myICM.writeDMPmems(B2S_MTX_11, 4, &b2sMountMultiplierPlus[0]) == ICM_20948_Stat_Ok); + success &= (myICM.writeDMPmems(B2S_MTX_12, 4, &b2sMountMultiplierZero[0]) == ICM_20948_Stat_Ok); + success &= (myICM.writeDMPmems(B2S_MTX_20, 4, &b2sMountMultiplierZero[0]) == ICM_20948_Stat_Ok); + success &= (myICM.writeDMPmems(B2S_MTX_21, 4, &b2sMountMultiplierZero[0]) == ICM_20948_Stat_Ok); + success &= (myICM.writeDMPmems(B2S_MTX_22, 4, &b2sMountMultiplierPlus[0]) == ICM_20948_Stat_Ok); // Configure the DMP Gyro Scaling Factor - //const unsigned char gyroScalingFactor[4] = {0x26, 0xFA, 0xB4, 0xB1}; // Value taken from InvenSense Nucleo example - //success &= (myICM.writeDMPmems(GYRO_SF, 4, &gyroScalingFactor[0]) == ICM_20948_Stat_Ok); // @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. @@ -263,7 +261,7 @@ void setup() { // 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] = {0x39, 0x99, 0x99, 0x9A}; // Value taken from InvenSense Nucleo example - success &= (myICM.writeDMPmems(ACCEL_A_VAR, 4, &accelAlphaVar[0]) == ICM_20948_Stat_Ok); + success &= (myICM.writeDMPmems(ACCEL_A_VAR, 4, &accelAVar[0]) == ICM_20948_Stat_Ok); // Configure the Accel Cal Rate const unsigned char accelCalRate[4] = {0x00, 0x00}; // Value taken from InvenSense Nucleo example @@ -338,6 +336,8 @@ void setup() { { 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 } } diff --git a/examples/Arduino/Example7_DMP_Quat6_EulerAngles/Example7_DMP_Quat6_EulerAngles.ino b/examples/Arduino/Example7_DMP_Quat6_EulerAngles/Example7_DMP_Quat6_EulerAngles.ino index 9028210..3741c67 100644 --- a/examples/Arduino/Example7_DMP_Quat6_EulerAngles/Example7_DMP_Quat6_EulerAngles.ino +++ b/examples/Arduino/Example7_DMP_Quat6_EulerAngles/Example7_DMP_Quat6_EulerAngles.ino @@ -223,19 +223,17 @@ void setup() { // 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 - success &= (myICM.writeDMPmems(B2S_MTX_00, 4, &mountMultiplierPlus[0]) == ICM_20948_Stat_Ok); - success &= (myICM.writeDMPmems(B2S_MTX_01, 4, &mountMultiplierZero[0]) == ICM_20948_Stat_Ok); - success &= (myICM.writeDMPmems(B2S_MTX_02, 4, &mountMultiplierZero[0]) == ICM_20948_Stat_Ok); - success &= (myICM.writeDMPmems(B2S_MTX_10, 4, &mountMultiplierZero[0]) == ICM_20948_Stat_Ok); - success &= (myICM.writeDMPmems(B2S_MTX_11, 4, &mountMultiplierPlus[0]) == ICM_20948_Stat_Ok); - success &= (myICM.writeDMPmems(B2S_MTX_12, 4, &mountMultiplierZero[0]) == ICM_20948_Stat_Ok); - success &= (myICM.writeDMPmems(B2S_MTX_20, 4, &mountMultiplierZero[0]) == ICM_20948_Stat_Ok); - success &= (myICM.writeDMPmems(B2S_MTX_21, 4, &mountMultiplierZero[0]) == ICM_20948_Stat_Ok); - success &= (myICM.writeDMPmems(B2S_MTX_22, 4, &mountMultiplierPlus[0]) == ICM_20948_Stat_Ok); + success &= (myICM.writeDMPmems(B2S_MTX_00, 4, &b2sMountMultiplierPlus[0]) == ICM_20948_Stat_Ok); + success &= (myICM.writeDMPmems(B2S_MTX_01, 4, &b2sMountMultiplierZero[0]) == ICM_20948_Stat_Ok); + success &= (myICM.writeDMPmems(B2S_MTX_02, 4, &b2sMountMultiplierZero[0]) == ICM_20948_Stat_Ok); + success &= (myICM.writeDMPmems(B2S_MTX_10, 4, &b2sMountMultiplierZero[0]) == ICM_20948_Stat_Ok); + success &= (myICM.writeDMPmems(B2S_MTX_11, 4, &b2sMountMultiplierPlus[0]) == ICM_20948_Stat_Ok); + success &= (myICM.writeDMPmems(B2S_MTX_12, 4, &b2sMountMultiplierZero[0]) == ICM_20948_Stat_Ok); + success &= (myICM.writeDMPmems(B2S_MTX_20, 4, &b2sMountMultiplierZero[0]) == ICM_20948_Stat_Ok); + success &= (myICM.writeDMPmems(B2S_MTX_21, 4, &b2sMountMultiplierZero[0]) == ICM_20948_Stat_Ok); + success &= (myICM.writeDMPmems(B2S_MTX_22, 4, &b2sMountMultiplierPlus[0]) == ICM_20948_Stat_Ok); // Configure the DMP Gyro Scaling Factor - //const unsigned char gyroScalingFactor[4] = {0x26, 0xFA, 0xB4, 0xB1}; // Value taken from InvenSense Nucleo example - //success &= (myICM.writeDMPmems(GYRO_SF, 4, &gyroScalingFactor[0]) == ICM_20948_Stat_Ok); // @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. @@ -263,7 +261,7 @@ void setup() { // 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] = {0x39, 0x99, 0x99, 0x9A}; // Value taken from InvenSense Nucleo example - success &= (myICM.writeDMPmems(ACCEL_A_VAR, 4, &accelAlphaVar[0]) == ICM_20948_Stat_Ok); + success &= (myICM.writeDMPmems(ACCEL_A_VAR, 4, &accelAVar[0]) == ICM_20948_Stat_Ok); // Configure the Accel Cal Rate const unsigned char accelCalRate[4] = {0x00, 0x00}; // Value taken from InvenSense Nucleo example @@ -338,6 +336,8 @@ void setup() { { 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 } } diff --git a/examples/Arduino/Example8_DMP_RawAccel/Example8_DMP_RawAccel.ino b/examples/Arduino/Example8_DMP_RawAccel/Example8_DMP_RawAccel.ino index 7a1740c..8ba5168 100644 --- a/examples/Arduino/Example8_DMP_RawAccel/Example8_DMP_RawAccel.ino +++ b/examples/Arduino/Example8_DMP_RawAccel/Example8_DMP_RawAccel.ino @@ -209,19 +209,17 @@ void setup() { // 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 - success &= (myICM.writeDMPmems(B2S_MTX_00, 4, &mountMultiplierPlus[0]) == ICM_20948_Stat_Ok); - success &= (myICM.writeDMPmems(B2S_MTX_01, 4, &mountMultiplierZero[0]) == ICM_20948_Stat_Ok); - success &= (myICM.writeDMPmems(B2S_MTX_02, 4, &mountMultiplierZero[0]) == ICM_20948_Stat_Ok); - success &= (myICM.writeDMPmems(B2S_MTX_10, 4, &mountMultiplierZero[0]) == ICM_20948_Stat_Ok); - success &= (myICM.writeDMPmems(B2S_MTX_11, 4, &mountMultiplierPlus[0]) == ICM_20948_Stat_Ok); - success &= (myICM.writeDMPmems(B2S_MTX_12, 4, &mountMultiplierZero[0]) == ICM_20948_Stat_Ok); - success &= (myICM.writeDMPmems(B2S_MTX_20, 4, &mountMultiplierZero[0]) == ICM_20948_Stat_Ok); - success &= (myICM.writeDMPmems(B2S_MTX_21, 4, &mountMultiplierZero[0]) == ICM_20948_Stat_Ok); - success &= (myICM.writeDMPmems(B2S_MTX_22, 4, &mountMultiplierPlus[0]) == ICM_20948_Stat_Ok); + success &= (myICM.writeDMPmems(B2S_MTX_00, 4, &b2sMountMultiplierPlus[0]) == ICM_20948_Stat_Ok); + success &= (myICM.writeDMPmems(B2S_MTX_01, 4, &b2sMountMultiplierZero[0]) == ICM_20948_Stat_Ok); + success &= (myICM.writeDMPmems(B2S_MTX_02, 4, &b2sMountMultiplierZero[0]) == ICM_20948_Stat_Ok); + success &= (myICM.writeDMPmems(B2S_MTX_10, 4, &b2sMountMultiplierZero[0]) == ICM_20948_Stat_Ok); + success &= (myICM.writeDMPmems(B2S_MTX_11, 4, &b2sMountMultiplierPlus[0]) == ICM_20948_Stat_Ok); + success &= (myICM.writeDMPmems(B2S_MTX_12, 4, &b2sMountMultiplierZero[0]) == ICM_20948_Stat_Ok); + success &= (myICM.writeDMPmems(B2S_MTX_20, 4, &b2sMountMultiplierZero[0]) == ICM_20948_Stat_Ok); + success &= (myICM.writeDMPmems(B2S_MTX_21, 4, &b2sMountMultiplierZero[0]) == ICM_20948_Stat_Ok); + success &= (myICM.writeDMPmems(B2S_MTX_22, 4, &b2sMountMultiplierPlus[0]) == ICM_20948_Stat_Ok); // Configure the DMP Gyro Scaling Factor - //const unsigned char gyroScalingFactor[4] = {0x26, 0xFA, 0xB4, 0xB1}; // Value taken from InvenSense Nucleo example - //success &= (myICM.writeDMPmems(GYRO_SF, 4, &gyroScalingFactor[0]) == ICM_20948_Stat_Ok); // @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. @@ -249,7 +247,7 @@ void setup() { // 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] = {0x39, 0x99, 0x99, 0x9A}; // Value taken from InvenSense Nucleo example - success &= (myICM.writeDMPmems(ACCEL_A_VAR, 4, &accelAlphaVar[0]) == ICM_20948_Stat_Ok); + success &= (myICM.writeDMPmems(ACCEL_A_VAR, 4, &accelAVar[0]) == ICM_20948_Stat_Ok); // Configure the Accel Cal Rate const unsigned char accelCalRate[4] = {0x00, 0x00}; // Value taken from InvenSense Nucleo example @@ -312,6 +310,8 @@ void setup() { { 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 } } diff --git a/examples/Arduino/Example9_DMP_MultipleSensors/Example9_DMP_MultipleSensors.ino b/examples/Arduino/Example9_DMP_MultipleSensors/Example9_DMP_MultipleSensors.ino index 4c9eaec..8c53720 100644 --- a/examples/Arduino/Example9_DMP_MultipleSensors/Example9_DMP_MultipleSensors.ino +++ b/examples/Arduino/Example9_DMP_MultipleSensors/Example9_DMP_MultipleSensors.ino @@ -209,19 +209,17 @@ void setup() { // 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 - success &= (myICM.writeDMPmems(B2S_MTX_00, 4, &mountMultiplierPlus[0]) == ICM_20948_Stat_Ok); - success &= (myICM.writeDMPmems(B2S_MTX_01, 4, &mountMultiplierZero[0]) == ICM_20948_Stat_Ok); - success &= (myICM.writeDMPmems(B2S_MTX_02, 4, &mountMultiplierZero[0]) == ICM_20948_Stat_Ok); - success &= (myICM.writeDMPmems(B2S_MTX_10, 4, &mountMultiplierZero[0]) == ICM_20948_Stat_Ok); - success &= (myICM.writeDMPmems(B2S_MTX_11, 4, &mountMultiplierPlus[0]) == ICM_20948_Stat_Ok); - success &= (myICM.writeDMPmems(B2S_MTX_12, 4, &mountMultiplierZero[0]) == ICM_20948_Stat_Ok); - success &= (myICM.writeDMPmems(B2S_MTX_20, 4, &mountMultiplierZero[0]) == ICM_20948_Stat_Ok); - success &= (myICM.writeDMPmems(B2S_MTX_21, 4, &mountMultiplierZero[0]) == ICM_20948_Stat_Ok); - success &= (myICM.writeDMPmems(B2S_MTX_22, 4, &mountMultiplierPlus[0]) == ICM_20948_Stat_Ok); + success &= (myICM.writeDMPmems(B2S_MTX_00, 4, &b2sMountMultiplierPlus[0]) == ICM_20948_Stat_Ok); + success &= (myICM.writeDMPmems(B2S_MTX_01, 4, &b2sMountMultiplierZero[0]) == ICM_20948_Stat_Ok); + success &= (myICM.writeDMPmems(B2S_MTX_02, 4, &b2sMountMultiplierZero[0]) == ICM_20948_Stat_Ok); + success &= (myICM.writeDMPmems(B2S_MTX_10, 4, &b2sMountMultiplierZero[0]) == ICM_20948_Stat_Ok); + success &= (myICM.writeDMPmems(B2S_MTX_11, 4, &b2sMountMultiplierPlus[0]) == ICM_20948_Stat_Ok); + success &= (myICM.writeDMPmems(B2S_MTX_12, 4, &b2sMountMultiplierZero[0]) == ICM_20948_Stat_Ok); + success &= (myICM.writeDMPmems(B2S_MTX_20, 4, &b2sMountMultiplierZero[0]) == ICM_20948_Stat_Ok); + success &= (myICM.writeDMPmems(B2S_MTX_21, 4, &b2sMountMultiplierZero[0]) == ICM_20948_Stat_Ok); + success &= (myICM.writeDMPmems(B2S_MTX_22, 4, &b2sMountMultiplierPlus[0]) == ICM_20948_Stat_Ok); // Configure the DMP Gyro Scaling Factor - //const unsigned char gyroScalingFactor[4] = {0x26, 0xFA, 0xB4, 0xB1}; // Value taken from InvenSense Nucleo example - //success &= (myICM.writeDMPmems(GYRO_SF, 4, &gyroScalingFactor[0]) == ICM_20948_Stat_Ok); // @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. @@ -249,7 +247,7 @@ void setup() { // 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] = {0x39, 0x99, 0x99, 0x9A}; // Value taken from InvenSense Nucleo example - success &= (myICM.writeDMPmems(ACCEL_A_VAR, 4, &accelAlphaVar[0]) == ICM_20948_Stat_Ok); + success &= (myICM.writeDMPmems(ACCEL_A_VAR, 4, &accelAVar[0]) == ICM_20948_Stat_Ok); // Configure the Accel Cal Rate const unsigned char accelCalRate[4] = {0x00, 0x00}; // Value taken from InvenSense Nucleo example @@ -281,7 +279,7 @@ 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 + // Enable the DMP Game Rotation Vector sensor (Quat6) success &= (myICM.enableDMPSensor(INV_ICM20948_SENSOR_GAME_ROTATION_VECTOR) == ICM_20948_Stat_Ok); // Enable additional sensors / features @@ -322,6 +320,8 @@ void setup() { { 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 } } @@ -345,18 +345,18 @@ void loop() //if ( data.header < 0x10) SERIAL_PORT.print( "0" ); //SERIAL_PORT.println( data.header, HEX ); - if ( (data.header & DMP_header_bitmap_Quat9) > 0 ) // Check for orientation data (Quat9) + if ( (data.header & DMP_header_bitmap_Quat6) > 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_PORT.printf("Quat9 data is: Q1:%ld Q2:%ld Q3:%ld Accuracy:%d\r\n", data.Quat9.Data.Q1, data.Quat9.Data.Q2, data.Quat9.Data.Q3, data.Quat9.Data.Accuracy); + //SERIAL_PORT.printf("Quat9 data is: Q1:%ld Q2:%ld Q3:%ld Accuracy:%d\r\n", data.Quat6.Data.Q1, data.Quat6.Data.Q2, data.Quat9.Data.Q3, data.Quat6.Data.Accuracy); // Scale to +/- 1 - double q1 = ((double)data.Quat9.Data.Q1) / 1073741824.0; // Convert to double. Divide by 2^30 - double q2 = ((double)data.Quat9.Data.Q2) / 1073741824.0; // Convert to double. Divide by 2^30 - double q3 = ((double)data.Quat9.Data.Q3) / 1073741824.0; // Convert to double. Divide by 2^30 + 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 SERIAL_PORT.print(F("Q1:")); SERIAL_PORT.print(q1, 3); diff --git a/library.properties b/library.properties index 40f5801..8d14d70 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=SparkFun 9DoF IMU Breakout - ICM 20948 - Arduino Library -version=1.2.2 +version=1.2.3 author=SparkFun Electronics maintainer=SparkFun Electronics sentence=Use the low-power high-resolution ICM 20948 9 DoF IMU from Invensense with I2C or SPI. Version 1.2 of the library includes support for the InvenSense Digital Motion Processor (DMP™). diff --git a/src/util/ICM_20948_C.c b/src/util/ICM_20948_C.c index 1522716..0123829 100644 --- a/src/util/ICM_20948_C.c +++ b/src/util/ICM_20948_C.c @@ -2,6 +2,110 @@ #include "ICM_20948_REGISTERS.h" #include "AK09916_REGISTERS.h" +/* + * Icm20948 device require a DMP image to be loaded on init + * Provide such images by mean of a byte array +*/ +#if defined(ICM_20948_USE_DMP) // Only include the 14301 Bytes of DMP if ICM_20948_USE_DMP is defined + const uint8_t dmp3_image[] = { + #include "icm20948_img.dmp3a.h" + }; +#endif + +// ICM-20948 data is big-endian. We need to make it little-endian when writing into icm_20948_DMP_data_t +const int DMP_Quat9_Byte_Ordering[icm_20948_DMP_Quat9_Bytes] = +{ + 3,2,1,0,7,6,5,4,11,10,9,8,13,12 // Also used for Geomag +}; +const int DMP_Quat6_Byte_Ordering[icm_20948_DMP_Quat6_Bytes] = +{ + 3,2,1,0,7,6,5,4,11,10,9,8 // Also used for Gyro_Calibr, Compass_Calibr +}; +const int DMP_PQuat6_Byte_Ordering[icm_20948_DMP_PQuat6_Bytes] = +{ + 1,0,3,2,5,4 // Also used for Raw_Accel, Compass +}; +const int DMP_Raw_Gyro_Byte_Ordering[icm_20948_DMP_Raw_Gyro_Bytes + icm_20948_DMP_Gyro_Bias_Bytes] = +{ + 1,0,3,2,5,4,7,6,9,8,11,10 +}; +const int DMP_Activity_Recognition_Byte_Ordering[icm_20948_DMP_Activity_Recognition_Bytes] = +{ + 0,1,5,4,3,2 +}; +const int DMP_Secondary_On_Off_Byte_Ordering[icm_20948_DMP_Secondary_On_Off_Bytes] = +{ + 1,0 +}; + +const uint16_t inv_androidSensor_to_control_bits[ANDROID_SENSOR_NUM_MAX]= +{ + // Data output control 1 register bit definition + // 16-bit accel 0x8000 + // 16-bit gyro 0x4000 + // 16-bit compass 0x2000 + // 16-bit ALS 0x1000 + // 32-bit 6-axis quaternion 0x0800 + // 32-bit 9-axis quaternion + heading accuracy 0x0400 + // 16-bit pedometer quaternion 0x0200 + // 32-bit Geomag rv + heading accuracy 0x0100 + // 16-bit Pressure 0x0080 + // 32-bit calibrated gyro 0x0040 + // 32-bit calibrated compass 0x0020 + // Pedometer Step Detector 0x0010 + // Header 2 0x0008 + // Pedometer Step Indicator Bit 2 0x0004 + // Pedometer Step Indicator Bit 1 0x0002 + // Pedometer Step Indicator Bit 0 0x0001 + // Unsupported Sensors are 0xFFFF + + 0xFFFF, // 0 Meta Data + 0x8008, // 1 Accelerometer + 0x0028, // 2 Magnetic Field + 0x0408, // 3 Orientation + 0x4048, // 4 Gyroscope + 0x1008, // 5 Light + 0x0088, // 6 Pressure + 0xFFFF, // 7 Temperature + 0xFFFF, // 8 Proximity <----------- fixme + 0x0808, // 9 Gravity + 0x8808, // 10 Linear Acceleration + 0x0408, // 11 Rotation Vector + 0xFFFF, // 12 Humidity + 0xFFFF, // 13 Ambient Temperature + 0x2008, // 14 Magnetic Field Uncalibrated + 0x0808, // 15 Game Rotation Vector + 0x4008, // 16 Gyroscope Uncalibrated + 0x0000, // 17 Significant Motion + 0x0018, // 18 Step Detector + 0x0010, // 19 Step Counter <----------- fixme + 0x0108, // 20 Geomagnetic Rotation Vector + 0xFFFF, // 21 ANDROID_SENSOR_HEART_RATE, + 0xFFFF, // 22 ANDROID_SENSOR_PROXIMITY, + + 0x8008, // 23 ANDROID_SENSOR_WAKEUP_ACCELEROMETER, + 0x0028, // 24 ANDROID_SENSOR_WAKEUP_MAGNETIC_FIELD, + 0x0408, // 25 ANDROID_SENSOR_WAKEUP_ORIENTATION, + 0x4048, // 26 ANDROID_SENSOR_WAKEUP_GYROSCOPE, + 0x1008, // 27 ANDROID_SENSOR_WAKEUP_LIGHT, + 0x0088, // 28 ANDROID_SENSOR_WAKEUP_PRESSURE, + 0x0808, // 29 ANDROID_SENSOR_WAKEUP_GRAVITY, + 0x8808, // 30 ANDROID_SENSOR_WAKEUP_LINEAR_ACCELERATION, + 0x0408, // 31 ANDROID_SENSOR_WAKEUP_ROTATION_VECTOR, + 0xFFFF, // 32 ANDROID_SENSOR_WAKEUP_RELATIVE_HUMIDITY, + 0xFFFF, // 33 ANDROID_SENSOR_WAKEUP_AMBIENT_TEMPERATURE, + 0x2008, // 34 ANDROID_SENSOR_WAKEUP_MAGNETIC_FIELD_UNCALIBRATED, + 0x0808, // 35 ANDROID_SENSOR_WAKEUP_GAME_ROTATION_VECTOR, + 0x4008, // 36 ANDROID_SENSOR_WAKEUP_GYROSCOPE_UNCALIBRATED, + 0x0018, // 37 ANDROID_SENSOR_WAKEUP_STEP_DETECTOR, + 0x0010, // 38 ANDROID_SENSOR_WAKEUP_STEP_COUNTER, + 0x0108, // 39 ANDROID_SENSOR_WAKEUP_GEOMAGNETIC_ROTATION_VECTOR + 0xFFFF, // 40 ANDROID_SENSOR_WAKEUP_HEART_RATE, + 0x0000, // 41 ANDROID_SENSOR_WAKEUP_TILT_DETECTOR, + 0x8008, // 42 Raw Acc + 0x4048, // 43 Raw Gyr +}; + const ICM_20948_Serif_t NullSerif = { NULL, // write NULL, // read diff --git a/src/util/ICM_20948_C.h b/src/util/ICM_20948_C.h index 94ea0da..f0aad4a 100644 --- a/src/util/ICM_20948_C.h +++ b/src/util/ICM_20948_C.h @@ -170,17 +170,6 @@ extern int memcmp(const void *, const void *, size_t); // Avoid compiler warning uint32_t _enabled_Android_intr_1; // Keep track of which Android sensor interrupts are enabled: 32- } ICM_20948_Device_t; // Definition of device struct type - /* - * Icm20948 device require a DMP image to be loaded on init - * Provide such images by mean of a byte array - */ -#if defined(ICM_20948_USE_DMP) // Only include the 93KBytes of DMP if ICM_20948_USE_DMP is defined - const uint8_t dmp3_image[] = { - #include "icm20948_img.dmp3a.h" - }; -#endif - - // ICM_20948_Status_e ICM_20948_Startup( ICM_20948_Device_t* pdev ); // For the time being this performs a standardized startup routine ICM_20948_Status_e ICM_20948_link_serif(ICM_20948_Device_t *pdev, const ICM_20948_Serif_t *s); // Links a SERIF structure to the device diff --git a/src/util/ICM_20948_DMP.h b/src/util/ICM_20948_DMP.h index 5ed733d..eba2b49 100644 --- a/src/util/ICM_20948_DMP.h +++ b/src/util/ICM_20948_DMP.h @@ -390,15 +390,15 @@ enum ANDROID_SENSORS { }; // Determines which base sensor needs to be on based upon ANDROID_SENSORS 0-31 -#define INV_NEEDS_ACCEL_MASK ((1L<<1)| (1L<<3)| (1L<<9)|(1L<<10)|(1L<<11)| (1L<<15)| (1L<<17)|(1L<<18)|(1L<<19)|(1L<<20)|(1<<23)| (1<<25)| (1<<29)|(1<<30)|(1<<31)) -#define INV_NEEDS_GYRO_MASK ( (1L<<3)|(1L<<4)|(1L<<9)|(1L<<10)|(1L<<11)| (1L<<15)|(1L<<16)| (1<<25)|(1<<26)|(1<<29)|(1<<30)|(1<<31)) -#define INV_NEEDS_COMPASS_MASK ( (1L<<2)|(1L<<3)| (1L<<11)|(1L<<14)| (1L<<20)| (1<<24)|(1<<25)| (1<<31)) -#define INV_NEEDS_PRESSURE ((1L<<6)|(1<<28)) +#define INV_NEEDS_ACCEL_MASK ((1L<<1)| (1L<<3)| (1L<<9)|(1L<<10)|(1L<<11)| (1L<<15)| (1L<<17)|(1L<<18)|(1L<<19)|(1L<<20)|(1L<<23)| (1L<<25)| (1L<<29)|(1L<<30)|(1L<<31)) +#define INV_NEEDS_GYRO_MASK ( (1L<<3)|(1L<<4)|(1L<<9)|(1L<<10)|(1L<<11)| (1L<<15)|(1L<<16)| (1L<<25)|(1L<<26)|(1L<<29)|(1L<<30)|(1L<<31)) +#define INV_NEEDS_COMPASS_MASK ( (1L<<2)|(1L<<3)| (1L<<11)|(1L<<14)| (1L<<20)| (1L<<24)|(1L<<25)| (1L<<31)) +#define INV_NEEDS_PRESSURE ((1L<<6)|(1L<<28)) // Determines which base sensor needs to be on based upon ANDROID_SENSORS 32- -#define INV_NEEDS_ACCEL_MASK1 ( (1<<3)| (1<<5)|(1<<6)|(1<<7)|(1<<9)|(1<<10)) // I.e. 35, 37, 38, 39, 41, 42 -#define INV_NEEDS_GYRO_MASK1 ( (1<<3)|(1<<4) |(1<<11)) // I.e. 35, 36, 43 -#define INV_NEEDS_COMPASS_MASK1 ((1<<2)| (1<<7)) // I.e. 34 and 39 +#define INV_NEEDS_ACCEL_MASK1 ( (1L<<3)| (1L<<5)|(1L<<6)|(1L<<7)|(1L<<9)|(1L<<10)) // I.e. 35, 37, 38, 39, 41, 42 +#define INV_NEEDS_GYRO_MASK1 ( (1L<<3)|(1L<<4) |(1L<<11)) // I.e. 35, 36, 43 +#define INV_NEEDS_COMPASS_MASK1 ((1L<<2)| (1L<<7)) // I.e. 34 and 39 enum DMP_Data_Ready_Status_Register_Bits { @@ -485,74 +485,6 @@ enum DMP_Header2_Bitmap DMP_header2_bitmap_Accel_Accuracy = 0x4000 }; -const uint16_t inv_androidSensor_to_control_bits[ANDROID_SENSOR_NUM_MAX]= -{ - // Data output control 1 register bit definition - // 16-bit accel 0x8000 - // 16-bit gyro 0x4000 - // 16-bit compass 0x2000 - // 16-bit ALS 0x1000 - // 32-bit 6-axis quaternion 0x0800 - // 32-bit 9-axis quaternion + heading accuracy 0x0400 - // 16-bit pedometer quaternion 0x0200 - // 32-bit Geomag rv + heading accuracy 0x0100 - // 16-bit Pressure 0x0080 - // 32-bit calibrated gyro 0x0040 - // 32-bit calibrated compass 0x0020 - // Pedometer Step Detector 0x0010 - // Header 2 0x0008 - // Pedometer Step Indicator Bit 2 0x0004 - // Pedometer Step Indicator Bit 1 0x0002 - // Pedometer Step Indicator Bit 0 0x0001 - // Unsupported Sensors are 0xFFFF - - 0xFFFF, // 0 Meta Data - 0x8008, // 1 Accelerometer - 0x0028, // 2 Magnetic Field - 0x0408, // 3 Orientation - 0x4048, // 4 Gyroscope - 0x1008, // 5 Light - 0x0088, // 6 Pressure - 0xFFFF, // 7 Temperature - 0xFFFF, // 8 Proximity <----------- fixme - 0x0808, // 9 Gravity - 0x8808, // 10 Linear Acceleration - 0x0408, // 11 Rotation Vector - 0xFFFF, // 12 Humidity - 0xFFFF, // 13 Ambient Temperature - 0x2008, // 14 Magnetic Field Uncalibrated - 0x0808, // 15 Game Rotation Vector - 0x4008, // 16 Gyroscope Uncalibrated - 0x0000, // 17 Significant Motion - 0x0018, // 18 Step Detector - 0x0010, // 19 Step Counter <----------- fixme - 0x0108, // 20 Geomagnetic Rotation Vector - 0xFFFF, // 21 ANDROID_SENSOR_HEART_RATE, - 0xFFFF, // 22 ANDROID_SENSOR_PROXIMITY, - - 0x8008, // 23 ANDROID_SENSOR_WAKEUP_ACCELEROMETER, - 0x0028, // 24 ANDROID_SENSOR_WAKEUP_MAGNETIC_FIELD, - 0x0408, // 25 ANDROID_SENSOR_WAKEUP_ORIENTATION, - 0x4048, // 26 ANDROID_SENSOR_WAKEUP_GYROSCOPE, - 0x1008, // 27 ANDROID_SENSOR_WAKEUP_LIGHT, - 0x0088, // 28 ANDROID_SENSOR_WAKEUP_PRESSURE, - 0x0808, // 29 ANDROID_SENSOR_WAKEUP_GRAVITY, - 0x8808, // 30 ANDROID_SENSOR_WAKEUP_LINEAR_ACCELERATION, - 0x0408, // 31 ANDROID_SENSOR_WAKEUP_ROTATION_VECTOR, - 0xFFFF, // 32 ANDROID_SENSOR_WAKEUP_RELATIVE_HUMIDITY, - 0xFFFF, // 33 ANDROID_SENSOR_WAKEUP_AMBIENT_TEMPERATURE, - 0x2008, // 34 ANDROID_SENSOR_WAKEUP_MAGNETIC_FIELD_UNCALIBRATED, - 0x0808, // 35 ANDROID_SENSOR_WAKEUP_GAME_ROTATION_VECTOR, - 0x4008, // 36 ANDROID_SENSOR_WAKEUP_GYROSCOPE_UNCALIBRATED, - 0x0018, // 37 ANDROID_SENSOR_WAKEUP_STEP_DETECTOR, - 0x0010, // 38 ANDROID_SENSOR_WAKEUP_STEP_COUNTER, - 0x0108, // 39 ANDROID_SENSOR_WAKEUP_GEOMAGNETIC_ROTATION_VECTOR - 0xFFFF, // 40 ANDROID_SENSOR_WAKEUP_HEART_RATE, - 0x0000, // 41 ANDROID_SENSOR_WAKEUP_TILT_DETECTOR, - 0x8008, // 42 Raw Acc - 0x4048, // 43 Raw Gyr -}; - typedef struct // DMP Activity Recognition data { uint8_t Drive : 1; @@ -601,32 +533,6 @@ typedef struct // DMP Secondary On/Off data #define icm_20948_DMP_Footer_Bytes 2 #define icm_20948_DMP_Maximum_Bytes 14 // The most bytes we will attempt to read from the FIFO in one go -// ICM-20948 data is big-endian. We need to make it little-endian when writing into icm_20948_DMP_data_t -const int DMP_Quat9_Byte_Ordering[icm_20948_DMP_Quat9_Bytes] = -{ - 3,2,1,0,7,6,5,4,11,10,9,8,13,12 // Also used for Geomag -}; -const int DMP_Quat6_Byte_Ordering[icm_20948_DMP_Quat6_Bytes] = -{ - 3,2,1,0,7,6,5,4,11,10,9,8 // Also used for Gyro_Calibr, Compass_Calibr -}; -const int DMP_PQuat6_Byte_Ordering[icm_20948_DMP_PQuat6_Bytes] = -{ - 1,0,3,2,5,4 // Also used for Raw_Accel, Compass -}; -const int DMP_Raw_Gyro_Byte_Ordering[icm_20948_DMP_Raw_Gyro_Bytes + icm_20948_DMP_Gyro_Bias_Bytes] = -{ - 1,0,3,2,5,4,7,6,9,8,11,10 -}; -const int DMP_Activity_Recognition_Byte_Ordering[icm_20948_DMP_Activity_Recognition_Bytes] = -{ - 0,1,5,4,3,2 -}; -const int DMP_Secondary_On_Off_Byte_Ordering[icm_20948_DMP_Secondary_On_Off_Bytes] = -{ - 1,0 -}; - typedef struct { uint16_t header;