Skip to content

Commit 68c55a4

Browse files
finger563finger563
committed
fleshing out some more
1 parent 822d177 commit 68c55a4

File tree

3 files changed

+218
-14
lines changed

3 files changed

+218
-14
lines changed

‎components/base_peripheral/include/base_peripheral.hpp

+22
Original file line numberDiff line numberDiff line change
@@ -636,6 +636,28 @@ class BasePeripheral : public BaseComponent {
636636
write_u8_to_register(register_address, data, ec);
637637
}
638638

639+
/// Set bits in a register on the peripheral by mask
640+
/// \param register_address The address of the register to modify
641+
/// \param mask The mask to use when setting the bits. All bits not in the
642+
/// mask will be unmodified, while all bits within the mask will
643+
/// be set to the value of the corresponding bit in the value
644+
/// \param value The value to set. Bits in the value should correspond to the
645+
/// bits in the mask
646+
/// \param ec The error code to set if there is an error
647+
void set_bits_in_register_by_mask(RegisterAddressType register_address, uint8_t mask,
648+
uint8_t value, std::error_code &ec) {
649+
logger_.debug("set_bits_in_register_by_mask 0x{:x} with mask 0x{:x} and value 0x{:x}",
650+
register_address, mask, value);
651+
std::lock_guard<std::recursive_mutex> lock(base_mutex_);
652+
uint8_t data = read_u8_from_register(register_address, ec);
653+
if (ec) {
654+
return;
655+
}
656+
data &= ~mask;
657+
data |= value & mask;
658+
write_u8_to_register(register_address, data, ec);
659+
}
660+
639661
/// Clear bits in a register on the peripheral
640662
/// \param register_address The address of the register to modify
641663
/// \param mask The mask to clear

‎components/esp-box/src/imu.cpp

+26
Original file line numberDiff line numberDiff line change
@@ -27,6 +27,32 @@ bool EspBox::initialize_imu() {
2727
logger_.info("Initializing IMU with default configuration");
2828
imu_ = std::make_shared<Imu>(config);
2929

30+
// configure the dmp
31+
std::error_code ec;
32+
// turn on DMP
33+
if (!imu_->set_dmp_power_save(false, ec)) {
34+
logger_.error("Failed to set DMP power save mode: {}", ec.message());
35+
return false;
36+
}
37+
if (!imu_->dmp_initialize(ec)) {
38+
logger_.error("Failed to initialize DMP: {}", ec.message());
39+
return false;
40+
}
41+
if (!imu_->set_dmp_odr(icm42607::DmpODR::ODR_25_HZ, ec)) {
42+
logger_.error("Failed to set DMP ODR: {}", ec.message());
43+
return false;
44+
}
45+
// set filters for the accel / gyro
46+
static constexpr auto filter_bw = icm42607::SensorFilterBandwidth::BW_16_HZ;
47+
if (!imu_->set_accelerometer_filter(filter_bw, ec)) {
48+
logger_.error("Failed to set accel filter: {}", ec.message());
49+
return false;
50+
}
51+
if (!imu_->set_gyroscope_filter(filter_bw, ec)) {
52+
logger_.error("Failed to set gyro filter: {}", ec.message());
53+
return false;
54+
}
55+
3056
return true;
3157
}
3258

‎components/icm42607/include/icm42607.hpp

+170-14
Original file line numberDiff line numberDiff line change
@@ -75,6 +75,14 @@ enum class GyroscopeODR : uint8_t {
7575
ODR_12_5_HZ = 12, ///< 12.5 Hz
7676
};
7777

