Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Yann/refactor/imu/standardize naming #1407

Merged
merged 6 commits into from
Jul 19, 2024
Merged

Yann/refactor/imu/standardize naming #1407

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
72d8904
🚚 (imu): Rename Gy(r)DataReady to DataReady
YannLocatelli Feb 19, 2024
5d94b3b
🚚 (imu): Rename drdy_irq by irq
YannLocatelli Feb 19, 2024
1c1d018
🚚 (imu): Rename drdy_callback to data_ready_callback
YannLocatelli Feb 19, 2024
90d8a0a
♻️ (imu): No abbreviation for drdy, use data_ready
YannLocatelli Feb 19, 2024
eb48c4f
♻️ (imu): Move lsm6dsox dataready pulsed config in init
YannLocatelli Feb 19, 2024
25dfefe
♻️ (imu): Break setDataReadyInterrupt in multiple methods
YannLocatelli Feb 19, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
6 changes: 3 additions & 3 deletions app/os/main.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -260,12 +260,12 @@ namespace imu {

namespace internal {

auto drdy_irq = CoreInterruptIn {PinName::SENSOR_IMU_IRQ};
auto i2c = CoreI2C(PinName::SENSOR_IMU_TH_I2C_SDA, PinName::SENSOR_IMU_TH_I2C_SCL);
auto irq = CoreInterruptIn {PinName::SENSOR_IMU_IRQ};
auto i2c = CoreI2C(PinName::SENSOR_IMU_TH_I2C_SDA, PinName::SENSOR_IMU_TH_I2C_SCL);

} // namespace internal

auto coreimu = CoreIMU(internal::i2c, internal::drdy_irq);
auto coreimu = CoreIMU(internal::i2c, internal::irq);

} // namespace imu

