Skip to content

Commit fa7027c

Browse files
finger563finger563
committed
continue fleshing out and add todos for remaining items
1 parent 5781678 commit fa7027c

File tree

2 files changed

+69
-8
lines changed

2 files changed

+69
-8
lines changed

‎components/icm20948/include/icm20948.hpp‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -541,7 +541,8 @@ class Icm20948 : public espp::BasePeripheral<uint8_t, Interface == icm20948::Int
541541
static float accelerometer_range_to_sensitivty(const AccelerometerRange &range);
542542
static float gyroscope_range_to_sensitivty(const GyroscopeRange &range);
543543

544-
bool write_magnetometer(uint8_t reg, uint8_t val, std::error_code &ec);
544+
uint8_t read_from_magnetometer(const Ak09916Register &reg, std::error_code &ec);
545+
bool write_to_magnetometer(const Ak09916Register &reg, uint8_t val, std::error_code &ec);
545546
bool enable_magnetometer_data_read(uint8_t reg_addr, uint8_t num_bytes, std::error_code &ec);
546547

547548
uint16_t get_temperature_raw(std::error_code &ec);

‎components/icm20948/src/icm20948.cpp‎

Lines changed: 67 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -66,6 +66,7 @@ bool Icm20948<I>::set_config(const Icm20948<I>::ImuConfig &imu_config, std::erro
6666
// save the config
6767
imu_config_ = imu_config;
6868

69+
// TODO: implement
6970
// // the config is set as two bytes containing the configuration for the
7071
// // accelerometer and gyroscope (full scale range and output data rate)
7172
// uint8_t data[2] = {// first byte is gyro fs + odr
@@ -120,6 +121,7 @@ bool Icm20948<I>::set_i2c_master_enabled(bool enable, std::error_code &ec) {
120121
// Configuration / Offsets
121122
/////////////////////////////////
122123

124+
// TODO: implement
123125
template <espp::icm20948::Interface I> bool Icm20948<I>::auto_offsets(std::error_code &ec) {
124126
return !ec;
125127
}
@@ -136,12 +138,14 @@ bool Icm20948<I>::set_odr_align_enabled(bool enable, std::error_code &ec) {
136138
return !ec;
137139
}
138140

141+
// TODO: implement
139142
template <espp::icm20948::Interface I>
140143
bool Icm20948<I>::set_accelerometer_offsets(const Range &x, const Range &y, const Range &z,
141144
std::error_code &ec) {
142145
return !ec;
143146
}
144147

148+
// TODO: implement
145149
template <espp::icm20948::Interface I>
146150
bool Icm20948<I>::get_accelerometer_offsets(Range &x, Range &y, Range &z, std::error_code &ec) {
147151
return !ec;
@@ -174,6 +178,7 @@ bool Icm20948<I>::set_gyroscope_offsets(const float &x, const float &y, const fl
174178
return !ec;
175179
}
176180

181+
// TODO: implement
177182
template <espp::icm20948::Interface I>
178183
bool Icm20948<I>::get_gyroscope_offsets(float &x, float &y, float &z, std::error_code &ec) {
179184
return !ec;
@@ -183,6 +188,7 @@ bool Icm20948<I>::get_gyroscope_offsets(float &x, float &y, float &z, std::error
183188
// Power / Sleep / Standby
184189
/////////////////////////////////
185190

191+
// TODO: implement
186192
template <espp::icm20948::Interface I>
187193
bool Icm20948<I>::set_power_mode(const PowerMode &mode, std::error_code &ec) {
188194
return !ec;
@@ -213,11 +219,13 @@ bool Icm20948<I>::set_low_power_duty_cycle_mode(const DutyCycleMode &mode, std::
213219
return !ec;
214220
}
215221

222+
// TODO: implement
216223
// bool Icm20948<I>::set_gyroscope_average_in_low_power_mode(const GyroscopeAverage &average,
217224
// std::error_code &ec) {
218225
// return !ec;
219226
// }
220227

228+
// TODO: implement
221229
// bool Icm20948<I>::set_accelerometer_average_in_low_power_mode(const AccelerometerAverage
222230
// &average,
223231
// std::error_code &ec) {
@@ -329,12 +337,14 @@ bool Icm20948<I>::set_accelerometer_dlpf_enabled(bool enable, std::error_code &e
329337
return !ec;
330338
}
331339

340+
// TODO: implement
332341
template <espp::icm20948::Interface I>
333342
bool Icm20948<I>::set_accelerometer_dlpf(const SensorFilterBandwidth &bandwidth,
334343
std::error_code &ec) {
335344
return !ec;
336345
}
337346

347+
// TODO: implement
338348
template <espp::icm20948::Interface I>
339349
bool Icm20948<I>::set_accelerometer_odr(const AccelerometerODR &odr, std::error_code &ec) {
340350
return !ec;
@@ -422,11 +432,13 @@ bool Icm20948<I>::set_gyroscope_dlpf_enabled(bool enable, std::error_code &ec) {
422432
return !ec;
423433
}
424434

435+
// TODO: implement
425436
template <espp::icm20948::Interface I>
426437
bool Icm20948<I>::set_gyroscope_dlpf(const SensorFilterBandwidth &bandwidth, std::error_code &ec) {
427438
return !ec;
428439
}
429440

441+
// TODO: implement
430442
template <espp::icm20948::Interface I>
431443
bool Icm20948<I>::set_gyroscope_odr(const GyroscopeODR &odr, std::error_code &ec) {
432444
return !ec;
@@ -436,6 +448,7 @@ bool Icm20948<I>::set_gyroscope_odr(const GyroscopeODR &odr, std::error_code &ec
436448
// Temperature
437449
/////////////////////////////////
438450

451+
// TODO: implement
439452
template <espp::icm20948::Interface I>
440453
bool Icm20948<I>::set_temperature_dlpf(const TemperatureFilterBandwidth &bandwidth,
441454
std::error_code &ec) {
@@ -451,20 +464,29 @@ bool Icm20948<I>::set_temperature_dlpf(const TemperatureFilterBandwidth &bandwid
451464
/////////////////////////////////
452465
// Magnetometer
453466
/////////////////////////////////
467+
468+
// TODO: implement
454469
template <espp::icm20948::Interface I> bool Icm20948<I>::init_magnetometer(std::error_code &ec) {
455470
return !ec;
456471
}
457472

458473
template <espp::icm20948::Interface I>
459474
uint16_t Icm20948<I>::get_magnetometer_device_id(std::error_code &ec) {
460-
return 0;
475+
uint8_t msb = read_from_magnetometer(Ak09916Register::WHO_AM_I_1, ec);
476+
if (ec) {
477+
return 0;
478+
}
479+
uint8_t lsb = read_from_magnetometer(Ak09916Register::WHO_AM_I_2, ec);
480+
if (ec) {
481+
return 0;
482+
}
483+
return (msb << 8) | lsb;
461484
}
462485

463486
template <espp::icm20948::Interface I>
464487
bool Icm20948<I>::set_magnetometer_mode(const icm20948::MagnetometerMode &mode,
465488
std::error_code &ec) {
466-
if (!write_magnetometer(static_cast<uint8_t>(Ak09916Register::CONTROL_2),
467-
static_cast<uint8_t>(mode), ec)) {
489+
if (!write_to_magnetometer(Ak09916Register::CONTROL_2, static_cast<uint8_t>(mode), ec)) {
468490
return false;
469491
}
470492
// make sure to enable reading the magnetometer data
@@ -481,7 +503,7 @@ template <espp::icm20948::Interface I> float Icm20948<I>::get_magnetometer_sensi
481503
}
482504

483505
template <espp::icm20948::Interface I> bool Icm20948<I>::reset_magnetometer(std::error_code &ec) {
484-
return !ec;
506+
return write_to_magnetometer(Ak09916Register::CONTROL_3, 0x01, ec);
485507
}
486508

487509
/////////////////////////////////
@@ -513,6 +535,7 @@ Icm20948<I>::Value Icm20948<I>::read_accelerometer(std::error_code &ec) {
513535
static_cast<float>(raw.y) / sensitivity,
514536
static_cast<float>(raw.z) / sensitivity,
515537
};
538+
// update values
516539
accel_values_ = value;
517540
return value;
518541
}
@@ -529,6 +552,7 @@ Icm20948<I>::Value Icm20948<I>::read_gyroscope(std::error_code &ec) {
529552
static_cast<float>(raw.y) / sensitivity,
530553
static_cast<float>(raw.z) / sensitivity,
531554
};
555+
// update values
532556
gyro_values_ = value;
533557
return value;
534558
}
@@ -545,6 +569,7 @@ Icm20948<I>::Value Icm20948<I>::read_magnetometer(std::error_code &ec) {
545569
static_cast<float>(raw.y) / sensitivity,
546570
static_cast<float>(raw.z) / sensitivity,
547571
};
572+
// update values
548573
mag_values_ = value;
549574
return value;
550575
}
@@ -554,7 +579,8 @@ template <espp::icm20948::Interface I> float Icm20948<I>::read_temperature(std::
554579
if (ec) {
555580
return 0.0f;
556581
}
557-
float temp = static_cast<float>(raw) / 128.0f + 25.0f; // 132.48 + 25
582+
float temp = static_cast<float>(raw) / 333.87f + 21.0f; /// 333.87 LSB/degC, 21 C offset
583+
// update values
558584
temperature_ = temp;
559585
return temp;
560586
}
@@ -741,7 +767,40 @@ float Icm20948<I>::gyroscope_range_to_sensitivty(const Icm20948<I>::GyroscopeRan
741767
}
742768

743769
template <espp::icm20948::Interface I>
744-
bool Icm20948<I>::write_magnetometer(uint8_t reg, uint8_t val, std::error_code &ec) {
770+
uint8_t Icm20948<I>::read_from_magnetometer(const Icm20948<I>::Ak09916Register &reg,
771+
std::error_code &ec) {
772+
// select bank 3
773+
if (!select_bank(Bank::_3, ec)) {
774+
return false;
775+
}
776+
// write the address of the magnetometer
777+
write_u8_to_register(static_cast<uint8_t>(RegisterBank3::I2C_SLV4_ADDR), AK09916C_ADDRESS | 0x80,
778+
ec);
779+
if (ec) {
780+
return false;
781+
}
782+
write_u8_to_register(static_cast<uint8_t>(RegisterBank3::I2C_SLV4_REG), static_cast<uint8_t>(reg),
783+
ec);
784+
if (ec) {
785+
return false;
786+
}
787+
write_u8_to_register(static_cast<uint8_t>(RegisterBank3::I2C_SLV4_CTRL), I2C_SLVX_EN, ec);
788+
789+
// wait for the write to complete
790+
while (read_u8_from_register(static_cast<uint8_t>(RegisterBank3::I2C_SLV4_CTRL), ec) &
791+
I2C_SLVX_EN) {
792+
if (ec) {
793+
return false;
794+
}
795+
}
796+
797+
// read the data
798+
return read_u8_from_register(static_cast<uint8_t>(RegisterBank3::I2C_SLV4_DI), ec);
799+
}
800+
801+
template <espp::icm20948::Interface I>
802+
bool Icm20948<I>::write_to_magnetometer(const Icm20948<I>::Ak09916Register &reg, uint8_t val,
803+
std::error_code &ec) {
745804
// select bank 3
746805
if (!select_bank(Bank::_3, ec)) {
747806
return false;
@@ -755,7 +814,8 @@ bool Icm20948<I>::write_magnetometer(uint8_t reg, uint8_t val, std::error_code &
755814
if (ec) {
756815
return false;
757816
}
758-
write_u8_to_register(static_cast<uint8_t>(RegisterBank3::I2C_SLV4_REG), reg, ec);
817+
write_u8_to_register(static_cast<uint8_t>(RegisterBank3::I2C_SLV4_REG), static_cast<uint8_t>(reg),
818+
ec);
759819
if (ec) {
760820
return false;
761821
}

0 commit comments

Comments
 (0)