78+
/// Digital Motion Processor (DMP) output data rate
79+
enum class DmpODR : uint8_t {
80+
ODR_25_HZ = 0, ///< 25 Hz
81+
ODR_400_HZ = 1, ///< 200 Hz
82+
ODR_50_HZ = 2, ///< 50 Hz
83+
ODR_100_HZ = 3, ///< 100 Hz
84+
};
85+
7886
/// Temperature DLPF Bandwidth
7987
enum class TemperatureFilterBandwidth : uint8_t {
8088
FILTER_OFF = 0, ///< Filter off
@@ -208,7 +216,10 @@ class Icm42607 : public espp::BasePeripheral<uint8_t, Interface == icm42607::Int
208216
using BasePeripheral<uint8_t, Interface == icm42607::Interface::I2C>::read_u8_from_register;
209217
using BasePeripheral<uint8_t, Interface == icm42607::Interface::I2C>::read_u16_from_register;
210218
using BasePeripheral<uint8_t, Interface == icm42607::Interface::I2C>::read_many_from_register;
219+
using BasePeripheral<uint8_t, Interface == icm42607::Interface::I2C>::clear_bits_in_register;
211220
using BasePeripheral<uint8_t, Interface == icm42607::Interface::I2C>::set_bits_in_register;
221+
using BasePeripheral<uint8_t,
222+
Interface == icm42607::Interface::I2C>::set_bits_in_register_by_mask;
212223
using BasePeripheral<uint8_t, Interface == icm42607::Interface::I2C>::read;
213224
using BasePeripheral<uint8_t, Interface == icm42607::Interface::I2C>::logger_;
214225

@@ -225,14 +236,18 @@ class Icm42607 : public espp::BasePeripheral<uint8_t, Interface == icm42607::Int
225236
using GyroscopeRange = icm42607::GyroscopeRange; ///< Gyroscope range
226237
using GyroscopePowerMode = icm42607::GyroscopePowerMode; ///< Gyroscope power mode
227238
using GyroscopeODR = icm42607::GyroscopeODR; ///< Gyroscope output data rate
228-
using ImuConfig = icm42607::ImuConfig; ///< IMU configuration
229-
using RawValue = icm42607::RawValue; ///< Raw IMU data
230-
using Value = icm42607::Value; ///< IMU data
231-
using ComplimentaryAngle = icm42607::ComplimentaryAngle; ///< Complimentary angle
232-
using InterruptDriveMode = icm42607::InterruptDriveMode; ///< Interrupt drive mode
233-
using InterruptPolarity = icm42607::InterruptPolarity; ///< Interrupt polarity
234-
using InterruptMode = icm42607::InterruptMode; ///< Interrupt mode
235-
using InterruptConfig = icm42607::InterruptConfig; ///< Interrupt configuration
239+
using DmpODR = icm42607::DmpODR; ///< DMP output data rate
240+
using TemperatureFilterBandwidth =
241+
icm42607::TemperatureFilterBandwidth; ///< Temperature filter bandwidth
242+
using SensorFilterBandwidth = icm42607::SensorFilterBandwidth; ///< Sensor filter bandwidth
243+
using ImuConfig = icm42607::ImuConfig; ///< IMU configuration
244+
using RawValue = icm42607::RawValue; ///< Raw IMU data
245+
using Value = icm42607::Value; ///< IMU data
246+
using ComplimentaryAngle = icm42607::ComplimentaryAngle; ///< Complimentary angle
247+
using InterruptDriveMode = icm42607::InterruptDriveMode; ///< Interrupt drive mode
248+
using InterruptPolarity = icm42607::InterruptPolarity; ///< Interrupt polarity
249+
using InterruptMode = icm42607::InterruptMode; ///< Interrupt mode
250+
using InterruptConfig = icm42607::InterruptConfig; ///< Interrupt configuration
236251

237252
/// Configuration struct for the ICM42607
238253
struct Config {
@@ -328,8 +343,9 @@ class Icm42607 : public espp::BasePeripheral<uint8_t, Interface == icm42607::Int
328343
/// @param ec The error code to set if an error occurs
329344
/// @return True if the power mode was set successfully, false otherwise
330345
bool set_accelerometer_power_mode(AccelerometerPowerMode power_mode, std::error_code &ec) {
331-
set_bits_in_register(static_cast<uint8_t>(Register::PWR_MGMT0),
332-
static_cast<uint8_t>(power_mode) & 0x03, ec);
346+
uint8_t bitmask = 0x03;
347+
set_bits_in_register_by_mask(static_cast<uint8_t>(Register::PWR_MGMT0), bitmask,
348+
static_cast<uint8_t>(power_mode) & bitmask, ec);
333349
return !ec;
334350
}
335351

@@ -338,8 +354,69 @@ class Icm42607 : public espp::BasePeripheral<uint8_t, Interface == icm42607::Int
338354
/// @param ec The error code to set if an error occurs
339355
/// @return True if the power mode was set successfully, false otherwise
340356
bool set_gyroscope_power_mode(GyroscopePowerMode power_mode, std::error_code &ec) {
341-
set_bits_in_register(static_cast<uint8_t>(Register::PWR_MGMT0),
342-
(static_cast<uint8_t>(power_mode) & 0x03) << 2, ec);
357+
uint8_t bitmask = 0x03;
358+
set_bits_in_register_by_mask(static_cast<uint8_t>(Register::PWR_MGMT0), bitmask << 2,
359+
(static_cast<uint8_t>(power_mode) & bitmask) << 2, ec);
360+
return !ec;
361+
}
362+
363+
/// Set the Accelerometer filter bandwidth
364+
/// @param bw The filter bandwidth
365+
/// @param ec The error code to set if an error occurs
366+
/// @return True if the filter bandwidth was set successfully, false otherwise
367+
bool set_accelerometer_filter(SensorFilterBandwidth bw, std::error_code &ec) {
368+
// ACCEL_FILT_BW is bits 2-0 in ACCEL_CONFIG1
369+
uint8_t mask = 0x07;
370+
uint8_t data = static_cast<uint8_t>(bw) & mask;
371+
set_bits_in_register_by_mask(static_cast<uint8_t>(Register::ACCEL_CONFIG1), mask, data, ec);
372+
return !ec;
373+
}
374+
375+
/// Set the Gyroscope filter bandwidth
376+
/// @param bw The filter bandwidth
377+
/// @param ec The error code to set if an error occurs
378+
/// @return True if the filter bandwidth was set successfully, false otherwise
379+
bool set_gyroscope_filter(SensorFilterBandwidth bw, std::error_code &ec) {
380+
// GYRO_FILT_BW is bits 2-0 in GYRO_CONFIG1
381+
uint8_t mask = 0x07;
382+
uint8_t data = static_cast<uint8_t>(bw) & mask;
383+
set_bits_in_register_by_mask(static_cast<uint8_t>(Register::GYRO_CONFIG1), mask, data, ec);
384+
return !ec;
385+
}
386+
387+
/// Initialize the DMP
388+
/// @param ec The error code to set if an error occurs
389+
/// @return True if the DMP was initialized successfully, false otherwise
390+
bool dmp_initialize(std::error_code &ec) {
391+
// DMP INIT EN is bit 2 in APEX_CONFIG0
392+
set_bits_in_register(static_cast<uint8_t>(Register::APEX_CONFIG0), 1 << 2, ec);
393+
return !ec;
394+
}
395+
396+
/// Set the DMP power save mode
397+
/// @param enable True to enable DMP power save mode, false to disable
398+
/// @param ec The error code to set if an error occurs
399+
/// @return True if the DMP power save mode was set successfully, false otherwise
400+
bool set_dmp_power_save(bool enable, std::error_code &ec) {
401+
// DMP POWER SAVE EN is bit 3 in APEX_CONFIG0
402+
if (enable) {
403+
set_bits_in_register(static_cast<uint8_t>(Register::APEX_CONFIG0), 1 << 3, ec);
404+
} else {
405+
clear_bits_in_register(static_cast<uint8_t>(Register::APEX_CONFIG0), 1 << 3, ec);
406+
}
407+
return !ec;
408+
};
409+
410+
/// Set the DMP output data rate
411+
/// @param odr The DMP output data rate
412+
/// @param ec The error code to set if an error occurs
413+
/// @return True if the DMP output data rate was set successfully, false
414+
/// otherwise
415+
bool set_dmp_odr(DmpODR odr, std::error_code &ec) {
416+
// DMP ODR is bits 1-0 in APEX_CONFIG1
417+
uint8_t mask = 0x03;
418+
uint8_t data = static_cast<uint8_t>(odr) & mask;
419+
set_bits_in_register_by_mask(static_cast<uint8_t>(Register::APEX_CONFIG1), mask, data, ec);
343420
return !ec;
344421
}
345422

@@ -490,16 +567,42 @@ class Icm42607 : public espp::BasePeripheral<uint8_t, Interface == icm42607::Int
490567
return read(static_cast<uint8_t>(Register::FIFO_DATA), data, size, ec);
491568
}
492569

570+
std::vector<uint8_t> fifo_data(std::error_code &ec) {
571+
// get the count
572+
uint16_t count = fifo_count(ec);
573+
if (ec) {
574+
return {};
575+
}
576+
577+
// allocate a buffer
578+
std::vector<uint8_t> buffer(count);
579+
580+
// read the data
581+
size_t read_count = fifo_data(buffer.data(), count, ec);
582+
if (ec) {
583+
return {};
584+
}
585+
586+
// check if the number of bytes read is the same as the count
587+
if (read_count != count) {
588+
ec = make_error_code(std::errc::io_error);
589+
return {};
590+
}
591+
592+
return buffer;
593+
}
594+
493595
/// Configure interrupt 1
494596
/// @param config The interrupt configuration
495597
/// @param ec The error code to set if an error occurs
496598
/// @return True if the interrupt was configured successfully, false otherwise
497599
bool configure_interrupt_1(const InterruptConfig &config, std::error_code &ec) {
498600
// interrupt 1 is bits 0-2 in INT_CONFIG (MODE << 2) | (POLARITY << 1) | DRIVE_MODE
601+
uint8_t mask = 0b111;
499602
uint8_t data = (static_cast<uint8_t>(config.mode) << 2) |
500603
(static_cast<uint8_t>(config.polarity) << 1) |
501604
static_cast<uint8_t>(config.drive_mode);
502-
set_bits_in_register(static_cast<uint8_t>(Register::INT_CONFIG), data, ec);
605+
set_bits_in_register_by_mask(static_cast<uint8_t>(Register::INT_CONFIG), mask, data, ec);
503606
return !ec;
504607
}
505608

@@ -509,10 +612,11 @@ class Icm42607 : public espp::BasePeripheral<uint8_t, Interface == icm42607::Int
509612
/// @return True if the interrupt was configured successfully, false otherwise
510613
bool configure_interrupt_2(const InterruptConfig &config, std::error_code &ec) {
511614
// interrupt 2 is bits 3-5 in INT_CONFIG (MODE << 5) | (POLARITY << 4) | (DRIVE_MODE << 3)
615+
uint8_t mask = 0b111 << 3;
512616
uint8_t data = (static_cast<uint8_t>(config.mode) << 5) |
513617
(static_cast<uint8_t>(config.polarity) << 4) |
514618
(static_cast<uint8_t>(config.drive_mode) << 3);
515-
set_bits_in_register(static_cast<uint8_t>(Register::INT_CONFIG), data, ec);
619+
set_bits_in_register_by_mask(static_cast<uint8_t>(Register::INT_CONFIG), mask, data, ec);
516620
return !ec;
517621
}
518622

@@ -674,6 +778,58 @@ class Icm42607 : public espp::BasePeripheral<uint8_t, Interface == icm42607::Int
674778
ZG_ST_DATA = 0x05, ///< Z-axis gyroscope self-test data register
675779
};
676780

