Modified Arduino library for ICM_20948 IMU for Nucleo boards
ICM_20948.cpp@0:894b603d32ee, 2022-01-31 (annotated)
- Committer:
- saloutos
- Date:
- Mon Jan 31 03:25:31 2022 +0000
- Revision:
- 0:894b603d32ee
modified ICM_20948 Arduino library for Nucleo boards
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
saloutos | 0:894b603d32ee | 1 | #include "ICM_20948.h" |
saloutos | 0:894b603d32ee | 2 | |
saloutos | 0:894b603d32ee | 3 | #include "util/ICM_20948_REGISTERS.h" |
saloutos | 0:894b603d32ee | 4 | #include "util/AK09916_REGISTERS.h" |
saloutos | 0:894b603d32ee | 5 | |
saloutos | 0:894b603d32ee | 6 | // Forward Declarations |
saloutos | 0:894b603d32ee | 7 | ICM_20948_Status_e ICM_20948_write_SPI(uint8_t reg, uint8_t *buff, uint32_t len, void *user); |
saloutos | 0:894b603d32ee | 8 | ICM_20948_Status_e ICM_20948_read_SPI(uint8_t reg, uint8_t *buff, uint32_t len, void *user); |
saloutos | 0:894b603d32ee | 9 | |
saloutos | 0:894b603d32ee | 10 | // Base |
saloutos | 0:894b603d32ee | 11 | ICM_20948::ICM_20948() |
saloutos | 0:894b603d32ee | 12 | { |
saloutos | 0:894b603d32ee | 13 | status = ICM_20948_init_struct(&_device); |
saloutos | 0:894b603d32ee | 14 | } |
saloutos | 0:894b603d32ee | 15 | |
saloutos | 0:894b603d32ee | 16 | void ICM_20948::enableDebugging(Serial &debugPort) |
saloutos | 0:894b603d32ee | 17 | { |
saloutos | 0:894b603d32ee | 18 | _debugSerial = &debugPort; //Grab which port the user wants us to use for debugging |
saloutos | 0:894b603d32ee | 19 | _printDebug = true; //Should we print the commands we send? Good for debugging |
saloutos | 0:894b603d32ee | 20 | } |
saloutos | 0:894b603d32ee | 21 | void ICM_20948::disableDebugging(void) |
saloutos | 0:894b603d32ee | 22 | { |
saloutos | 0:894b603d32ee | 23 | _printDebug = false; //Turn off extra print statements |
saloutos | 0:894b603d32ee | 24 | } |
saloutos | 0:894b603d32ee | 25 | |
saloutos | 0:894b603d32ee | 26 | void ICM_20948::debugPrintStatus(ICM_20948_Status_e stat) |
saloutos | 0:894b603d32ee | 27 | { |
saloutos | 0:894b603d32ee | 28 | // if (_printDebug == true) { |
saloutos | 0:894b603d32ee | 29 | switch (stat) |
saloutos | 0:894b603d32ee | 30 | { |
saloutos | 0:894b603d32ee | 31 | case ICM_20948_Stat_Ok: |
saloutos | 0:894b603d32ee | 32 | _debugSerial->printf("All is well.\n\r"); |
saloutos | 0:894b603d32ee | 33 | break; |
saloutos | 0:894b603d32ee | 34 | case ICM_20948_Stat_Err: |
saloutos | 0:894b603d32ee | 35 | _debugSerial->printf("General Error\n\r"); |
saloutos | 0:894b603d32ee | 36 | break; |
saloutos | 0:894b603d32ee | 37 | case ICM_20948_Stat_NotImpl: |
saloutos | 0:894b603d32ee | 38 | _debugSerial->printf("Not Implemented\n\r"); |
saloutos | 0:894b603d32ee | 39 | break; |
saloutos | 0:894b603d32ee | 40 | case ICM_20948_Stat_ParamErr: |
saloutos | 0:894b603d32ee | 41 | _debugSerial->printf("Parameter Error\n\r"); |
saloutos | 0:894b603d32ee | 42 | break; |
saloutos | 0:894b603d32ee | 43 | case ICM_20948_Stat_WrongID: |
saloutos | 0:894b603d32ee | 44 | _debugSerial->printf("Wrong ID\n\r"); |
saloutos | 0:894b603d32ee | 45 | break; |
saloutos | 0:894b603d32ee | 46 | case ICM_20948_Stat_InvalSensor: |
saloutos | 0:894b603d32ee | 47 | _debugSerial->printf("Invalid Sensor\n\r"); |
saloutos | 0:894b603d32ee | 48 | break; |
saloutos | 0:894b603d32ee | 49 | case ICM_20948_Stat_NoData: |
saloutos | 0:894b603d32ee | 50 | _debugSerial->printf("Data Underflow\n\r"); |
saloutos | 0:894b603d32ee | 51 | break; |
saloutos | 0:894b603d32ee | 52 | case ICM_20948_Stat_SensorNotSupported: |
saloutos | 0:894b603d32ee | 53 | _debugSerial->printf("Sensor Not Supported\n\r"); |
saloutos | 0:894b603d32ee | 54 | break; |
saloutos | 0:894b603d32ee | 55 | case ICM_20948_Stat_DMPNotSupported: |
saloutos | 0:894b603d32ee | 56 | _debugSerial->printf("DMP Firmware Not Supported. Is #define ICM_20948_USE_DMP commented in util/ICM_20948_C.h?\n\r"); |
saloutos | 0:894b603d32ee | 57 | break; |
saloutos | 0:894b603d32ee | 58 | case ICM_20948_Stat_DMPVerifyFail: |
saloutos | 0:894b603d32ee | 59 | _debugSerial->printf("DMP Firmware Verification Failed\n\r"); |
saloutos | 0:894b603d32ee | 60 | break; |
saloutos | 0:894b603d32ee | 61 | case ICM_20948_Stat_FIFONoDataAvail: |
saloutos | 0:894b603d32ee | 62 | _debugSerial->printf("No FIFO Data Available\n\r"); |
saloutos | 0:894b603d32ee | 63 | break; |
saloutos | 0:894b603d32ee | 64 | case ICM_20948_Stat_FIFOIncompleteData: |
saloutos | 0:894b603d32ee | 65 | _debugSerial->printf("DMP data in FIFO was incomplete\n\r"); |
saloutos | 0:894b603d32ee | 66 | break; |
saloutos | 0:894b603d32ee | 67 | case ICM_20948_Stat_FIFOMoreDataAvail: |
saloutos | 0:894b603d32ee | 68 | _debugSerial->printf("More FIFO Data Available\n\r"); |
saloutos | 0:894b603d32ee | 69 | break; |
saloutos | 0:894b603d32ee | 70 | case ICM_20948_Stat_UnrecognisedDMPHeader: |
saloutos | 0:894b603d32ee | 71 | _debugSerial->printf("Unrecognised DMP Header\n\r"); |
saloutos | 0:894b603d32ee | 72 | break; |
saloutos | 0:894b603d32ee | 73 | case ICM_20948_Stat_UnrecognisedDMPHeader2: |
saloutos | 0:894b603d32ee | 74 | _debugSerial->printf("Unrecognised DMP Header2\n\r"); |
saloutos | 0:894b603d32ee | 75 | break; |
saloutos | 0:894b603d32ee | 76 | case ICM_20948_Stat_InvalDMPRegister: |
saloutos | 0:894b603d32ee | 77 | _debugSerial->printf("Invalid DMP Register\n\r"); |
saloutos | 0:894b603d32ee | 78 | break; |
saloutos | 0:894b603d32ee | 79 | default: |
saloutos | 0:894b603d32ee | 80 | _debugSerial->printf("Unknown Status\n\r"); |
saloutos | 0:894b603d32ee | 81 | break; |
saloutos | 0:894b603d32ee | 82 | } |
saloutos | 0:894b603d32ee | 83 | // } // end if debugging==true |
saloutos | 0:894b603d32ee | 84 | } |
saloutos | 0:894b603d32ee | 85 | |
saloutos | 0:894b603d32ee | 86 | ICM_20948_AGMT_t ICM_20948::getAGMT(void) |
saloutos | 0:894b603d32ee | 87 | { |
saloutos | 0:894b603d32ee | 88 | status = ICM_20948_get_agmt(&_device, &agmt); |
saloutos | 0:894b603d32ee | 89 | |
saloutos | 0:894b603d32ee | 90 | return agmt; |
saloutos | 0:894b603d32ee | 91 | } |
saloutos | 0:894b603d32ee | 92 | |
saloutos | 0:894b603d32ee | 93 | float ICM_20948::magX(void) |
saloutos | 0:894b603d32ee | 94 | { |
saloutos | 0:894b603d32ee | 95 | return getMagUT(agmt.mag.axes.x); |
saloutos | 0:894b603d32ee | 96 | } |
saloutos | 0:894b603d32ee | 97 | |
saloutos | 0:894b603d32ee | 98 | float ICM_20948::magY(void) |
saloutos | 0:894b603d32ee | 99 | { |
saloutos | 0:894b603d32ee | 100 | return getMagUT(agmt.mag.axes.y); |
saloutos | 0:894b603d32ee | 101 | } |
saloutos | 0:894b603d32ee | 102 | |
saloutos | 0:894b603d32ee | 103 | float ICM_20948::magZ(void) |
saloutos | 0:894b603d32ee | 104 | { |
saloutos | 0:894b603d32ee | 105 | return getMagUT(agmt.mag.axes.z); |
saloutos | 0:894b603d32ee | 106 | } |
saloutos | 0:894b603d32ee | 107 | |
saloutos | 0:894b603d32ee | 108 | float ICM_20948::getMagUT(int16_t axis_val) |
saloutos | 0:894b603d32ee | 109 | { |
saloutos | 0:894b603d32ee | 110 | return (((float)axis_val) * 0.15); |
saloutos | 0:894b603d32ee | 111 | } |
saloutos | 0:894b603d32ee | 112 | |
saloutos | 0:894b603d32ee | 113 | float ICM_20948::accX(void) |
saloutos | 0:894b603d32ee | 114 | { |
saloutos | 0:894b603d32ee | 115 | return getAccMG(agmt.acc.axes.x); |
saloutos | 0:894b603d32ee | 116 | } |
saloutos | 0:894b603d32ee | 117 | |
saloutos | 0:894b603d32ee | 118 | float ICM_20948::accY(void) |
saloutos | 0:894b603d32ee | 119 | { |
saloutos | 0:894b603d32ee | 120 | return getAccMG(agmt.acc.axes.y); |
saloutos | 0:894b603d32ee | 121 | } |
saloutos | 0:894b603d32ee | 122 | |
saloutos | 0:894b603d32ee | 123 | float ICM_20948::accZ(void) |
saloutos | 0:894b603d32ee | 124 | { |
saloutos | 0:894b603d32ee | 125 | return getAccMG(agmt.acc.axes.z); |
saloutos | 0:894b603d32ee | 126 | } |
saloutos | 0:894b603d32ee | 127 | |
saloutos | 0:894b603d32ee | 128 | float ICM_20948::getAccMG(int16_t axis_val) |
saloutos | 0:894b603d32ee | 129 | { |
saloutos | 0:894b603d32ee | 130 | switch (agmt.fss.a) |
saloutos | 0:894b603d32ee | 131 | { |
saloutos | 0:894b603d32ee | 132 | case 0: |
saloutos | 0:894b603d32ee | 133 | return (((float)axis_val) / 16.384); |
saloutos | 0:894b603d32ee | 134 | break; |
saloutos | 0:894b603d32ee | 135 | case 1: |
saloutos | 0:894b603d32ee | 136 | return (((float)axis_val) / 8.192); |
saloutos | 0:894b603d32ee | 137 | break; |
saloutos | 0:894b603d32ee | 138 | case 2: |
saloutos | 0:894b603d32ee | 139 | return (((float)axis_val) / 4.096); |
saloutos | 0:894b603d32ee | 140 | break; |
saloutos | 0:894b603d32ee | 141 | case 3: |
saloutos | 0:894b603d32ee | 142 | return (((float)axis_val) / 2.048); |
saloutos | 0:894b603d32ee | 143 | break; |
saloutos | 0:894b603d32ee | 144 | default: |
saloutos | 0:894b603d32ee | 145 | return 0; |
saloutos | 0:894b603d32ee | 146 | break; |
saloutos | 0:894b603d32ee | 147 | } |
saloutos | 0:894b603d32ee | 148 | } |
saloutos | 0:894b603d32ee | 149 | |
saloutos | 0:894b603d32ee | 150 | float ICM_20948::gyrX(void) |
saloutos | 0:894b603d32ee | 151 | { |
saloutos | 0:894b603d32ee | 152 | return getGyrDPS(agmt.gyr.axes.x); |
saloutos | 0:894b603d32ee | 153 | } |
saloutos | 0:894b603d32ee | 154 | |
saloutos | 0:894b603d32ee | 155 | float ICM_20948::gyrY(void) |
saloutos | 0:894b603d32ee | 156 | { |
saloutos | 0:894b603d32ee | 157 | return getGyrDPS(agmt.gyr.axes.y); |
saloutos | 0:894b603d32ee | 158 | } |
saloutos | 0:894b603d32ee | 159 | |
saloutos | 0:894b603d32ee | 160 | float ICM_20948::gyrZ(void) |
saloutos | 0:894b603d32ee | 161 | { |
saloutos | 0:894b603d32ee | 162 | return getGyrDPS(agmt.gyr.axes.z); |
saloutos | 0:894b603d32ee | 163 | } |
saloutos | 0:894b603d32ee | 164 | |
saloutos | 0:894b603d32ee | 165 | float ICM_20948::getGyrDPS(int16_t axis_val) |
saloutos | 0:894b603d32ee | 166 | { |
saloutos | 0:894b603d32ee | 167 | switch (agmt.fss.g) |
saloutos | 0:894b603d32ee | 168 | { |
saloutos | 0:894b603d32ee | 169 | case 0: |
saloutos | 0:894b603d32ee | 170 | return (((float)axis_val) / 131); |
saloutos | 0:894b603d32ee | 171 | break; |
saloutos | 0:894b603d32ee | 172 | case 1: |
saloutos | 0:894b603d32ee | 173 | return (((float)axis_val) / 65.5); |
saloutos | 0:894b603d32ee | 174 | break; |
saloutos | 0:894b603d32ee | 175 | case 2: |
saloutos | 0:894b603d32ee | 176 | return (((float)axis_val) / 32.8); |
saloutos | 0:894b603d32ee | 177 | break; |
saloutos | 0:894b603d32ee | 178 | case 3: |
saloutos | 0:894b603d32ee | 179 | return (((float)axis_val) / 16.4); |
saloutos | 0:894b603d32ee | 180 | break; |
saloutos | 0:894b603d32ee | 181 | default: |
saloutos | 0:894b603d32ee | 182 | return 0; |
saloutos | 0:894b603d32ee | 183 | break; |
saloutos | 0:894b603d32ee | 184 | } |
saloutos | 0:894b603d32ee | 185 | } |
saloutos | 0:894b603d32ee | 186 | |
saloutos | 0:894b603d32ee | 187 | float ICM_20948::temp(void) |
saloutos | 0:894b603d32ee | 188 | { |
saloutos | 0:894b603d32ee | 189 | return getTempC(agmt.tmp.val); |
saloutos | 0:894b603d32ee | 190 | } |
saloutos | 0:894b603d32ee | 191 | |
saloutos | 0:894b603d32ee | 192 | float ICM_20948::getTempC(int16_t val) |
saloutos | 0:894b603d32ee | 193 | { |
saloutos | 0:894b603d32ee | 194 | return (((float)val - 21) / 333.87) + 21; |
saloutos | 0:894b603d32ee | 195 | } |
saloutos | 0:894b603d32ee | 196 | |
saloutos | 0:894b603d32ee | 197 | // Device Level |
saloutos | 0:894b603d32ee | 198 | ICM_20948_Status_e ICM_20948::setBank(uint8_t bank) |
saloutos | 0:894b603d32ee | 199 | { |
saloutos | 0:894b603d32ee | 200 | status = ICM_20948_set_bank(&_device, bank); |
saloutos | 0:894b603d32ee | 201 | return status; |
saloutos | 0:894b603d32ee | 202 | } |
saloutos | 0:894b603d32ee | 203 | |
saloutos | 0:894b603d32ee | 204 | ICM_20948_Status_e ICM_20948::swReset(void) |
saloutos | 0:894b603d32ee | 205 | { |
saloutos | 0:894b603d32ee | 206 | status = ICM_20948_sw_reset(&_device); |
saloutos | 0:894b603d32ee | 207 | return status; |
saloutos | 0:894b603d32ee | 208 | } |
saloutos | 0:894b603d32ee | 209 | |
saloutos | 0:894b603d32ee | 210 | ICM_20948_Status_e ICM_20948::sleep(bool on) |
saloutos | 0:894b603d32ee | 211 | { |
saloutos | 0:894b603d32ee | 212 | status = ICM_20948_sleep(&_device, on); |
saloutos | 0:894b603d32ee | 213 | return status; |
saloutos | 0:894b603d32ee | 214 | } |
saloutos | 0:894b603d32ee | 215 | |
saloutos | 0:894b603d32ee | 216 | ICM_20948_Status_e ICM_20948::lowPower(bool on) |
saloutos | 0:894b603d32ee | 217 | { |
saloutos | 0:894b603d32ee | 218 | status = ICM_20948_low_power(&_device, on); |
saloutos | 0:894b603d32ee | 219 | return status; |
saloutos | 0:894b603d32ee | 220 | } |
saloutos | 0:894b603d32ee | 221 | |
saloutos | 0:894b603d32ee | 222 | ICM_20948_Status_e ICM_20948::setClockSource(ICM_20948_PWR_MGMT_1_CLKSEL_e source) |
saloutos | 0:894b603d32ee | 223 | { |
saloutos | 0:894b603d32ee | 224 | status = ICM_20948_set_clock_source(&_device, source); |
saloutos | 0:894b603d32ee | 225 | return status; |
saloutos | 0:894b603d32ee | 226 | } |
saloutos | 0:894b603d32ee | 227 | |
saloutos | 0:894b603d32ee | 228 | ICM_20948_Status_e ICM_20948::checkID(void) |
saloutos | 0:894b603d32ee | 229 | { |
saloutos | 0:894b603d32ee | 230 | status = ICM_20948_check_id(&_device); |
saloutos | 0:894b603d32ee | 231 | if (status != ICM_20948_Stat_Ok) |
saloutos | 0:894b603d32ee | 232 | { |
saloutos | 0:894b603d32ee | 233 | if (_printDebug) { |
saloutos | 0:894b603d32ee | 234 | _debugSerial->printf("ICM_20948::checkID: ICM_20948_check_id returned: "); |
saloutos | 0:894b603d32ee | 235 | debugPrintStatus(status); |
saloutos | 0:894b603d32ee | 236 | } |
saloutos | 0:894b603d32ee | 237 | } |
saloutos | 0:894b603d32ee | 238 | return status; |
saloutos | 0:894b603d32ee | 239 | } |
saloutos | 0:894b603d32ee | 240 | |
saloutos | 0:894b603d32ee | 241 | bool ICM_20948::dataReady(void) |
saloutos | 0:894b603d32ee | 242 | { |
saloutos | 0:894b603d32ee | 243 | status = ICM_20948_data_ready(&_device); |
saloutos | 0:894b603d32ee | 244 | if (status == ICM_20948_Stat_Ok) |
saloutos | 0:894b603d32ee | 245 | { |
saloutos | 0:894b603d32ee | 246 | return true; |
saloutos | 0:894b603d32ee | 247 | } |
saloutos | 0:894b603d32ee | 248 | return false; |
saloutos | 0:894b603d32ee | 249 | } |
saloutos | 0:894b603d32ee | 250 | |
saloutos | 0:894b603d32ee | 251 | uint8_t ICM_20948::getWhoAmI(void) |
saloutos | 0:894b603d32ee | 252 | { |
saloutos | 0:894b603d32ee | 253 | uint8_t retval = 0x00; |
saloutos | 0:894b603d32ee | 254 | status = ICM_20948_get_who_am_i(&_device, &retval); |
saloutos | 0:894b603d32ee | 255 | return retval; |
saloutos | 0:894b603d32ee | 256 | } |
saloutos | 0:894b603d32ee | 257 | |
saloutos | 0:894b603d32ee | 258 | bool ICM_20948::isConnected(void) |
saloutos | 0:894b603d32ee | 259 | { |
saloutos | 0:894b603d32ee | 260 | status = checkID(); |
saloutos | 0:894b603d32ee | 261 | if (status == ICM_20948_Stat_Ok) |
saloutos | 0:894b603d32ee | 262 | { |
saloutos | 0:894b603d32ee | 263 | return true; |
saloutos | 0:894b603d32ee | 264 | } |
saloutos | 0:894b603d32ee | 265 | if (_printDebug) { |
saloutos | 0:894b603d32ee | 266 | _debugSerial->printf("ICM_20948::isConnected: checkID returned: "); |
saloutos | 0:894b603d32ee | 267 | debugPrintStatus(status); |
saloutos | 0:894b603d32ee | 268 | } |
saloutos | 0:894b603d32ee | 269 | return false; |
saloutos | 0:894b603d32ee | 270 | } |
saloutos | 0:894b603d32ee | 271 | |
saloutos | 0:894b603d32ee | 272 | // Internal Sensor Options |
saloutos | 0:894b603d32ee | 273 | ICM_20948_Status_e ICM_20948::setSampleMode(uint8_t sensor_id_bm, uint8_t lp_config_cycle_mode) |
saloutos | 0:894b603d32ee | 274 | { |
saloutos | 0:894b603d32ee | 275 | status = ICM_20948_set_sample_mode(&_device, (ICM_20948_InternalSensorID_bm)sensor_id_bm, (ICM_20948_LP_CONFIG_CYCLE_e)lp_config_cycle_mode); |
saloutos | 0:894b603d32ee | 276 | wait_ms(1); // Give the ICM20948 time to change the sample mode (see issue #8) |
saloutos | 0:894b603d32ee | 277 | return status; |
saloutos | 0:894b603d32ee | 278 | } |
saloutos | 0:894b603d32ee | 279 | |
saloutos | 0:894b603d32ee | 280 | ICM_20948_Status_e ICM_20948::setFullScale(uint8_t sensor_id_bm, ICM_20948_fss_t fss) |
saloutos | 0:894b603d32ee | 281 | { |
saloutos | 0:894b603d32ee | 282 | status = ICM_20948_set_full_scale(&_device, (ICM_20948_InternalSensorID_bm)sensor_id_bm, fss); |
saloutos | 0:894b603d32ee | 283 | return status; |
saloutos | 0:894b603d32ee | 284 | } |
saloutos | 0:894b603d32ee | 285 | |
saloutos | 0:894b603d32ee | 286 | ICM_20948_Status_e ICM_20948::setDLPFcfg(uint8_t sensor_id_bm, ICM_20948_dlpcfg_t cfg) |
saloutos | 0:894b603d32ee | 287 | { |
saloutos | 0:894b603d32ee | 288 | status = ICM_20948_set_dlpf_cfg(&_device, (ICM_20948_InternalSensorID_bm)sensor_id_bm, cfg); |
saloutos | 0:894b603d32ee | 289 | return status; |
saloutos | 0:894b603d32ee | 290 | } |
saloutos | 0:894b603d32ee | 291 | |
saloutos | 0:894b603d32ee | 292 | ICM_20948_Status_e ICM_20948::enableDLPF(uint8_t sensor_id_bm, bool enable) |
saloutos | 0:894b603d32ee | 293 | { |
saloutos | 0:894b603d32ee | 294 | status = ICM_20948_enable_dlpf(&_device, (ICM_20948_InternalSensorID_bm)sensor_id_bm, enable); |
saloutos | 0:894b603d32ee | 295 | return status; |
saloutos | 0:894b603d32ee | 296 | } |
saloutos | 0:894b603d32ee | 297 | |
saloutos | 0:894b603d32ee | 298 | ICM_20948_Status_e ICM_20948::setSampleRate(uint8_t sensor_id_bm, ICM_20948_smplrt_t smplrt) |
saloutos | 0:894b603d32ee | 299 | { |
saloutos | 0:894b603d32ee | 300 | status = ICM_20948_set_sample_rate(&_device, (ICM_20948_InternalSensorID_bm)sensor_id_bm, smplrt); |
saloutos | 0:894b603d32ee | 301 | return status; |
saloutos | 0:894b603d32ee | 302 | } |
saloutos | 0:894b603d32ee | 303 | |
saloutos | 0:894b603d32ee | 304 | // Interrupts on INT Pin |
saloutos | 0:894b603d32ee | 305 | ICM_20948_Status_e ICM_20948::clearInterrupts(void) |
saloutos | 0:894b603d32ee | 306 | { |
saloutos | 0:894b603d32ee | 307 | ICM_20948_INT_STATUS_t int_stat; |
saloutos | 0:894b603d32ee | 308 | ICM_20948_INT_STATUS_1_t int_stat_1; |
saloutos | 0:894b603d32ee | 309 | |
saloutos | 0:894b603d32ee | 310 | // read to clear interrupts |
saloutos | 0:894b603d32ee | 311 | status = ICM_20948_set_bank(&_device, 0); |
saloutos | 0:894b603d32ee | 312 | if (status != ICM_20948_Stat_Ok) |
saloutos | 0:894b603d32ee | 313 | { |
saloutos | 0:894b603d32ee | 314 | return status; |
saloutos | 0:894b603d32ee | 315 | } |
saloutos | 0:894b603d32ee | 316 | status = ICM_20948_execute_r(&_device, AGB0_REG_INT_STATUS, (uint8_t *)&int_stat, sizeof(ICM_20948_INT_STATUS_t)); |
saloutos | 0:894b603d32ee | 317 | if (status != ICM_20948_Stat_Ok) |
saloutos | 0:894b603d32ee | 318 | { |
saloutos | 0:894b603d32ee | 319 | return status; |
saloutos | 0:894b603d32ee | 320 | } |
saloutos | 0:894b603d32ee | 321 | status = ICM_20948_execute_r(&_device, AGB0_REG_INT_STATUS_1, (uint8_t *)&int_stat_1, sizeof(ICM_20948_INT_STATUS_1_t)); |
saloutos | 0:894b603d32ee | 322 | if (status != ICM_20948_Stat_Ok) |
saloutos | 0:894b603d32ee | 323 | { |
saloutos | 0:894b603d32ee | 324 | return status; |
saloutos | 0:894b603d32ee | 325 | } |
saloutos | 0:894b603d32ee | 326 | |
saloutos | 0:894b603d32ee | 327 | // todo: there may be additional interrupts that need to be cleared, like FIFO overflow/watermark |
saloutos | 0:894b603d32ee | 328 | |
saloutos | 0:894b603d32ee | 329 | return status; |
saloutos | 0:894b603d32ee | 330 | } |
saloutos | 0:894b603d32ee | 331 | |
saloutos | 0:894b603d32ee | 332 | ICM_20948_Status_e ICM_20948::cfgIntActiveLow(bool active_low) |
saloutos | 0:894b603d32ee | 333 | { |
saloutos | 0:894b603d32ee | 334 | ICM_20948_INT_PIN_CFG_t reg; |
saloutos | 0:894b603d32ee | 335 | status = ICM_20948_int_pin_cfg(&_device, NULL, ®); // read phase |
saloutos | 0:894b603d32ee | 336 | if (status != ICM_20948_Stat_Ok) |
saloutos | 0:894b603d32ee | 337 | { |
saloutos | 0:894b603d32ee | 338 | return status; |
saloutos | 0:894b603d32ee | 339 | } |
saloutos | 0:894b603d32ee | 340 | reg.INT1_ACTL = active_low; // set the setting |
saloutos | 0:894b603d32ee | 341 | status = ICM_20948_int_pin_cfg(&_device, ®, NULL); // write phase |
saloutos | 0:894b603d32ee | 342 | if (status != ICM_20948_Stat_Ok) |
saloutos | 0:894b603d32ee | 343 | { |
saloutos | 0:894b603d32ee | 344 | return status; |
saloutos | 0:894b603d32ee | 345 | } |
saloutos | 0:894b603d32ee | 346 | return status; |
saloutos | 0:894b603d32ee | 347 | } |
saloutos | 0:894b603d32ee | 348 | |
saloutos | 0:894b603d32ee | 349 | ICM_20948_Status_e ICM_20948::cfgIntOpenDrain(bool open_drain) |
saloutos | 0:894b603d32ee | 350 | { |
saloutos | 0:894b603d32ee | 351 | ICM_20948_INT_PIN_CFG_t reg; |
saloutos | 0:894b603d32ee | 352 | status = ICM_20948_int_pin_cfg(&_device, NULL, ®); // read phase |
saloutos | 0:894b603d32ee | 353 | if (status != ICM_20948_Stat_Ok) |
saloutos | 0:894b603d32ee | 354 | { |
saloutos | 0:894b603d32ee | 355 | return status; |
saloutos | 0:894b603d32ee | 356 | } |
saloutos | 0:894b603d32ee | 357 | reg.INT1_OPEN = open_drain; // set the setting |
saloutos | 0:894b603d32ee | 358 | status = ICM_20948_int_pin_cfg(&_device, ®, NULL); // write phase |
saloutos | 0:894b603d32ee | 359 | if (status != ICM_20948_Stat_Ok) |
saloutos | 0:894b603d32ee | 360 | { |
saloutos | 0:894b603d32ee | 361 | return status; |
saloutos | 0:894b603d32ee | 362 | } |
saloutos | 0:894b603d32ee | 363 | return status; |
saloutos | 0:894b603d32ee | 364 | } |
saloutos | 0:894b603d32ee | 365 | |
saloutos | 0:894b603d32ee | 366 | ICM_20948_Status_e ICM_20948::cfgIntLatch(bool latching) |
saloutos | 0:894b603d32ee | 367 | { |
saloutos | 0:894b603d32ee | 368 | ICM_20948_INT_PIN_CFG_t reg; |
saloutos | 0:894b603d32ee | 369 | status = ICM_20948_int_pin_cfg(&_device, NULL, ®); // read phase |
saloutos | 0:894b603d32ee | 370 | if (status != ICM_20948_Stat_Ok) |
saloutos | 0:894b603d32ee | 371 | { |
saloutos | 0:894b603d32ee | 372 | return status; |
saloutos | 0:894b603d32ee | 373 | } |
saloutos | 0:894b603d32ee | 374 | reg.INT1_LATCH_EN = latching; // set the setting |
saloutos | 0:894b603d32ee | 375 | status = ICM_20948_int_pin_cfg(&_device, ®, NULL); // write phase |
saloutos | 0:894b603d32ee | 376 | if (status != ICM_20948_Stat_Ok) |
saloutos | 0:894b603d32ee | 377 | { |
saloutos | 0:894b603d32ee | 378 | return status; |
saloutos | 0:894b603d32ee | 379 | } |
saloutos | 0:894b603d32ee | 380 | return status; |
saloutos | 0:894b603d32ee | 381 | } |
saloutos | 0:894b603d32ee | 382 | |
saloutos | 0:894b603d32ee | 383 | ICM_20948_Status_e ICM_20948::cfgIntAnyReadToClear(bool enabled) |
saloutos | 0:894b603d32ee | 384 | { |
saloutos | 0:894b603d32ee | 385 | ICM_20948_INT_PIN_CFG_t reg; |
saloutos | 0:894b603d32ee | 386 | status = ICM_20948_int_pin_cfg(&_device, NULL, ®); // read phase |
saloutos | 0:894b603d32ee | 387 | if (status != ICM_20948_Stat_Ok) |
saloutos | 0:894b603d32ee | 388 | { |
saloutos | 0:894b603d32ee | 389 | return status; |
saloutos | 0:894b603d32ee | 390 | } |
saloutos | 0:894b603d32ee | 391 | reg.INT_ANYRD_2CLEAR = enabled; // set the setting |
saloutos | 0:894b603d32ee | 392 | status = ICM_20948_int_pin_cfg(&_device, ®, NULL); // write phase |
saloutos | 0:894b603d32ee | 393 | if (status != ICM_20948_Stat_Ok) |
saloutos | 0:894b603d32ee | 394 | { |
saloutos | 0:894b603d32ee | 395 | return status; |
saloutos | 0:894b603d32ee | 396 | } |
saloutos | 0:894b603d32ee | 397 | return status; |
saloutos | 0:894b603d32ee | 398 | } |
saloutos | 0:894b603d32ee | 399 | |
saloutos | 0:894b603d32ee | 400 | ICM_20948_Status_e ICM_20948::cfgFsyncActiveLow(bool active_low) |
saloutos | 0:894b603d32ee | 401 | { |
saloutos | 0:894b603d32ee | 402 | ICM_20948_INT_PIN_CFG_t reg; |
saloutos | 0:894b603d32ee | 403 | status = ICM_20948_int_pin_cfg(&_device, NULL, ®); // read phase |
saloutos | 0:894b603d32ee | 404 | if (status != ICM_20948_Stat_Ok) |
saloutos | 0:894b603d32ee | 405 | { |
saloutos | 0:894b603d32ee | 406 | return status; |
saloutos | 0:894b603d32ee | 407 | } |
saloutos | 0:894b603d32ee | 408 | reg.ACTL_FSYNC = active_low; // set the setting |
saloutos | 0:894b603d32ee | 409 | status = ICM_20948_int_pin_cfg(&_device, ®, NULL); // write phase |
saloutos | 0:894b603d32ee | 410 | if (status != ICM_20948_Stat_Ok) |
saloutos | 0:894b603d32ee | 411 | { |
saloutos | 0:894b603d32ee | 412 | return status; |
saloutos | 0:894b603d32ee | 413 | } |
saloutos | 0:894b603d32ee | 414 | return status; |
saloutos | 0:894b603d32ee | 415 | } |
saloutos | 0:894b603d32ee | 416 | |
saloutos | 0:894b603d32ee | 417 | ICM_20948_Status_e ICM_20948::cfgFsyncIntMode(bool interrupt_mode) |
saloutos | 0:894b603d32ee | 418 | { |
saloutos | 0:894b603d32ee | 419 | ICM_20948_INT_PIN_CFG_t reg; |
saloutos | 0:894b603d32ee | 420 | status = ICM_20948_int_pin_cfg(&_device, NULL, ®); // read phase |
saloutos | 0:894b603d32ee | 421 | if (status != ICM_20948_Stat_Ok) |
saloutos | 0:894b603d32ee | 422 | { |
saloutos | 0:894b603d32ee | 423 | return status; |
saloutos | 0:894b603d32ee | 424 | } |
saloutos | 0:894b603d32ee | 425 | reg.FSYNC_INT_MODE_EN = interrupt_mode; // set the setting |
saloutos | 0:894b603d32ee | 426 | status = ICM_20948_int_pin_cfg(&_device, ®, NULL); // write phase |
saloutos | 0:894b603d32ee | 427 | if (status != ICM_20948_Stat_Ok) |
saloutos | 0:894b603d32ee | 428 | { |
saloutos | 0:894b603d32ee | 429 | return status; |
saloutos | 0:894b603d32ee | 430 | } |
saloutos | 0:894b603d32ee | 431 | return status; |
saloutos | 0:894b603d32ee | 432 | } |
saloutos | 0:894b603d32ee | 433 | |
saloutos | 0:894b603d32ee | 434 | // All these individual functions will use a read->set->write method to leave other settings untouched |
saloutos | 0:894b603d32ee | 435 | ICM_20948_Status_e ICM_20948::intEnableI2C(bool enable) |
saloutos | 0:894b603d32ee | 436 | { |
saloutos | 0:894b603d32ee | 437 | ICM_20948_INT_enable_t en; // storage |
saloutos | 0:894b603d32ee | 438 | status = ICM_20948_int_enable(&_device, NULL, &en); // read phase |
saloutos | 0:894b603d32ee | 439 | if (status != ICM_20948_Stat_Ok) |
saloutos | 0:894b603d32ee | 440 | { |
saloutos | 0:894b603d32ee | 441 | return status; |
saloutos | 0:894b603d32ee | 442 | } |
saloutos | 0:894b603d32ee | 443 | en.I2C_MST_INT_EN = enable; // change the setting |
saloutos | 0:894b603d32ee | 444 | status = ICM_20948_int_enable(&_device, &en, &en); // write phase w/ readback |
saloutos | 0:894b603d32ee | 445 | if (status != ICM_20948_Stat_Ok) |
saloutos | 0:894b603d32ee | 446 | { |
saloutos | 0:894b603d32ee | 447 | return status; |
saloutos | 0:894b603d32ee | 448 | } |
saloutos | 0:894b603d32ee | 449 | if (en.I2C_MST_INT_EN != enable) |
saloutos | 0:894b603d32ee | 450 | { |
saloutos | 0:894b603d32ee | 451 | status = ICM_20948_Stat_Err; |
saloutos | 0:894b603d32ee | 452 | return status; |
saloutos | 0:894b603d32ee | 453 | } |
saloutos | 0:894b603d32ee | 454 | return status; |
saloutos | 0:894b603d32ee | 455 | } |
saloutos | 0:894b603d32ee | 456 | |
saloutos | 0:894b603d32ee | 457 | ICM_20948_Status_e ICM_20948::intEnableDMP(bool enable) |
saloutos | 0:894b603d32ee | 458 | { |
saloutos | 0:894b603d32ee | 459 | ICM_20948_INT_enable_t en; // storage |
saloutos | 0:894b603d32ee | 460 | status = ICM_20948_int_enable(&_device, NULL, &en); // read phase |
saloutos | 0:894b603d32ee | 461 | if (status != ICM_20948_Stat_Ok) |
saloutos | 0:894b603d32ee | 462 | { |
saloutos | 0:894b603d32ee | 463 | return status; |
saloutos | 0:894b603d32ee | 464 | } |
saloutos | 0:894b603d32ee | 465 | en.DMP_INT1_EN = enable; // change the setting |
saloutos | 0:894b603d32ee | 466 | status = ICM_20948_int_enable(&_device, &en, &en); // write phase w/ readback |
saloutos | 0:894b603d32ee | 467 | if (status != ICM_20948_Stat_Ok) |
saloutos | 0:894b603d32ee | 468 | { |
saloutos | 0:894b603d32ee | 469 | return status; |
saloutos | 0:894b603d32ee | 470 | } |
saloutos | 0:894b603d32ee | 471 | if (en.DMP_INT1_EN != enable) |
saloutos | 0:894b603d32ee | 472 | { |
saloutos | 0:894b603d32ee | 473 | status = ICM_20948_Stat_Err; |
saloutos | 0:894b603d32ee | 474 | return status; |
saloutos | 0:894b603d32ee | 475 | } |
saloutos | 0:894b603d32ee | 476 | return status; |
saloutos | 0:894b603d32ee | 477 | } |
saloutos | 0:894b603d32ee | 478 | |
saloutos | 0:894b603d32ee | 479 | ICM_20948_Status_e ICM_20948::intEnablePLL(bool enable) |
saloutos | 0:894b603d32ee | 480 | { |
saloutos | 0:894b603d32ee | 481 | ICM_20948_INT_enable_t en; // storage |
saloutos | 0:894b603d32ee | 482 | status = ICM_20948_int_enable(&_device, NULL, &en); // read phase |
saloutos | 0:894b603d32ee | 483 | if (status != ICM_20948_Stat_Ok) |
saloutos | 0:894b603d32ee | 484 | { |
saloutos | 0:894b603d32ee | 485 | return status; |
saloutos | 0:894b603d32ee | 486 | } |
saloutos | 0:894b603d32ee | 487 | en.PLL_RDY_EN = enable; // change the setting |
saloutos | 0:894b603d32ee | 488 | status = ICM_20948_int_enable(&_device, &en, &en); // write phase w/ readback |
saloutos | 0:894b603d32ee | 489 | if (status != ICM_20948_Stat_Ok) |
saloutos | 0:894b603d32ee | 490 | { |
saloutos | 0:894b603d32ee | 491 | return status; |
saloutos | 0:894b603d32ee | 492 | } |
saloutos | 0:894b603d32ee | 493 | if (en.PLL_RDY_EN != enable) |
saloutos | 0:894b603d32ee | 494 | { |
saloutos | 0:894b603d32ee | 495 | status = ICM_20948_Stat_Err; |
saloutos | 0:894b603d32ee | 496 | return status; |
saloutos | 0:894b603d32ee | 497 | } |
saloutos | 0:894b603d32ee | 498 | return status; |
saloutos | 0:894b603d32ee | 499 | } |
saloutos | 0:894b603d32ee | 500 | |
saloutos | 0:894b603d32ee | 501 | ICM_20948_Status_e ICM_20948::intEnableWOM(bool enable) |
saloutos | 0:894b603d32ee | 502 | { |
saloutos | 0:894b603d32ee | 503 | ICM_20948_INT_enable_t en; // storage |
saloutos | 0:894b603d32ee | 504 | status = ICM_20948_int_enable(&_device, NULL, &en); // read phase |
saloutos | 0:894b603d32ee | 505 | if (status != ICM_20948_Stat_Ok) |
saloutos | 0:894b603d32ee | 506 | { |
saloutos | 0:894b603d32ee | 507 | return status; |
saloutos | 0:894b603d32ee | 508 | } |
saloutos | 0:894b603d32ee | 509 | en.WOM_INT_EN = enable; // change the setting |
saloutos | 0:894b603d32ee | 510 | status = ICM_20948_int_enable(&_device, &en, &en); // write phase w/ readback |
saloutos | 0:894b603d32ee | 511 | if (status != ICM_20948_Stat_Ok) |
saloutos | 0:894b603d32ee | 512 | { |
saloutos | 0:894b603d32ee | 513 | return status; |
saloutos | 0:894b603d32ee | 514 | } |
saloutos | 0:894b603d32ee | 515 | if (en.WOM_INT_EN != enable) |
saloutos | 0:894b603d32ee | 516 | { |
saloutos | 0:894b603d32ee | 517 | status = ICM_20948_Stat_Err; |
saloutos | 0:894b603d32ee | 518 | return status; |
saloutos | 0:894b603d32ee | 519 | } |
saloutos | 0:894b603d32ee | 520 | return status; |
saloutos | 0:894b603d32ee | 521 | } |
saloutos | 0:894b603d32ee | 522 | |
saloutos | 0:894b603d32ee | 523 | ICM_20948_Status_e ICM_20948::intEnableWOF(bool enable) |
saloutos | 0:894b603d32ee | 524 | { |
saloutos | 0:894b603d32ee | 525 | ICM_20948_INT_enable_t en; // storage |
saloutos | 0:894b603d32ee | 526 | status = ICM_20948_int_enable(&_device, NULL, &en); // read phase |
saloutos | 0:894b603d32ee | 527 | if (status != ICM_20948_Stat_Ok) |
saloutos | 0:894b603d32ee | 528 | { |
saloutos | 0:894b603d32ee | 529 | return status; |
saloutos | 0:894b603d32ee | 530 | } |
saloutos | 0:894b603d32ee | 531 | en.REG_WOF_EN = enable; // change the setting |
saloutos | 0:894b603d32ee | 532 | status = ICM_20948_int_enable(&_device, &en, &en); // write phase w/ readback |
saloutos | 0:894b603d32ee | 533 | if (status != ICM_20948_Stat_Ok) |
saloutos | 0:894b603d32ee | 534 | { |
saloutos | 0:894b603d32ee | 535 | return status; |
saloutos | 0:894b603d32ee | 536 | } |
saloutos | 0:894b603d32ee | 537 | if (en.REG_WOF_EN != enable) |
saloutos | 0:894b603d32ee | 538 | { |
saloutos | 0:894b603d32ee | 539 | status = ICM_20948_Stat_Err; |
saloutos | 0:894b603d32ee | 540 | return status; |
saloutos | 0:894b603d32ee | 541 | } |
saloutos | 0:894b603d32ee | 542 | return status; |
saloutos | 0:894b603d32ee | 543 | } |
saloutos | 0:894b603d32ee | 544 | |
saloutos | 0:894b603d32ee | 545 | ICM_20948_Status_e ICM_20948::intEnableRawDataReady(bool enable) |
saloutos | 0:894b603d32ee | 546 | { |
saloutos | 0:894b603d32ee | 547 | ICM_20948_INT_enable_t en; // storage |
saloutos | 0:894b603d32ee | 548 | status = ICM_20948_int_enable(&_device, NULL, &en); // read phase |
saloutos | 0:894b603d32ee | 549 | if (status != ICM_20948_Stat_Ok) |
saloutos | 0:894b603d32ee | 550 | { |
saloutos | 0:894b603d32ee | 551 | return status; |
saloutos | 0:894b603d32ee | 552 | } |
saloutos | 0:894b603d32ee | 553 | en.RAW_DATA_0_RDY_EN = enable; // change the setting |
saloutos | 0:894b603d32ee | 554 | status = ICM_20948_int_enable(&_device, &en, &en); // write phase w/ readback |
saloutos | 0:894b603d32ee | 555 | if (status != ICM_20948_Stat_Ok) |
saloutos | 0:894b603d32ee | 556 | { |
saloutos | 0:894b603d32ee | 557 | return status; |
saloutos | 0:894b603d32ee | 558 | } |
saloutos | 0:894b603d32ee | 559 | if (en.RAW_DATA_0_RDY_EN != enable) |
saloutos | 0:894b603d32ee | 560 | { |
saloutos | 0:894b603d32ee | 561 | status = ICM_20948_Stat_Err; |
saloutos | 0:894b603d32ee | 562 | return status; |
saloutos | 0:894b603d32ee | 563 | } |
saloutos | 0:894b603d32ee | 564 | return status; |
saloutos | 0:894b603d32ee | 565 | } |
saloutos | 0:894b603d32ee | 566 | |
saloutos | 0:894b603d32ee | 567 | ICM_20948_Status_e ICM_20948::intEnableOverflowFIFO(uint8_t bm_enable) |
saloutos | 0:894b603d32ee | 568 | { |
saloutos | 0:894b603d32ee | 569 | ICM_20948_INT_enable_t en; // storage |
saloutos | 0:894b603d32ee | 570 | status = ICM_20948_int_enable(&_device, NULL, &en); // read phase |
saloutos | 0:894b603d32ee | 571 | if (status != ICM_20948_Stat_Ok) |
saloutos | 0:894b603d32ee | 572 | { |
saloutos | 0:894b603d32ee | 573 | return status; |
saloutos | 0:894b603d32ee | 574 | } |
saloutos | 0:894b603d32ee | 575 | en.FIFO_OVERFLOW_EN_0 = ((bm_enable >> 0) & 0x01); // change the settings |
saloutos | 0:894b603d32ee | 576 | en.FIFO_OVERFLOW_EN_1 = ((bm_enable >> 1) & 0x01); |
saloutos | 0:894b603d32ee | 577 | en.FIFO_OVERFLOW_EN_2 = ((bm_enable >> 2) & 0x01); |
saloutos | 0:894b603d32ee | 578 | en.FIFO_OVERFLOW_EN_3 = ((bm_enable >> 3) & 0x01); |
saloutos | 0:894b603d32ee | 579 | en.FIFO_OVERFLOW_EN_4 = ((bm_enable >> 4) & 0x01); |
saloutos | 0:894b603d32ee | 580 | status = ICM_20948_int_enable(&_device, &en, &en); // write phase w/ readback |
saloutos | 0:894b603d32ee | 581 | if (status != ICM_20948_Stat_Ok) |
saloutos | 0:894b603d32ee | 582 | { |
saloutos | 0:894b603d32ee | 583 | return status; |
saloutos | 0:894b603d32ee | 584 | } |
saloutos | 0:894b603d32ee | 585 | return status; |
saloutos | 0:894b603d32ee | 586 | } |
saloutos | 0:894b603d32ee | 587 | |
saloutos | 0:894b603d32ee | 588 | ICM_20948_Status_e ICM_20948::intEnableWatermarkFIFO(uint8_t bm_enable) |
saloutos | 0:894b603d32ee | 589 | { |
saloutos | 0:894b603d32ee | 590 | ICM_20948_INT_enable_t en; // storage |
saloutos | 0:894b603d32ee | 591 | status = ICM_20948_int_enable(&_device, NULL, &en); // read phase |
saloutos | 0:894b603d32ee | 592 | if (status != ICM_20948_Stat_Ok) |
saloutos | 0:894b603d32ee | 593 | { |
saloutos | 0:894b603d32ee | 594 | return status; |
saloutos | 0:894b603d32ee | 595 | } |
saloutos | 0:894b603d32ee | 596 | en.FIFO_WM_EN_0 = ((bm_enable >> 0) & 0x01); // change the settings |
saloutos | 0:894b603d32ee | 597 | en.FIFO_WM_EN_1 = ((bm_enable >> 1) & 0x01); |
saloutos | 0:894b603d32ee | 598 | en.FIFO_WM_EN_2 = ((bm_enable >> 2) & 0x01); |
saloutos | 0:894b603d32ee | 599 | en.FIFO_WM_EN_3 = ((bm_enable >> 3) & 0x01); |
saloutos | 0:894b603d32ee | 600 | en.FIFO_WM_EN_4 = ((bm_enable >> 4) & 0x01); |
saloutos | 0:894b603d32ee | 601 | status = ICM_20948_int_enable(&_device, &en, &en); // write phase w/ readback |
saloutos | 0:894b603d32ee | 602 | if (status != ICM_20948_Stat_Ok) |
saloutos | 0:894b603d32ee | 603 | { |
saloutos | 0:894b603d32ee | 604 | return status; |
saloutos | 0:894b603d32ee | 605 | } |
saloutos | 0:894b603d32ee | 606 | return status; |
saloutos | 0:894b603d32ee | 607 | } |
saloutos | 0:894b603d32ee | 608 | |
saloutos | 0:894b603d32ee | 609 | ICM_20948_Status_e ICM_20948::WOMThreshold(uint8_t threshold) |
saloutos | 0:894b603d32ee | 610 | { |
saloutos | 0:894b603d32ee | 611 | ICM_20948_ACCEL_WOM_THR_t thr; // storage |
saloutos | 0:894b603d32ee | 612 | status = ICM_20948_wom_threshold(&_device, NULL, &thr); // read phase |
saloutos | 0:894b603d32ee | 613 | if (status != ICM_20948_Stat_Ok) |
saloutos | 0:894b603d32ee | 614 | { |
saloutos | 0:894b603d32ee | 615 | return status; |
saloutos | 0:894b603d32ee | 616 | } |
saloutos | 0:894b603d32ee | 617 | thr.WOM_THRESHOLD = threshold; // change the setting |
saloutos | 0:894b603d32ee | 618 | status = ICM_20948_wom_threshold(&_device, &thr, &thr); // write phase w/ readback |
saloutos | 0:894b603d32ee | 619 | if (status != ICM_20948_Stat_Ok) |
saloutos | 0:894b603d32ee | 620 | { |
saloutos | 0:894b603d32ee | 621 | return status; |
saloutos | 0:894b603d32ee | 622 | } |
saloutos | 0:894b603d32ee | 623 | if (thr.WOM_THRESHOLD != threshold) |
saloutos | 0:894b603d32ee | 624 | { |
saloutos | 0:894b603d32ee | 625 | status = ICM_20948_Stat_Err; |
saloutos | 0:894b603d32ee | 626 | return status; |
saloutos | 0:894b603d32ee | 627 | } |
saloutos | 0:894b603d32ee | 628 | return status; |
saloutos | 0:894b603d32ee | 629 | } |
saloutos | 0:894b603d32ee | 630 | |
saloutos | 0:894b603d32ee | 631 | // Interface Options |
saloutos | 0:894b603d32ee | 632 | ICM_20948_Status_e ICM_20948::i2cMasterPassthrough(bool passthrough) |
saloutos | 0:894b603d32ee | 633 | { |
saloutos | 0:894b603d32ee | 634 | status = ICM_20948_i2c_master_passthrough(&_device, passthrough); |
saloutos | 0:894b603d32ee | 635 | return status; |
saloutos | 0:894b603d32ee | 636 | } |
saloutos | 0:894b603d32ee | 637 | |
saloutos | 0:894b603d32ee | 638 | ICM_20948_Status_e ICM_20948::i2cMasterEnable(bool enable) |
saloutos | 0:894b603d32ee | 639 | { |
saloutos | 0:894b603d32ee | 640 | status = ICM_20948_i2c_master_enable(&_device, enable); |
saloutos | 0:894b603d32ee | 641 | return status; |
saloutos | 0:894b603d32ee | 642 | } |
saloutos | 0:894b603d32ee | 643 | |
saloutos | 0:894b603d32ee | 644 | ICM_20948_Status_e ICM_20948::i2cMasterReset() |
saloutos | 0:894b603d32ee | 645 | { |
saloutos | 0:894b603d32ee | 646 | status = ICM_20948_i2c_master_reset(&_device); |
saloutos | 0:894b603d32ee | 647 | return status; |
saloutos | 0:894b603d32ee | 648 | } |
saloutos | 0:894b603d32ee | 649 | |
saloutos | 0:894b603d32ee | 650 | ICM_20948_Status_e ICM_20948::i2cControllerConfigurePeripheral(uint8_t peripheral, uint8_t addr, uint8_t reg, uint8_t len, bool Rw, bool enable, bool data_only, bool grp, bool swap, uint8_t dataOut) |
saloutos | 0:894b603d32ee | 651 | { |
saloutos | 0:894b603d32ee | 652 | status = ICM_20948_i2c_controller_configure_peripheral(&_device, peripheral, addr, reg, len, Rw, enable, data_only, grp, swap, dataOut); |
saloutos | 0:894b603d32ee | 653 | return status; |
saloutos | 0:894b603d32ee | 654 | } |
saloutos | 0:894b603d32ee | 655 | |
saloutos | 0:894b603d32ee | 656 | ICM_20948_Status_e ICM_20948::i2cControllerPeriph4Transaction(uint8_t addr, uint8_t reg, uint8_t *data, uint8_t len, bool Rw, bool send_reg_addr) |
saloutos | 0:894b603d32ee | 657 | { |
saloutos | 0:894b603d32ee | 658 | status = ICM_20948_i2c_controller_periph4_txn(&_device, addr, reg, data, len, Rw, send_reg_addr); |
saloutos | 0:894b603d32ee | 659 | return status; |
saloutos | 0:894b603d32ee | 660 | } |
saloutos | 0:894b603d32ee | 661 | |
saloutos | 0:894b603d32ee | 662 | ICM_20948_Status_e ICM_20948::i2cMasterSingleW(uint8_t addr, uint8_t reg, uint8_t data) |
saloutos | 0:894b603d32ee | 663 | { |
saloutos | 0:894b603d32ee | 664 | status = ICM_20948_i2c_master_single_w(&_device, addr, reg, &data); |
saloutos | 0:894b603d32ee | 665 | return status; |
saloutos | 0:894b603d32ee | 666 | } |
saloutos | 0:894b603d32ee | 667 | uint8_t ICM_20948::i2cMasterSingleR(uint8_t addr, uint8_t reg) |
saloutos | 0:894b603d32ee | 668 | { |
saloutos | 0:894b603d32ee | 669 | uint8_t data = 0; |
saloutos | 0:894b603d32ee | 670 | status = ICM_20948_i2c_master_single_r(&_device, addr, reg, &data); |
saloutos | 0:894b603d32ee | 671 | if (status != ICM_20948_Stat_Ok) |
saloutos | 0:894b603d32ee | 672 | { |
saloutos | 0:894b603d32ee | 673 | if (_printDebug) { |
saloutos | 0:894b603d32ee | 674 | _debugSerial->printf("ICM_20948::i2cMasterSingleR: ICM_20948_i2c_master_single_r returned: "); |
saloutos | 0:894b603d32ee | 675 | debugPrintStatus(status); |
saloutos | 0:894b603d32ee | 676 | } |
saloutos | 0:894b603d32ee | 677 | } |
saloutos | 0:894b603d32ee | 678 | return data; |
saloutos | 0:894b603d32ee | 679 | } |
saloutos | 0:894b603d32ee | 680 | |
saloutos | 0:894b603d32ee | 681 | ICM_20948_Status_e ICM_20948::startupDefault(bool minimal) |
saloutos | 0:894b603d32ee | 682 | { |
saloutos | 0:894b603d32ee | 683 | ICM_20948_Status_e retval = ICM_20948_Stat_Ok; |
saloutos | 0:894b603d32ee | 684 | |
saloutos | 0:894b603d32ee | 685 | retval = checkID(); |
saloutos | 0:894b603d32ee | 686 | if (retval != ICM_20948_Stat_Ok) |
saloutos | 0:894b603d32ee | 687 | { |
saloutos | 0:894b603d32ee | 688 | if (_printDebug) { |
saloutos | 0:894b603d32ee | 689 | _debugSerial->printf("ICM_20948::startupDefault: checkID returned: "); |
saloutos | 0:894b603d32ee | 690 | debugPrintStatus(retval); |
saloutos | 0:894b603d32ee | 691 | } |
saloutos | 0:894b603d32ee | 692 | status = retval; |
saloutos | 0:894b603d32ee | 693 | return status; |
saloutos | 0:894b603d32ee | 694 | } |
saloutos | 0:894b603d32ee | 695 | |
saloutos | 0:894b603d32ee | 696 | retval = swReset(); |
saloutos | 0:894b603d32ee | 697 | if (retval != ICM_20948_Stat_Ok) |
saloutos | 0:894b603d32ee | 698 | { |
saloutos | 0:894b603d32ee | 699 | if (_printDebug) { |
saloutos | 0:894b603d32ee | 700 | _debugSerial->printf("ICM_20948::startupDefault: swReset returned: "); |
saloutos | 0:894b603d32ee | 701 | debugPrintStatus(retval); |
saloutos | 0:894b603d32ee | 702 | } |
saloutos | 0:894b603d32ee | 703 | status = retval; |
saloutos | 0:894b603d32ee | 704 | return status; |
saloutos | 0:894b603d32ee | 705 | } |
saloutos | 0:894b603d32ee | 706 | wait_ms(50); |
saloutos | 0:894b603d32ee | 707 | |
saloutos | 0:894b603d32ee | 708 | retval = sleep(false); |
saloutos | 0:894b603d32ee | 709 | if (retval != ICM_20948_Stat_Ok) |
saloutos | 0:894b603d32ee | 710 | { |
saloutos | 0:894b603d32ee | 711 | if (_printDebug) { |
saloutos | 0:894b603d32ee | 712 | _debugSerial->printf("ICM_20948::startupDefault: sleep returned: "); |
saloutos | 0:894b603d32ee | 713 | debugPrintStatus(retval); |
saloutos | 0:894b603d32ee | 714 | } |
saloutos | 0:894b603d32ee | 715 | status = retval; |
saloutos | 0:894b603d32ee | 716 | return status; |
saloutos | 0:894b603d32ee | 717 | } |
saloutos | 0:894b603d32ee | 718 | |
saloutos | 0:894b603d32ee | 719 | retval = lowPower(false); |
saloutos | 0:894b603d32ee | 720 | if (retval != ICM_20948_Stat_Ok) |
saloutos | 0:894b603d32ee | 721 | { |
saloutos | 0:894b603d32ee | 722 | if (_printDebug) { |
saloutos | 0:894b603d32ee | 723 | _debugSerial->printf("ICM_20948::startupDefault: lowPower returned: "); |
saloutos | 0:894b603d32ee | 724 | debugPrintStatus(retval); |
saloutos | 0:894b603d32ee | 725 | } |
saloutos | 0:894b603d32ee | 726 | status = retval; |
saloutos | 0:894b603d32ee | 727 | return status; |
saloutos | 0:894b603d32ee | 728 | } |
saloutos | 0:894b603d32ee | 729 | |
saloutos | 0:894b603d32ee | 730 | retval = startupMagnetometer(minimal); // Pass the minimal startup flag to startupMagnetometer |
saloutos | 0:894b603d32ee | 731 | if (retval != ICM_20948_Stat_Ok) |
saloutos | 0:894b603d32ee | 732 | { |
saloutos | 0:894b603d32ee | 733 | if (_printDebug) { |
saloutos | 0:894b603d32ee | 734 | _debugSerial->printf("ICM_20948::startupDefault: startupMagnetometer returned: "); |
saloutos | 0:894b603d32ee | 735 | debugPrintStatus(retval); |
saloutos | 0:894b603d32ee | 736 | } |
saloutos | 0:894b603d32ee | 737 | status = retval; |
saloutos | 0:894b603d32ee | 738 | return status; |
saloutos | 0:894b603d32ee | 739 | } |
saloutos | 0:894b603d32ee | 740 | |
saloutos | 0:894b603d32ee | 741 | if (minimal) // Return now if minimal is true |
saloutos | 0:894b603d32ee | 742 | { |
saloutos | 0:894b603d32ee | 743 | if (_printDebug) { |
saloutos | 0:894b603d32ee | 744 | _debugSerial->printf("ICM_20948::startupDefault: minimal startup complete!"); |
saloutos | 0:894b603d32ee | 745 | } |
saloutos | 0:894b603d32ee | 746 | return status; |
saloutos | 0:894b603d32ee | 747 | } |
saloutos | 0:894b603d32ee | 748 | |
saloutos | 0:894b603d32ee | 749 | retval = setSampleMode((ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), ICM_20948_Sample_Mode_Continuous); // options: ICM_20948_Sample_Mode_Continuous or ICM_20948_Sample_Mode_Cycled |
saloutos | 0:894b603d32ee | 750 | if (retval != ICM_20948_Stat_Ok) |
saloutos | 0:894b603d32ee | 751 | { |
saloutos | 0:894b603d32ee | 752 | if (_printDebug) { |
saloutos | 0:894b603d32ee | 753 | _debugSerial->printf("ICM_20948::startupDefault: setSampleMode returned: "); |
saloutos | 0:894b603d32ee | 754 | debugPrintStatus(retval); |
saloutos | 0:894b603d32ee | 755 | } |
saloutos | 0:894b603d32ee | 756 | status = retval; |
saloutos | 0:894b603d32ee | 757 | return status; |
saloutos | 0:894b603d32ee | 758 | } // sensors: ICM_20948_Internal_Acc, ICM_20948_Internal_Gyr, ICM_20948_Internal_Mst |
saloutos | 0:894b603d32ee | 759 | |
saloutos | 0:894b603d32ee | 760 | ICM_20948_fss_t FSS; |
saloutos | 0:894b603d32ee | 761 | FSS.a = gpm2; // (ICM_20948_ACCEL_CONFIG_FS_SEL_e) |
saloutos | 0:894b603d32ee | 762 | FSS.g = dps250; // (ICM_20948_GYRO_CONFIG_1_FS_SEL_e) |
saloutos | 0:894b603d32ee | 763 | retval = setFullScale((ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), FSS); |
saloutos | 0:894b603d32ee | 764 | if (retval != ICM_20948_Stat_Ok) |
saloutos | 0:894b603d32ee | 765 | { |
saloutos | 0:894b603d32ee | 766 | if (_printDebug) { |
saloutos | 0:894b603d32ee | 767 | _debugSerial->printf("ICM_20948::startupDefault: setFullScale returned: "); |
saloutos | 0:894b603d32ee | 768 | debugPrintStatus(retval); |
saloutos | 0:894b603d32ee | 769 | } |
saloutos | 0:894b603d32ee | 770 | status = retval; |
saloutos | 0:894b603d32ee | 771 | return status; |
saloutos | 0:894b603d32ee | 772 | } |
saloutos | 0:894b603d32ee | 773 | |
saloutos | 0:894b603d32ee | 774 | ICM_20948_dlpcfg_t dlpcfg; |
saloutos | 0:894b603d32ee | 775 | dlpcfg.a = acc_d473bw_n499bw; |
saloutos | 0:894b603d32ee | 776 | dlpcfg.g = gyr_d361bw4_n376bw5; |
saloutos | 0:894b603d32ee | 777 | retval = setDLPFcfg((ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), dlpcfg); |
saloutos | 0:894b603d32ee | 778 | if (retval != ICM_20948_Stat_Ok) |
saloutos | 0:894b603d32ee | 779 | { |
saloutos | 0:894b603d32ee | 780 | if (_printDebug) { |
saloutos | 0:894b603d32ee | 781 | _debugSerial->printf("ICM_20948::startupDefault: setDLPFcfg returned: "); |
saloutos | 0:894b603d32ee | 782 | debugPrintStatus(retval); |
saloutos | 0:894b603d32ee | 783 | } |
saloutos | 0:894b603d32ee | 784 | status = retval; |
saloutos | 0:894b603d32ee | 785 | return status; |
saloutos | 0:894b603d32ee | 786 | } |
saloutos | 0:894b603d32ee | 787 | |
saloutos | 0:894b603d32ee | 788 | retval = enableDLPF(ICM_20948_Internal_Acc, false); |
saloutos | 0:894b603d32ee | 789 | if (retval != ICM_20948_Stat_Ok) |
saloutos | 0:894b603d32ee | 790 | { |
saloutos | 0:894b603d32ee | 791 | if (_printDebug) { |
saloutos | 0:894b603d32ee | 792 | _debugSerial->printf("ICM_20948::startupDefault: enableDLPF (Acc) returned: "); |
saloutos | 0:894b603d32ee | 793 | debugPrintStatus(retval); |
saloutos | 0:894b603d32ee | 794 | } |
saloutos | 0:894b603d32ee | 795 | status = retval; |
saloutos | 0:894b603d32ee | 796 | return status; |
saloutos | 0:894b603d32ee | 797 | } |
saloutos | 0:894b603d32ee | 798 | |
saloutos | 0:894b603d32ee | 799 | retval = enableDLPF(ICM_20948_Internal_Gyr, false); |
saloutos | 0:894b603d32ee | 800 | if (retval != ICM_20948_Stat_Ok) |
saloutos | 0:894b603d32ee | 801 | { |
saloutos | 0:894b603d32ee | 802 | if (_printDebug) { |
saloutos | 0:894b603d32ee | 803 | _debugSerial->printf("ICM_20948::startupDefault: enableDLPF (Gyr) returned: "); |
saloutos | 0:894b603d32ee | 804 | debugPrintStatus(retval); |
saloutos | 0:894b603d32ee | 805 | } |
saloutos | 0:894b603d32ee | 806 | status = retval; |
saloutos | 0:894b603d32ee | 807 | return status; |
saloutos | 0:894b603d32ee | 808 | } |
saloutos | 0:894b603d32ee | 809 | |
saloutos | 0:894b603d32ee | 810 | return status; |
saloutos | 0:894b603d32ee | 811 | } |
saloutos | 0:894b603d32ee | 812 | |
saloutos | 0:894b603d32ee | 813 | // direct read/write |
saloutos | 0:894b603d32ee | 814 | ICM_20948_Status_e ICM_20948::read(uint8_t reg, uint8_t *pdata, uint32_t len) |
saloutos | 0:894b603d32ee | 815 | { |
saloutos | 0:894b603d32ee | 816 | status = ICM_20948_execute_r(&_device, reg, pdata, len); |
saloutos | 0:894b603d32ee | 817 | return (status); |
saloutos | 0:894b603d32ee | 818 | } |
saloutos | 0:894b603d32ee | 819 | |
saloutos | 0:894b603d32ee | 820 | ICM_20948_Status_e ICM_20948::write(uint8_t reg, uint8_t *pdata, uint32_t len) |
saloutos | 0:894b603d32ee | 821 | { |
saloutos | 0:894b603d32ee | 822 | status = ICM_20948_execute_w(&_device, reg, pdata, len); |
saloutos | 0:894b603d32ee | 823 | return (status); |
saloutos | 0:894b603d32ee | 824 | } |
saloutos | 0:894b603d32ee | 825 | |
saloutos | 0:894b603d32ee | 826 | uint8_t ICM_20948::readMag(AK09916_Reg_Addr_e reg) |
saloutos | 0:894b603d32ee | 827 | { |
saloutos | 0:894b603d32ee | 828 | uint8_t data = i2cMasterSingleR(MAG_AK09916_I2C_ADDR, reg); // i2cMasterSingleR updates status too |
saloutos | 0:894b603d32ee | 829 | return data; |
saloutos | 0:894b603d32ee | 830 | } |
saloutos | 0:894b603d32ee | 831 | |
saloutos | 0:894b603d32ee | 832 | ICM_20948_Status_e ICM_20948::writeMag(AK09916_Reg_Addr_e reg, uint8_t *pdata) |
saloutos | 0:894b603d32ee | 833 | { |
saloutos | 0:894b603d32ee | 834 | status = i2cMasterSingleW(MAG_AK09916_I2C_ADDR, reg, *pdata); |
saloutos | 0:894b603d32ee | 835 | return status; |
saloutos | 0:894b603d32ee | 836 | } |
saloutos | 0:894b603d32ee | 837 | |
saloutos | 0:894b603d32ee | 838 | ICM_20948_Status_e ICM_20948::resetMag() |
saloutos | 0:894b603d32ee | 839 | { |
saloutos | 0:894b603d32ee | 840 | uint8_t SRST = 1; |
saloutos | 0:894b603d32ee | 841 | // SRST: Soft reset |
saloutos | 0:894b603d32ee | 842 | // “0”: Normal |
saloutos | 0:894b603d32ee | 843 | // “1”: Reset |
saloutos | 0:894b603d32ee | 844 | // When “1” is set, all registers are initialized. After reset, SRST bit turns to “0” automatically. |
saloutos | 0:894b603d32ee | 845 | status = i2cMasterSingleW(MAG_AK09916_I2C_ADDR, AK09916_REG_CNTL3, SRST); |
saloutos | 0:894b603d32ee | 846 | return status; |
saloutos | 0:894b603d32ee | 847 | } |
saloutos | 0:894b603d32ee | 848 | |
saloutos | 0:894b603d32ee | 849 | // FIFO |
saloutos | 0:894b603d32ee | 850 | |
saloutos | 0:894b603d32ee | 851 | ICM_20948_Status_e ICM_20948::enableFIFO(bool enable) |
saloutos | 0:894b603d32ee | 852 | { |
saloutos | 0:894b603d32ee | 853 | status = ICM_20948_enable_FIFO(&_device, enable); |
saloutos | 0:894b603d32ee | 854 | return status; |
saloutos | 0:894b603d32ee | 855 | } |
saloutos | 0:894b603d32ee | 856 | |
saloutos | 0:894b603d32ee | 857 | ICM_20948_Status_e ICM_20948::resetFIFO(void) |
saloutos | 0:894b603d32ee | 858 | { |
saloutos | 0:894b603d32ee | 859 | status = ICM_20948_reset_FIFO(&_device); |
saloutos | 0:894b603d32ee | 860 | return status; |
saloutos | 0:894b603d32ee | 861 | } |
saloutos | 0:894b603d32ee | 862 | |
saloutos | 0:894b603d32ee | 863 | ICM_20948_Status_e ICM_20948::setFIFOmode(bool snapshot) |
saloutos | 0:894b603d32ee | 864 | { |
saloutos | 0:894b603d32ee | 865 | // Default to Stream (non-Snapshot) mode |
saloutos | 0:894b603d32ee | 866 | status = ICM_20948_set_FIFO_mode(&_device, snapshot); |
saloutos | 0:894b603d32ee | 867 | return status; |
saloutos | 0:894b603d32ee | 868 | } |
saloutos | 0:894b603d32ee | 869 | |
saloutos | 0:894b603d32ee | 870 | ICM_20948_Status_e ICM_20948::getFIFOcount(uint16_t *count) |
saloutos | 0:894b603d32ee | 871 | { |
saloutos | 0:894b603d32ee | 872 | status = ICM_20948_get_FIFO_count(&_device, count); |
saloutos | 0:894b603d32ee | 873 | return status; |
saloutos | 0:894b603d32ee | 874 | } |
saloutos | 0:894b603d32ee | 875 | |
saloutos | 0:894b603d32ee | 876 | ICM_20948_Status_e ICM_20948::readFIFO(uint8_t *data, uint8_t len) |
saloutos | 0:894b603d32ee | 877 | { |
saloutos | 0:894b603d32ee | 878 | status = ICM_20948_read_FIFO(&_device, data, len); |
saloutos | 0:894b603d32ee | 879 | return status; |
saloutos | 0:894b603d32ee | 880 | } |
saloutos | 0:894b603d32ee | 881 | |
saloutos | 0:894b603d32ee | 882 | // DMP |
saloutos | 0:894b603d32ee | 883 | |
saloutos | 0:894b603d32ee | 884 | ICM_20948_Status_e ICM_20948::enableDMP(bool enable) |
saloutos | 0:894b603d32ee | 885 | { |
saloutos | 0:894b603d32ee | 886 | if (_device._dmp_firmware_available == true) // Should we attempt to enable the DMP? |
saloutos | 0:894b603d32ee | 887 | { |
saloutos | 0:894b603d32ee | 888 | status = ICM_20948_enable_DMP(&_device, enable == true ? 1 : 0); |
saloutos | 0:894b603d32ee | 889 | return status; |
saloutos | 0:894b603d32ee | 890 | } |
saloutos | 0:894b603d32ee | 891 | return ICM_20948_Stat_DMPNotSupported; |
saloutos | 0:894b603d32ee | 892 | } |
saloutos | 0:894b603d32ee | 893 | |
saloutos | 0:894b603d32ee | 894 | ICM_20948_Status_e ICM_20948::resetDMP(void) |
saloutos | 0:894b603d32ee | 895 | { |
saloutos | 0:894b603d32ee | 896 | status = ICM_20948_reset_DMP(&_device); |
saloutos | 0:894b603d32ee | 897 | return status; |
saloutos | 0:894b603d32ee | 898 | } |
saloutos | 0:894b603d32ee | 899 | |
saloutos | 0:894b603d32ee | 900 | ICM_20948_Status_e ICM_20948::loadDMPFirmware(void) |
saloutos | 0:894b603d32ee | 901 | { |
saloutos | 0:894b603d32ee | 902 | if (_device._dmp_firmware_available == true) // Should we attempt to load the DMP firmware? |
saloutos | 0:894b603d32ee | 903 | { |
saloutos | 0:894b603d32ee | 904 | status = ICM_20948_firmware_load(&_device); |
saloutos | 0:894b603d32ee | 905 | return status; |
saloutos | 0:894b603d32ee | 906 | } |
saloutos | 0:894b603d32ee | 907 | return ICM_20948_Stat_DMPNotSupported; |
saloutos | 0:894b603d32ee | 908 | } |
saloutos | 0:894b603d32ee | 909 | |
saloutos | 0:894b603d32ee | 910 | ICM_20948_Status_e ICM_20948::setDMPstartAddress(unsigned short address) |
saloutos | 0:894b603d32ee | 911 | { |
saloutos | 0:894b603d32ee | 912 | if (_device._dmp_firmware_available == true) // Should we attempt to set the start address? |
saloutos | 0:894b603d32ee | 913 | { |
saloutos | 0:894b603d32ee | 914 | status = ICM_20948_set_dmp_start_address(&_device, address); |
saloutos | 0:894b603d32ee | 915 | return status; |
saloutos | 0:894b603d32ee | 916 | } |
saloutos | 0:894b603d32ee | 917 | return ICM_20948_Stat_DMPNotSupported; |
saloutos | 0:894b603d32ee | 918 | } |
saloutos | 0:894b603d32ee | 919 | |
saloutos | 0:894b603d32ee | 920 | ICM_20948_Status_e ICM_20948::enableDMPSensor(enum inv_icm20948_sensor sensor, bool enable) |
saloutos | 0:894b603d32ee | 921 | { |
saloutos | 0:894b603d32ee | 922 | if (_device._dmp_firmware_available == true) // Should we attempt to enable the sensor? |
saloutos | 0:894b603d32ee | 923 | { |
saloutos | 0:894b603d32ee | 924 | status = inv_icm20948_enable_dmp_sensor(&_device, sensor, enable == true ? 1 : 0); |
saloutos | 0:894b603d32ee | 925 | if (_printDebug) { |
saloutos | 0:894b603d32ee | 926 | _debugSerial->printf("ICM_20948::enableDMPSensor: _enabled_Android_0: %d", (int)_device._enabled_Android_0); |
saloutos | 0:894b603d32ee | 927 | _debugSerial->printf(" _enabled_Android_1: %d", (int)_device._enabled_Android_1); |
saloutos | 0:894b603d32ee | 928 | _debugSerial->printf(" _dataOutCtl1: %d", (int)_device._dataOutCtl1); |
saloutos | 0:894b603d32ee | 929 | _debugSerial->printf(" _dataOutCtl2: %d", (int)_device._dataOutCtl2); |
saloutos | 0:894b603d32ee | 930 | _debugSerial->printf(" _dataRdyStatus: %d\n\r", (int)_device._dataRdyStatus); |
saloutos | 0:894b603d32ee | 931 | } |
saloutos | 0:894b603d32ee | 932 | return status; |
saloutos | 0:894b603d32ee | 933 | } |
saloutos | 0:894b603d32ee | 934 | return ICM_20948_Stat_DMPNotSupported; |
saloutos | 0:894b603d32ee | 935 | } |
saloutos | 0:894b603d32ee | 936 | |
saloutos | 0:894b603d32ee | 937 | ICM_20948_Status_e ICM_20948::enableDMPSensorInt(enum inv_icm20948_sensor sensor, bool enable) |
saloutos | 0:894b603d32ee | 938 | { |
saloutos | 0:894b603d32ee | 939 | if (_device._dmp_firmware_available == true) // Should we attempt to enable the sensor interrupt? |
saloutos | 0:894b603d32ee | 940 | { |
saloutos | 0:894b603d32ee | 941 | status = inv_icm20948_enable_dmp_sensor_int(&_device, sensor, enable == true ? 1 : 0); |
saloutos | 0:894b603d32ee | 942 | if (_printDebug) { |
saloutos | 0:894b603d32ee | 943 | _debugSerial->printf("ICM_20948::enableDMPSensorInt: _enabled_Android_intr_0: %d", (int)_device._enabled_Android_intr_0); |
saloutos | 0:894b603d32ee | 944 | _debugSerial->printf(" _enabled_Android_intr_1: %d", (int)_device._enabled_Android_intr_1); |
saloutos | 0:894b603d32ee | 945 | _debugSerial->printf(" _dataIntrCtl: %d\n\r", (int)_device._dataIntrCtl); |
saloutos | 0:894b603d32ee | 946 | } |
saloutos | 0:894b603d32ee | 947 | return status; |
saloutos | 0:894b603d32ee | 948 | } |
saloutos | 0:894b603d32ee | 949 | return ICM_20948_Stat_DMPNotSupported; |
saloutos | 0:894b603d32ee | 950 | } |
saloutos | 0:894b603d32ee | 951 | |
saloutos | 0:894b603d32ee | 952 | ICM_20948_Status_e ICM_20948::writeDMPmems(unsigned short reg, unsigned int length, const unsigned char *data) |
saloutos | 0:894b603d32ee | 953 | { |
saloutos | 0:894b603d32ee | 954 | if (_device._dmp_firmware_available == true) // Should we attempt to write to the DMP? |
saloutos | 0:894b603d32ee | 955 | { |
saloutos | 0:894b603d32ee | 956 | status = inv_icm20948_write_mems(&_device, reg, length, data); |
saloutos | 0:894b603d32ee | 957 | return status; |
saloutos | 0:894b603d32ee | 958 | } |
saloutos | 0:894b603d32ee | 959 | return ICM_20948_Stat_DMPNotSupported; |
saloutos | 0:894b603d32ee | 960 | } |
saloutos | 0:894b603d32ee | 961 | |
saloutos | 0:894b603d32ee | 962 | ICM_20948_Status_e ICM_20948::readDMPmems(unsigned short reg, unsigned int length, unsigned char *data) |
saloutos | 0:894b603d32ee | 963 | { |
saloutos | 0:894b603d32ee | 964 | if (_device._dmp_firmware_available == true) // Should we attempt to read from the DMP? |
saloutos | 0:894b603d32ee | 965 | { |
saloutos | 0:894b603d32ee | 966 | status = inv_icm20948_read_mems(&_device, reg, length, data); |
saloutos | 0:894b603d32ee | 967 | return status; |
saloutos | 0:894b603d32ee | 968 | } |
saloutos | 0:894b603d32ee | 969 | return ICM_20948_Stat_DMPNotSupported; |
saloutos | 0:894b603d32ee | 970 | } |
saloutos | 0:894b603d32ee | 971 | |
saloutos | 0:894b603d32ee | 972 | ICM_20948_Status_e ICM_20948::setDMPODRrate(enum DMP_ODR_Registers odr_reg, int interval) |
saloutos | 0:894b603d32ee | 973 | { |
saloutos | 0:894b603d32ee | 974 | if (_device._dmp_firmware_available == true) // Should we attempt to set the DMP ODR? |
saloutos | 0:894b603d32ee | 975 | { |
saloutos | 0:894b603d32ee | 976 | // In order to set an ODR for a given sensor data, write 2-byte value to DMP using key defined above for a particular sensor. |
saloutos | 0:894b603d32ee | 977 | // Setting value can be calculated as follows: |
saloutos | 0:894b603d32ee | 978 | // Value = (DMP running rate (225Hz) / ODR ) - 1 |
saloutos | 0:894b603d32ee | 979 | // E.g. For a 25Hz ODR rate, value= (225/25) - 1 = 8. |
saloutos | 0:894b603d32ee | 980 | |
saloutos | 0:894b603d32ee | 981 | status = inv_icm20948_set_dmp_sensor_period(&_device, odr_reg, interval); |
saloutos | 0:894b603d32ee | 982 | return status; |
saloutos | 0:894b603d32ee | 983 | } |
saloutos | 0:894b603d32ee | 984 | return ICM_20948_Stat_DMPNotSupported; |
saloutos | 0:894b603d32ee | 985 | } |
saloutos | 0:894b603d32ee | 986 | |
saloutos | 0:894b603d32ee | 987 | ICM_20948_Status_e ICM_20948::readDMPdataFromFIFO(icm_20948_DMP_data_t *data) |
saloutos | 0:894b603d32ee | 988 | { |
saloutos | 0:894b603d32ee | 989 | if (_device._dmp_firmware_available == true) // Should we attempt to set the data from the FIFO? |
saloutos | 0:894b603d32ee | 990 | { |
saloutos | 0:894b603d32ee | 991 | status = inv_icm20948_read_dmp_data(&_device, data); |
saloutos | 0:894b603d32ee | 992 | return status; |
saloutos | 0:894b603d32ee | 993 | } |
saloutos | 0:894b603d32ee | 994 | return ICM_20948_Stat_DMPNotSupported; |
saloutos | 0:894b603d32ee | 995 | } |
saloutos | 0:894b603d32ee | 996 | |
saloutos | 0:894b603d32ee | 997 | ICM_20948_Status_e ICM_20948::setGyroSF(unsigned char div, int gyro_level) |
saloutos | 0:894b603d32ee | 998 | { |
saloutos | 0:894b603d32ee | 999 | if (_device._dmp_firmware_available == true) // Should we attempt to set the Gyro SF? |
saloutos | 0:894b603d32ee | 1000 | { |
saloutos | 0:894b603d32ee | 1001 | status = inv_icm20948_set_gyro_sf(&_device, div, gyro_level); |
saloutos | 0:894b603d32ee | 1002 | if (_printDebug) { |
saloutos | 0:894b603d32ee | 1003 | _debugSerial->printf("ICM_20948::setGyroSF: pll: %d", (int)_device._gyroSFpll); |
saloutos | 0:894b603d32ee | 1004 | _debugSerial->printf(" Gyro SF is: %d\n\r", (int)_device._gyroSF); |
saloutos | 0:894b603d32ee | 1005 | } |
saloutos | 0:894b603d32ee | 1006 | return status; |
saloutos | 0:894b603d32ee | 1007 | } |
saloutos | 0:894b603d32ee | 1008 | return ICM_20948_Stat_DMPNotSupported; |
saloutos | 0:894b603d32ee | 1009 | } |
saloutos | 0:894b603d32ee | 1010 | |
saloutos | 0:894b603d32ee | 1011 | // Combine all of the DMP start-up code from the earlier DMP examples |
saloutos | 0:894b603d32ee | 1012 | // This function is defined as __attribute__((weak)) so you can overwrite it if you want to, |
saloutos | 0:894b603d32ee | 1013 | // e.g. to modify the sample rate |
saloutos | 0:894b603d32ee | 1014 | ICM_20948_Status_e ICM_20948::initializeDMP(void) |
saloutos | 0:894b603d32ee | 1015 | { |
saloutos | 0:894b603d32ee | 1016 | // First, let's check if the DMP is available |
saloutos | 0:894b603d32ee | 1017 | if (_device._dmp_firmware_available != true) |
saloutos | 0:894b603d32ee | 1018 | { |
saloutos | 0:894b603d32ee | 1019 | if (_printDebug) { |
saloutos | 0:894b603d32ee | 1020 | _debugSerial->printf("ICM_20948::startupDMP: DMP is not available. Please check that you have uncommented line 29 (#define ICM_20948_USE_DMP) in ICM_20948_C.h...\n\r"); |
saloutos | 0:894b603d32ee | 1021 | } |
saloutos | 0:894b603d32ee | 1022 | return ICM_20948_Stat_DMPNotSupported; |
saloutos | 0:894b603d32ee | 1023 | } |
saloutos | 0:894b603d32ee | 1024 | |
saloutos | 0:894b603d32ee | 1025 | ICM_20948_Status_e worstResult = ICM_20948_Stat_Ok; |
saloutos | 0:894b603d32ee | 1026 | |
saloutos | 0:894b603d32ee | 1027 | #if defined(ICM_20948_USE_DMP) |
saloutos | 0:894b603d32ee | 1028 | |
saloutos | 0:894b603d32ee | 1029 | // The ICM-20948 is awake and ready but hasn't been configured. Let's step through the configuration |
saloutos | 0:894b603d32ee | 1030 | // sequence from InvenSense's _confidential_ Application Note "Programming Sequence for DMP Hardware Functions". |
saloutos | 0:894b603d32ee | 1031 | |
saloutos | 0:894b603d32ee | 1032 | ICM_20948_Status_e result = ICM_20948_Stat_Ok; // Use result and worstResult to show if the configuration was successful |
saloutos | 0:894b603d32ee | 1033 | |
saloutos | 0:894b603d32ee | 1034 | // Normally, when the DMP is not enabled, startupMagnetometer (called by startupDefault, which is called by begin) configures the AK09916 magnetometer |
saloutos | 0:894b603d32ee | 1035 | // to run at 100Hz by setting the CNTL2 register (0x31) to 0x08. Then the ICM20948's I2C_SLV0 is configured to read |
saloutos | 0:894b603d32ee | 1036 | // nine bytes from the mag every sample, starting from the STATUS1 register (0x10). ST1 includes the DRDY (Data Ready) bit. |
saloutos | 0:894b603d32ee | 1037 | // Next are the six magnetometer readings (little endian). After a dummy byte, the STATUS2 register (0x18) contains the HOFL (Overflow) bit. |
saloutos | 0:894b603d32ee | 1038 | // |
saloutos | 0:894b603d32ee | 1039 | // But looking very closely at the InvenSense example code, we can see in inv_icm20948_resume_akm (in Icm20948AuxCompassAkm.c) that, |
saloutos | 0:894b603d32ee | 1040 | // when the DMP is running, the magnetometer is set to Single Measurement (SM) mode and that ten bytes are read, starting from the reserved |
saloutos | 0:894b603d32ee | 1041 | // RSV2 register (0x03). The datasheet does not define what registers 0x04 to 0x0C contain. There is definitely some secret sauce in here... |
saloutos | 0:894b603d32ee | 1042 | // The magnetometer data appears to be big endian (not little endian like the HX/Y/Z registers) and starts at register 0x04. |
saloutos | 0:894b603d32ee | 1043 | // We had to examine the I2C traffic between the master and the AK09916 on the AUX_DA and AUX_CL pins to discover this... |
saloutos | 0:894b603d32ee | 1044 | // |
saloutos | 0:894b603d32ee | 1045 | // So, we need to set up I2C_SLV0 to do the ten byte reading. The parameters passed to i2cControllerConfigurePeripheral are: |
saloutos | 0:894b603d32ee | 1046 | // 0: use I2C_SLV0 |
saloutos | 0:894b603d32ee | 1047 | // MAG_AK09916_I2C_ADDR: the I2C address of the AK09916 magnetometer (0x0C unshifted) |
saloutos | 0:894b603d32ee | 1048 | // AK09916_REG_RSV2: we start reading here (0x03). Secret sauce... |
saloutos | 0:894b603d32ee | 1049 | // 10: we read 10 bytes each cycle |
saloutos | 0:894b603d32ee | 1050 | // true: set the I2C_SLV0_RNW ReadNotWrite bit so we read the 10 bytes (not write them) |
saloutos | 0:894b603d32ee | 1051 | // true: set the I2C_SLV0_CTRL I2C_SLV0_EN bit to enable reading from the peripheral at the sample rate |
saloutos | 0:894b603d32ee | 1052 | // false: clear the I2C_SLV0_CTRL I2C_SLV0_REG_DIS (we want to write the register value) |
saloutos | 0:894b603d32ee | 1053 | // true: set the I2C_SLV0_CTRL I2C_SLV0_GRP bit to show the register pairing starts at byte 1+2 (copied from inv_icm20948_resume_akm) |
saloutos | 0:894b603d32ee | 1054 | // true: set the I2C_SLV0_CTRL I2C_SLV0_BYTE_SW to byte-swap the data from the mag (copied from inv_icm20948_resume_akm) |
saloutos | 0:894b603d32ee | 1055 | result = i2cControllerConfigurePeripheral(0, MAG_AK09916_I2C_ADDR, AK09916_REG_RSV2, 10, true, true, false, true, true); if (result > worstResult) worstResult = result; |
saloutos | 0:894b603d32ee | 1056 | // |
saloutos | 0:894b603d32ee | 1057 | // We also need to set up I2C_SLV1 to do the Single Measurement triggering: |
saloutos | 0:894b603d32ee | 1058 | // 1: use I2C_SLV1 |
saloutos | 0:894b603d32ee | 1059 | // MAG_AK09916_I2C_ADDR: the I2C address of the AK09916 magnetometer (0x0C unshifted) |
saloutos | 0:894b603d32ee | 1060 | // AK09916_REG_CNTL2: we start writing here (0x31) |
saloutos | 0:894b603d32ee | 1061 | // 1: not sure why, but the write does not happen if this is set to zero |
saloutos | 0:894b603d32ee | 1062 | // false: clear the I2C_SLV0_RNW ReadNotWrite bit so we write the dataOut byte |
saloutos | 0:894b603d32ee | 1063 | // true: set the I2C_SLV0_CTRL I2C_SLV0_EN bit. Not sure why, but the write does not happen if this is clear |
saloutos | 0:894b603d32ee | 1064 | // false: clear the I2C_SLV0_CTRL I2C_SLV0_REG_DIS (we want to write the register value) |
saloutos | 0:894b603d32ee | 1065 | // false: clear the I2C_SLV0_CTRL I2C_SLV0_GRP bit |
saloutos | 0:894b603d32ee | 1066 | // false: clear the I2C_SLV0_CTRL I2C_SLV0_BYTE_SW bit |
saloutos | 0:894b603d32ee | 1067 | // AK09916_mode_single: tell I2C_SLV1 to write the Single Measurement command each sample |
saloutos | 0:894b603d32ee | 1068 | result = i2cControllerConfigurePeripheral(1, MAG_AK09916_I2C_ADDR, AK09916_REG_CNTL2, 1, false, true, false, false, false, AK09916_mode_single); if (result > worstResult) worstResult = result; |
saloutos | 0:894b603d32ee | 1069 | |
saloutos | 0:894b603d32ee | 1070 | // Set the I2C Master ODR configuration |
saloutos | 0:894b603d32ee | 1071 | // It is not clear why we need to do this... But it appears to be essential! From the datasheet: |
saloutos | 0:894b603d32ee | 1072 | // "I2C_MST_ODR_CONFIG[3:0]: ODR configuration for external sensor when gyroscope and accelerometer are disabled. |
saloutos | 0:894b603d32ee | 1073 | // ODR is computed as follows: 1.1 kHz/(2^((odr_config[3:0])) ) |
saloutos | 0:894b603d32ee | 1074 | // When gyroscope is enabled, all sensors (including I2C_MASTER) use the gyroscope ODR. |
saloutos | 0:894b603d32ee | 1075 | // If gyroscope is disabled, then all sensors (including I2C_MASTER) use the accelerometer ODR." |
saloutos | 0:894b603d32ee | 1076 | // Since both gyro and accel are running, setting this register should have no effect. But it does. Maybe because the Gyro and Accel are placed in Low Power Mode (cycled)? |
saloutos | 0:894b603d32ee | 1077 | // You can see by monitoring the Aux I2C pins that the next three lines reduce the bus traffic (magnetometer reads) from 1125Hz to the chosen rate: 68.75Hz in this case. |
saloutos | 0:894b603d32ee | 1078 | result = setBank(3); if (result > worstResult) worstResult = result; // Select Bank 3 |
saloutos | 0:894b603d32ee | 1079 | uint8_t mstODRconfig = 0x04; // Set the ODR configuration to 1100/2^4 = 68.75Hz |
saloutos | 0:894b603d32ee | 1080 | result = write(AGB3_REG_I2C_MST_ODR_CONFIG, &mstODRconfig, 1); if (result > worstResult) worstResult = result; // Write one byte to the I2C_MST_ODR_CONFIG register |
saloutos | 0:894b603d32ee | 1081 | |
saloutos | 0:894b603d32ee | 1082 | // Configure clock source through PWR_MGMT_1 |
saloutos | 0:894b603d32ee | 1083 | // ICM_20948_Clock_Auto selects the best available clock source – PLL if ready, else use the Internal oscillator |
saloutos | 0:894b603d32ee | 1084 | result = setClockSource(ICM_20948_Clock_Auto); if (result > worstResult) worstResult = result; // This is shorthand: success will be set to false if setClockSource fails |
saloutos | 0:894b603d32ee | 1085 | |
saloutos | 0:894b603d32ee | 1086 | // Enable accel and gyro sensors through PWR_MGMT_2 |
saloutos | 0:894b603d32ee | 1087 | // Enable Accelerometer (all axes) and Gyroscope (all axes) by writing zero to PWR_MGMT_2 |
saloutos | 0:894b603d32ee | 1088 | result = setBank(0); if (result > worstResult) worstResult = result; // Select Bank 0 |
saloutos | 0:894b603d32ee | 1089 | uint8_t pwrMgmt2 = 0x40; // Set the reserved bit 6 (pressure sensor disable?) |
saloutos | 0:894b603d32ee | 1090 | result = write(AGB0_REG_PWR_MGMT_2, &pwrMgmt2, 1); if (result > worstResult) worstResult = result; // Write one byte to the PWR_MGMT_2 register |
saloutos | 0:894b603d32ee | 1091 | |
saloutos | 0:894b603d32ee | 1092 | // Place _only_ I2C_Master in Low Power Mode (cycled) via LP_CONFIG |
saloutos | 0:894b603d32ee | 1093 | // The InvenSense Nucleo example initially puts the accel and gyro into low power mode too, but then later updates LP_CONFIG so only the I2C_Master is in Low Power Mode |
saloutos | 0:894b603d32ee | 1094 | result = setSampleMode(ICM_20948_Internal_Mst, ICM_20948_Sample_Mode_Cycled); if (result > worstResult) worstResult = result; |
saloutos | 0:894b603d32ee | 1095 | |
saloutos | 0:894b603d32ee | 1096 | // Disable the FIFO |
saloutos | 0:894b603d32ee | 1097 | result = enableFIFO(false); if (result > worstResult) worstResult = result; |
saloutos | 0:894b603d32ee | 1098 | |
saloutos | 0:894b603d32ee | 1099 | // Disable the DMP |
saloutos | 0:894b603d32ee | 1100 | result = enableDMP(false); if (result > worstResult) worstResult = result; |
saloutos | 0:894b603d32ee | 1101 | |
saloutos | 0:894b603d32ee | 1102 | // Set Gyro FSR (Full scale range) to 2000dps through GYRO_CONFIG_1 |
saloutos | 0:894b603d32ee | 1103 | // Set Accel FSR (Full scale range) to 4g through ACCEL_CONFIG |
saloutos | 0:894b603d32ee | 1104 | ICM_20948_fss_t myFSS; // This uses a "Full Scale Settings" structure that can contain values for all configurable sensors |
saloutos | 0:894b603d32ee | 1105 | myFSS.a = gpm4; // (ICM_20948_ACCEL_CONFIG_FS_SEL_e) |
saloutos | 0:894b603d32ee | 1106 | // gpm2 |
saloutos | 0:894b603d32ee | 1107 | // gpm4 |
saloutos | 0:894b603d32ee | 1108 | // gpm8 |
saloutos | 0:894b603d32ee | 1109 | // gpm16 |
saloutos | 0:894b603d32ee | 1110 | myFSS.g = dps2000; // (ICM_20948_GYRO_CONFIG_1_FS_SEL_e) |
saloutos | 0:894b603d32ee | 1111 | // dps250 |
saloutos | 0:894b603d32ee | 1112 | // dps500 |
saloutos | 0:894b603d32ee | 1113 | // dps1000 |
saloutos | 0:894b603d32ee | 1114 | // dps2000 |
saloutos | 0:894b603d32ee | 1115 | result = setFullScale((ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), myFSS); if (result > worstResult) worstResult = result; |
saloutos | 0:894b603d32ee | 1116 | |
saloutos | 0:894b603d32ee | 1117 | // The InvenSense Nucleo code also enables the gyro DLPF (but leaves GYRO_DLPFCFG set to zero = 196.6Hz (3dB)) |
saloutos | 0:894b603d32ee | 1118 | // We found this by going through the SPI data generated by ZaneL's Teensy-ICM-20948 library byte by byte... |
saloutos | 0:894b603d32ee | 1119 | // The gyro DLPF is enabled by default (GYRO_CONFIG_1 = 0x01) so the following line should have no effect, but we'll include it anyway |
saloutos | 0:894b603d32ee | 1120 | result = enableDLPF(ICM_20948_Internal_Gyr, true); if (result > worstResult) worstResult = result; |
saloutos | 0:894b603d32ee | 1121 | |
saloutos | 0:894b603d32ee | 1122 | // Enable interrupt for FIFO overflow from FIFOs through INT_ENABLE_2 |
saloutos | 0:894b603d32ee | 1123 | // If we see this interrupt, we'll need to reset the FIFO |
saloutos | 0:894b603d32ee | 1124 | //result = intEnableOverflowFIFO( 0x1F ); if (result > worstResult) worstResult = result; // Enable the interrupt on all FIFOs |
saloutos | 0:894b603d32ee | 1125 | |
saloutos | 0:894b603d32ee | 1126 | // Turn off what goes into the FIFO through FIFO_EN_1, FIFO_EN_2 |
saloutos | 0:894b603d32ee | 1127 | // Stop the peripheral data from being written to the FIFO by writing zero to FIFO_EN_1 |
saloutos | 0:894b603d32ee | 1128 | result = setBank(0); if (result > worstResult) worstResult = result; // Select Bank 0 |
saloutos | 0:894b603d32ee | 1129 | uint8_t zero = 0; |
saloutos | 0:894b603d32ee | 1130 | result = write(AGB0_REG_FIFO_EN_1, &zero, 1); if (result > worstResult) worstResult = result; |
saloutos | 0:894b603d32ee | 1131 | // Stop the accelerometer, gyro and temperature data from being written to the FIFO by writing zero to FIFO_EN_2 |
saloutos | 0:894b603d32ee | 1132 | result = write(AGB0_REG_FIFO_EN_2, &zero, 1); if (result > worstResult) worstResult = result; |
saloutos | 0:894b603d32ee | 1133 | |
saloutos | 0:894b603d32ee | 1134 | // Turn off data ready interrupt through INT_ENABLE_1 |
saloutos | 0:894b603d32ee | 1135 | result = intEnableRawDataReady(false); if (result > worstResult) worstResult = result; |
saloutos | 0:894b603d32ee | 1136 | |
saloutos | 0:894b603d32ee | 1137 | // Reset FIFO through FIFO_RST |
saloutos | 0:894b603d32ee | 1138 | result = resetFIFO(); if (result > worstResult) worstResult = result; |
saloutos | 0:894b603d32ee | 1139 | |
saloutos | 0:894b603d32ee | 1140 | // Set gyro sample rate divider with GYRO_SMPLRT_DIV |
saloutos | 0:894b603d32ee | 1141 | // Set accel sample rate divider with ACCEL_SMPLRT_DIV_2 |
saloutos | 0:894b603d32ee | 1142 | ICM_20948_smplrt_t mySmplrt; |
saloutos | 0:894b603d32ee | 1143 | mySmplrt.g = 19; // ODR is computed as follows: 1.1 kHz/(1+GYRO_SMPLRT_DIV[7:0]). 19 = 55Hz. InvenSense Nucleo example uses 19 (0x13). |
saloutos | 0:894b603d32ee | 1144 | mySmplrt.a = 19; // ODR is computed as follows: 1.125 kHz/(1+ACCEL_SMPLRT_DIV[11:0]). 19 = 56.25Hz. InvenSense Nucleo example uses 19 (0x13). |
saloutos | 0:894b603d32ee | 1145 | //mySmplrt.g = 4; // 225Hz |
saloutos | 0:894b603d32ee | 1146 | //mySmplrt.a = 4; // 225Hz |
saloutos | 0:894b603d32ee | 1147 | //mySmplrt.g = 8; // 112Hz |
saloutos | 0:894b603d32ee | 1148 | //mySmplrt.a = 8; // 112Hz |
saloutos | 0:894b603d32ee | 1149 | result = setSampleRate((ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), mySmplrt); if (result > worstResult) worstResult = result; |
saloutos | 0:894b603d32ee | 1150 | |
saloutos | 0:894b603d32ee | 1151 | // Setup DMP start address through PRGM_STRT_ADDRH/PRGM_STRT_ADDRL |
saloutos | 0:894b603d32ee | 1152 | result = setDMPstartAddress(); if (result > worstResult) worstResult = result; // Defaults to DMP_START_ADDRESS |
saloutos | 0:894b603d32ee | 1153 | |
saloutos | 0:894b603d32ee | 1154 | // Now load the DMP firmware |
saloutos | 0:894b603d32ee | 1155 | result = loadDMPFirmware(); if (result > worstResult) worstResult = result; |
saloutos | 0:894b603d32ee | 1156 | |
saloutos | 0:894b603d32ee | 1157 | // Write the 2 byte Firmware Start Value to ICM PRGM_STRT_ADDRH/PRGM_STRT_ADDRL |
saloutos | 0:894b603d32ee | 1158 | result = setDMPstartAddress(); if (result > worstResult) worstResult = result; // Defaults to DMP_START_ADDRESS |
saloutos | 0:894b603d32ee | 1159 | |
saloutos | 0:894b603d32ee | 1160 | // Set the Hardware Fix Disable register to 0x48 |
saloutos | 0:894b603d32ee | 1161 | result = setBank(0); if (result > worstResult) worstResult = result; // Select Bank 0 |
saloutos | 0:894b603d32ee | 1162 | uint8_t fix = 0x48; |
saloutos | 0:894b603d32ee | 1163 | result = write(AGB0_REG_HW_FIX_DISABLE, &fix, 1); if (result > worstResult) worstResult = result; |
saloutos | 0:894b603d32ee | 1164 | |
saloutos | 0:894b603d32ee | 1165 | // Set the Single FIFO Priority Select register to 0xE4 |
saloutos | 0:894b603d32ee | 1166 | result = setBank(0); if (result > worstResult) worstResult = result; // Select Bank 0 |
saloutos | 0:894b603d32ee | 1167 | uint8_t fifoPrio = 0xE4; |
saloutos | 0:894b603d32ee | 1168 | result = write(AGB0_REG_SINGLE_FIFO_PRIORITY_SEL, &fifoPrio, 1); if (result > worstResult) worstResult = result; |
saloutos | 0:894b603d32ee | 1169 | |
saloutos | 0:894b603d32ee | 1170 | // Configure Accel scaling to DMP |
saloutos | 0:894b603d32ee | 1171 | // The DMP scales accel raw data internally to align 1g as 2^25 |
saloutos | 0:894b603d32ee | 1172 | // In order to align internal accel raw data 2^25 = 1g write 0x04000000 when FSR is 4g |
saloutos | 0:894b603d32ee | 1173 | const unsigned char accScale[4] = {0x04, 0x00, 0x00, 0x00}; |
saloutos | 0:894b603d32ee | 1174 | result = writeDMPmems(ACC_SCALE, 4, &accScale[0]); if (result > worstResult) worstResult = result; // Write accScale to ACC_SCALE DMP register |
saloutos | 0:894b603d32ee | 1175 | // In order to output hardware unit data as configured FSR write 0x00040000 when FSR is 4g |
saloutos | 0:894b603d32ee | 1176 | const unsigned char accScale2[4] = {0x00, 0x04, 0x00, 0x00}; |
saloutos | 0:894b603d32ee | 1177 | result = writeDMPmems(ACC_SCALE2, 4, &accScale2[0]); if (result > worstResult) worstResult = result; // Write accScale2 to ACC_SCALE2 DMP register |
saloutos | 0:894b603d32ee | 1178 | |
saloutos | 0:894b603d32ee | 1179 | // Configure Compass mount matrix and scale to DMP |
saloutos | 0:894b603d32ee | 1180 | // The mount matrix write to DMP register is used to align the compass axes with accel/gyro. |
saloutos | 0:894b603d32ee | 1181 | // This mechanism is also used to convert hardware unit to uT. The value is expressed as 1uT = 2^30. |
saloutos | 0:894b603d32ee | 1182 | // Each compass axis will be converted as below: |
saloutos | 0:894b603d32ee | 1183 | // X = raw_x * CPASS_MTX_00 + raw_y * CPASS_MTX_01 + raw_z * CPASS_MTX_02 |
saloutos | 0:894b603d32ee | 1184 | // Y = raw_x * CPASS_MTX_10 + raw_y * CPASS_MTX_11 + raw_z * CPASS_MTX_12 |
saloutos | 0:894b603d32ee | 1185 | // Z = raw_x * CPASS_MTX_20 + raw_y * CPASS_MTX_21 + raw_z * CPASS_MTX_22 |
saloutos | 0:894b603d32ee | 1186 | // The AK09916 produces a 16-bit signed output in the range +/-32752 corresponding to +/-4912uT. 1uT = 6.66 ADU. |
saloutos | 0:894b603d32ee | 1187 | // 2^30 / 6.66666 = 161061273 = 0x9999999 |
saloutos | 0:894b603d32ee | 1188 | const unsigned char mountMultiplierZero[4] = {0x00, 0x00, 0x00, 0x00}; |
saloutos | 0:894b603d32ee | 1189 | const unsigned char mountMultiplierPlus[4] = {0x09, 0x99, 0x99, 0x99}; // Value taken from InvenSense Nucleo example |
saloutos | 0:894b603d32ee | 1190 | const unsigned char mountMultiplierMinus[4] = {0xF6, 0x66, 0x66, 0x67}; // Value taken from InvenSense Nucleo example |
saloutos | 0:894b603d32ee | 1191 | result = writeDMPmems(CPASS_MTX_00, 4, &mountMultiplierPlus[0]); if (result > worstResult) worstResult = result; |
saloutos | 0:894b603d32ee | 1192 | result = writeDMPmems(CPASS_MTX_01, 4, &mountMultiplierZero[0]); if (result > worstResult) worstResult = result; |
saloutos | 0:894b603d32ee | 1193 | result = writeDMPmems(CPASS_MTX_02, 4, &mountMultiplierZero[0]); if (result > worstResult) worstResult = result; |
saloutos | 0:894b603d32ee | 1194 | result = writeDMPmems(CPASS_MTX_10, 4, &mountMultiplierZero[0]); if (result > worstResult) worstResult = result; |
saloutos | 0:894b603d32ee | 1195 | result = writeDMPmems(CPASS_MTX_11, 4, &mountMultiplierMinus[0]); if (result > worstResult) worstResult = result; |
saloutos | 0:894b603d32ee | 1196 | result = writeDMPmems(CPASS_MTX_12, 4, &mountMultiplierZero[0]); if (result > worstResult) worstResult = result; |
saloutos | 0:894b603d32ee | 1197 | result = writeDMPmems(CPASS_MTX_20, 4, &mountMultiplierZero[0]); if (result > worstResult) worstResult = result; |
saloutos | 0:894b603d32ee | 1198 | result = writeDMPmems(CPASS_MTX_21, 4, &mountMultiplierZero[0]); if (result > worstResult) worstResult = result; |
saloutos | 0:894b603d32ee | 1199 | result = writeDMPmems(CPASS_MTX_22, 4, &mountMultiplierMinus[0]); if (result > worstResult) worstResult = result; |
saloutos | 0:894b603d32ee | 1200 | |
saloutos | 0:894b603d32ee | 1201 | // Configure the B2S Mounting Matrix |
saloutos | 0:894b603d32ee | 1202 | const unsigned char b2sMountMultiplierZero[4] = {0x00, 0x00, 0x00, 0x00}; |
saloutos | 0:894b603d32ee | 1203 | const unsigned char b2sMountMultiplierPlus[4] = {0x40, 0x00, 0x00, 0x00}; // Value taken from InvenSense Nucleo example |
saloutos | 0:894b603d32ee | 1204 | result = writeDMPmems(B2S_MTX_00, 4, &b2sMountMultiplierPlus[0]); if (result > worstResult) worstResult = result; |
saloutos | 0:894b603d32ee | 1205 | result = writeDMPmems(B2S_MTX_01, 4, &b2sMountMultiplierZero[0]); if (result > worstResult) worstResult = result; |
saloutos | 0:894b603d32ee | 1206 | result = writeDMPmems(B2S_MTX_02, 4, &b2sMountMultiplierZero[0]); if (result > worstResult) worstResult = result; |
saloutos | 0:894b603d32ee | 1207 | result = writeDMPmems(B2S_MTX_10, 4, &b2sMountMultiplierZero[0]); if (result > worstResult) worstResult = result; |
saloutos | 0:894b603d32ee | 1208 | result = writeDMPmems(B2S_MTX_11, 4, &b2sMountMultiplierPlus[0]); if (result > worstResult) worstResult = result; |
saloutos | 0:894b603d32ee | 1209 | result = writeDMPmems(B2S_MTX_12, 4, &b2sMountMultiplierZero[0]); if (result > worstResult) worstResult = result; |
saloutos | 0:894b603d32ee | 1210 | result = writeDMPmems(B2S_MTX_20, 4, &b2sMountMultiplierZero[0]); if (result > worstResult) worstResult = result; |
saloutos | 0:894b603d32ee | 1211 | result = writeDMPmems(B2S_MTX_21, 4, &b2sMountMultiplierZero[0]); if (result > worstResult) worstResult = result; |
saloutos | 0:894b603d32ee | 1212 | result = writeDMPmems(B2S_MTX_22, 4, &b2sMountMultiplierPlus[0]); if (result > worstResult) worstResult = result; |
saloutos | 0:894b603d32ee | 1213 | |
saloutos | 0:894b603d32ee | 1214 | // Configure the DMP Gyro Scaling Factor |
saloutos | 0:894b603d32ee | 1215 | // @param[in] gyro_div Value written to GYRO_SMPLRT_DIV register, where |
saloutos | 0:894b603d32ee | 1216 | // 0=1125Hz sample rate, 1=562.5Hz sample rate, ... 4=225Hz sample rate, ... |
saloutos | 0:894b603d32ee | 1217 | // 10=102.2727Hz sample rate, ... etc. |
saloutos | 0:894b603d32ee | 1218 | // @param[in] gyro_level 0=250 dps, 1=500 dps, 2=1000 dps, 3=2000 dps |
saloutos | 0:894b603d32ee | 1219 | result = setGyroSF(19, 3); if (result > worstResult) worstResult = result; // 19 = 55Hz (see above), 3 = 2000dps (see above) |
saloutos | 0:894b603d32ee | 1220 | |
saloutos | 0:894b603d32ee | 1221 | // Configure the Gyro full scale |
saloutos | 0:894b603d32ee | 1222 | // 2000dps : 2^28 |
saloutos | 0:894b603d32ee | 1223 | // 1000dps : 2^27 |
saloutos | 0:894b603d32ee | 1224 | // 500dps : 2^26 |
saloutos | 0:894b603d32ee | 1225 | // 250dps : 2^25 |
saloutos | 0:894b603d32ee | 1226 | const unsigned char gyroFullScale[4] = {0x10, 0x00, 0x00, 0x00}; // 2000dps : 2^28 |
saloutos | 0:894b603d32ee | 1227 | result = writeDMPmems(GYRO_FULLSCALE, 4, &gyroFullScale[0]); if (result > worstResult) worstResult = result; |
saloutos | 0:894b603d32ee | 1228 | |
saloutos | 0:894b603d32ee | 1229 | // Configure the Accel Only Gain: 15252014 (225Hz) 30504029 (112Hz) 61117001 (56Hz) |
saloutos | 0:894b603d32ee | 1230 | const unsigned char accelOnlyGain[4] = {0x03, 0xA4, 0x92, 0x49}; // 56Hz |
saloutos | 0:894b603d32ee | 1231 | //const unsigned char accelOnlyGain[4] = {0x00, 0xE8, 0xBA, 0x2E}; // 225Hz |
saloutos | 0:894b603d32ee | 1232 | //const unsigned char accelOnlyGain[4] = {0x01, 0xD1, 0x74, 0x5D}; // 112Hz |
saloutos | 0:894b603d32ee | 1233 | result = writeDMPmems(ACCEL_ONLY_GAIN, 4, &accelOnlyGain[0]); if (result > worstResult) worstResult = result; |
saloutos | 0:894b603d32ee | 1234 | |
saloutos | 0:894b603d32ee | 1235 | // Configure the Accel Alpha Var: 1026019965 (225Hz) 977872018 (112Hz) 882002213 (56Hz) |
saloutos | 0:894b603d32ee | 1236 | const unsigned char accelAlphaVar[4] = {0x34, 0x92, 0x49, 0x25}; // 56Hz |
saloutos | 0:894b603d32ee | 1237 | //const unsigned char accelAlphaVar[4] = {0x3D, 0x27, 0xD2, 0x7D}; // 225Hz |
saloutos | 0:894b603d32ee | 1238 | //const unsigned char accelAlphaVar[4] = {0x3A, 0x49, 0x24, 0x92}; // 112Hz |
saloutos | 0:894b603d32ee | 1239 | result = writeDMPmems(ACCEL_ALPHA_VAR, 4, &accelAlphaVar[0]); if (result > worstResult) worstResult = result; |
saloutos | 0:894b603d32ee | 1240 | |
saloutos | 0:894b603d32ee | 1241 | // Configure the Accel A Var: 47721859 (225Hz) 95869806 (112Hz) 191739611 (56Hz) |
saloutos | 0:894b603d32ee | 1242 | const unsigned char accelAVar[4] = {0x0B, 0x6D, 0xB6, 0xDB}; // 56Hz |
saloutos | 0:894b603d32ee | 1243 | //const unsigned char accelAVar[4] = {0x02, 0xD8, 0x2D, 0x83}; // 225Hz |
saloutos | 0:894b603d32ee | 1244 | //const unsigned char accelAVar[4] = {0x05, 0xB6, 0xDB, 0x6E}; // 112Hz |
saloutos | 0:894b603d32ee | 1245 | result = writeDMPmems(ACCEL_A_VAR, 4, &accelAVar[0]); if (result > worstResult) worstResult = result; |
saloutos | 0:894b603d32ee | 1246 | |
saloutos | 0:894b603d32ee | 1247 | // Configure the Accel Cal Rate |
saloutos | 0:894b603d32ee | 1248 | const unsigned char accelCalRate[4] = {0x00, 0x00}; // Value taken from InvenSense Nucleo example |
saloutos | 0:894b603d32ee | 1249 | result = writeDMPmems(ACCEL_CAL_RATE, 2, &accelCalRate[0]); if (result > worstResult) worstResult = result; |
saloutos | 0:894b603d32ee | 1250 | |
saloutos | 0:894b603d32ee | 1251 | // Configure the Compass Time Buffer. The I2C Master ODR Configuration (see above) sets the magnetometer read rate to 68.75Hz. |
saloutos | 0:894b603d32ee | 1252 | // Let's set the Compass Time Buffer to 69 (Hz). |
saloutos | 0:894b603d32ee | 1253 | const unsigned char compassRate[2] = {0x00, 0x45}; // 69Hz |
saloutos | 0:894b603d32ee | 1254 | result = writeDMPmems(CPASS_TIME_BUFFER, 2, &compassRate[0]); if (result > worstResult) worstResult = result; |
saloutos | 0:894b603d32ee | 1255 | |
saloutos | 0:894b603d32ee | 1256 | // Enable DMP interrupt |
saloutos | 0:894b603d32ee | 1257 | // This would be the most efficient way of getting the DMP data, instead of polling the FIFO |
saloutos | 0:894b603d32ee | 1258 | //result = intEnableDMP(true); if (result > worstResult) worstResult = result; |
saloutos | 0:894b603d32ee | 1259 | |
saloutos | 0:894b603d32ee | 1260 | #endif |
saloutos | 0:894b603d32ee | 1261 | |
saloutos | 0:894b603d32ee | 1262 | return worstResult; |
saloutos | 0:894b603d32ee | 1263 | } |
saloutos | 0:894b603d32ee | 1264 | |
saloutos | 0:894b603d32ee | 1265 | ICM_20948_Status_e ICM_20948::startupMagnetometer(bool minimal) |
saloutos | 0:894b603d32ee | 1266 | { |
saloutos | 0:894b603d32ee | 1267 | ICM_20948_Status_e retval = ICM_20948_Stat_Ok; |
saloutos | 0:894b603d32ee | 1268 | |
saloutos | 0:894b603d32ee | 1269 | i2cMasterPassthrough(false); //Do not connect the SDA/SCL pins to AUX_DA/AUX_CL |
saloutos | 0:894b603d32ee | 1270 | i2cMasterEnable(true); |
saloutos | 0:894b603d32ee | 1271 | |
saloutos | 0:894b603d32ee | 1272 | resetMag(); |
saloutos | 0:894b603d32ee | 1273 | |
saloutos | 0:894b603d32ee | 1274 | //After a ICM reset the Mag sensor may stop responding over the I2C master |
saloutos | 0:894b603d32ee | 1275 | //Reset the Master I2C until it responds |
saloutos | 0:894b603d32ee | 1276 | uint8_t tries = 0; |
saloutos | 0:894b603d32ee | 1277 | while (tries < MAX_MAGNETOMETER_STARTS) |
saloutos | 0:894b603d32ee | 1278 | { |
saloutos | 0:894b603d32ee | 1279 | tries++; |
saloutos | 0:894b603d32ee | 1280 | |
saloutos | 0:894b603d32ee | 1281 | //See if we can read the WhoIAm register correctly |
saloutos | 0:894b603d32ee | 1282 | retval = magWhoIAm(); |
saloutos | 0:894b603d32ee | 1283 | if (retval == ICM_20948_Stat_Ok) |
saloutos | 0:894b603d32ee | 1284 | break; //WIA matched! |
saloutos | 0:894b603d32ee | 1285 | |
saloutos | 0:894b603d32ee | 1286 | i2cMasterReset(); //Otherwise, reset the master I2C and try again |
saloutos | 0:894b603d32ee | 1287 | |
saloutos | 0:894b603d32ee | 1288 | wait_ms(10); |
saloutos | 0:894b603d32ee | 1289 | } |
saloutos | 0:894b603d32ee | 1290 | |
saloutos | 0:894b603d32ee | 1291 | if (tries == MAX_MAGNETOMETER_STARTS) |
saloutos | 0:894b603d32ee | 1292 | { |
saloutos | 0:894b603d32ee | 1293 | if (_printDebug) { |
saloutos | 0:894b603d32ee | 1294 | _debugSerial->printf("ICM_20948::startupMagnetometer: reached MAX_MAGNETOMETER_STARTS (%d). Returning ICM_20948_Stat_WrongID\n\r", (int)MAX_MAGNETOMETER_STARTS); |
saloutos | 0:894b603d32ee | 1295 | } |
saloutos | 0:894b603d32ee | 1296 | status = ICM_20948_Stat_WrongID; |
saloutos | 0:894b603d32ee | 1297 | return status; |
saloutos | 0:894b603d32ee | 1298 | } |
saloutos | 0:894b603d32ee | 1299 | else |
saloutos | 0:894b603d32ee | 1300 | { |
saloutos | 0:894b603d32ee | 1301 | if (_printDebug) { |
saloutos | 0:894b603d32ee | 1302 | _debugSerial->printf("ICM_20948::startupMagnetometer: successful magWhoIAm after %d", (int)tries); |
saloutos | 0:894b603d32ee | 1303 | if (tries == 1) { |
saloutos | 0:894b603d32ee | 1304 | _debugSerial->printf(" try\n\r"); |
saloutos | 0:894b603d32ee | 1305 | } else { |
saloutos | 0:894b603d32ee | 1306 | _debugSerial->printf(" tries\n\r"); |
saloutos | 0:894b603d32ee | 1307 | } |
saloutos | 0:894b603d32ee | 1308 | } |
saloutos | 0:894b603d32ee | 1309 | } |
saloutos | 0:894b603d32ee | 1310 | |
saloutos | 0:894b603d32ee | 1311 | //Return now if minimal is true. The mag will be configured manually for the DMP |
saloutos | 0:894b603d32ee | 1312 | if (minimal) // Return now if minimal is true |
saloutos | 0:894b603d32ee | 1313 | { |
saloutos | 0:894b603d32ee | 1314 | if (_printDebug) { |
saloutos | 0:894b603d32ee | 1315 | _debugSerial->printf("ICM_20948::startupMagnetometer: minimal startup complete!\n\r"); |
saloutos | 0:894b603d32ee | 1316 | } |
saloutos | 0:894b603d32ee | 1317 | return status; |
saloutos | 0:894b603d32ee | 1318 | } |
saloutos | 0:894b603d32ee | 1319 | |
saloutos | 0:894b603d32ee | 1320 | //Set up magnetometer |
saloutos | 0:894b603d32ee | 1321 | AK09916_CNTL2_Reg_t reg; |
saloutos | 0:894b603d32ee | 1322 | reg.MODE = AK09916_mode_cont_100hz; |
saloutos | 0:894b603d32ee | 1323 | reg.reserved_0 = 0; // Make sure the unused bits are clear. Probably redundant, but prevents confusion when looking at the I2C traffic |
saloutos | 0:894b603d32ee | 1324 | retval = writeMag(AK09916_REG_CNTL2, (uint8_t *)®); |
saloutos | 0:894b603d32ee | 1325 | if (retval != ICM_20948_Stat_Ok) |
saloutos | 0:894b603d32ee | 1326 | { |
saloutos | 0:894b603d32ee | 1327 | if (_printDebug) { |
saloutos | 0:894b603d32ee | 1328 | _debugSerial->printf("ICM_20948::startupMagnetometer: writeMag returned: "); |
saloutos | 0:894b603d32ee | 1329 | debugPrintStatus(retval); |
saloutos | 0:894b603d32ee | 1330 | } |
saloutos | 0:894b603d32ee | 1331 | status = retval; |
saloutos | 0:894b603d32ee | 1332 | return status; |
saloutos | 0:894b603d32ee | 1333 | } |
saloutos | 0:894b603d32ee | 1334 | |
saloutos | 0:894b603d32ee | 1335 | retval = i2cControllerConfigurePeripheral(0, MAG_AK09916_I2C_ADDR, AK09916_REG_ST1, 9, true, true, false, false, false); |
saloutos | 0:894b603d32ee | 1336 | if (retval != ICM_20948_Stat_Ok) |
saloutos | 0:894b603d32ee | 1337 | { |
saloutos | 0:894b603d32ee | 1338 | if (_printDebug) { |
saloutos | 0:894b603d32ee | 1339 | _debugSerial->printf("ICM_20948::startupMagnetometer: i2cMasterConfigurePeripheral returned: "); |
saloutos | 0:894b603d32ee | 1340 | debugPrintStatus(retval); |
saloutos | 0:894b603d32ee | 1341 | } |
saloutos | 0:894b603d32ee | 1342 | status = retval; |
saloutos | 0:894b603d32ee | 1343 | return status; |
saloutos | 0:894b603d32ee | 1344 | } |
saloutos | 0:894b603d32ee | 1345 | |
saloutos | 0:894b603d32ee | 1346 | return status; |
saloutos | 0:894b603d32ee | 1347 | } |
saloutos | 0:894b603d32ee | 1348 | |
saloutos | 0:894b603d32ee | 1349 | ICM_20948_Status_e ICM_20948::magWhoIAm(void) |
saloutos | 0:894b603d32ee | 1350 | { |
saloutos | 0:894b603d32ee | 1351 | ICM_20948_Status_e retval = ICM_20948_Stat_Ok; |
saloutos | 0:894b603d32ee | 1352 | |
saloutos | 0:894b603d32ee | 1353 | uint8_t whoiam1, whoiam2; |
saloutos | 0:894b603d32ee | 1354 | whoiam1 = readMag(AK09916_REG_WIA1); |
saloutos | 0:894b603d32ee | 1355 | // readMag calls i2cMasterSingleR which calls ICM_20948_i2c_master_single_r |
saloutos | 0:894b603d32ee | 1356 | // i2cMasterSingleR updates status so it is OK to set retval to status here |
saloutos | 0:894b603d32ee | 1357 | retval = status; |
saloutos | 0:894b603d32ee | 1358 | if (retval != ICM_20948_Stat_Ok) |
saloutos | 0:894b603d32ee | 1359 | { |
saloutos | 0:894b603d32ee | 1360 | if (_printDebug) { |
saloutos | 0:894b603d32ee | 1361 | _debugSerial->printf("ICM_20948::magWhoIAm: whoiam1: %d (should be 72) readMag set status to: ", (int)whoiam1); |
saloutos | 0:894b603d32ee | 1362 | debugPrintStatus(status); |
saloutos | 0:894b603d32ee | 1363 | } |
saloutos | 0:894b603d32ee | 1364 | return retval; |
saloutos | 0:894b603d32ee | 1365 | } |
saloutos | 0:894b603d32ee | 1366 | whoiam2 = readMag(AK09916_REG_WIA2); |
saloutos | 0:894b603d32ee | 1367 | // readMag calls i2cMasterSingleR which calls ICM_20948_i2c_master_single_r |
saloutos | 0:894b603d32ee | 1368 | // i2cMasterSingleR updates status so it is OK to set retval to status here |
saloutos | 0:894b603d32ee | 1369 | retval = status; |
saloutos | 0:894b603d32ee | 1370 | if (retval != ICM_20948_Stat_Ok) |
saloutos | 0:894b603d32ee | 1371 | { |
saloutos | 0:894b603d32ee | 1372 | if (_printDebug) { |
saloutos | 0:894b603d32ee | 1373 | _debugSerial->printf("ICM_20948::magWhoIAm: whoiam1: %d", (int)whoiam1); |
saloutos | 0:894b603d32ee | 1374 | _debugSerial->printf(" (should be 72) whoiam2: %d", (int)whoiam2); |
saloutos | 0:894b603d32ee | 1375 | _debugSerial->printf(" (should be 9) readMag set status to: "); |
saloutos | 0:894b603d32ee | 1376 | debugPrintStatus(status); |
saloutos | 0:894b603d32ee | 1377 | } |
saloutos | 0:894b603d32ee | 1378 | return retval; |
saloutos | 0:894b603d32ee | 1379 | } |
saloutos | 0:894b603d32ee | 1380 | |
saloutos | 0:894b603d32ee | 1381 | if ((whoiam1 == (MAG_AK09916_WHO_AM_I >> 8)) && (whoiam2 == (MAG_AK09916_WHO_AM_I & 0xFF))) |
saloutos | 0:894b603d32ee | 1382 | { |
saloutos | 0:894b603d32ee | 1383 | retval = ICM_20948_Stat_Ok; |
saloutos | 0:894b603d32ee | 1384 | status = retval; |
saloutos | 0:894b603d32ee | 1385 | return status; |
saloutos | 0:894b603d32ee | 1386 | } |
saloutos | 0:894b603d32ee | 1387 | if (_printDebug) { |
saloutos | 0:894b603d32ee | 1388 | _debugSerial->printf("ICM_20948::magWhoIAm: whoiam1: %d", (int)whoiam1); |
saloutos | 0:894b603d32ee | 1389 | _debugSerial->printf(" (should be 72) whoiam2: %d", (int)whoiam2); |
saloutos | 0:894b603d32ee | 1390 | _debugSerial->printf(" (should be 9). Returning ICM_20948_Stat_WrongID\n\r"); |
saloutos | 0:894b603d32ee | 1391 | } |
saloutos | 0:894b603d32ee | 1392 | retval = ICM_20948_Stat_WrongID; |
saloutos | 0:894b603d32ee | 1393 | status = retval; |
saloutos | 0:894b603d32ee | 1394 | return status; |
saloutos | 0:894b603d32ee | 1395 | } |
saloutos | 0:894b603d32ee | 1396 | |
saloutos | 0:894b603d32ee | 1397 | // SPI |
saloutos | 0:894b603d32ee | 1398 | |
saloutos | 0:894b603d32ee | 1399 | // SPISettings ICM_20948_SPI_DEFAULT_SETTINGS(ICM_20948_SPI_DEFAULT_FREQ, ICM_20948_SPI_DEFAULT_ORDER, ICM_20948_SPI_DEFAULT_MODE); |
saloutos | 0:894b603d32ee | 1400 | |
saloutos | 0:894b603d32ee | 1401 | ICM_20948_SPI::ICM_20948_SPI() |
saloutos | 0:894b603d32ee | 1402 | { |
saloutos | 0:894b603d32ee | 1403 | } |
saloutos | 0:894b603d32ee | 1404 | |
saloutos | 0:894b603d32ee | 1405 | //ICM_20948_Status_e begin(DigitalOut &cspin, SPI &spiPort, uint32_t SPIFreq = ICM_20948_SPI_DEFAULT_FREQ); // TODO: check this! |
saloutos | 0:894b603d32ee | 1406 | |
saloutos | 0:894b603d32ee | 1407 | ICM_20948_Status_e ICM_20948_SPI::begin(DigitalOut &csPin, SPI &spiPort, uint32_t SPIFreq) |
saloutos | 0:894b603d32ee | 1408 | { |
saloutos | 0:894b603d32ee | 1409 | if (SPIFreq > 7000000) |
saloutos | 0:894b603d32ee | 1410 | SPIFreq = 7000000; // Limit SPI frequency to 7MHz |
saloutos | 0:894b603d32ee | 1411 | |
saloutos | 0:894b603d32ee | 1412 | // Associate |
saloutos | 0:894b603d32ee | 1413 | _spi = &spiPort; |
saloutos | 0:894b603d32ee | 1414 | _spi->frequency(SPIFreq); // TODO: could also set mode here? |
saloutos | 0:894b603d32ee | 1415 | _cs = &csPin; |
saloutos | 0:894b603d32ee | 1416 | |
saloutos | 0:894b603d32ee | 1417 | // Set pins to default positions |
saloutos | 0:894b603d32ee | 1418 | _cs->write(1); |
saloutos | 0:894b603d32ee | 1419 | |
saloutos | 0:894b603d32ee | 1420 | // 'Kickstart' the SPI hardware. |
saloutos | 0:894b603d32ee | 1421 | _spi->write(0x00); |
saloutos | 0:894b603d32ee | 1422 | |
saloutos | 0:894b603d32ee | 1423 | // Set up the serif |
saloutos | 0:894b603d32ee | 1424 | _serif.write = ICM_20948_write_SPI; |
saloutos | 0:894b603d32ee | 1425 | _serif.read = ICM_20948_read_SPI; |
saloutos | 0:894b603d32ee | 1426 | _serif.user = (void *)this; // refer to yourself in the user field |
saloutos | 0:894b603d32ee | 1427 | |
saloutos | 0:894b603d32ee | 1428 | // Link the serif |
saloutos | 0:894b603d32ee | 1429 | _device._serif = &_serif; |
saloutos | 0:894b603d32ee | 1430 | |
saloutos | 0:894b603d32ee | 1431 | //#if defined(ICM_20948_USE_DMP) |
saloutos | 0:894b603d32ee | 1432 | // _device._dmp_firmware_available = true; // Initialize _dmp_firmware_available |
saloutos | 0:894b603d32ee | 1433 | //#else |
saloutos | 0:894b603d32ee | 1434 | _device._dmp_firmware_available = false; // Initialize _dmp_firmware_available |
saloutos | 0:894b603d32ee | 1435 | //#endif |
saloutos | 0:894b603d32ee | 1436 | |
saloutos | 0:894b603d32ee | 1437 | _device._firmware_loaded = false; // Initialize _firmware_loaded |
saloutos | 0:894b603d32ee | 1438 | _device._last_bank = 255; // Initialize _last_bank. Make it invalid. It will be set by the first call of ICM_20948_set_bank. |
saloutos | 0:894b603d32ee | 1439 | _device._last_mems_bank = 255; // Initialize _last_mems_bank. Make it invalid. It will be set by the first call of inv_icm20948_write_mems. |
saloutos | 0:894b603d32ee | 1440 | _device._gyroSF = 0; // Use this to record the GyroSF, calculated by inv_icm20948_set_gyro_sf |
saloutos | 0:894b603d32ee | 1441 | _device._gyroSFpll = 0; |
saloutos | 0:894b603d32ee | 1442 | _device._enabled_Android_0 = 0; // Keep track of which Android sensors are enabled: 0-31 |
saloutos | 0:894b603d32ee | 1443 | _device._enabled_Android_1 = 0; // Keep track of which Android sensors are enabled: 32- |
saloutos | 0:894b603d32ee | 1444 | _device._enabled_Android_intr_0 = 0; // Keep track of which Android sensor interrupts are enabled: 0-31 |
saloutos | 0:894b603d32ee | 1445 | _device._enabled_Android_intr_1 = 0; // Keep track of which Android sensor interrupts are enabled: 32- |
saloutos | 0:894b603d32ee | 1446 | |
saloutos | 0:894b603d32ee | 1447 | // Perform default startup |
saloutos | 0:894b603d32ee | 1448 | // Do a minimal startupDefault if using the DMP. User can always call startupDefault(false) manually if required. |
saloutos | 0:894b603d32ee | 1449 | status = startupDefault(_device._dmp_firmware_available); |
saloutos | 0:894b603d32ee | 1450 | if (status != ICM_20948_Stat_Ok) |
saloutos | 0:894b603d32ee | 1451 | { |
saloutos | 0:894b603d32ee | 1452 | if (_printDebug) { |
saloutos | 0:894b603d32ee | 1453 | _debugSerial->printf("ICM_20948_SPI::begin: startupDefault returned: "); |
saloutos | 0:894b603d32ee | 1454 | debugPrintStatus(status); |
saloutos | 0:894b603d32ee | 1455 | } |
saloutos | 0:894b603d32ee | 1456 | } |
saloutos | 0:894b603d32ee | 1457 | |
saloutos | 0:894b603d32ee | 1458 | return status; |
saloutos | 0:894b603d32ee | 1459 | } |
saloutos | 0:894b603d32ee | 1460 | |
saloutos | 0:894b603d32ee | 1461 | ICM_20948_Status_e ICM_20948_write_SPI(uint8_t reg, uint8_t *data, uint32_t len, void *user) |
saloutos | 0:894b603d32ee | 1462 | { |
saloutos | 0:894b603d32ee | 1463 | if (user == NULL) |
saloutos | 0:894b603d32ee | 1464 | { |
saloutos | 0:894b603d32ee | 1465 | return ICM_20948_Stat_ParamErr; |
saloutos | 0:894b603d32ee | 1466 | } |
saloutos | 0:894b603d32ee | 1467 | |
saloutos | 0:894b603d32ee | 1468 | // TODO: check this? |
saloutos | 0:894b603d32ee | 1469 | SPI *_spi = ((ICM_20948_SPI *)user)->_spi; // Cast user field to ICM_20948_SPI type and extract the SPI interface pointer |
saloutos | 0:894b603d32ee | 1470 | DigitalOut *_cs = ((ICM_20948_SPI *)user)->_cs; |
saloutos | 0:894b603d32ee | 1471 | if (_spi == NULL) |
saloutos | 0:894b603d32ee | 1472 | { |
saloutos | 0:894b603d32ee | 1473 | return ICM_20948_Stat_ParamErr; |
saloutos | 0:894b603d32ee | 1474 | } |
saloutos | 0:894b603d32ee | 1475 | |
saloutos | 0:894b603d32ee | 1476 | // 'Kickstart' the SPI hardware. This is a fairly high amount of overhead, but it guarantees that the lines will start in the correct states even when sharing the SPI bus with devices that use other modes |
saloutos | 0:894b603d32ee | 1477 | _spi->write(0x00); |
saloutos | 0:894b603d32ee | 1478 | |
saloutos | 0:894b603d32ee | 1479 | _cs->write(0); |
saloutos | 0:894b603d32ee | 1480 | // delayMicroseconds(5); |
saloutos | 0:894b603d32ee | 1481 | _spi->write(((reg & 0x7F) | 0x00)); |
saloutos | 0:894b603d32ee | 1482 | // SPI.transfer(data, len); // Can't do this thanks to Arduino's poor implementation |
saloutos | 0:894b603d32ee | 1483 | for (uint32_t indi = 0; indi < len; indi++) |
saloutos | 0:894b603d32ee | 1484 | { |
saloutos | 0:894b603d32ee | 1485 | _spi->write(*(data + indi)); |
saloutos | 0:894b603d32ee | 1486 | } |
saloutos | 0:894b603d32ee | 1487 | // delayMicroseconds(5); |
saloutos | 0:894b603d32ee | 1488 | _cs->write(1); |
saloutos | 0:894b603d32ee | 1489 | |
saloutos | 0:894b603d32ee | 1490 | return ICM_20948_Stat_Ok; |
saloutos | 0:894b603d32ee | 1491 | } |
saloutos | 0:894b603d32ee | 1492 | |
saloutos | 0:894b603d32ee | 1493 | ICM_20948_Status_e ICM_20948_read_SPI(uint8_t reg, uint8_t *buff, uint32_t len, void *user) |
saloutos | 0:894b603d32ee | 1494 | { |
saloutos | 0:894b603d32ee | 1495 | if (user == NULL) |
saloutos | 0:894b603d32ee | 1496 | { |
saloutos | 0:894b603d32ee | 1497 | return ICM_20948_Stat_ParamErr; |
saloutos | 0:894b603d32ee | 1498 | } |
saloutos | 0:894b603d32ee | 1499 | // TODO: also check this |
saloutos | 0:894b603d32ee | 1500 | SPI *_spi = ((ICM_20948_SPI *)user)->_spi; |
saloutos | 0:894b603d32ee | 1501 | DigitalOut *_cs = ((ICM_20948_SPI *)user)->_cs; |
saloutos | 0:894b603d32ee | 1502 | if (_spi == NULL) |
saloutos | 0:894b603d32ee | 1503 | { |
saloutos | 0:894b603d32ee | 1504 | return ICM_20948_Stat_ParamErr; |
saloutos | 0:894b603d32ee | 1505 | } |
saloutos | 0:894b603d32ee | 1506 | |
saloutos | 0:894b603d32ee | 1507 | // 'Kickstart' the SPI hardware. This is a fairly high amount of overhead, but it guarantees that the lines will start in the correct states |
saloutos | 0:894b603d32ee | 1508 | _spi->write(0x00); |
saloutos | 0:894b603d32ee | 1509 | |
saloutos | 0:894b603d32ee | 1510 | _cs->write(0); |
saloutos | 0:894b603d32ee | 1511 | // delayMicroseconds(5); |
saloutos | 0:894b603d32ee | 1512 | _spi->write(((reg & 0x7F) | 0x80)); |
saloutos | 0:894b603d32ee | 1513 | // SPI.transfer(data, len); // Can't do this thanks to Arduino's stupid implementation |
saloutos | 0:894b603d32ee | 1514 | for (uint32_t indi = 0; indi < len; indi++) |
saloutos | 0:894b603d32ee | 1515 | { |
saloutos | 0:894b603d32ee | 1516 | *(buff + indi) = _spi->write(0x00); |
saloutos | 0:894b603d32ee | 1517 | } |
saloutos | 0:894b603d32ee | 1518 | // delayMicroseconds(5); |
saloutos | 0:894b603d32ee | 1519 | _cs->write(1); |
saloutos | 0:894b603d32ee | 1520 | |
saloutos | 0:894b603d32ee | 1521 | return ICM_20948_Stat_Ok; |
saloutos | 0:894b603d32ee | 1522 | } |