Andrew SaLoutos / ICM_20948

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?

UserRevisionLine numberNew 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, &reg); // 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, &reg, 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, &reg); // 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, &reg, 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, &reg); // 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, &reg, 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, &reg); // 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, &reg, 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, &reg); // 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, &reg, 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, &reg); // 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, &reg, 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 *)&reg);
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 }