Expand Down
15 changes: 9 additions & 6 deletions drivers/CoreIMU/include/CoreIMU.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -18,11 +18,13 @@ namespace leka {
class CoreIMU : public interface::IMU, public interface::DeepSleepEnabled
{
public:
explicit CoreIMU(interface::I2C &i2c, CoreInterruptIn &drdy_irq);
explicit CoreIMU(interface::I2C &i2c, CoreInterruptIn &irq);

void init() final;

void registerOnGyDataReadyCallback(drdy_callback_t const &callback) final;
void registerOnDataReadyCallback(data_ready_callback_t const &callback) final;
void enableOnDataReadyInterrupt() final;
void disableOnDataReadyInterrupt() final;

void setPowerMode(PowerMode mode) final;

Expand All @@ -38,20 +40,21 @@ class CoreIMU : public interface::IMU, public interface::DeepSleepEnabled
static auto ptr_io_read(CoreIMU *handle, uint8_t read_address, uint8_t *p_buffer,
uint16_t number_bytes_to_read) -> int32_t;

void onGyrDataReadyHandler(auto timestamp);
void setGyrDataReadyInterrupt();
void onDataReadyHandler(auto timestamp);

void setDataReadyInterruptCallback(std::function<void()> const &callback);

interface::I2C &_i2c;
CoreEventQueue _event_queue {};
lsm6dsox_md_t _config {};
stmdev_ctx_t _register_io_function {};
SensorData _sensor_data {};
CoreInterruptIn &_drdy_irq;
CoreInterruptIn &_irq;
const char _address = LSM6DSOX_I2C_ADD_L;

std::array<int16_t, 3> data_raw_xl {};
std::array<int16_t, 3> data_raw_gy {};
drdy_callback_t _on_gy_data_ready_callback {};
data_ready_callback_t _on_data_ready_callback {};

static constexpr uint8_t kMaxBufferLength = 32;
std::array<uint8_t, kMaxBufferLength> _rx_buffer {};
Expand Down
64 changes: 41 additions & 23 deletions drivers/CoreIMU/source/CoreIMU.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,7 @@

namespace leka {

CoreIMU::CoreIMU(interface::I2C &i2c, CoreInterruptIn &drdy_irq) : _i2c(i2c), _drdy_irq(drdy_irq)
CoreIMU::CoreIMU(interface::I2C &i2c, CoreInterruptIn &irq) : _i2c(i2c), _irq(irq)
{
// ? NOLINTNEXTLINE - allow reinterpret_cast as there are no alternatives
_register_io_function.write_reg = reinterpret_cast<stmdev_write_ptr>(ptr_io_write);
Expand All @@ -30,7 +30,10 @@ void CoreIMU::init()

lsm6dsox_mode_set(&_register_io_function, nullptr, &_config);

setGyrDataReadyInterrupt();
lsm6dsox_dataready_pulsed_t data_ready_pulsed {LSM6DSOX_DRDY_PULSED};
lsm6dsox_data_ready_mode_set(&_register_io_function, data_ready_pulsed);

enableOnDataReadyInterrupt();
}

void CoreIMU::setPowerMode(PowerMode mode)
Expand Down Expand Up @@ -71,12 +74,12 @@ void CoreIMU::setPowerMode(PowerMode mode)
lsm6dsox_gy_data_rate_set(&_register_io_function, gy_odr);
}

void CoreIMU::registerOnGyDataReadyCallback(drdy_callback_t const &callback)
void CoreIMU::registerOnDataReadyCallback(data_ready_callback_t const &callback)
{
_on_gy_data_ready_callback = callback;
_on_data_ready_callback = callback;
}

void CoreIMU::onGyrDataReadyHandler(auto timestamp)
void CoreIMU::onDataReadyHandler(auto timestamp)
{
static constexpr auto _1k = float {1000.F};

Expand All @@ -92,11 +95,38 @@ void CoreIMU::onGyrDataReadyHandler(auto timestamp)
_sensor_data.xl.y = lsm6dsox_from_fs4_to_mg(data_raw_xl.at(1)) / _1k;
_sensor_data.xl.z = lsm6dsox_from_fs4_to_mg(data_raw_xl.at(2)) / _1k;

if (_on_gy_data_ready_callback) {
_on_gy_data_ready_callback(_sensor_data);
if (_on_data_ready_callback) {
_on_data_ready_callback(_sensor_data);
}
}

void CoreIMU::enableOnDataReadyInterrupt()
{
lsm6dsox_pin_int1_route_t lsm6dsox_int1 {
.drdy_xl = PROPERTY_ENABLE,
.den_flag = PROPERTY_ENABLE,
};
lsm6dsox_pin_int1_route_set(&_register_io_function, lsm6dsox_int1);

auto on_data_ready_callback = [this] {
auto timestamp = rtos::Kernel::Clock::now();
_event_queue.call([this, timestamp] { onDataReadyHandler(timestamp); });
};

setDataReadyInterruptCallback(on_data_ready_callback);
}

void CoreIMU::disableOnDataReadyInterrupt()
{
lsm6dsox_pin_int1_route_t lsm6dsox_int1 {
.drdy_xl = PROPERTY_DISABLE,
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

ici aussi ça doit être .drdy_g je pense

.den_flag = PROPERTY_DISABLE,
};
lsm6dsox_pin_int1_route_set(&_register_io_function, lsm6dsox_int1);

setDataReadyInterruptCallback({});
}

void CoreIMU::enableDeepSleep()
{
setPowerMode(interface::IMU::PowerMode::Off);
Expand Down Expand Up @@ -143,23 +173,11 @@ auto CoreIMU::ptr_io_read(CoreIMU *handle, uint8_t read_address, uint8_t *p_buff
return handle->read(read_address, number_bytes_to_read, p_buffer);
}

void CoreIMU::setGyrDataReadyInterrupt()
void CoreIMU::setDataReadyInterruptCallback(std::function<void()> const &callback)
{
lsm6dsox_dataready_pulsed_t drdy_pulsed {LSM6DSOX_DRDY_PULSED};
lsm6dsox_data_ready_mode_set(&_register_io_function, drdy_pulsed);

lsm6dsox_pin_int1_route_t gyro_int1 {
.drdy_xl = PROPERTY_ENABLE,
.den_flag = PROPERTY_ENABLE,
};
lsm6dsox_pin_int1_route_set(&_register_io_function, gyro_int1);

auto gyr_drdy_callback = [this] {
auto timestamp = rtos::Kernel::Clock::now();
_event_queue.call([this, timestamp] { onGyrDataReadyHandler(timestamp); });
};

_drdy_irq.onRise(gyr_drdy_callback);
if (callback) {
_irq.onRise(callback);
}
}

} // namespace leka
28 changes: 22 additions & 6 deletions drivers/CoreIMU/tests/CoreIMU_test.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -31,9 +31,9 @@ class CoreIMUTest : public ::testing::Test
// void TearDown() override {}

mock::CoreI2C mocki2c {};
CoreInterruptIn drdy_irq {NC};
CoreInterruptIn irq {NC};

CoreIMU coreimu {mocki2c, drdy_irq};
CoreIMU coreimu {mocki2c, irq};

// ? Instantiation of mock::EventQueue is needed to setup the underlying stubs that will make the mock work
// ? correctly. Without it UT are failing
Expand Down Expand Up @@ -65,28 +65,44 @@ TEST_F(CoreIMUTest, setPowerMode)
coreimu.setPowerMode(CoreIMU::PowerMode::High);
}

TEST_F(CoreIMUTest, onGyrDRDY)
TEST_F(CoreIMUTest, onDataReady)
{
MockFunction<void(const leka::interface::IMU::SensorData &data)> mock_callback;

EXPECT_CALL(mocki2c, write).Times(AtLeast(1));
EXPECT_CALL(mocki2c, read).Times(AtLeast(1));
EXPECT_CALL(mock_callback, Call).Times(1);

coreimu.registerOnGyDataReadyCallback(mock_callback.AsStdFunction());
coreimu.registerOnDataReadyCallback(mock_callback.AsStdFunction());

auto on_rise_callback = spy_InterruptIn_getRiseCallback();
on_rise_callback();
}

TEST_F(CoreIMUTest, emptyOnGyrDrdyCallback)
TEST_F(CoreIMUTest, emptyOnDataReadyCallback)
{
coreimu.registerOnGyDataReadyCallback({});
coreimu.registerOnDataReadyCallback({});

auto on_rise_callback = spy_InterruptIn_getRiseCallback();
on_rise_callback();
}

TEST_F(CoreIMUTest, enableOnDataReadyInterrupt)
{
EXPECT_CALL(mocki2c, write).Times(AtLeast(1));
EXPECT_CALL(mocki2c, read).Times(AtLeast(1));

coreimu.enableOnDataReadyInterrupt();
}

TEST_F(CoreIMUTest, disableOnDataReadyInterrupt)
{
EXPECT_CALL(mocki2c, write).Times(AtLeast(1));
EXPECT_CALL(mocki2c, read).Times(AtLeast(1));

coreimu.disableOnDataReadyInterrupt();
}

TEST_F(CoreIMUTest, enableDeepSleep)
{
EXPECT_CALL(mocki2c, write).Times(AtLeast(1));
Expand Down
13 changes: 9 additions & 4 deletions include/interface/drivers/IMU.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -45,10 +45,15 @@ class IMU
time_point_t timestamp {};
};

using drdy_callback_t = std::function<void(const SensorData)>;
using data_ready_callback_t = std::function<void(const SensorData)>;

virtual void init() = 0;
virtual void registerOnGyDataReadyCallback(drdy_callback_t const &callback) = 0;
virtual void setPowerMode(PowerMode) = 0;
virtual void init() = 0;

virtual void registerOnDataReadyCallback(data_ready_callback_t const &callback) = 0;

virtual void enableOnDataReadyInterrupt() = 0;
virtual void disableOnDataReadyInterrupt() = 0;

virtual void setPowerMode(PowerMode) = 0;
};
} // namespace leka::interface
2 changes: 1 addition & 1 deletion libs/IMUKit/include/IMUKit.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -23,7 +23,7 @@ class IMUKit : public interface::IMUKit
[[nodiscard]] auto getEulerAngles() const -> EulerAngles final;

