test_code / Mbed OS test_icm20948

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

icm20948.h@1:8459e28d77a1, 2021-03-29 (annotated)

Committer:
eric11fr
Date:
Mon Mar 29 21:16:25 2021 +0000
Revision:
1:8459e28d77a1
Parent:
0:efb1550773f1 
icm20948

Who changed what in which revision?

UserRevisionLine numberNew contents of line
eric11fr 0:efb1550773f1 1
eric11fr 0:efb1550773f1 2 #define SERIAL_DEBUG true
eric11fr 1:8459e28d77a1 3 #include <stdint.h>
eric11fr 1:8459e28d77a1 4 #include <inttypes.h>
eric11fr 1:8459e28d77a1 5 #include <I2C.h>
eric11fr 1:8459e28d77a1 6 #include "mbed.h"
eric11fr 1:8459e28d77a1 7 #include "mbed.h"
eric11fr 1:8459e28d77a1 8 #include <math.h>
eric11fr 1:8459e28d77a1 9 #include <stdint.h>
eric11fr 1:8459e28d77a1 10 #include <inttypes.h>
eric11fr 1:8459e28d77a1 11 #define DEG_TO_RAD (1/57.2957795)
eric11fr 1:8459e28d77a1 12 #define RAD_TO_DEG 57.2957795
eric11fr 1:8459e28d77a1 13 using namespace std::chrono;
eric11fr 1:8459e28d77a1 14 Timer t;
eric11fr 1:8459e28d77a1 15 I2C i2c(PB_7, PB_6);
eric11fr 1:8459e28d77a1 16 //static BufferedSerial pc(USBTX, USBRX);
eric11fr 0:efb1550773f1 17
eric11fr 1:8459e28d77a1 18 /*float clock_s() { return us_ticker_read() / 1000000.0f; }
eric11fr 1:8459e28d77a1 19 uint64_t clock_ms() { return us_ticker_read() / 1000; }
eric11fr 1:8459e28d77a1 20 uint64_t clock_us() { return us_ticker_read(); }
eric11fr 1:8459e28d77a1 21 */
eric11fr 0:efb1550773f1 22 // See also ICM-20948 Datasheet, Register Map and Descriptions, Revision 1.3,
eric11fr 0:efb1550773f1 23 // https://www.invensense.com/wp-content/uploads/2016/06/DS-000189-ICM-20948-v1.3.pdf
eric11fr 0:efb1550773f1 24 // and AK09916 Datasheet and Register Map
eric11fr 0:efb1550773f1 25 // https://www.akm.com/akm/en/file/datasheet/AK09916C.pdf
eric11fr 0:efb1550773f1 26
eric11fr 0:efb1550773f1 27 //Magnetometer Registers
eric11fr 0:efb1550773f1 28 #define AK09916_ADDRESS 0x0C
eric11fr 0:efb1550773f1 29 #define WHO_AM_I_AK09916 0x01 // (AKA WIA2) should return 0x09
eric11fr 0:efb1550773f1 30 #define AK09916_ST1 0x10 // data ready status bit 0
eric11fr 0:efb1550773f1 31 #define AK09916_XOUT_L 0x11 // data
eric11fr 0:efb1550773f1 32 #define AK09916_XOUT_H 0x12
eric11fr 0:efb1550773f1 33 #define AK09916_YOUT_L 0x13
eric11fr 0:efb1550773f1 34 #define AK09916_YOUT_H 0x14
eric11fr 0:efb1550773f1 35 #define AK09916_ZOUT_L 0x15
eric11fr 0:efb1550773f1 36 #define AK09916_ZOUT_H 0x16
eric11fr 0:efb1550773f1 37 #define AK09916_ST2 0x18 // Data overflow bit 3 and data read error status bit 2
eric11fr 0:efb1550773f1 38 #define AK09916_CNTL 0x30 // Power down (0000), single-measurement (0001), self-test (1000) and Fuse ROM (1111) modes on bits 3:0
eric11fr 0:efb1550773f1 39 #define AK09916_CNTL2 0x31 // Normal (0), Reset (1)
eric11fr 0:efb1550773f1 40
eric11fr 0:efb1550773f1 41 // ICM-20948
eric11fr 0:efb1550773f1 42
eric11fr 0:efb1550773f1 43 // USER BANK 0 REGISTER MAP
eric11fr 0:efb1550773f1 44 #define WHO_AM_I_ICM20948 0x00 // Should return 0xEA
eric11fr 0:efb1550773f1 45 #define USER_CTRL 0x03 // Bit 7 enable DMP, bit 3 reset DMP
eric11fr 0:efb1550773f1 46 #define LP_CONFIG 0x05 // Not found in MPU-9250
eric11fr 0:efb1550773f1 47 #define PWR_MGMT_1 0x06 // Device defaults to the SLEEP mode
eric11fr 0:efb1550773f1 48 #define PWR_MGMT_2 0x07
eric11fr 0:efb1550773f1 49 #define INT_PIN_CFG 0x0F
eric11fr 0:efb1550773f1 50 #define INT_ENABLE 0x10
eric11fr 0:efb1550773f1 51 #define INT_ENABLE_1 0x11 // Not found in MPU-9250
eric11fr 0:efb1550773f1 52 #define INT_ENABLE_2 0x12 // Not found in MPU-9250
eric11fr 0:efb1550773f1 53 #define INT_ENABLE_3 0x13 // Not found in MPU-9250
eric11fr 0:efb1550773f1 54 #define I2C_MST_STATUS 0x17
eric11fr 0:efb1550773f1 55 #define INT_STATUS 0x19
eric11fr 0:efb1550773f1 56 #define INT_STATUS_1 0x1A // Not found in MPU-9250
eric11fr 0:efb1550773f1 57 #define INT_STATUS_2 0x1B // Not found in MPU-9250
eric11fr 0:efb1550773f1 58 #define INT_STATUS_3 0x1C // Not found in MPU-9250
eric11fr 0:efb1550773f1 59 #define DELAY_TIMEH 0x28 // Not found in MPU-9250
eric11fr 0:efb1550773f1 60 #define DELAY_TIMEL 0x29 // Not found in MPU-9250
eric11fr 0:efb1550773f1 61 #define ACCEL_XOUT_H 0x2D
eric11fr 0:efb1550773f1 62 #define ACCEL_XOUT_L 0x2E
eric11fr 0:efb1550773f1 63 #define ACCEL_YOUT_H 0x2F
eric11fr 0:efb1550773f1 64 #define ACCEL_YOUT_L 0x30
eric11fr 0:efb1550773f1 65 #define ACCEL_ZOUT_H 0x31
eric11fr 0:efb1550773f1 66 #define ACCEL_ZOUT_L 0x32
eric11fr 0:efb1550773f1 67 #define GYRO_XOUT_H 0x33
eric11fr 0:efb1550773f1 68 #define GYRO_XOUT_L 0x34
eric11fr 0:efb1550773f1 69 #define GYRO_YOUT_H 0x35
eric11fr 0:efb1550773f1 70 #define GYRO_YOUT_L 0x36
eric11fr 0:efb1550773f1 71 #define GYRO_ZOUT_H 0x37
eric11fr 0:efb1550773f1 72 #define GYRO_ZOUT_L 0x38
eric11fr 0:efb1550773f1 73 #define TEMP_OUT_H 0x39
eric11fr 0:efb1550773f1 74 #define TEMP_OUT_L 0x3A
eric11fr 0:efb1550773f1 75 #define EXT_SENS_DATA_00 0x3B
eric11fr 0:efb1550773f1 76 #define EXT_SENS_DATA_01 0x3C
eric11fr 0:efb1550773f1 77 #define EXT_SENS_DATA_02 0x3D
eric11fr 0:efb1550773f1 78 #define EXT_SENS_DATA_03 0x3E
eric11fr 0:efb1550773f1 79 #define EXT_SENS_DATA_04 0x3F
eric11fr 0:efb1550773f1 80 #define EXT_SENS_DATA_05 0x40
eric11fr 0:efb1550773f1 81 #define EXT_SENS_DATA_06 0x41
eric11fr 0:efb1550773f1 82 #define EXT_SENS_DATA_07 0x42
eric11fr 0:efb1550773f1 83 #define EXT_SENS_DATA_08 0x43
eric11fr 0:efb1550773f1 84 #define EXT_SENS_DATA_09 0x44
eric11fr 0:efb1550773f1 85 #define EXT_SENS_DATA_10 0x45
eric11fr 0:efb1550773f1 86 #define EXT_SENS_DATA_11 0x46
eric11fr 0:efb1550773f1 87 #define EXT_SENS_DATA_12 0x47
eric11fr 0:efb1550773f1 88 #define EXT_SENS_DATA_13 0x48
eric11fr 0:efb1550773f1 89 #define EXT_SENS_DATA_14 0x49
eric11fr 0:efb1550773f1 90 #define EXT_SENS_DATA_15 0x4A
eric11fr 0:efb1550773f1 91 #define EXT_SENS_DATA_16 0x4B
eric11fr 0:efb1550773f1 92 #define EXT_SENS_DATA_17 0x4C
eric11fr 0:efb1550773f1 93 #define EXT_SENS_DATA_18 0x4D
eric11fr 0:efb1550773f1 94 #define EXT_SENS_DATA_19 0x4E
eric11fr 0:efb1550773f1 95 #define EXT_SENS_DATA_20 0x4F
eric11fr 0:efb1550773f1 96 #define EXT_SENS_DATA_21 0x50
eric11fr 0:efb1550773f1 97 #define EXT_SENS_DATA_22 0x51
eric11fr 0:efb1550773f1 98 #define EXT_SENS_DATA_23 0x52
eric11fr 0:efb1550773f1 99 #define FIFO_EN_1 0x66
eric11fr 0:efb1550773f1 100 #define FIFO_EN_2 0x67 // Not found in MPU-9250
eric11fr 0:efb1550773f1 101 #define FIFO_RST 0x68 // Not found in MPU-9250
eric11fr 0:efb1550773f1 102 #define FIFO_MODE 0x69 // Not found in MPU-9250
eric11fr 0:efb1550773f1 103 #define FIFO_COUNTH 0x70
eric11fr 0:efb1550773f1 104 #define FIFO_COUNTL 0x71
eric11fr 0:efb1550773f1 105 #define FIFO_R_W 0x72
eric11fr 0:efb1550773f1 106 #define DATA_RDY_STATUS 0x74 // Not found in MPU-9250
eric11fr 0:efb1550773f1 107 #define FIFO_CFG 0x76 // Not found in MPU-9250
eric11fr 0:efb1550773f1 108 #define REG_BANK_SEL 0x7F // Not found in MPU-9250
eric11fr 0:efb1550773f1 109
eric11fr 0:efb1550773f1 110 // USER BANK 1 REGISTER MAP
eric11fr 0:efb1550773f1 111 #define SELF_TEST_X_GYRO 0x02
eric11fr 0:efb1550773f1 112 #define SELF_TEST_Y_GYRO 0x03
eric11fr 0:efb1550773f1 113 #define SELF_TEST_Z_GYRO 0x04
eric11fr 0:efb1550773f1 114 #define SELF_TEST_X_ACCEL 0x0E
eric11fr 0:efb1550773f1 115 #define SELF_TEST_Y_ACCEL 0x0F
eric11fr 0:efb1550773f1 116 #define SELF_TEST_Z_ACCEL 0x10
eric11fr 0:efb1550773f1 117 #define XA_OFFSET_H 0x14
eric11fr 0:efb1550773f1 118 #define XA_OFFSET_L 0x15
eric11fr 0:efb1550773f1 119 #define YA_OFFSET_H 0x17
eric11fr 0:efb1550773f1 120 #define YA_OFFSET_L 0x18
eric11fr 0:efb1550773f1 121 #define ZA_OFFSET_H 0x1A
eric11fr 0:efb1550773f1 122 #define ZA_OFFSET_L 0x1B
eric11fr 0:efb1550773f1 123 #define TIMEBASE_CORRECTION_PLL 0x28
eric11fr 0:efb1550773f1 124
eric11fr 0:efb1550773f1 125 // USER BANK 2 REGISTER MAP
eric11fr 0:efb1550773f1 126 #define GYRO_SMPLRT_DIV 0x00 // Not found in MPU-9250
eric11fr 0:efb1550773f1 127 #define GYRO_CONFIG_1 0x01 // Not found in MPU-9250
eric11fr 0:efb1550773f1 128 #define GYRO_CONFIG_2 0x02 // Not found in MPU-9250
eric11fr 0:efb1550773f1 129 #define XG_OFFSET_H 0x03 // User-defined trim values for gyroscope
eric11fr 0:efb1550773f1 130 #define XG_OFFSET_L 0x04
eric11fr 0:efb1550773f1 131 #define YG_OFFSET_H 0x05
eric11fr 0:efb1550773f1 132 #define YG_OFFSET_L 0x06
eric11fr 0:efb1550773f1 133 #define ZG_OFFSET_H 0x07
eric11fr 0:efb1550773f1 134 #define ZG_OFFSET_L 0x08
eric11fr 0:efb1550773f1 135 #define ODR_ALIGN_EN 0x09 // Not found in MPU-9250
eric11fr 0:efb1550773f1 136 #define ACCEL_SMPLRT_DIV_1 0x10 // Not found in MPU-9250
eric11fr 0:efb1550773f1 137 #define ACCEL_SMPLRT_DIV_2 0x11 // Not found in MPU-9250
eric11fr 0:efb1550773f1 138 #define ACCEL_INTEL_CTRL 0x12 // Not found in MPU-9250
eric11fr 0:efb1550773f1 139 #define ACCEL_WOM_THR 0x13 // Not found in MPU-9250 (could be WOM_THR)
eric11fr 0:efb1550773f1 140 #define ACCEL_CONFIG 0x14
eric11fr 0:efb1550773f1 141 #define ACCEL_CONFIG_2 0x15 // Not found in MPU-9250 (could be ACCEL_CONFIG2)
eric11fr 0:efb1550773f1 142 #define FSYNC_CONFIG 0x52 // Not found in MPU-9250
eric11fr 0:efb1550773f1 143 #define TEMP_CONFIG 0x53 // Not found in MPU-9250
eric11fr 0:efb1550773f1 144 #define MOD_CTRL_USR 0x54 // Not found in MPU-9250
eric11fr 0:efb1550773f1 145
eric11fr 0:efb1550773f1 146 // USER BANK 3 REGISTER MAP
eric11fr 0:efb1550773f1 147 #define I2C_MST_ODR_CONFIG 0x00 // Not found in MPU-9250
eric11fr 0:efb1550773f1 148 #define I2C_MST_CTRL 0x01
eric11fr 0:efb1550773f1 149 #define I2C_MST_DELAY_CTRL 0x02
eric11fr 0:efb1550773f1 150 #define I2C_SLV0_ADDR 0x03
eric11fr 0:efb1550773f1 151 #define I2C_SLV0_REG 0x04
eric11fr 0:efb1550773f1 152 #define I2C_SLV0_CTRL 0x05
eric11fr 0:efb1550773f1 153 #define I2C_SLV0_DO 0x06
eric11fr 0:efb1550773f1 154 #define I2C_SLV1_ADDR 0x07
eric11fr 0:efb1550773f1 155 #define I2C_SLV1_REG 0x08
eric11fr 0:efb1550773f1 156 #define I2C_SLV1_CTRL 0x09
eric11fr 0:efb1550773f1 157 #define I2C_SLV1_DO 0x0A
eric11fr 0:efb1550773f1 158 #define I2C_SLV2_ADDR 0x0B
eric11fr 0:efb1550773f1 159 #define I2C_SLV2_REG 0x0C
eric11fr 0:efb1550773f1 160 #define I2C_SLV2_CTRL 0x0D
eric11fr 0:efb1550773f1 161 #define I2C_SLV2_DO 0x0E
eric11fr 0:efb1550773f1 162 #define I2C_SLV3_ADDR 0x0F
eric11fr 0:efb1550773f1 163 #define I2C_SLV3_REG 0x10
eric11fr 0:efb1550773f1 164 #define I2C_SLV3_CTRL 0x11
eric11fr 0:efb1550773f1 165 #define I2C_SLV3_DO 0x12
eric11fr 0:efb1550773f1 166 #define I2C_SLV4_ADDR 0x13
eric11fr 0:efb1550773f1 167 #define I2C_SLV4_REG 0x14
eric11fr 0:efb1550773f1 168 #define I2C_SLV4_CTRL 0x15
eric11fr 0:efb1550773f1 169 #define I2C_SLV4_DO 0x16
eric11fr 0:efb1550773f1 170 #define I2C_SLV4_DI 0x17
eric11fr 0:efb1550773f1 171
eric11fr 0:efb1550773f1 172 // Using the ICM-20948 breakout board, ADO is set to 1
eric11fr 0:efb1550773f1 173 // Seven-bit device address is 1000100 for ADO = 0 and 1000101 for ADO = 1
eric11fr 1:8459e28d77a1 174 #define ADO 0
eric11fr 0:efb1550773f1 175 #if ADO
eric11fr 1:8459e28d77a1 176 #define ICM20948_ADDRESS 0x69<<1 // Device address when ADO = 1
eric11fr 0:efb1550773f1 177 #else
eric11fr 1:8459e28d77a1 178 #define ICM20948_ADDRESS 0x68<<1 // Device address when ADO = 0
eric11fr 0:efb1550773f1 179 #define AK09916_ADDRESS 0x0C // Address of magnetometer
eric11fr 0:efb1550773f1 180 #endif // AD0
eric11fr 0:efb1550773f1 181
eric11fr 1:8459e28d77a1 182 #define READ_FLAGS 0x80
eric11fr 0:efb1550773f1 183
eric11fr 0:efb1550773f1 184 enum Ascale
eric11fr 0:efb1550773f1 185 {
eric11fr 0:efb1550773f1 186 AFS_2G = 0,
eric11fr 0:efb1550773f1 187 AFS_4G,
eric11fr 0:efb1550773f1 188 AFS_8G,
eric11fr 0:efb1550773f1 189 AFS_16G
eric11fr 0:efb1550773f1 190 };
eric11fr 0:efb1550773f1 191
eric11fr 0:efb1550773f1 192 enum Gscale {
eric11fr 0:efb1550773f1 193 GFS_250DPS = 0,
eric11fr 0:efb1550773f1 194 GFS_500DPS,
eric11fr 0:efb1550773f1 195 GFS_1000DPS,
eric11fr 0:efb1550773f1 196 GFS_2000DPS
eric11fr 0:efb1550773f1 197 };
eric11fr 0:efb1550773f1 198
eric11fr 0:efb1550773f1 199 enum Mscale {
eric11fr 0:efb1550773f1 200 MFS_14BITS = 0, // 0.6 mG per LSB
eric11fr 0:efb1550773f1 201 MFS_16BITS // 0.15 mG per LSB
eric11fr 0:efb1550773f1 202 };
eric11fr 0:efb1550773f1 203
eric11fr 0:efb1550773f1 204 enum M_MODE {
eric11fr 0:efb1550773f1 205 M_8HZ = 0x02, // 8 Hz update
eric11fr 0:efb1550773f1 206 M_100HZ = 0x06 // 100 Hz continuous magnetometer
eric11fr 0:efb1550773f1 207 };
eric11fr 0:efb1550773f1 208
eric11fr 0:efb1550773f1 209 // TODO: Add setter methods for this hard coded stuff
eric11fr 0:efb1550773f1 210 // Specify sensor full scale
eric11fr 0:efb1550773f1 211 uint8_t Gscale = GFS_250DPS;
eric11fr 0:efb1550773f1 212 uint8_t Ascale = AFS_2G;
eric11fr 0:efb1550773f1 213
eric11fr 0:efb1550773f1 214 // 2 for 8 Hz, 6 for 100 Hz continuous magnetometer data read
eric11fr 0:efb1550773f1 215 uint8_t Mmode = M_100HZ;
eric11fr 0:efb1550773f1 216
eric11fr 0:efb1550773f1 217 uint8_t writeByteWire(uint8_t, uint8_t, uint8_t);
eric11fr 1:8459e28d77a1 218
eric11fr 0:efb1550773f1 219 uint8_t readByteWire(uint8_t address, uint8_t subAddress);
eric11fr 1:8459e28d77a1 220
eric11fr 0:efb1550773f1 221
eric11fr 1:8459e28d77a1 222
eric11fr 0:efb1550773f1 223 float pitch, yaw, roll;
eric11fr 0:efb1550773f1 224 float temperature; // Stores the real internal chip temperature in Celsius
eric11fr 0:efb1550773f1 225 int16_t tempCount; // Temperature raw count output
eric11fr 0:efb1550773f1 226 uint32_t delt_t = 0; // Used to control display output rate
eric11fr 0:efb1550773f1 227
eric11fr 1:8459e28d77a1 228 uint32_t counts = 0, sumCount = 0; // used to control display output rate
eric11fr 0:efb1550773f1 229 float deltat = 0.0f, sum = 0.0f; // integration interval for both filter schemes
eric11fr 0:efb1550773f1 230 uint32_t lastUpdate = 0, firstUpdate = 0; // used to calculate integration interval
eric11fr 0:efb1550773f1 231 uint32_t Now = 0; // used to calculate integration interval
eric11fr 0:efb1550773f1 232
eric11fr 0:efb1550773f1 233 int16_t gyroCount[3]; // Stores the 16-bit signed gyro sensor output
eric11fr 0:efb1550773f1 234 int16_t magCount[3]; // Stores the 16-bit signed magnetometer sensor output
eric11fr 0:efb1550773f1 235 // Scale resolutions per LSB for the sensors
eric11fr 0:efb1550773f1 236 float aRes, gRes, mRes;
eric11fr 0:efb1550773f1 237 // Variables to hold latest sensor data values
eric11fr 0:efb1550773f1 238 float ax, ay, az, gx, gy, gz, mx, my, mz;
eric11fr 0:efb1550773f1 239 // Factory mag calibration and mag bias
eric11fr 0:efb1550773f1 240 float factoryMagCalibration[3] = {0, 0, 0}, factoryMagBias[3] = {0, 0, 0};
eric11fr 0:efb1550773f1 241 // Bias corrections for gyro, accelerometer, and magnetometer
eric11fr 0:efb1550773f1 242 float gyroBias[3] = {0, 0, 0},
eric11fr 0:efb1550773f1 243 accelBias[3] = {0, 0, 0},
eric11fr 0:efb1550773f1 244 magBias[3] = {0, 0, 0},
eric11fr 0:efb1550773f1 245 magScale[3] = {0, 0, 0};
eric11fr 1:8459e28d77a1 246 // float selfTest[6];
eric11fr 0:efb1550773f1 247 // Stores the 16-bit signed accelerometer sensor output
eric11fr 0:efb1550773f1 248 int16_t accelCount[3];
eric11fr 0:efb1550773f1 249
eric11fr 0:efb1550773f1 250 // Public method declarations
eric11fr 0:efb1550773f1 251 void getMres();
eric11fr 0:efb1550773f1 252 void getGres();
eric11fr 0:efb1550773f1 253 void getAres();
eric11fr 0:efb1550773f1 254 void readAccelData(int16_t *);
eric11fr 0:efb1550773f1 255 void readGyroData(int16_t *);
eric11fr 0:efb1550773f1 256 void readMagData(int16_t *);
eric11fr 0:efb1550773f1 257 int16_t readTempData();
eric11fr 0:efb1550773f1 258 void updateTime();
eric11fr 0:efb1550773f1 259 void initAK09916();
eric11fr 0:efb1550773f1 260 void initICM20948();
eric11fr 0:efb1550773f1 261 void calibrateICM20948(float * gyroBias, float * accelBias);
eric11fr 0:efb1550773f1 262 void ICM20948SelfTest(float * destination);
eric11fr 0:efb1550773f1 263 void magCalICM20948(float * dest1, float * dest2);
eric11fr 0:efb1550773f1 264 uint8_t writeByte(uint8_t, uint8_t, uint8_t);
eric11fr 0:efb1550773f1 265 uint8_t readByte(uint8_t, uint8_t);
eric11fr 0:efb1550773f1 266 uint8_t readBytes(uint8_t, uint8_t, uint8_t, uint8_t *);
eric11fr 0:efb1550773f1 267 uint8_t readBytesWire(uint8_t, uint8_t, uint8_t, uint8_t *);
eric11fr 0:efb1550773f1 268 bool begin();
eric11fr 1:8459e28d77a1 269
eric11fr 1:8459e28d77a1 270
eric11fr 1:8459e28d77a1 271 bool begin(void)
eric11fr 1:8459e28d77a1 272 {
eric11fr 1:8459e28d77a1 273 i2c.frequency(400000); // use fast (400 kHz) I2C
eric11fr 1:8459e28d77a1 274 t.start();
eric11fr 1:8459e28d77a1 275 return true;
eric11fr 1:8459e28d77a1 276 }
eric11fr 1:8459e28d77a1 277
eric11fr 1:8459e28d77a1 278 void getMres()
eric11fr 1:8459e28d77a1 279 {
eric11fr 1:8459e28d77a1 280 mRes = 10.0f * 4912.0f / 32760.0f; // Proper scale to return milliGauss
eric11fr 1:8459e28d77a1 281 }
eric11fr 1:8459e28d77a1 282
eric11fr 1:8459e28d77a1 283 void getGres()
eric11fr 1:8459e28d77a1 284 {
eric11fr 1:8459e28d77a1 285 switch (Gscale)
eric11fr 1:8459e28d77a1 286 {
eric11fr 1:8459e28d77a1 287 // Possible gyro scales (and their register bit settings) are:
eric11fr 1:8459e28d77a1 288 // 250 DPS (00), 500 DPS (01), 1000 DPS (10), and 2000 DPS (11).
eric11fr 1:8459e28d77a1 289 // Here's a bit of an algorith to calculate DPS/(ADC tick) based on that
eric11fr 1:8459e28d77a1 290 // 2-bit value:
eric11fr 1:8459e28d77a1 291 case GFS_250DPS:
eric11fr 1:8459e28d77a1 292 gRes = 250.0f / 32768.0f;
eric11fr 1:8459e28d77a1 293 break;
eric11fr 1:8459e28d77a1 294 case GFS_500DPS:
eric11fr 1:8459e28d77a1 295 gRes = 500.0f / 32768.0f;
eric11fr 1:8459e28d77a1 296 break;
eric11fr 1:8459e28d77a1 297 case GFS_1000DPS:
eric11fr 1:8459e28d77a1 298 gRes = 1000.0f / 32768.0f;
eric11fr 1:8459e28d77a1 299 break;
eric11fr 1:8459e28d77a1 300 case GFS_2000DPS:
eric11fr 1:8459e28d77a1 301 gRes = 2000.0f / 32768.0f;
eric11fr 1:8459e28d77a1 302 break;
eric11fr 1:8459e28d77a1 303 }
eric11fr 1:8459e28d77a1 304 }
eric11fr 1:8459e28d77a1 305
eric11fr 1:8459e28d77a1 306 void getAres()
eric11fr 1:8459e28d77a1 307 {
eric11fr 1:8459e28d77a1 308 switch (Ascale)
eric11fr 1:8459e28d77a1 309 {
eric11fr 1:8459e28d77a1 310 // Possible accelerometer scales (and their register bit settings) are:
eric11fr 1:8459e28d77a1 311 // 2 Gs (00), 4 Gs (01), 8 Gs (10), and 16 Gs (11).
eric11fr 1:8459e28d77a1 312 // Here's a bit of an algorith to calculate DPS/(ADC tick) based on that
eric11fr 1:8459e28d77a1 313 // 2-bit value:
eric11fr 1:8459e28d77a1 314 case AFS_2G:
eric11fr 1:8459e28d77a1 315 aRes = 2.0f / 32768.0f;
eric11fr 1:8459e28d77a1 316 break;
eric11fr 1:8459e28d77a1 317 case AFS_4G:
eric11fr 1:8459e28d77a1 318 aRes = 4.0f / 32768.0f;
eric11fr 1:8459e28d77a1 319 break;
eric11fr 1:8459e28d77a1 320 case AFS_8G:
eric11fr 1:8459e28d77a1 321 aRes = 8.0f / 32768.0f;
eric11fr 1:8459e28d77a1 322 break;
eric11fr 1:8459e28d77a1 323 case AFS_16G:
eric11fr 1:8459e28d77a1 324 aRes = 16.0f / 32768.0f;
eric11fr 1:8459e28d77a1 325 break;
eric11fr 1:8459e28d77a1 326 }
eric11fr 1:8459e28d77a1 327 }
eric11fr 1:8459e28d77a1 328
eric11fr 1:8459e28d77a1 329
eric11fr 1:8459e28d77a1 330 void readAccelData(int16_t * destination)
eric11fr 1:8459e28d77a1 331 {
eric11fr 1:8459e28d77a1 332 uint8_t rawData[6]; // x/y/z accel register data stored here
eric11fr 1:8459e28d77a1 333 // Read the six raw data registers into data array
eric11fr 1:8459e28d77a1 334 // readBytes(ICM20948_ADDRESS, ACCEL_XOUT_H, 6, &rawData[0]);
eric11fr 1:8459e28d77a1 335 for(int z=0;z<6;z++)
eric11fr 1:8459e28d77a1 336 {
eric11fr 1:8459e28d77a1 337 rawData[z]=readByte(ICM20948_ADDRESS, ACCEL_XOUT_H+z);
eric11fr 1:8459e28d77a1 338 }
eric11fr 1:8459e28d77a1 339 // Turn the MSB and LSB into a signed 16-bit value
eric11fr 1:8459e28d77a1 340 destination[0] = (int16_t)(rawData[0] << 8) | rawData[1];
eric11fr 1:8459e28d77a1 341 destination[1] = (int16_t)(rawData[2] << 8) | rawData[3];
eric11fr 1:8459e28d77a1 342 destination[2] = (int16_t)(rawData[4] << 8) | rawData[5];
eric11fr 1:8459e28d77a1 343 }
eric11fr 1:8459e28d77a1 344
eric11fr 1:8459e28d77a1 345
eric11fr 1:8459e28d77a1 346 void readGyroData(int16_t * destination)
eric11fr 1:8459e28d77a1 347 {
eric11fr 1:8459e28d77a1 348 uint8_t rawData[6]; // x/y/z gyro register data stored here
eric11fr 1:8459e28d77a1 349 // Read the six raw data registers sequentially into data array
eric11fr 1:8459e28d77a1 350 readBytes(ICM20948_ADDRESS, GYRO_XOUT_H, 6, &rawData[0]);
eric11fr 1:8459e28d77a1 351
eric11fr 1:8459e28d77a1 352 // Turn the MSB and LSB into a signed 16-bit value
eric11fr 1:8459e28d77a1 353 destination[0] = (int16_t)(rawData[0] << 8) | rawData[1];
eric11fr 1:8459e28d77a1 354 destination[1] = (int16_t)(rawData[2] << 8) | rawData[3];
eric11fr 1:8459e28d77a1 355 destination[2] = (int16_t)(rawData[4] << 8) | rawData[5];
eric11fr 1:8459e28d77a1 356 }
eric11fr 1:8459e28d77a1 357
eric11fr 1:8459e28d77a1 358 void readMagData(int16_t * destination)
eric11fr 1:8459e28d77a1 359 {
eric11fr 1:8459e28d77a1 360 // x/y/z gyro register data, ST2 register stored here, must read ST2 at end
eric11fr 1:8459e28d77a1 361 // of data acquisition
eric11fr 1:8459e28d77a1 362 uint8_t rawData[8];
eric11fr 1:8459e28d77a1 363 // thread_sleep_for for magnetometer data ready bit to be set
eric11fr 1:8459e28d77a1 364 if (readByte(AK09916_ADDRESS, AK09916_ST1) & 0x01)
eric11fr 1:8459e28d77a1 365 {
eric11fr 1:8459e28d77a1 366
eric11fr 1:8459e28d77a1 367 // Read the six raw data and ST2 registers sequentially into data array
eric11fr 1:8459e28d77a1 368 readBytes(AK09916_ADDRESS, AK09916_XOUT_L, 8, &rawData[0]);
eric11fr 1:8459e28d77a1 369 uint8_t c = rawData[7]; // End data read by reading ST2 register
eric11fr 1:8459e28d77a1 370 // Check if magnetic sensor overflow set, if not then report data
eric11fr 1:8459e28d77a1 371 // Remove once finished
eric11fr 1:8459e28d77a1 372
eric11fr 1:8459e28d77a1 373 if (!(c & 0x08))
eric11fr 1:8459e28d77a1 374 {
eric11fr 1:8459e28d77a1 375 // Turn the MSB and LSB into a signed 16-bit value
eric11fr 1:8459e28d77a1 376 destination[0] = ((int16_t)rawData[1] << 8) | rawData[0];
eric11fr 1:8459e28d77a1 377 // Data stored as little Endian
eric11fr 1:8459e28d77a1 378 destination[1] = ((int16_t)rawData[3] << 8) | rawData[2];
eric11fr 1:8459e28d77a1 379 destination[2] = ((int16_t)rawData[5] << 8) | rawData[4];
eric11fr 1:8459e28d77a1 380 }
eric11fr 1:8459e28d77a1 381 }
eric11fr 1:8459e28d77a1 382 }
eric11fr 1:8459e28d77a1 383
eric11fr 1:8459e28d77a1 384 int16_t readTempData()
eric11fr 1:8459e28d77a1 385 {
eric11fr 1:8459e28d77a1 386 uint8_t rawData[2]; // x/y/z gyro register data stored here
eric11fr 1:8459e28d77a1 387 // Read the two raw data registers sequentially into data array
eric11fr 1:8459e28d77a1 388 readBytes(ICM20948_ADDRESS, TEMP_OUT_H, 2, &rawData[0]);
eric11fr 1:8459e28d77a1 389 // Turn the MSB and LSB into a 16-bit value
eric11fr 1:8459e28d77a1 390 return ((int16_t)rawData[0] << 8) | rawData[1];
eric11fr 1:8459e28d77a1 391 }
eric11fr 1:8459e28d77a1 392
eric11fr 1:8459e28d77a1 393 // Calculate the time the last update took for use in the quaternion filters
eric11fr 1:8459e28d77a1 394 // TODO: This doesn't really belong in this class.
eric11fr 1:8459e28d77a1 395 void updateTime()
eric11fr 1:8459e28d77a1 396 {
eric11fr 1:8459e28d77a1 397 Now = t.elapsed_time().count();;
eric11fr 1:8459e28d77a1 398
eric11fr 1:8459e28d77a1 399 // Set integration time by time elapsed since last filter update
eric11fr 1:8459e28d77a1 400 deltat = ((Now - lastUpdate) / 1000000.0f);
eric11fr 1:8459e28d77a1 401 lastUpdate = Now;
eric11fr 1:8459e28d77a1 402
eric11fr 1:8459e28d77a1 403 sum += deltat; // sum for averaging filter update rate
eric11fr 1:8459e28d77a1 404 sumCount++;
eric11fr 1:8459e28d77a1 405 }
eric11fr 1:8459e28d77a1 406
eric11fr 1:8459e28d77a1 407 void initAK09916()
eric11fr 1:8459e28d77a1 408 {
eric11fr 1:8459e28d77a1 409
eric11fr 1:8459e28d77a1 410 // Write code to initialise magnetometer
eric11fr 1:8459e28d77a1 411 // Bypass I2C master interface and turn on magnetometer
eric11fr 1:8459e28d77a1 412 // writeByte(ICM20948_ADDRESS, INT_PIN_CFG, 0x02); //Already set in initICM20948
eric11fr 1:8459e28d77a1 413
eric11fr 1:8459e28d77a1 414 // Configure the magnetometer for continuous read and highest resolution.
eric11fr 1:8459e28d77a1 415 // Enable continuous mode data acquisition Mmode (bits [3:0]),
eric11fr 1:8459e28d77a1 416 // 0010 for 8 Hz and 0110 for 100 Hz sample rates.
eric11fr 1:8459e28d77a1 417
eric11fr 1:8459e28d77a1 418 // Set magnetometer data resolution and sample ODR
eric11fr 1:8459e28d77a1 419 writeByte(AK09916_ADDRESS, AK09916_CNTL2, 0x08);
eric11fr 1:8459e28d77a1 420 thread_sleep_for(10);
eric11fr 1:8459e28d77a1 421 }
eric11fr 1:8459e28d77a1 422
eric11fr 1:8459e28d77a1 423 void initICM20948()
eric11fr 1:8459e28d77a1 424 {
eric11fr 1:8459e28d77a1 425 // Get stable time source
eric11fr 1:8459e28d77a1 426 // Auto select clock source to be PLL gyroscope reference if ready else
eric11fr 1:8459e28d77a1 427 writeByte(ICM20948_ADDRESS, PWR_MGMT_1, 0x01);
eric11fr 1:8459e28d77a1 428 thread_sleep_for(200);
eric11fr 1:8459e28d77a1 429 // Switch to user bank 2
eric11fr 1:8459e28d77a1 430 writeByte(ICM20948_ADDRESS, REG_BANK_SEL, 0x20);
eric11fr 1:8459e28d77a1 431 // Configure Gyro and Thermometer
eric11fr 1:8459e28d77a1 432 // Disable FSYNC and set gyro bandwidth to 51.2 Hz,
eric11fr 1:8459e28d77a1 433 // respectively;
eric11fr 1:8459e28d77a1 434 // minimum delay time for this setting is 5.9 ms, which means sensor fusion
eric11fr 1:8459e28d77a1 435 // update rates cannot be higher than 1 / 0.0059 = 170 Hz
eric11fr 1:8459e28d77a1 436 // DLPF_CFG = bits 2:0 = 011; this limits the sample rate to 1000 Hz for both
eric11fr 1:8459e28d77a1 437 // With the ICM20948, it is possible to get gyro sample rates of 32 kHz (!),
eric11fr 1:8459e28d77a1 438 // 8 kHz, or 1 kHz
eric11fr 1:8459e28d77a1 439 // Set gyroscope full scale range to 250 dps
eric11fr 1:8459e28d77a1 440 writeByte(ICM20948_ADDRESS, GYRO_CONFIG_1, 0x19);
eric11fr 1:8459e28d77a1 441 writeByte(ICM20948_ADDRESS, TEMP_CONFIG, 0x03);
eric11fr 1:8459e28d77a1 442
eric11fr 1:8459e28d77a1 443 // Set sample rate = gyroscope output rate/(1 + GYRO_SMPLRT_DIV)
eric11fr 1:8459e28d77a1 444 // Use a 220 Hz rate; a rate consistent with the filter update rate
eric11fr 1:8459e28d77a1 445 // determined inset in CONFIG above.
eric11fr 1:8459e28d77a1 446
eric11fr 1:8459e28d77a1 447 writeByte(ICM20948_ADDRESS, GYRO_SMPLRT_DIV, 0x04);
eric11fr 1:8459e28d77a1 448
eric11fr 1:8459e28d77a1 449 // Set gyroscope full scale range
eric11fr 1:8459e28d77a1 450 // Range selects FS_SEL and AFS_SEL are 0 - 3, so 2-bit values are
eric11fr 1:8459e28d77a1 451 // left-shifted into positions 4:3
eric11fr 1:8459e28d77a1 452
eric11fr 1:8459e28d77a1 453 // Set accelerometer full-scale range configuration
eric11fr 1:8459e28d77a1 454 // Get current ACCEL_CONFIG register value
eric11fr 1:8459e28d77a1 455 uint8_t c = readByte(ICM20948_ADDRESS, ACCEL_CONFIG);
eric11fr 1:8459e28d77a1 456 // c = c & ~0xE0; // Clear self-test bits [7:5]
eric11fr 1:8459e28d77a1 457 c = c & ~0x06; // Clear AFS bits [4:3]
eric11fr 1:8459e28d77a1 458 c = c | Ascale << 1; // Set full scale range for the accelerometer
eric11fr 1:8459e28d77a1 459 c = c | 0x01; // Set enable accel DLPF for the accelerometer
eric11fr 1:8459e28d77a1 460 c = c | 0x18; // and set DLFPFCFG to 50.4 hz
eric11fr 1:8459e28d77a1 461 // Write new ACCEL_CONFIG register value
eric11fr 1:8459e28d77a1 462 writeByte(ICM20948_ADDRESS, ACCEL_CONFIG, c);
eric11fr 1:8459e28d77a1 463
eric11fr 1:8459e28d77a1 464 // Set accelerometer sample rate configuration
eric11fr 1:8459e28d77a1 465 // It is possible to get a 4 kHz sample rate from the accelerometer by
eric11fr 1:8459e28d77a1 466 // choosing 1 for accel_fchoice_b bit [3]; in this case the bandwidth is
eric11fr 1:8459e28d77a1 467 // 1.13 kHz
eric11fr 1:8459e28d77a1 468 writeByte(ICM20948_ADDRESS, ACCEL_SMPLRT_DIV_2, 0x04);
eric11fr 1:8459e28d77a1 469 // The accelerometer, gyro, and thermometer are set to 1 kHz sample rates,
eric11fr 1:8459e28d77a1 470 // but all these rates are further reduced by a factor of 5 to 200 Hz because
eric11fr 1:8459e28d77a1 471 // of the GYRO_SMPLRT_DIV setting
eric11fr 1:8459e28d77a1 472
eric11fr 1:8459e28d77a1 473 // Switch to user bank 0
eric11fr 1:8459e28d77a1 474 writeByte(ICM20948_ADDRESS, REG_BANK_SEL, 0x00);
eric11fr 1:8459e28d77a1 475
eric11fr 1:8459e28d77a1 476 // Configure Interrupts and Bypass Enable
eric11fr 1:8459e28d77a1 477 // Set interrupt pin active high, push-pull, hold interrupt pin level HIGH
eric11fr 1:8459e28d77a1 478 // until interrupt cleared, clear on read of INT_STATUS, and enable
eric11fr 1:8459e28d77a1 479 // I2C_BYPASS_EN so additional chips can join the I2C bus and all can be
eric11fr 1:8459e28d77a1 480 // controlled by the Arduino as master.
eric11fr 1:8459e28d77a1 481 writeByte(ICM20948_ADDRESS, INT_PIN_CFG, 0x22);
eric11fr 1:8459e28d77a1 482 // Enable data ready (bit 0) interrupt
eric11fr 1:8459e28d77a1 483 writeByte(ICM20948_ADDRESS, INT_ENABLE_1, 0x01);
eric11fr 1:8459e28d77a1 484 }
eric11fr 1:8459e28d77a1 485
eric11fr 1:8459e28d77a1 486
eric11fr 1:8459e28d77a1 487 // Function which accumulates gyro and accelerometer data after device
eric11fr 1:8459e28d77a1 488 // initialization. It calculates the average of the at-rest readings and then
eric11fr 1:8459e28d77a1 489 // loads the resulting offsets into accelerometer and gyro bias registers.
eric11fr 1:8459e28d77a1 490 void calibrateICM20948(float * gyroBias, float * accelBias)
eric11fr 1:8459e28d77a1 491 {
eric11fr 1:8459e28d77a1 492 uint8_t data[12]; // data array to hold accelerometer and gyro x, y, z, data
eric11fr 1:8459e28d77a1 493 uint16_t ii, packet_count, fifo_count;
eric11fr 1:8459e28d77a1 494 int32_t gyro_bias[3] = {0, 0, 0}, accel_bias[3] = {0, 0, 0};
eric11fr 1:8459e28d77a1 495 // reset device
eric11fr 1:8459e28d77a1 496 // Write a one to bit 7 reset bit; toggle reset device
eric11fr 1:8459e28d77a1 497 writeByte(ICM20948_ADDRESS, PWR_MGMT_1, READ_FLAGS);
eric11fr 1:8459e28d77a1 498 thread_sleep_for(200);
eric11fr 1:8459e28d77a1 499
eric11fr 1:8459e28d77a1 500 // get stable time source; Auto select clock source to be PLL gyroscope
eric11fr 1:8459e28d77a1 501 // reference if ready else use the internal oscillator, bits 2:0 = 001
eric11fr 1:8459e28d77a1 502 writeByte(ICM20948_ADDRESS, PWR_MGMT_1, 0x01);
eric11fr 1:8459e28d77a1 503 thread_sleep_for(200);
eric11fr 1:8459e28d77a1 504
eric11fr 1:8459e28d77a1 505 // Configure device for bias calculation
eric11fr 1:8459e28d77a1 506 // Disable all interrupts
eric11fr 1:8459e28d77a1 507 writeByte(ICM20948_ADDRESS, INT_ENABLE, 0x00);
eric11fr 1:8459e28d77a1 508 // Disable FIFO
eric11fr 1:8459e28d77a1 509 writeByte(ICM20948_ADDRESS, FIFO_EN_1, 0x00);
eric11fr 1:8459e28d77a1 510 writeByte(ICM20948_ADDRESS, FIFO_EN_2, 0x00);
eric11fr 1:8459e28d77a1 511 // Turn on internal clock source
eric11fr 1:8459e28d77a1 512 writeByte(ICM20948_ADDRESS, PWR_MGMT_1, 0x00);
eric11fr 1:8459e28d77a1 513 // Disable I2C master
eric11fr 1:8459e28d77a1 514 //writeByte(ICM20948_ADDRESS, I2C_MST_CTRL, 0x00); Already disabled
eric11fr 1:8459e28d77a1 515 // Disable FIFO and I2C master modes
eric11fr 1:8459e28d77a1 516 writeByte(ICM20948_ADDRESS, USER_CTRL, 0x00);
eric11fr 1:8459e28d77a1 517 // Reset FIFO and DMP
eric11fr 1:8459e28d77a1 518 writeByte(ICM20948_ADDRESS, USER_CTRL, 0x08);
eric11fr 1:8459e28d77a1 519 writeByte(ICM20948_ADDRESS, FIFO_RST, 0x1F);
eric11fr 1:8459e28d77a1 520 thread_sleep_for(10);
eric11fr 1:8459e28d77a1 521 writeByte(ICM20948_ADDRESS, FIFO_RST, 0x00);
eric11fr 1:8459e28d77a1 522 thread_sleep_for(15);
eric11fr 1:8459e28d77a1 523
eric11fr 1:8459e28d77a1 524 // Set FIFO mode to snapshot
eric11fr 1:8459e28d77a1 525 writeByte(ICM20948_ADDRESS, FIFO_MODE, 0x1F);
eric11fr 1:8459e28d77a1 526 // Switch to user bank 2
eric11fr 1:8459e28d77a1 527 writeByte(ICM20948_ADDRESS, REG_BANK_SEL, 0x20);
eric11fr 1:8459e28d77a1 528 // Configure ICM20948 gyro and accelerometer for bias calculation
eric11fr 1:8459e28d77a1 529 // Set low-pass filter to 188 Hz
eric11fr 1:8459e28d77a1 530 writeByte(ICM20948_ADDRESS, GYRO_CONFIG_1, 0x01);
eric11fr 1:8459e28d77a1 531 // Set sample rate to 1 kHz
eric11fr 1:8459e28d77a1 532 writeByte(ICM20948_ADDRESS, GYRO_SMPLRT_DIV, 0x00);
eric11fr 1:8459e28d77a1 533 // Set gyro full-scale to 250 degrees per second, maximum sensitivity
eric11fr 1:8459e28d77a1 534 writeByte(ICM20948_ADDRESS, GYRO_CONFIG_1, 0x00);
eric11fr 1:8459e28d77a1 535 // Set accelerometer full-scale to 2 g, maximum sensitivity
eric11fr 1:8459e28d77a1 536 writeByte(ICM20948_ADDRESS, ACCEL_CONFIG, 0x00);
eric11fr 1:8459e28d77a1 537
eric11fr 1:8459e28d77a1 538 uint16_t gyrosensitivity = 131; // = 131 LSB/degrees/sec
eric11fr 1:8459e28d77a1 539 uint16_t accelsensitivity = 16384; // = 16384 LSB/g
eric11fr 1:8459e28d77a1 540
eric11fr 1:8459e28d77a1 541 // Switch to user bank 0
eric11fr 1:8459e28d77a1 542 writeByte(ICM20948_ADDRESS, REG_BANK_SEL, 0x00);
eric11fr 1:8459e28d77a1 543 // Configure FIFO to capture accelerometer and gyro data for bias calculation
eric11fr 1:8459e28d77a1 544 writeByte(ICM20948_ADDRESS, USER_CTRL, 0x40); // Enable FIFO
eric11fr 1:8459e28d77a1 545 // Enable gyro and accelerometer sensors for FIFO (max size 512 bytes in
eric11fr 1:8459e28d77a1 546 // ICM20948)
eric11fr 1:8459e28d77a1 547 writeByte(ICM20948_ADDRESS, FIFO_EN_2, 0x1E);
eric11fr 1:8459e28d77a1 548 thread_sleep_for(40); // accumulate 40 samples in 40 milliseconds = 480 bytes
eric11fr 1:8459e28d77a1 549
eric11fr 1:8459e28d77a1 550 // At end of sample accumulation, turn off FIFO sensor read
eric11fr 1:8459e28d77a1 551 // Disable gyro and accelerometer sensors for FIFO
eric11fr 1:8459e28d77a1 552 writeByte(ICM20948_ADDRESS, FIFO_EN_2, 0x00);
eric11fr 1:8459e28d77a1 553 // Read FIFO sample count
eric11fr 1:8459e28d77a1 554 readBytes(ICM20948_ADDRESS, FIFO_COUNTH, 2, &data[0]);
eric11fr 1:8459e28d77a1 555 fifo_count = ((uint16_t)data[0] << 8) | data[1];
eric11fr 1:8459e28d77a1 556 // How many sets of full gyro and accelerometer data for averaging
eric11fr 1:8459e28d77a1 557 packet_count = fifo_count/12;
eric11fr 1:8459e28d77a1 558
eric11fr 1:8459e28d77a1 559 for (ii = 0; ii < packet_count; ii++)
eric11fr 1:8459e28d77a1 560 {
eric11fr 1:8459e28d77a1 561 int16_t accel_temp[3] = {0, 0, 0}, gyro_temp[3] = {0, 0, 0};
eric11fr 1:8459e28d77a1 562 // Read data for averaging
eric11fr 1:8459e28d77a1 563 readBytes(ICM20948_ADDRESS, FIFO_R_W, 12, &data[0]);
eric11fr 1:8459e28d77a1 564 // Form signed 16-bit integer for each sample in FIFO
eric11fr 1:8459e28d77a1 565 accel_temp[0] = (int16_t) (((int16_t)data[0] << 8) | data[1] );
eric11fr 1:8459e28d77a1 566 accel_temp[1] = (int16_t) (((int16_t)data[2] << 8) | data[3] );
eric11fr 1:8459e28d77a1 567 accel_temp[2] = (int16_t) (((int16_t)data[4] << 8) | data[5] );
eric11fr 1:8459e28d77a1 568 gyro_temp[0] = (int16_t) (((int16_t)data[6] << 8) | data[7] );
eric11fr 1:8459e28d77a1 569 gyro_temp[1] = (int16_t) (((int16_t)data[8] << 8) | data[9] );
eric11fr 1:8459e28d77a1 570 gyro_temp[2] = (int16_t) (((int16_t)data[10] << 8) | data[11]);
eric11fr 1:8459e28d77a1 571
eric11fr 1:8459e28d77a1 572 // Sum individual signed 16-bit biases to get accumulated signed 32-bit
eric11fr 1:8459e28d77a1 573 // biases.
eric11fr 1:8459e28d77a1 574 accel_bias[0] += (int32_t) accel_temp[0];
eric11fr 1:8459e28d77a1 575 accel_bias[1] += (int32_t) accel_temp[1];
eric11fr 1:8459e28d77a1 576 accel_bias[2] += (int32_t) accel_temp[2];
eric11fr 1:8459e28d77a1 577 gyro_bias[0] += (int32_t) gyro_temp[0];
eric11fr 1:8459e28d77a1 578 gyro_bias[1] += (int32_t) gyro_temp[1];
eric11fr 1:8459e28d77a1 579 gyro_bias[2] += (int32_t) gyro_temp[2];
eric11fr 1:8459e28d77a1 580 }
eric11fr 1:8459e28d77a1 581 // Sum individual signed 16-bit biases to get accumulated signed 32-bit biases
eric11fr 1:8459e28d77a1 582 accel_bias[0] /= (int32_t) packet_count;
eric11fr 1:8459e28d77a1 583 accel_bias[1] /= (int32_t) packet_count;
eric11fr 1:8459e28d77a1 584 accel_bias[2] /= (int32_t) packet_count;
eric11fr 1:8459e28d77a1 585 gyro_bias[0] /= (int32_t) packet_count;
eric11fr 1:8459e28d77a1 586 gyro_bias[1] /= (int32_t) packet_count;
eric11fr 1:8459e28d77a1 587 gyro_bias[2] /= (int32_t) packet_count;
eric11fr 1:8459e28d77a1 588
eric11fr 1:8459e28d77a1 589 // Sum individual signed 16-bit biases to get accumulated signed 32-bit biases
eric11fr 1:8459e28d77a1 590 if (accel_bias[2] > 0L)
eric11fr 1:8459e28d77a1 591 {
eric11fr 1:8459e28d77a1 592 accel_bias[2] -= (int32_t) accelsensitivity;
eric11fr 1:8459e28d77a1 593 }
eric11fr 1:8459e28d77a1 594 else
eric11fr 1:8459e28d77a1 595 {
eric11fr 1:8459e28d77a1 596 accel_bias[2] += (int32_t) accelsensitivity;
eric11fr 1:8459e28d77a1 597 }
eric11fr 1:8459e28d77a1 598
eric11fr 1:8459e28d77a1 599 // Construct the gyro biases for push to the hardware gyro bias registers,
eric11fr 1:8459e28d77a1 600 // which are reset to zero upon device startup.
eric11fr 1:8459e28d77a1 601 // Divide by 4 to get 32.9 LSB per deg/s to conform to expected bias input
eric11fr 1:8459e28d77a1 602 // format.
eric11fr 1:8459e28d77a1 603 data[0] = (-gyro_bias[0]/4 >> 8) & 0xFF;
eric11fr 1:8459e28d77a1 604 // Biases are additive, so change sign on calculated average gyro biases
eric11fr 1:8459e28d77a1 605 data[1] = (-gyro_bias[0]/4) & 0xFF;
eric11fr 1:8459e28d77a1 606 data[2] = (-gyro_bias[1]/4 >> 8) & 0xFF;
eric11fr 1:8459e28d77a1 607 data[3] = (-gyro_bias[1]/4) & 0xFF;
eric11fr 1:8459e28d77a1 608 data[4] = (-gyro_bias[2]/4 >> 8) & 0xFF;
eric11fr 1:8459e28d77a1 609 data[5] = (-gyro_bias[2]/4) & 0xFF;
eric11fr 1:8459e28d77a1 610
eric11fr 1:8459e28d77a1 611 // Switch to user bank 2
eric11fr 1:8459e28d77a1 612 writeByte(ICM20948_ADDRESS, REG_BANK_SEL, 0x20);
eric11fr 1:8459e28d77a1 613
eric11fr 1:8459e28d77a1 614 // Push gyro biases to hardware registers
eric11fr 1:8459e28d77a1 615 writeByte(ICM20948_ADDRESS, XG_OFFSET_H, data[0]);
eric11fr 1:8459e28d77a1 616 writeByte(ICM20948_ADDRESS, XG_OFFSET_L, data[1]);
eric11fr 1:8459e28d77a1 617 writeByte(ICM20948_ADDRESS, YG_OFFSET_H, data[2]);
eric11fr 1:8459e28d77a1 618 writeByte(ICM20948_ADDRESS, YG_OFFSET_L, data[3]);
eric11fr 1:8459e28d77a1 619 writeByte(ICM20948_ADDRESS, ZG_OFFSET_H, data[4]);
eric11fr 1:8459e28d77a1 620 writeByte(ICM20948_ADDRESS, ZG_OFFSET_L, data[5]);
eric11fr 1:8459e28d77a1 621
eric11fr 1:8459e28d77a1 622 // Output scaled gyro biases for display in the main program
eric11fr 1:8459e28d77a1 623 gyroBias[0] = (float) gyro_bias[0]/(float) gyrosensitivity;
eric11fr 1:8459e28d77a1 624 gyroBias[1] = (float) gyro_bias[1]/(float) gyrosensitivity;
eric11fr 1:8459e28d77a1 625 gyroBias[2] = (float) gyro_bias[2]/(float) gyrosensitivity;
eric11fr 1:8459e28d77a1 626
eric11fr 1:8459e28d77a1 627 // Construct the accelerometer biases for push to the hardware accelerometer
eric11fr 1:8459e28d77a1 628 // bias registers. These registers contain factory trim values which must be
eric11fr 1:8459e28d77a1 629 // added to the calculated accelerometer biases; on boot up these registers
eric11fr 1:8459e28d77a1 630 // will hold non-zero values. In addition, bit 0 of the lower byte must be
eric11fr 1:8459e28d77a1 631 // preserved since it is used for temperature compensation calculations.
eric11fr 1:8459e28d77a1 632 // Accelerometer bias registers expect bias input as 2048 LSB per g, so that
eric11fr 1:8459e28d77a1 633 // the accelerometer biases calculated above must be divided by 8.
eric11fr 1:8459e28d77a1 634
eric11fr 1:8459e28d77a1 635 // Switch to user bank 1
eric11fr 1:8459e28d77a1 636 writeByte(ICM20948_ADDRESS, REG_BANK_SEL, 0x10);
eric11fr 1:8459e28d77a1 637 // A place to hold the factory accelerometer trim biases
eric11fr 1:8459e28d77a1 638 int32_t accel_bias_reg[3] = {0, 0, 0};
eric11fr 1:8459e28d77a1 639 // Read factory accelerometer trim values
eric11fr 1:8459e28d77a1 640 readBytes(ICM20948_ADDRESS, XA_OFFSET_H, 2, &data[0]);
eric11fr 1:8459e28d77a1 641 accel_bias_reg[0] = (int32_t) (((int16_t)data[0] << 8) | data[1]);
eric11fr 1:8459e28d77a1 642 readBytes(ICM20948_ADDRESS, YA_OFFSET_H, 2, &data[0]);
eric11fr 1:8459e28d77a1 643 accel_bias_reg[1] = (int32_t) (((int16_t)data[0] << 8) | data[1]);
eric11fr 1:8459e28d77a1 644 readBytes(ICM20948_ADDRESS, ZA_OFFSET_H, 2, &data[0]);
eric11fr 1:8459e28d77a1 645 accel_bias_reg[2] = (int32_t) (((int16_t)data[0] << 8) | data[1]);
eric11fr 0:efb1550773f1 646
eric11fr 1:8459e28d77a1 647 // Define mask for temperature compensation bit 0 of lower byte of
eric11fr 1:8459e28d77a1 648 // accelerometer bias registers
eric11fr 1:8459e28d77a1 649 uint32_t mask = 1uL;
eric11fr 1:8459e28d77a1 650 // Define array to hold mask bit for each accelerometer bias axis
eric11fr 1:8459e28d77a1 651 uint8_t mask_bit[3] = {0, 0, 0};
eric11fr 1:8459e28d77a1 652
eric11fr 1:8459e28d77a1 653 for (ii = 0; ii < 3; ii++)
eric11fr 1:8459e28d77a1 654 {
eric11fr 1:8459e28d77a1 655 // If temperature compensation bit is set, record that fact in mask_bit
eric11fr 1:8459e28d77a1 656 if ((accel_bias_reg[ii] & mask))
eric11fr 1:8459e28d77a1 657 {
eric11fr 1:8459e28d77a1 658 mask_bit[ii] = 0x01;
eric11fr 1:8459e28d77a1 659 }
eric11fr 1:8459e28d77a1 660 }
eric11fr 1:8459e28d77a1 661
eric11fr 1:8459e28d77a1 662 // Construct total accelerometer bias, including calculated average
eric11fr 1:8459e28d77a1 663 // accelerometer bias from above
eric11fr 1:8459e28d77a1 664 // Subtract calculated averaged accelerometer bias scaled to 2048 LSB/g
eric11fr 1:8459e28d77a1 665 // (16 g full scale)
eric11fr 1:8459e28d77a1 666 accel_bias_reg[0] -= (accel_bias[0]/8);
eric11fr 1:8459e28d77a1 667 accel_bias_reg[1] -= (accel_bias[1]/8);
eric11fr 1:8459e28d77a1 668 accel_bias_reg[2] -= (accel_bias[2]/8);
eric11fr 1:8459e28d77a1 669
eric11fr 1:8459e28d77a1 670 data[0] = (accel_bias_reg[0] >> 8) & 0xFF;
eric11fr 1:8459e28d77a1 671 data[1] = (accel_bias_reg[0]) & 0xFF;
eric11fr 1:8459e28d77a1 672 // preserve temperature compensation bit when writing back to accelerometer
eric11fr 1:8459e28d77a1 673 // bias registers
eric11fr 1:8459e28d77a1 674 data[1] = data[1] | mask_bit[0];
eric11fr 1:8459e28d77a1 675 data[2] = (accel_bias_reg[1] >> 8) & 0xFF;
eric11fr 1:8459e28d77a1 676 data[3] = (accel_bias_reg[1]) & 0xFF;
eric11fr 1:8459e28d77a1 677 // Preserve temperature compensation bit when writing back to accelerometer
eric11fr 1:8459e28d77a1 678 // bias registers
eric11fr 1:8459e28d77a1 679 data[3] = data[3] | mask_bit[1];
eric11fr 1:8459e28d77a1 680 data[4] = (accel_bias_reg[2] >> 8) & 0xFF;
eric11fr 1:8459e28d77a1 681 data[5] = (accel_bias_reg[2]) & 0xFF;
eric11fr 1:8459e28d77a1 682 // Preserve temperature compensation bit when writing back to accelerometer
eric11fr 1:8459e28d77a1 683 // bias registers
eric11fr 1:8459e28d77a1 684 data[5] = data[5] | mask_bit[2];
eric11fr 1:8459e28d77a1 685
eric11fr 1:8459e28d77a1 686 // Apparently this is not working for the acceleration biases in the ICM-20948
eric11fr 1:8459e28d77a1 687 // Are we handling the temperature correction bit properly?
eric11fr 1:8459e28d77a1 688 // Push accelerometer biases to hardware registers
eric11fr 1:8459e28d77a1 689 writeByte(ICM20948_ADDRESS, XA_OFFSET_H, data[0]);
eric11fr 1:8459e28d77a1 690 writeByte(ICM20948_ADDRESS, XA_OFFSET_L, data[1]);
eric11fr 1:8459e28d77a1 691 writeByte(ICM20948_ADDRESS, YA_OFFSET_H, data[2]);
eric11fr 1:8459e28d77a1 692 writeByte(ICM20948_ADDRESS, YA_OFFSET_L, data[3]);
eric11fr 1:8459e28d77a1 693 writeByte(ICM20948_ADDRESS, ZA_OFFSET_H, data[4]);
eric11fr 1:8459e28d77a1 694 writeByte(ICM20948_ADDRESS, ZA_OFFSET_L, data[5]);
eric11fr 1:8459e28d77a1 695
eric11fr 1:8459e28d77a1 696 // Output scaled accelerometer biases for display in the main program
eric11fr 1:8459e28d77a1 697 accelBias[0] = (float)accel_bias[0]/(float)accelsensitivity;
eric11fr 1:8459e28d77a1 698 accelBias[1] = (float)accel_bias[1]/(float)accelsensitivity;
eric11fr 1:8459e28d77a1 699 accelBias[2] = (float)accel_bias[2]/(float)accelsensitivity;
eric11fr 1:8459e28d77a1 700 // Switch to user bank 0
eric11fr 1:8459e28d77a1 701 writeByte(ICM20948_ADDRESS, REG_BANK_SEL, 0x00);
eric11fr 1:8459e28d77a1 702 }
eric11fr 1:8459e28d77a1 703
eric11fr 1:8459e28d77a1 704
eric11fr 1:8459e28d77a1 705 // Accelerometer and gyroscope self test; check calibration wrt factory settings
eric11fr 1:8459e28d77a1 706 // Should return percent deviation from factory trim values, +/- 14 or less
eric11fr 1:8459e28d77a1 707 // deviation is a pass.
eric11fr 1:8459e28d77a1 708 void ICM20948SelfTest(float * destination)
eric11fr 1:8459e28d77a1 709 {
eric11fr 1:8459e28d77a1 710 uint8_t rawData[6] = {0, 0, 0, 0, 0, 0};
eric11fr 1:8459e28d77a1 711 uint8_t selfTest[6];
eric11fr 1:8459e28d77a1 712 int32_t gAvg[3] = {0}, aAvg[3] = {0}, aSTAvg[3] = {0}, gSTAvg[3] = {0};
eric11fr 1:8459e28d77a1 713 float factoryTrim[6];
eric11fr 1:8459e28d77a1 714 uint8_t FS = 0;
eric11fr 1:8459e28d77a1 715 // Get stable time source
eric11fr 1:8459e28d77a1 716 // Auto select clock source to be PLL gyroscope reference if ready else
eric11fr 1:8459e28d77a1 717 writeByte(ICM20948_ADDRESS, PWR_MGMT_1, 0x01);
eric11fr 1:8459e28d77a1 718 thread_sleep_for(200);
eric11fr 1:8459e28d77a1 719 // Switch to user bank 2
eric11fr 1:8459e28d77a1 720 writeByte(ICM20948_ADDRESS, REG_BANK_SEL, 0x20);
eric11fr 1:8459e28d77a1 721 // Set gyro sample rate to 1 kHz
eric11fr 1:8459e28d77a1 722 writeByte(ICM20948_ADDRESS, GYRO_SMPLRT_DIV, 0x00);
eric11fr 1:8459e28d77a1 723 // Set gyro sample rate to 1 kHz, DLPF to 119.5 Hz and FSR to 250 dps
eric11fr 1:8459e28d77a1 724 writeByte(ICM20948_ADDRESS, GYRO_CONFIG_1, 0x11);
eric11fr 1:8459e28d77a1 725 // Set accelerometer rate to 1 kHz and bandwidth to 111.4 Hz
eric11fr 1:8459e28d77a1 726 // Set full scale range for the accelerometer to 2 g
eric11fr 1:8459e28d77a1 727 writeByte(ICM20948_ADDRESS, ACCEL_CONFIG, 0x11);
eric11fr 1:8459e28d77a1 728 // Switch to user bank 0
eric11fr 1:8459e28d77a1 729 writeByte(ICM20948_ADDRESS, REG_BANK_SEL, 0x00);
eric11fr 1:8459e28d77a1 730 // Get average current values of gyro and acclerometer
eric11fr 1:8459e28d77a1 731 for (int ii = 0; ii < 200; ii++)
eric11fr 1:8459e28d77a1 732 {
eric11fr 1:8459e28d77a1 733
eric11fr 1:8459e28d77a1 734 // Read the six raw data registers into data array
eric11fr 1:8459e28d77a1 735 for(int z=0;z<6;z++)
eric11fr 1:8459e28d77a1 736 {
eric11fr 1:8459e28d77a1 737 rawData[z]=readByte(ICM20948_ADDRESS, ACCEL_XOUT_H+z);
eric11fr 1:8459e28d77a1 738 }
eric11fr 1:8459e28d77a1 739 // Turn the MSB and LSB into a signed 16-bit value
eric11fr 1:8459e28d77a1 740 aAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ;
eric11fr 1:8459e28d77a1 741 aAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;
eric11fr 1:8459e28d77a1 742 aAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ;
eric11fr 1:8459e28d77a1 743
eric11fr 1:8459e28d77a1 744 // Read the six raw data registers sequentially into data array
eric11fr 1:8459e28d77a1 745 for(int z=0;z<6;z++)
eric11fr 1:8459e28d77a1 746 {
eric11fr 1:8459e28d77a1 747 rawData[z]=readByte(ICM20948_ADDRESS, GYRO_XOUT_H+z);
eric11fr 1:8459e28d77a1 748 }
eric11fr 1:8459e28d77a1 749 // Turn the MSB and LSB into a signed 16-bit value
eric11fr 1:8459e28d77a1 750 gAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ;
eric11fr 1:8459e28d77a1 751 gAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;
eric11fr 1:8459e28d77a1 752 gAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ;
eric11fr 1:8459e28d77a1 753 }
eric11fr 1:8459e28d77a1 754
eric11fr 1:8459e28d77a1 755 // Get average of 200 values and store as average current readings
eric11fr 1:8459e28d77a1 756 for (int ii =0; ii < 3; ii++)
eric11fr 1:8459e28d77a1 757 {
eric11fr 1:8459e28d77a1 758 aAvg[ii] /= 200;
eric11fr 1:8459e28d77a1 759 gAvg[ii] /= 200;
eric11fr 1:8459e28d77a1 760 }
eric11fr 1:8459e28d77a1 761 // Switch to user bank 2
eric11fr 1:8459e28d77a1 762 writeByte(ICM20948_ADDRESS, REG_BANK_SEL, 0x20);
eric11fr 1:8459e28d77a1 763 // Configure the accelerometer for self-test
eric11fr 1:8459e28d77a1 764 // Enable self test on all three axes and set accelerometer range to +/- 2 g
eric11fr 1:8459e28d77a1 765 writeByte(ICM20948_ADDRESS, ACCEL_CONFIG_2, 0x1C);
eric11fr 1:8459e28d77a1 766 // Enable self test on all three axes and set gyro range to +/- 250 degrees/s
eric11fr 1:8459e28d77a1 767 writeByte(ICM20948_ADDRESS, GYRO_CONFIG_2, 0x38);
eric11fr 1:8459e28d77a1 768 thread_sleep_for(25); // Delay a while to let the device stabilize
eric11fr 1:8459e28d77a1 769 // Switch to user bank 0
eric11fr 1:8459e28d77a1 770 writeByte(ICM20948_ADDRESS, REG_BANK_SEL, 0x00);
eric11fr 1:8459e28d77a1 771 // Get average self-test values of gyro and acclerometer
eric11fr 1:8459e28d77a1 772 for (int ii = 0; ii < 200; ii++)
eric11fr 1:8459e28d77a1 773 {
eric11fr 1:8459e28d77a1 774 // Read the six raw data registers into data array
eric11fr 1:8459e28d77a1 775 // readBytes(ICM20948_ADDRESS, ACCEL_XOUT_H, 6, &rawData[0]);
eric11fr 1:8459e28d77a1 776 for(int z=0;z<6;z++)
eric11fr 1:8459e28d77a1 777 {
eric11fr 1:8459e28d77a1 778 rawData[z]=readByte(ICM20948_ADDRESS, ACCEL_XOUT_H+z);
eric11fr 1:8459e28d77a1 779 }
eric11fr 1:8459e28d77a1 780 // Turn the MSB and LSB into a signed 16-bit value
eric11fr 1:8459e28d77a1 781 aSTAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ;
eric11fr 1:8459e28d77a1 782 aSTAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;
eric11fr 1:8459e28d77a1 783 aSTAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ;
eric11fr 1:8459e28d77a1 784
eric11fr 1:8459e28d77a1 785 // Read the six raw data registers sequentially into data array
eric11fr 1:8459e28d77a1 786 //readBytes(ICM20948_ADDRESS, GYRO_XOUT_H, 6, &rawData[0]);
eric11fr 1:8459e28d77a1 787 for(int z=0;z<6;z++)
eric11fr 1:8459e28d77a1 788 {
eric11fr 1:8459e28d77a1 789 rawData[z]=readByte(ICM20948_ADDRESS, GYRO_XOUT_H+z);
eric11fr 1:8459e28d77a1 790 }
eric11fr 1:8459e28d77a1 791 // Turn the MSB and LSB into a signed 16-bit value
eric11fr 1:8459e28d77a1 792 gSTAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ;
eric11fr 1:8459e28d77a1 793 gSTAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;
eric11fr 1:8459e28d77a1 794 gSTAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ;
eric11fr 1:8459e28d77a1 795 }
eric11fr 1:8459e28d77a1 796
eric11fr 1:8459e28d77a1 797 // Get average of 200 values and store as average self-test readings
eric11fr 1:8459e28d77a1 798 for (int ii =0; ii < 3; ii++)
eric11fr 1:8459e28d77a1 799 {
eric11fr 1:8459e28d77a1 800 aSTAvg[ii] /= 200;
eric11fr 1:8459e28d77a1 801 gSTAvg[ii] /= 200;
eric11fr 1:8459e28d77a1 802 }
eric11fr 1:8459e28d77a1 803
eric11fr 1:8459e28d77a1 804 // Switch to user bank 2
eric11fr 1:8459e28d77a1 805 writeByte(ICM20948_ADDRESS, REG_BANK_SEL, 0x20);
eric11fr 1:8459e28d77a1 806 // Configure the gyro and accelerometer for normal operation
eric11fr 1:8459e28d77a1 807 writeByte(ICM20948_ADDRESS, ACCEL_CONFIG_2, 0x00);
eric11fr 1:8459e28d77a1 808 writeByte(ICM20948_ADDRESS, GYRO_CONFIG_2, 0x00);
eric11fr 1:8459e28d77a1 809 thread_sleep_for(25); // Delay a while to let the device stabilize
eric11fr 1:8459e28d77a1 810 // Switch to user bank 1
eric11fr 1:8459e28d77a1 811 writeByte(ICM20948_ADDRESS, REG_BANK_SEL, 0x10);
eric11fr 1:8459e28d77a1 812 // Retrieve accelerometer and gyro factory Self-Test Code from USR_Reg
eric11fr 1:8459e28d77a1 813 // X-axis accel self-test results
eric11fr 1:8459e28d77a1 814 selfTest[0] = readByte(ICM20948_ADDRESS, SELF_TEST_X_ACCEL);
eric11fr 1:8459e28d77a1 815 // Y-axis accel self-test results
eric11fr 1:8459e28d77a1 816 selfTest[1] = readByte(ICM20948_ADDRESS, SELF_TEST_Y_ACCEL);
eric11fr 1:8459e28d77a1 817 // Z-axis accel self-test results
eric11fr 1:8459e28d77a1 818 selfTest[2] = readByte(ICM20948_ADDRESS, SELF_TEST_Z_ACCEL);
eric11fr 1:8459e28d77a1 819 // X-axis gyro self-test results
eric11fr 1:8459e28d77a1 820 selfTest[3] = readByte(ICM20948_ADDRESS, SELF_TEST_X_GYRO);
eric11fr 1:8459e28d77a1 821 // Y-axis gyro self-test results
eric11fr 1:8459e28d77a1 822 selfTest[4] = readByte(ICM20948_ADDRESS, SELF_TEST_Y_GYRO);
eric11fr 1:8459e28d77a1 823 // Z-axis gyro self-test results
eric11fr 1:8459e28d77a1 824 selfTest[5] = readByte(ICM20948_ADDRESS, SELF_TEST_Z_GYRO);
eric11fr 1:8459e28d77a1 825 // Switch to user bank 0
eric11fr 1:8459e28d77a1 826 writeByte(ICM20948_ADDRESS, REG_BANK_SEL, 0x00);
eric11fr 1:8459e28d77a1 827 // Retrieve factory self-test value from self-test code reads
eric11fr 1:8459e28d77a1 828 // FT[Xa] factory trim calculation
eric11fr 1:8459e28d77a1 829 factoryTrim[0] = (float)(2620/1<<FS)*(pow(1.01 ,((float)selfTest[0] - 1.0) ));
eric11fr 1:8459e28d77a1 830 // FT[Ya] factory trim calculation
eric11fr 1:8459e28d77a1 831 factoryTrim[1] = (float)(2620/1<<FS)*(pow(1.01 ,((float)selfTest[1] - 1.0) ));
eric11fr 1:8459e28d77a1 832 // FT[Za] factory trim calculation
eric11fr 1:8459e28d77a1 833 factoryTrim[2] = (float)(2620/1<<FS)*(pow(1.01 ,((float)selfTest[2] - 1.0) ));
eric11fr 1:8459e28d77a1 834 // FT[Xg] factory trim calculation
eric11fr 1:8459e28d77a1 835 factoryTrim[3] = (float)(2620/1<<FS)*(pow(1.01 ,((float)selfTest[3] - 1.0) ));
eric11fr 1:8459e28d77a1 836 // FT[Yg] factory trim calculation
eric11fr 1:8459e28d77a1 837 factoryTrim[4] = (float)(2620/1<<FS)*(pow(1.01 ,((float)selfTest[4] - 1.0) ));
eric11fr 1:8459e28d77a1 838 // FT[Zg] factory trim calculation
eric11fr 1:8459e28d77a1 839 factoryTrim[5] = (float)(2620/1<<FS)*(pow(1.01 ,((float)selfTest[5] - 1.0) ));
eric11fr 1:8459e28d77a1 840
eric11fr 1:8459e28d77a1 841 // Report results as a ratio of (STR - FT)/FT; the change from Factory Trim
eric11fr 1:8459e28d77a1 842 // of the Self-Test Response
eric11fr 1:8459e28d77a1 843 // To get percent, must multiply by 100
eric11fr 1:8459e28d77a1 844 for (int i = 0; i < 3; i++)
eric11fr 1:8459e28d77a1 845 {
eric11fr 1:8459e28d77a1 846 // Report percent differences
eric11fr 1:8459e28d77a1 847 destination[i] = 100.0 * ((float)(aSTAvg[i] - aAvg[i]))/factoryTrim[i]- 100./*selfTest[i]*/;
eric11fr 1:8459e28d77a1 848 // Report percent differences
eric11fr 1:8459e28d77a1 849 destination[i+3] =100.0*((float)(gSTAvg[i] - gAvg[i]))/factoryTrim[i+3]- 100./*selfTest[i+3]*/;
eric11fr 1:8459e28d77a1 850 }
eric11fr 1:8459e28d77a1 851 }
eric11fr 1:8459e28d77a1 852
eric11fr 1:8459e28d77a1 853 // Function which accumulates magnetometer data after device initialization.
eric11fr 1:8459e28d77a1 854 // It calculates the bias and scale in the x, y, and z axes.
eric11fr 1:8459e28d77a1 855 void magCalICM20948(float * bias_dest, float * scale_dest)
eric11fr 1:8459e28d77a1 856 {
eric11fr 1:8459e28d77a1 857 uint16_t ii = 0, sample_count = 0;
eric11fr 1:8459e28d77a1 858 int32_t mag_bias[3] = {0, 0, 0},
eric11fr 1:8459e28d77a1 859 mag_scale[3] = {0, 0, 0};
eric11fr 1:8459e28d77a1 860 int32_t mag_max[3] = {0x8000, 0x8000, 0x8000},
eric11fr 1:8459e28d77a1 861 mag_min[3] = {0x7FFF, 0x7FFF, 0x7FFF},
eric11fr 1:8459e28d77a1 862 mag_temp[3] = {0, 0, 0};
eric11fr 1:8459e28d77a1 863
eric11fr 1:8459e28d77a1 864 // Make sure resolution has been calculated
eric11fr 1:8459e28d77a1 865 getMres();
eric11fr 1:8459e28d77a1 866 thread_sleep_for(4000);
eric11fr 1:8459e28d77a1 867
eric11fr 1:8459e28d77a1 868 // shoot for ~fifteen seconds of mag data
eric11fr 1:8459e28d77a1 869 // at 8 Hz ODR, new mag data is available every 125 ms
eric11fr 1:8459e28d77a1 870 if (Mmode == M_8HZ)
eric11fr 1:8459e28d77a1 871 {
eric11fr 1:8459e28d77a1 872 sample_count = 128;
eric11fr 1:8459e28d77a1 873 }
eric11fr 1:8459e28d77a1 874 // at 100 Hz ODR, new mag data is available every 10 ms
eric11fr 1:8459e28d77a1 875 if (Mmode == M_100HZ)
eric11fr 1:8459e28d77a1 876 {
eric11fr 1:8459e28d77a1 877 sample_count = 1500;
eric11fr 1:8459e28d77a1 878 }
eric11fr 1:8459e28d77a1 879
eric11fr 1:8459e28d77a1 880 for (ii = 0; ii < sample_count; ii++)
eric11fr 1:8459e28d77a1 881 {
eric11fr 1:8459e28d77a1 882 readMagData((int16_t *) mag_temp); // Read the mag data
eric11fr 1:8459e28d77a1 883
eric11fr 1:8459e28d77a1 884 for (int jj = 0; jj < 3; jj++)
eric11fr 1:8459e28d77a1 885 {
eric11fr 1:8459e28d77a1 886 if (mag_temp[jj] > mag_max[jj])
eric11fr 1:8459e28d77a1 887 {
eric11fr 1:8459e28d77a1 888 mag_max[jj] = mag_temp[jj];
eric11fr 1:8459e28d77a1 889 }
eric11fr 1:8459e28d77a1 890 if (mag_temp[jj] < mag_min[jj])
eric11fr 1:8459e28d77a1 891 {
eric11fr 1:8459e28d77a1 892 mag_min[jj] = mag_temp[jj];
eric11fr 1:8459e28d77a1 893 }
eric11fr 1:8459e28d77a1 894 }
eric11fr 1:8459e28d77a1 895
eric11fr 1:8459e28d77a1 896 if (Mmode == M_8HZ)
eric11fr 1:8459e28d77a1 897 {
eric11fr 1:8459e28d77a1 898 thread_sleep_for(135); // At 8 Hz ODR, new mag data is available every 125 ms
eric11fr 1:8459e28d77a1 899 }
eric11fr 1:8459e28d77a1 900 if (Mmode == M_100HZ)
eric11fr 1:8459e28d77a1 901 {
eric11fr 1:8459e28d77a1 902 thread_sleep_for(12); // At 100 Hz ODR, new mag data is available every 10 ms
eric11fr 1:8459e28d77a1 903 }
eric11fr 1:8459e28d77a1 904 }
eric11fr 1:8459e28d77a1 905
eric11fr 1:8459e28d77a1 906 // pc.println("mag x min/max:"); pc.println(mag_max[0]); pc.println(mag_min[0]);
eric11fr 1:8459e28d77a1 907 // pc.println("mag y min/max:"); pc.println(mag_max[1]); pc.println(mag_min[1]);
eric11fr 1:8459e28d77a1 908 // pc.println("mag z min/max:"); pc.println(mag_max[2]); pc.println(mag_min[2]);
eric11fr 1:8459e28d77a1 909
eric11fr 1:8459e28d77a1 910 // Get hard iron correction
eric11fr 1:8459e28d77a1 911 // Get 'average' x mag bias in counts
eric11fr 1:8459e28d77a1 912 mag_bias[0] = (mag_max[0] + mag_min[0]) / 2;
eric11fr 1:8459e28d77a1 913 // Get 'average' y mag bias in counts
eric11fr 1:8459e28d77a1 914 mag_bias[1] = (mag_max[1] + mag_min[1]) / 2;
eric11fr 1:8459e28d77a1 915 // Get 'average' z mag bias in counts
eric11fr 1:8459e28d77a1 916 mag_bias[2] = (mag_max[2] + mag_min[2]) / 2;
eric11fr 1:8459e28d77a1 917
eric11fr 1:8459e28d77a1 918 // Save mag biases in G for main program
eric11fr 1:8459e28d77a1 919 bias_dest[0] = (float)mag_bias[0] * mRes;// * factoryMagCalibration[0];
eric11fr 1:8459e28d77a1 920 bias_dest[1] = (float)mag_bias[1] * mRes;// * factoryMagCalibration[1];
eric11fr 1:8459e28d77a1 921 bias_dest[2] = (float)mag_bias[2] * mRes;// * factoryMagCalibration[2];
eric11fr 1:8459e28d77a1 922
eric11fr 1:8459e28d77a1 923 // Get soft iron correction estimate
eric11fr 1:8459e28d77a1 924 // Get average x axis max chord length in counts
eric11fr 1:8459e28d77a1 925 mag_scale[0] = (mag_max[0] - mag_min[0]) / 2;
eric11fr 1:8459e28d77a1 926 // Get average y axis max chord length in counts
eric11fr 1:8459e28d77a1 927 mag_scale[1] = (mag_max[1] - mag_min[1]) / 2;
eric11fr 1:8459e28d77a1 928 // Get average z axis max chord length in counts
eric11fr 1:8459e28d77a1 929 mag_scale[2] = (mag_max[2] - mag_min[2]) / 2;
eric11fr 1:8459e28d77a1 930
eric11fr 1:8459e28d77a1 931 float avg_rad = mag_scale[0] + mag_scale[1] + mag_scale[2];
eric11fr 1:8459e28d77a1 932 avg_rad /= 3.0;
eric11fr 1:8459e28d77a1 933
eric11fr 1:8459e28d77a1 934 scale_dest[0] = avg_rad / ((float)mag_scale[0]);
eric11fr 1:8459e28d77a1 935 scale_dest[1] = avg_rad / ((float)mag_scale[1]);
eric11fr 1:8459e28d77a1 936 scale_dest[2] = avg_rad / ((float)mag_scale[2]);
eric11fr 1:8459e28d77a1 937 }
eric11fr 1:8459e28d77a1 938
eric11fr 1:8459e28d77a1 939 // Wire.h read and write protocols
eric11fr 1:8459e28d77a1 940 uint8_t writeByte(uint8_t deviceAddress, uint8_t registerAddress,uint8_t data)
eric11fr 1:8459e28d77a1 941 {
eric11fr 1:8459e28d77a1 942 //writeByteWire(deviceAddress,registerAddress, data);
eric11fr 1:8459e28d77a1 943 char tmp[2];
eric11fr 1:8459e28d77a1 944 tmp[0]=registerAddress;
eric11fr 1:8459e28d77a1 945 tmp[1]=data;
eric11fr 1:8459e28d77a1 946 i2c.write(deviceAddress, tmp, 2, 0); // no stop
eric11fr 1:8459e28d77a1 947 return NULL;
eric11fr 1:8459e28d77a1 948 }
eric11fr 1:8459e28d77a1 949
eric11fr 1:8459e28d77a1 950 uint8_t writeByteOne(uint8_t deviceAddress, uint8_t registerAddress)
eric11fr 1:8459e28d77a1 951 {
eric11fr 1:8459e28d77a1 952 char tmp[2];
eric11fr 1:8459e28d77a1 953 tmp[0]=registerAddress;
eric11fr 1:8459e28d77a1 954 i2c.write(deviceAddress, tmp, 1, 1);
eric11fr 1:8459e28d77a1 955 return NULL;
eric11fr 1:8459e28d77a1 956 }
eric11fr 1:8459e28d77a1 957
eric11fr 1:8459e28d77a1 958 /*
eric11fr 1:8459e28d77a1 959 uint8_t writeByteWire(uint8_t deviceAddress, uint8_t registerAddress,
eric11fr 1:8459e28d77a1 960 uint8_t data)
eric11fr 1:8459e28d77a1 961 { // i2c.write(address, data_write, 1, 1); // no stop
eric11fr 1:8459e28d77a1 962 char tmp[2];
eric11fr 1:8459e28d77a1 963 tmp[0]=registerAddress;
eric11fr 1:8459e28d77a1 964 i2c.write(deviceAddress, tmp, 1, 1); // no stop
eric11fr 1:8459e28d77a1 965 tmp[0]=data;
eric11fr 1:8459e28d77a1 966 i2c.write(deviceAddress, tmp, 1, 0); // stop
eric11fr 1:8459e28d77a1 967 // TODO: Fix this to return something meaningful
eric11fr 1:8459e28d77a1 968 return NULL;
eric11fr 1:8459e28d77a1 969 }
eric11fr 1:8459e28d77a1 970 */
eric11fr 1:8459e28d77a1 971 // Read a byte from given register on device. Calls necessary SPI or I2C
eric11fr 1:8459e28d77a1 972 // implementation. This was configured in the constructor.
eric11fr 1:8459e28d77a1 973 uint8_t readByte(uint8_t deviceAddress, uint8_t registerAddress)
eric11fr 1:8459e28d77a1 974 {
eric11fr 1:8459e28d77a1 975 char tmp[1];
eric11fr 1:8459e28d77a1 976 tmp[0]=registerAddress;
eric11fr 1:8459e28d77a1 977 i2c.write(deviceAddress,tmp, 1, 1); // no stop
eric11fr 1:8459e28d77a1 978 //tmp[0]=data;
eric11fr 1:8459e28d77a1 979 i2c.read(deviceAddress, tmp, 1, 0);//stop
eric11fr 1:8459e28d77a1 980 // Return data read from slave register
eric11fr 1:8459e28d77a1 981 return (uint8_t) tmp[0];
eric11fr 1:8459e28d77a1 982 }
eric11fr 1:8459e28d77a1 983 /*
eric11fr 1:8459e28d77a1 984
eric11fr 1:8459e28d77a1 985 uint8_t readByteWire(uint8_t deviceAddress, uint8_t registerAddress)
eric11fr 1:8459e28d77a1 986 {
eric11fr 1:8459e28d77a1 987 uint8_t data; // `data` will store the register data
eric11fr 1:8459e28d77a1 988 // i2c.write(address, data_write, 1, 1); // no stop
eric11fr 1:8459e28d77a1 989 // i2c.read(address, data, count, 0);
eric11fr 1:8459e28d77a1 990 // Initialize the Tx buffer
eric11fr 1:8459e28d77a1 991 char tmp[2];
eric11fr 1:8459e28d77a1 992 tmp[0]=registerAddress;
eric11fr 1:8459e28d77a1 993 i2c.write(deviceAddress,tmp, 1, 0); // no stop
eric11fr 1:8459e28d77a1 994 //tmp[0]=data;
eric11fr 1:8459e28d77a1 995 i2c.read(deviceAddress, tmp, 1, 0);//stop
eric11fr 1:8459e28d77a1 996 // Return data read from slave register
eric11fr 1:8459e28d77a1 997 return tmp[0];
eric11fr 1:8459e28d77a1 998 }
eric11fr 1:8459e28d77a1 999
eric11fr 1:8459e28d77a1 1000 */
eric11fr 1:8459e28d77a1 1001 // Read 1 or more bytes from given register and device using I2C
eric11fr 1:8459e28d77a1 1002 uint8_t readBytesWire(uint8_t deviceAddress, uint8_t registerAddress,
eric11fr 1:8459e28d77a1 1003 uint8_t count, uint8_t * dest)
eric11fr 1:8459e28d77a1 1004 {
eric11fr 1:8459e28d77a1 1005 char tmp[2];
eric11fr 1:8459e28d77a1 1006 tmp[0]=registerAddress;
eric11fr 1:8459e28d77a1 1007 i2c.write(deviceAddress, tmp, 1, 1); // no stop
eric11fr 1:8459e28d77a1 1008 i2c.read(deviceAddress,(char *) dest, count, 0);//stop
eric11fr 1:8459e28d77a1 1009 // Initialize the Tx buffer
eric11fr 1:8459e28d77a1 1010 /* Wire.beginTransmission(deviceAddress);
eric11fr 1:8459e28d77a1 1011 // Put slave register address in Tx buffer
eric11fr 1:8459e28d77a1 1012 Wire.write(registerAddress);
eric11fr 1:8459e28d77a1 1013 // Send the Tx buffer, but send a restart to keep connection alive
eric11fr 1:8459e28d77a1 1014 Wire.endTransmission(false);
eric11fr 1:8459e28d77a1 1015
eric11fr 1:8459e28d77a1 1016 uint8_t i = 0;
eric11fr 1:8459e28d77a1 1017 // Read bytes from slave register address
eric11fr 1:8459e28d77a1 1018 Wire.requestFrom(deviceAddress, count);
eric11fr 1:8459e28d77a1 1019 while (Wire.available())
eric11fr 1:8459e28d77a1 1020 {
eric11fr 1:8459e28d77a1 1021 // Put read results in the Rx buffer
eric11fr 1:8459e28d77a1 1022 dest[i++] = Wire.read();
eric11fr 1:8459e28d77a1 1023 }
eric11fr 1:8459e28d77a1 1024 */
eric11fr 1:8459e28d77a1 1025 return count; // Return number of bytes written
eric11fr 1:8459e28d77a1 1026 }
eric11fr 1:8459e28d77a1 1027
eric11fr 1:8459e28d77a1 1028 uint8_t readBytes(uint8_t deviceAddress, uint8_t registerAddress,
eric11fr 1:8459e28d77a1 1029 uint8_t count, uint8_t * dest)
eric11fr 1:8459e28d77a1 1030 {
eric11fr 1:8459e28d77a1 1031
eric11fr 1:8459e28d77a1 1032 return readBytesWire(deviceAddress, registerAddress, count, dest);
eric11fr 1:8459e28d77a1 1033
eric11fr 1:8459e28d77a1 1034 }