48 #define JEVOIS_DMP_BAC_WEARABLE_EN 0x8000
49 #define JEVOIS_DMP_PEDOMETER_EN 0x4000
50 #define JEVOIS_DMP_PEDOMETER_INT_EN 0x2000
51 #define JEVOIS_DMP_SMD_EN 0x0800
52 #define JEVOIS_DMP_BTS_EN 0x0020
53 #define JEVOIS_DMP_FLIP_PICKUP_EN 0x0010
54 #define JEVOIS_DMP_GEOMAG_EN 0x0008
55 #define JEVOIS_DMP_ACCEL_CAL_EN 0x0200
56 #define JEVOIS_DMP_GYRO_CAL_EN 0x0100
57 #define JEVOIS_DMP_COMPASS_CAL_EN 0x0080
58 #define JEVOIS_DMP_NINE_AXIS_EN 0x0040
59 #define JEVOIS_DMP_BRING_AND_LOOK_T0_SEE_EN 0x0004
71 unsigned char jevois::ICM20948::readMagRegister(
unsigned char magreg)
74 itsIMU->writeRegister(ICM20948_REG_I2C_SLV4_ADDR, ICM20948_BIT_I2C_READ | COMPASS_SLAVEADDR);
75 itsIMU->writeRegister(ICM20948_REG_I2C_SLV4_REG, magreg);
76 itsIMU->writeRegister(ICM20948_REG_I2C_SLV4_CTRL, ICM20948_BIT_I2C_SLV_EN);
80 return itsIMU->readRegister(ICM20948_REG_I2C_SLV4_DI);
84 void jevois::ICM20948::waitForSlave4()
87 auto tooLate = std::chrono::steady_clock::now() + std::chrono::milliseconds(300);
91 std::this_thread::sleep_for(std::chrono::milliseconds(10));
92 unsigned char status = itsIMU->readRegister(ICM20948_REG_I2C_MST_STATUS);
93 if (status & ICM20948_BIT_SLV4_NACK)
LFATAL(
"Failed to communicate with compass: NACK");
94 if (status & ICM20948_BIT_SLV4_DONE)
return;
96 while (std::chrono::steady_clock::now() < tooLate);
98 LFATAL(
"Failed to communicate with compass: timeout");
102 void jevois::ICM20948::writeMagRegister(
unsigned char magreg,
unsigned char val)
105 itsIMU->writeRegister(ICM20948_REG_I2C_SLV4_ADDR, COMPASS_SLAVEADDR);
106 itsIMU->writeRegister(ICM20948_REG_I2C_SLV4_REG, magreg);
107 itsIMU->writeRegister(ICM20948_REG_I2C_SLV4_DO, val);
108 itsIMU->writeRegister(ICM20948_REG_I2C_SLV4_CTRL, ICM20948_BIT_I2C_SLV_EN);
117 itsIMU = engine()->imu();
118 if (!itsIMU)
LFATAL(
"IMU chip was not detected but is required for this module -- ABORT");
124 if (ready() ==
false)
125 LFATAL(
"Cannot access ICM20948 inertial measurement unit (IMU) chip. This chip is only available on JeVois-A33 "
126 "with a modified Global Shutter OnSemi (Aptina) AR0135 camera sensor. It is not included with standard "
127 "JeVois-A33 cameras. It is included in JeVois-Pro.");
132 itsIMU->writeRegister(ICM20948_REG_I2C_SLV0_CTRL, 0);
133 itsIMU->writeRegister(ICM20948_REG_I2C_SLV1_CTRL, 0);
134 itsIMU->writeRegister(ICM20948_REG_I2C_SLV2_CTRL, 0);
135 itsIMU->writeRegister(ICM20948_REG_I2C_SLV3_CTRL, 0);
136 itsIMU->writeRegister(ICM20948_REG_I2C_SLV4_CTRL, 0);
139 itsIMU->writeRegister(ICM20948_REG_I2C_MST_CTRL, 0x07 | ICM20948_BIT_I2C_MST_P_NSR);
142 unsigned char v = itsIMU->readRegister(ICM20948_REG_USER_CTRL);
143 v |= ICM20948_BIT_I2C_MST_EN;
144 if (itsIMU->isSPI()) v |= ICM20948_BIT_I2C_IF_DIS;
145 itsIMU->writeRegister(ICM20948_REG_USER_CTRL, v);
146 std::this_thread::sleep_for(std::chrono::milliseconds(1));
149 unsigned char wia2 = readMagRegister(REG_AK09916_WIA);
150 if (wia2 != VAL_AK09916_WIA)
LFATAL(
"Cannot communicate with magnetometer");
151 LINFO(
"AK09916 magnetometer ok.");
154 itsIMU->writeRegister(ICM20948_REG_PWR_MGMT_2, itsIMU->readRegister(ICM20948_REG_PWR_MGMT_2) | 0x80);
157 itsIMU->writeRegister(ICM20948_REG_FSYNC_CONFIG, ICM20948_BIT_FSYNC_TIME_EN | ICM20948_BIT_FSYNC_DEGLITCH_EN |
158 ICM20948_BIT_FSYNC_EDGE_EN | ICM20948_BITS_FSYNC_SET_TEMP);
159 itsIMU->writeRegister(ICM20948_REG_INT_PIN_CFG, ICM20948_BIT_INT_FSYNC_EN);
160 itsIMU->writeRegister(ICM20948_REG_INT_ENABLE, ICM20948_BIT_FSYNC_INT_EN);
170 case jevois::imu::Mode::DMP:
174 unsigned char v = itsIMU->readRegister(ICM20948_REG_USER_CTRL);
175 v &= ~(ICM20948_BIT_DMP_EN | ICM20948_BIT_FIFO_EN); v |= ICM20948_BIT_DMP_RST;
176 if (itsIMU->isSPI()) v |= ICM20948_BIT_I2C_IF_DIS;
177 itsIMU->writeRegister(ICM20948_REG_USER_CTRL, v);
178 std::this_thread::sleep_for(std::chrono::milliseconds(1));
181 itsIMU->writeRegister(ICM20948_REG_FIFO_CFG, ICM20948_BIT_MULTI_FIFO_CFG);
182 itsIMU->writeRegister(ICM20948_REG_FIFO_RST, 0x1f);
183 itsIMU->writeRegister(ICM20948_REG_FIFO_RST, 0x1e);
184 itsIMU->writeRegister(ICM20948_REG_FIFO_EN_1, 0x0);
185 itsIMU->writeRegister(ICM20948_REG_FIFO_EN_2, 0x0);
187 itsIMU->writeRegister(ICM20948_REG_SINGLE_FIFO_PRIORITY_SEL, 0xe4);
190 itsIMU->writeRegister(ICM20948_REG_INT_ENABLE,
191 ICM20948_BIT_FSYNC_INT_EN | ICM20948_BIT_DMP_INT_EN);
192 itsIMU->writeRegister(ICM20948_REG_INT_ENABLE_1, ICM20948_BIT_RAW_DATA_0_RDY_EN | ICM20948_BIT_RAW_DATA_1_RDY_EN |
193 ICM20948_BIT_RAW_DATA_2_RDY_EN |
194 ICM20948_BIT_RAW_DATA_3_RDY_EN);
195 itsIMU->writeRegister(ICM20948_REG_INT_ENABLE_2, ICM20948_BIT_FIFO_OVERFLOW_EN_0);
201 v |= ICM20948_BIT_DMP_EN | ICM20948_BIT_FIFO_EN; v &= ~ICM20948_BIT_DMP_RST;
202 if (itsIMU->isSPI()) v |= ICM20948_BIT_I2C_IF_DIS;
203 itsIMU->writeRegister(ICM20948_REG_USER_CTRL, v);
205 LINFO(
"IMU Digital Motion Processor (DMP) enabled.");
209 case jevois::imu::Mode::FIFO:
212 unsigned char v = itsIMU->readRegister(ICM20948_REG_USER_CTRL);
213 v &= ~ICM20948_BIT_DMP_EN; v |= ICM20948_BIT_DMP_RST;
214 if (itsIMU->isSPI()) v |= ICM20948_BIT_I2C_IF_DIS;
215 itsIMU->writeRegister(ICM20948_REG_USER_CTRL, v);
218 computeFIFOpktSize(-1.0
F, -1.0
F, -1);
223 LINFO(
"IMU FIFO mode enabled.");
227 case jevois::imu::Mode::RAW:
230 unsigned char v = itsIMU->readRegister(ICM20948_REG_USER_CTRL);
231 v &= ~(ICM20948_BIT_DMP_EN | ICM20948_BIT_FIFO_EN); v |= ICM20948_BIT_DMP_RST;
232 if (itsIMU->isSPI()) v |= ICM20948_BIT_I2C_IF_DIS;
233 itsIMU->writeRegister(ICM20948_REG_USER_CTRL, v);
239 LINFO(
"IMU raw data mode enabled.");
254 return (itsIMU->readRegister(ICM20948_REG_WHO_AM_I) == ICM20948_DEVICE_ID);
262 case jevois::imu::Mode::FIFO:
263 case jevois::imu::Mode::DMP:
265 unsigned char datalen[2];
266 itsIMU->readRegisterArray(ICM20948_REG_FIFO_COUNT_H, &datalen[0], 2);
267 return (datalen[0] << 8) + datalen[1];
269 case jevois::imu::Mode::RAW:
271 unsigned char val = itsIMU->readRegister(ICM20948_REG_INT_STATUS_1);
272 if (val & ICM20948_BIT_RAW_DATA_0_RDY_INT)
return 1;
287 case jevois::imu::Mode::FIFO:
289 int siz = dataReady();
294 LERROR(
"IMU FIFO filling up. You need to call get() more often or reduce rate. Data will be lost.");
297 unsigned char trash[itsFIFOpktSiz];
298 itsIMU->readRegisterArray(ICM20948_REG_FIFO_R_W, &trash[0], itsFIFOpktSiz);
303 if (siz < itsFIFOpktSiz)
305 if (blocking ==
false)
return d;
307 auto tooLate = std::chrono::steady_clock::now() + std::chrono::seconds(2);
310 std::this_thread::sleep_for(std::chrono::milliseconds(1));
311 if (std::chrono::steady_clock::now() > tooLate)
312 {
LERROR(
"TIMEOUT waiting for IMU FIFO data - RETURNING BLANK");
return d; }
315 while (siz < itsFIFOpktSiz);
319 unsigned char packet[itsFIFOpktSiz];
320 itsIMU->readRegisterArray(ICM20948_REG_FIFO_R_W, &packet[0], itsFIFOpktSiz);
326 d.
ax() = (packet[off + 0] << 8) | packet[off + 1];
327 d.
ay() = (packet[off + 2] << 8) | packet[off + 3];
328 d.
az() = (packet[off + 4] << 8) | packet[off + 5];
333 d.
gx() = (packet[off + 0] << 8) | packet[off + 1];
334 d.
gy() = (packet[off + 2] << 8) | packet[off + 3];
335 d.
gz() = (packet[off + 4] << 8) | packet[off + 5];
340 d.
mx() = (packet[off + 0] << 8) | packet[off + 1];
341 d.
my() = (packet[off + 2] << 8) | packet[off + 3];
342 d.
mz() = (packet[off + 4] << 8) | packet[off + 5];
343 d.
mst2() = (packet[off + 6] << 8) | packet[off + 7];
351 std::stringstream stream; stream <<
"RAW" << std::hex;
352 for (
int i = 0; i < itsFIFOpktSiz; ++i) stream <<
' ' << (
unsigned int)(packet[i]);
359 case jevois::imu::Mode::RAW:
360 case jevois::imu::Mode::DMP:
363 itsIMU->readRegisterArray(ICM20948_REG_ACCEL_XOUT_H_SH,
reinterpret_cast<unsigned char *
>(&d.
v[0]), 11*2);
366 for (
short & s : d.
v) {
short hi = (s & 0xff00) >> 8;
short lo = s & 0x00ff; s = (lo << 8) | hi; }
383 if (
mode::get() != jevois::imu::Mode::DMP)
LFATAL(
"getDMP() only available when mode=DMP, see params.cfg");
391 size_t got = getDMPsome(blocking, 4);
392 if (got < 4)
return d;
396 unsigned short ctl1 = (itsDMPpacket[0] << 8) | itsDMPpacket[1];
398 unsigned short ctl2 = 0;
399 if (ctl1 &
JEVOIS_DMP_HEADER2) { ctl2 = (itsDMPpacket[off] << 8) | itsDMPpacket[off + 1]; off += 2; }
408 size_t need2 = std::min(need,
size_t(32));
409 size_t got = getDMPsome(blocking, need2);
410 if (got < need2)
return d;
420 std::stringstream stream; stream <<
"RAW" << std::hex;
421 for (
int i = 0; i < itsDMPsz; ++i) stream <<
' ' << (
unsigned int)(itsDMPpacket[i]);
432 size_t jevois::ICM20948::getDMPsome(
bool blocking,
size_t desired)
434 size_t siz = dataReady();
438 if (blocking ==
false)
return 0;
440 auto tooLate = std::chrono::steady_clock::now() + std::chrono::seconds(2);
443 std::this_thread::sleep_for(std::chrono::milliseconds(1));
445 if (std::chrono::steady_clock::now() >= tooLate)
446 {
LERROR(
"TIMEOUT waiting for IMU DMP data - RETURNING BLANK");
return 0; }
450 while (siz < desired);
454 itsIMU->readRegisterArray(ICM20948_REG_FIFO_R_W, &itsDMPpacket[itsDMPsz], desired);
465 itsIMU->writeRegister(ICM20948_REG_PWR_MGMT_1, ICM20948_BIT_H_RESET | ICM20948_BIT_CLK_PLL);
468 std::this_thread::sleep_for(std::chrono::milliseconds(30));
472 void jevois::ICM20948::onParamChange(jevois::imu::arate
const & JEVOIS_UNUSED_PARAM(param),
float const & newval)
475 uint8_t pwr = itsIMU->readRegister(ICM20948_REG_PWR_MGMT_2);
476 if (newval == 0.0
F) pwr |= ICM20948_BIT_PWR_ACCEL_STBY;
else pwr &= ~ICM20948_BIT_PWR_ACCEL_STBY;
477 itsIMU->writeRegister(ICM20948_REG_PWR_MGMT_2, pwr);
482 float accelSampleRate = (1125.0F / newval) - 1.0
F;
485 if (accelSampleRate > 4095.0
F) accelSampleRate = 4095.0F;
else if (accelSampleRate < 0.0
F) accelSampleRate = 0.0F;
488 uint16_t
const accelDiv = uint16_t(accelSampleRate);
489 itsIMU->writeRegister(ICM20948_REG_ACCEL_SMPLRT_DIV_1, uint8_t(accelDiv >> 8) );
490 itsIMU->writeRegister(ICM20948_REG_ACCEL_SMPLRT_DIV_2, uint8_t(accelDiv & 0xFF) );
493 LINFO(
"Accelerometer sampling rate set to " << 1125.0
F / (accelDiv + 1.0
F) <<
" Hz");
496 computeFIFOpktSize(newval, -1.0
F, -1);
500 void jevois::ICM20948::onParamChange(jevois::imu::grate
const & JEVOIS_UNUSED_PARAM(param),
float const & newval)
503 uint8_t pwr = itsIMU->readRegister(ICM20948_REG_PWR_MGMT_2);
504 if (newval == 0.0
F) pwr |= ICM20948_BIT_PWR_GYRO_STBY;
else pwr &= ~ICM20948_BIT_PWR_GYRO_STBY;
505 itsIMU->writeRegister(ICM20948_REG_PWR_MGMT_2, pwr);
510 float gyroSampleRate = (1125.0F / newval) - 1.0
F;
513 if (gyroSampleRate > 255.0
F) gyroSampleRate = 255.0F;
else if (gyroSampleRate < 0.0
F) gyroSampleRate = 0.0F;
516 uint8_t
const gyroDiv = uint8_t(gyroSampleRate);
517 itsIMU->writeRegister(ICM20948_REG_GYRO_SMPLRT_DIV, gyroDiv);
520 LINFO(
"Gyroscope sampling rate set to " << 1125.0
F / (gyroDiv + 1.0
F) <<
" Hz");
522 else std::this_thread::sleep_for(std::chrono::microseconds(22));
524 computeFIFOpktSize(-1.0
F, newval, -1);
528 void jevois::ICM20948::onParamChange(jevois::imu::mrate
const & JEVOIS_UNUSED_PARAM(param),
532 writeMagRegister(REG_AK09916_CNTL2, VAL_AK09916_CNTL2_PD);
538 case jevois::imu::MagRate::Off: mod = VAL_AK09916_CNTL2_PD;
break;
539 case jevois::imu::MagRate::Once: mod = VAL_AK09916_CNTL2_SNGL;
break;
540 case jevois::imu::MagRate::M10Hz: mod = VAL_AK09916_CNTL2_MOD1;
break;
541 case jevois::imu::MagRate::M20Hz: mod = VAL_AK09916_CNTL2_MOD2;
break;
542 case jevois::imu::MagRate::M50Hz: mod = VAL_AK09916_CNTL2_MOD3;
break;
543 case jevois::imu::MagRate::M100Hz: mod = VAL_AK09916_CNTL2_MOD4;
break;
544 default:
LFATAL(
"Invalid mode value: " << newval);
548 std::this_thread::sleep_for(std::chrono::milliseconds(10));
551 writeMagRegister(REG_AK09916_CNTL2, mod);
554 itsIMU->writeRegister(ICM20948_REG_I2C_MST_ODR_CONFIG, 0x04);
557 std::this_thread::sleep_for(std::chrono::milliseconds(10));
559 computeFIFOpktSize(-1.0
F, -1.0
F, mod);
562 itsIMU->writeRegister(ICM20948_REG_I2C_SLV0_ADDR, ICM20948_BIT_I2C_READ | COMPASS_SLAVEADDR);
563 itsIMU->writeRegister(ICM20948_REG_I2C_SLV0_REG, REG_AK09916_HXL);
564 int siz = (
mode::get() == jevois::imu::Mode::DMP) ? 6 : 8;
565 itsIMU->writeRegister(ICM20948_REG_I2C_SLV0_CTRL,
566 ICM20948_BIT_I2C_SLV_EN | ICM20948_BIT_I2C_BYTE_SW | ICM20948_BIT_I2C_GRP | siz);
570 void jevois::ICM20948::onParamChange(jevois::imu::abw
const & JEVOIS_UNUSED_PARAM(param),
571 unsigned int const & newval)
573 uint8_t reg = itsIMU->readRegister(ICM20948_REG_ACCEL_CONFIG);
577 case 0: reg &= ~ICM20948_BIT_ACCEL_FCHOICE;
break;
578 case 6: reg &= ~ICM20948_MASK_ACCEL_BW; reg |= ICM20948_ACCEL_BW_6HZ;
break;
579 case 12: reg &= ~ICM20948_MASK_ACCEL_BW; reg |= ICM20948_ACCEL_BW_12HZ;
break;
580 case 24: reg &= ~ICM20948_MASK_ACCEL_BW; reg |= ICM20948_ACCEL_BW_24HZ;
break;
581 case 50: reg &= ~ICM20948_MASK_ACCEL_BW; reg |= ICM20948_ACCEL_BW_50HZ;
break;
582 case 111: reg &= ~ICM20948_MASK_ACCEL_BW; reg |= ICM20948_ACCEL_BW_111HZ;
break;
583 case 246: reg &= ~ICM20948_MASK_ACCEL_BW; reg |= ICM20948_ACCEL_BW_246HZ;
break;
584 case 470: reg &= ~ICM20948_MASK_ACCEL_BW; reg |= ICM20948_ACCEL_BW_470HZ;
break;
585 case 1210: reg &= ~ICM20948_MASK_ACCEL_BW; reg |= ICM20948_ACCEL_BW_1210HZ;
break;
586 default:
LFATAL(
"Invalid value");
589 itsIMU->writeRegister(ICM20948_REG_ACCEL_CONFIG, reg);
593 void jevois::ICM20948::onParamChange(jevois::imu::gbw
const & JEVOIS_UNUSED_PARAM(param),
594 unsigned int const & newval)
596 uint8_t reg = itsIMU->readRegister(ICM20948_REG_GYRO_CONFIG_1);
600 case 0: reg &= ~ICM20948_BIT_GYRO_FCHOICE;
break;
601 case 6: reg &= ~ICM20948_MASK_GYRO_BW; reg |= ICM20948_GYRO_BW_6HZ;
break;
602 case 12: reg &= ~ICM20948_MASK_GYRO_BW; reg |= ICM20948_GYRO_BW_12HZ;
break;
603 case 24: reg &= ~ICM20948_MASK_GYRO_BW; reg |= ICM20948_GYRO_BW_24HZ;
break;
604 case 51: reg &= ~ICM20948_MASK_GYRO_BW; reg |= ICM20948_GYRO_BW_51HZ;
break;
605 case 120: reg &= ~ICM20948_MASK_GYRO_BW; reg |= ICM20948_GYRO_BW_120HZ;
break;
606 case 150: reg &= ~ICM20948_MASK_GYRO_BW; reg |= ICM20948_GYRO_BW_150HZ;
break;
607 case 200: reg &= ~ICM20948_MASK_GYRO_BW; reg |= ICM20948_GYRO_BW_200HZ;
break;
608 case 360: reg &= ~ICM20948_MASK_GYRO_BW; reg |= ICM20948_GYRO_BW_360HZ;
break;
609 case 12100: reg &= ~ICM20948_MASK_GYRO_BW; reg |= ICM20948_GYRO_BW_12100HZ;
break;
610 default:
LFATAL(
"Invalid value");
613 itsIMU->writeRegister(ICM20948_REG_GYRO_CONFIG_1, reg);
617 void jevois::ICM20948::onParamChange(jevois::imu::tbw
const & JEVOIS_UNUSED_PARAM(param),
618 unsigned int const & newval)
621 uint8_t pwr = itsIMU->readRegister(ICM20948_REG_PWR_MGMT_1);
622 if (newval == 0) pwr |= ICM20948_BIT_TEMP_DIS;
else pwr &= ~ICM20948_BIT_TEMP_DIS;
623 itsIMU->writeRegister(ICM20948_REG_PWR_MGMT_1, pwr);
625 if (newval == 0)
return;
629 case 9: itsIMU->writeRegister(ICM20948_REG_TEMP_CONFIG, 6);
break;
630 case 17: itsIMU->writeRegister(ICM20948_REG_TEMP_CONFIG, 5);
break;
631 case 34: itsIMU->writeRegister(ICM20948_REG_TEMP_CONFIG, 4);
break;
632 case 66: itsIMU->writeRegister(ICM20948_REG_TEMP_CONFIG, 3);
break;
633 case 123: itsIMU->writeRegister(ICM20948_REG_TEMP_CONFIG, 2);
break;
634 case 218: itsIMU->writeRegister(ICM20948_REG_TEMP_CONFIG, 1);
break;
635 case 7932: itsIMU->writeRegister(ICM20948_REG_TEMP_CONFIG, 0);
break;
636 default:
LFATAL(
"Invalid value");
641 void jevois::ICM20948::onParamChange(jevois::imu::arange
const & JEVOIS_UNUSED_PARAM(param),
642 unsigned int const & newval)
644 uint8_t reg = itsIMU->readRegister(ICM20948_REG_ACCEL_CONFIG) & ~ICM20948_MASK_ACCEL_FULLSCALE;
648 case 2: reg |= ICM20948_ACCEL_FULLSCALE_2G;
break;
649 case 4: reg |= ICM20948_ACCEL_FULLSCALE_4G;
break;
650 case 8: reg |= ICM20948_ACCEL_FULLSCALE_8G;
break;
651 case 16: reg |= ICM20948_ACCEL_FULLSCALE_16G;
break;
652 default:
LFATAL(
"Invalid value");
654 itsIMU->writeRegister(ICM20948_REG_ACCEL_CONFIG, reg);
658 void jevois::ICM20948::onParamChange(jevois::imu::grange
const & JEVOIS_UNUSED_PARAM(param),
659 unsigned int const & newval)
661 uint8_t reg = itsIMU->readRegister(ICM20948_REG_GYRO_CONFIG_1) & ~ICM20948_MASK_GYRO_FULLSCALE;
665 case 250: reg |= ICM20948_GYRO_FULLSCALE_250DPS;
break;
666 case 500: reg |= ICM20948_GYRO_FULLSCALE_500DPS;
break;
667 case 1000: reg |= ICM20948_GYRO_FULLSCALE_1000DPS;
break;
668 case 2000: reg |= ICM20948_GYRO_FULLSCALE_2000DPS;
break;
669 default:
LFATAL(
"Invalid value");
671 itsIMU->writeRegister(ICM20948_REG_GYRO_CONFIG_1, reg);
677 uint8_t reg = itsIMU->readRegister(ICM20948_REG_PWR_MGMT_1);
679 if (enable) reg |= ICM20948_BIT_SLEEP;
680 else reg &= ~(ICM20948_BIT_SLEEP);
682 reg |= ICM20948_BIT_CLK_PLL;
684 itsIMU->writeRegister(ICM20948_REG_PWR_MGMT_1, reg);
686 std::this_thread::sleep_for(std::chrono::milliseconds(2));
692 uint8_t reg = ICM20948_REG_LP_CONFIG;
693 uint8_t
const mask = ICM20948_BIT_I2C_MST_CYCLE | ICM20948_BIT_ACCEL_CYCLE | ICM20948_BIT_GYRO_CYCLE;
695 if (enable) reg |= mask;
else reg &= ~mask;
697 itsIMU->writeRegister(ICM20948_REG_LP_CONFIG, reg);
702 {
return itsIMU->readRegister(ICM20948_REG_WHO_AM_I); }
705 void jevois::ICM20948::onParamChange(imu::dmp
const & JEVOIS_UNUSED_PARAM(param), std::string
const & newval)
707 unsigned short ctl1 = 0, ctl2 = 0, mec = 0;
708 bool a =
false, g =
false, m =
false, h2 =
false;
710 LINFO(
"Setting dmp parameter to " << newval);
712 for (
unsigned char c : newval)
734 default:
LERROR(
"Phony character '" << c <<
"' ignored while parsing parameter dmp.");
751 itsIMU->writeDMPregister(
DMP_DATA_RDY_STATUS, (a ? 0x02 : 0x00) | (g ? 0x01 : 0x00) | (m ? 0x08 : 0x00));
755 void jevois::ICM20948::computeFIFOpktSize(
float ar,
float gr,
int mm)
760 if (
mode::get() == jevois::imu::Mode::FIFO)
763 float acc;
if (ar < 0.0
F) acc =
arate::get();
else acc = ar;
764 float gyr;
if (gr < 0.0
F) gyr =
grate::get();
else gyr = gr;
768 unsigned char ctl = itsIMU->readRegister(ICM20948_REG_USER_CTRL);
769 ctl &= ~ICM20948_BIT_FIFO_EN;
770 if (itsIMU->isSPI()) ctl |= ICM20948_BIT_I2C_IF_DIS;
771 itsIMU->writeRegister(ICM20948_REG_USER_CTRL, ctl);
773 itsIMU->writeRegister(ICM20948_REG_FIFO_CFG, ICM20948_BIT_SINGLE_FIFO_CFG);
774 itsIMU->writeRegister(ICM20948_REG_FIFO_RST, 0x1f);
775 itsIMU->writeRegister(ICM20948_REG_FIFO_EN_1, 0x0);
776 itsIMU->writeRegister(ICM20948_REG_FIFO_EN_2, 0x0);
778 std::this_thread::sleep_for(std::chrono::milliseconds(100));
780 int siz = dataReady();
unsigned char trash[16];
783 itsIMU->readRegisterArray(ICM20948_REG_FIFO_R_W, &trash[0], std::min(siz, 16));
787 itsIMU->writeRegister(ICM20948_REG_FIFO_RST, 0x00);
789 unsigned char v = itsIMU->readRegister(ICM20948_REG_FIFO_EN_2);
790 if (acc > 0.0
F) { v |= ICM20948_BIT_ACCEL_FIFO_EN; itsFIFOpktSiz += 6; }
791 else v &= ~ICM20948_BIT_ACCEL_FIFO_EN;
792 if (gyr > 0.0
F) { v |= ICM20948_BITS_GYRO_FIFO_EN; itsFIFOpktSiz += 6; }
793 else v &= ~ICM20948_BITS_GYRO_FIFO_EN;
794 itsIMU->writeRegister(ICM20948_REG_FIFO_EN_2, v);
796 v = itsIMU->readRegister(ICM20948_REG_FIFO_EN_1);
797 if (mag != jevois::imu::MagRate::Off && mag != jevois::imu::MagRate::Once)
798 { v |= ICM20948_BIT_SLV_0_FIFO_EN; itsFIFOpktSiz += 8; }
799 else v &= ~ICM20948_BIT_SLV_0_FIFO_EN;
800 itsIMU->writeRegister(ICM20948_REG_FIFO_EN_1, v);
803 ctl |= ICM20948_BIT_FIFO_EN;
804 if (itsIMU->isSPI()) ctl |= ICM20948_BIT_I2C_IF_DIS;
805 itsIMU->writeRegister(ICM20948_REG_USER_CTRL, ctl);