private:
void drdy_callback(interface::IMU::SensorData data);
void data_ready_callback(interface::IMU::SensorData data);

interface::IMU &_imu;
EulerAngles _euler_angles {};
Expand Down
6 changes: 3 additions & 3 deletions libs/IMUKit/source/IMUKit.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -55,9 +55,9 @@ void IMUKit::init()
FusionOffsetInitialise(&fusion::global_offset, fusion::kODR_HZ);
}

auto on_drdy_callback = [this](const interface::IMU::SensorData &data) { drdy_callback(data); };
auto on_data_ready_callback = [this](const interface::IMU::SensorData &data) { data_ready_callback(data); };

_imu.registerOnGyDataReadyCallback(on_drdy_callback);
_imu.registerOnDataReadyCallback(on_data_ready_callback);
}

void IMUKit::start()
Expand Down Expand Up @@ -88,7 +88,7 @@ void IMUKit::onEulerAnglesReady(angles_ready_callback_t const &callback)
_on_euler_angles_rdy_callback = callback;
}

void IMUKit::drdy_callback(const interface::IMU::SensorData data)
void IMUKit::data_ready_callback(const interface::IMU::SensorData data)
{
// ? Note: For a detailed explanation on the code below, checkout
// ? https://github.com/leka/LekaOS/tree/develop/spikes/lk_sensors_imu_imu_fusion_calibration
Expand Down
8 changes: 4 additions & 4 deletions libs/IMUKit/tests/IMUKit_test.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -75,7 +75,7 @@ TEST_F(IMUKitTest, onDataReady)

EXPECT_CALL(mock_callback, Call);

mock_imu.call_drdy_callback(data_initial);
mock_imu.call_data_ready_callback(data_initial);

const auto angles_initial = imukit.getEulerAngles();

Expand All @@ -86,7 +86,7 @@ TEST_F(IMUKitTest, onDataReady)

EXPECT_CALL(mock_callback, Call);

mock_imu.call_drdy_callback(data_updated);
mock_imu.call_data_ready_callback(data_updated);

auto angles_updated = imukit.getEulerAngles();

Expand All @@ -102,7 +102,7 @@ TEST_F(IMUKitTest, onDataReadyEmptyEulerAngleCallback)
const auto data_initial =
interface::IMU::SensorData {.xl = {.x = 0.F, .y = 0.F, .z = 0.F}, .gy = {.x = 0.F, .y = 0.F, .z = 0.F}};

mock_imu.call_drdy_callback(data_initial);
mock_imu.call_data_ready_callback(data_initial);

const auto angles_initial = imukit.getEulerAngles();

Expand All @@ -111,7 +111,7 @@ TEST_F(IMUKitTest, onDataReadyEmptyEulerAngleCallback)
const auto data_updated =
interface::IMU::SensorData {.xl = {.x = 1.F, .y = 2.F, .z = 3.F}, .gy = {.x = 1.F, .y = 2.F, .z = 3.F}};

mock_imu.call_drdy_callback(data_updated);
mock_imu.call_data_ready_callback(data_updated);

auto angles_updated = imukit.getEulerAngles();

Expand Down
6 changes: 3 additions & 3 deletions spikes/lk_activity_kit/main.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -190,12 +190,12 @@ namespace imu {

namespace internal {

auto drdy_irq = CoreInterruptIn {PinName::SENSOR_IMU_IRQ};
auto i2c = CoreI2C(PinName::SENSOR_IMU_TH_I2C_SDA, PinName::SENSOR_IMU_TH_I2C_SCL);
auto irq = CoreInterruptIn {PinName::SENSOR_IMU_IRQ};
auto i2c = CoreI2C(PinName::SENSOR_IMU_TH_I2C_SDA, PinName::SENSOR_IMU_TH_I2C_SCL);

} // namespace internal

auto coreimu = CoreIMU(internal::i2c, internal::drdy_irq);
auto coreimu = CoreIMU(internal::i2c, internal::irq);

} // namespace imu

Expand Down
6 changes: 3 additions & 3 deletions spikes/lk_command_kit/main.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -99,12 +99,12 @@ auto right = CoreMotor {internal::right::dir_1, internal::right::dir_2, internal
namespace imu {
namespace internal {

auto drdy_irq = CoreInterruptIn {PinName::SENSOR_IMU_IRQ};
auto i2c = CoreI2C(PinName::SENSOR_IMU_TH_I2C_SDA, PinName::SENSOR_IMU_TH_I2C_SCL);
auto irq = CoreInterruptIn {PinName::SENSOR_IMU_IRQ};
auto i2c = CoreI2C(PinName::SENSOR_IMU_TH_I2C_SDA, PinName::SENSOR_IMU_TH_I2C_SCL);

} // namespace internal

CoreIMU coreimu(internal::i2c, internal::drdy_irq);
CoreIMU coreimu(internal::i2c, internal::irq);

} // namespace imu

Expand Down
6 changes: 3 additions & 3 deletions spikes/lk_imu_kit/main.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,12 +19,12 @@ namespace imu {

namespace internal {

auto drdy_irq = CoreInterruptIn {PinName::SENSOR_IMU_IRQ};
auto i2c = CoreI2C(PinName::SENSOR_IMU_TH_I2C_SDA, PinName::SENSOR_IMU_TH_I2C_SCL);
auto irq = CoreInterruptIn {PinName::SENSOR_IMU_IRQ};
auto i2c = CoreI2C(PinName::SENSOR_IMU_TH_I2C_SDA, PinName::SENSOR_IMU_TH_I2C_SCL);

} // namespace internal

CoreIMU coreimu(internal::i2c, internal::drdy_irq);
CoreIMU coreimu(internal::i2c, internal::irq);

} // namespace imu

Expand Down
4 changes: 2 additions & 2 deletions spikes/lk_motion_kit/main.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -59,13 +59,13 @@ namespace imu {

namespace internal {

auto drdy_irq = CoreInterruptIn {PinName::SENSOR_IMU_IRQ};
auto irq = CoreInterruptIn {PinName::SENSOR_IMU_IRQ};
CoreI2C i2c(PinName::SENSOR_IMU_TH_I2C_SDA, PinName::SENSOR_IMU_TH_I2C_SCL);
auto event_queue = CoreEventQueue();

} // namespace internal

CoreIMU coreimu(internal::i2c, internal::drdy_irq);
CoreIMU coreimu(internal::i2c, internal::irq);

} // namespace imu

Expand Down
6 changes: 3 additions & 3 deletions spikes/lk_reinforcer/main.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -137,12 +137,12 @@ namespace imu {

namespace internal {

auto drdy_irq = CoreInterruptIn {PinName::SENSOR_IMU_IRQ};
auto i2c = CoreI2C(PinName::SENSOR_IMU_TH_I2C_SDA, PinName::SENSOR_IMU_TH_I2C_SCL);
auto irq = CoreInterruptIn {PinName::SENSOR_IMU_IRQ};
auto i2c = CoreI2C(PinName::SENSOR_IMU_TH_I2C_SDA, PinName::SENSOR_IMU_TH_I2C_SCL);

} // namespace internal

CoreIMU coreimu(internal::i2c, internal::drdy_irq);
CoreIMU coreimu(internal::i2c, internal::irq);

} // namespace imu

Expand Down
Loading