781+
/// @brief Structure for the ICM42607 FIFO header
782+
struct FifoHeader {
783+
uint8_t odr_gyro : 1; ///< 1: ODfor gyro is different fro this packet comapred to previous gyro
784+
///< packet
785+
uint8_t odr_accel : 1; ///< 1: ODfor accel is different fro this packet comapred to previous
786+
///< accel packet
787+
uint8_t timestamp_fsync : 2; ///< 0b00: no timestamp or fsync data, 0b01: reserved, 0b10: ODR
788+
///< timestamp, 0b11: FSYNC timestamp
789+
uint8_t extended : 1; ///< 1: packet contains 20-bit data for gyro and/or accel
790+
uint8_t has_gyro : 1; ///< 1: Packet is sized so that it contains gyroscope data
791+
uint8_t has_accel : 1; ///< 1: Packet is sized so that it contains accelerometer data
792+
uint8_t empty : 1; ///< 1: Packet is empty, 0: packet contains sensor data
793+
} __attribute__((packed));
794+
795+
/// @brief Struct for the FIFO Packet 1 data
796+
struct FifoPacket1 {
797+
FifoHeader header; ///< FIFO header
798+
std::array<uint16_t, 3> accel; ///< Accelerometer data (x,y,z)
799+
uint8_t temperature; ///< Temperature data
800+
};
801+
802+
/// @brief Struct for the FIFO Packet 2 data
803+
struct FifoPacket2 {
804+
FifoHeader header; ///< FIFO header
805+
std::array<uint16_t, 3> gyro; ///< Gyroscope data (x,y,z)
806+
uint8_t temperature; ///< Temperature data
807+
};
808+
809+
/// @brief Struct for the FIFO Packet 3 data
810+
struct FifoPacket3 {
811+
FifoHeader header; ///< FIFO header
812+
std::array<uint16_t, 3> accel; ///< Accelerometer data (x,y,z)
813+
std::array<uint16_t, 3> gyro; ///< Gyroscope data (x,y,z)
814+
uint8_t temperature; ///< Temperature data
815+
uint16_t timestamp_us; ///< Timestamp in microseconds
816+
};
817+
818+
/// @brief Struct for the FIFO Packet 4 data
819+
struct FifoPacket4 {
820+
FifoHeader header; ///< FIFO header
821+
std::array<uint16_t, 3> accel; ///< Accelerometer data (x,y,z) bits [19:4]
822+
std::array<uint16_t, 3> gyro; ///< Gyroscope data (x,y,z) bits [19:4]
823+
uint16_t temperature; ///< Temperature data
824+
uint16_t timestamp_us; ///< Timestamp in microseconds
825+
uint8_t
826+
x_extension; ///< X-axis extension data (accelx [3:0] high nibble + gyrox [3:0] low nibble)
827+
uint8_t
828+
y_extension; ///< Y-axis extension data (accely [3:0] high nibble + gyroy [3:0] low nibble)
829+
uint8_t
830+
z_extension; ///< Z-axis extension data (accelz [3:0] high nibble + gyroz [3:0] low nibble)
831+
};
832+
677833
static float accelerometer_range_to_sensitivty(AccelerometerRange range) {
678834
switch (range) {
679835
case AccelerometerRange::RANGE_16G:

0 commit comments

Comments
 (0)