Skip to content

Commit a3ae74e

Browse files
YannLocatelliYannLocatelli
committed
✨ (imu): Add wake up interrupt
1 parent 0f38bca commit a3ae74e

File tree

5 files changed

+158
-0
lines changed

5 files changed

+158
-0
lines changed

‎drivers/CoreIMU/include/CoreIMU.hpp

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -25,6 +25,10 @@ class CoreIMU : public interface::IMU
2525
void enableOnDataAvailable() final;
2626
void disableOnDataAvailable() final;
2727

28+
void registerOnWakeUpCallback(std::function<void()> const &callback) final;
29+
void enableOnWakeUpInterrupt() final;
30+
void disableOnWakeUpInterrupt() final;
31+
2832
void setPowerMode(PowerMode mode) final;
2933

3034
private:
@@ -55,6 +59,9 @@ class CoreIMU : public interface::IMU
5559

5660
static constexpr uint8_t kMaxBufferLength = 32;
5761
std::array<uint8_t, kMaxBufferLength> _rx_buffer {};
62+
63+
std::function<void()> _on_wake_up_callback {};
64+
std::function<void()> _on_wake_up_wrapper_callback {};
5865
};
5966

6067
} // namespace leka

‎drivers/CoreIMU/source/CoreIMU.cpp

Lines changed: 53 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -129,6 +129,57 @@ void CoreIMU::disableOnDataAvailable()
129129
setInterruptCallback({});
130130
}
131131

132+
void CoreIMU::registerOnWakeUpCallback(std::function<void()> const &callback)
133+
{
134+
_on_wake_up_callback = callback;
135+
136+
_on_wake_up_wrapper_callback = [this] {
137+
_event_queue.call([this] {
138+
lsm6dsox_all_sources_t all_source;
139+
lsm6dsox_all_sources_get(&_register_io_function, &all_source);
140+
141+
if (all_source.sleep_change && all_source.sleep_state == 0 && _on_wake_up_callback != nullptr) {
142+
_on_wake_up_callback();
143+
}
144+
});
145+
};
146+
147+
setInterruptCallback(_on_wake_up_wrapper_callback);
148+
}
149+
150+
void CoreIMU::enableOnWakeUpInterrupt()
151+
{
152+
// ? Set filter and disable user offset
153+
lsm6dsox_xl_hp_path_internal_set(&_register_io_function, LSM6DSOX_USE_SLOPE);
154+
lsm6dsox_xl_usr_offset_on_wkup_set(&_register_io_function, 0);
155+
156+
// ? Set Wakeup config
157+
lsm6dsox_wkup_threshold_set(&_register_io_function, 2);
158+
lsm6dsox_wkup_ths_weight_set(&_register_io_function, LSM6DSOX_LSb_FS_DIV_64);
159+
lsm6dsox_wkup_dur_set(&_register_io_function, 0x02);
160+
161+
// ? Set Activity config
162+
lsm6dsox_act_sleep_dur_set(&_register_io_function, 0x02);
163+
lsm6dsox_act_mode_set(&_register_io_function, LSM6DSOX_XL_AND_GY_NOT_AFFECTED);
164+
165+
lsm6dsox_pin_int1_route_t lsm6dsox_int1 {
166+
.sleep_change = PROPERTY_ENABLE,
167+
};
168+
lsm6dsox_pin_int1_route_set(&_register_io_function, lsm6dsox_int1);
169+
170+
setInterruptCallback(_on_wake_up_wrapper_callback);
171+
}
172+
173+
void CoreIMU::disableOnWakeUpInterrupt()
174+
{
175+
lsm6dsox_pin_int1_route_t lsm6dsox_int1 {
176+
.sleep_change = PROPERTY_DISABLE,
177+
};
178+
lsm6dsox_pin_int1_route_set(&_register_io_function, lsm6dsox_int1);
179+
180+
setInterruptCallback({});
181+
}
182+
132183
auto CoreIMU::read(uint8_t register_address, uint16_t number_bytes_to_read, uint8_t *p_buffer) -> int32_t
133184
{
134185
// Send component address, without STOP condition
@@ -169,6 +220,8 @@ void CoreIMU::setInterruptCallback(std::function<void()> const &callback)
169220
{
170221
if (callback != nullptr) {
171222
_irq.onRise(callback);
223+
} else {
224+
_irq.onRise([] {});
172225
}
173226
}
174227

‎drivers/CoreIMU/tests/CoreIMU_test.cpp

Lines changed: 88 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -13,6 +13,7 @@
1313
using namespace leka;
1414

1515
using testing::_;
16+
using testing::AnyNumber;
1617
using testing::AtLeast;
1718
using testing::MockFunction;
1819

@@ -102,3 +103,90 @@ TEST_F(CoreIMUTest, disableOnDataAvailable)
102103

103104
coreimu.disableOnDataAvailable();
104105
}
106+
107+
TEST_F(CoreIMUTest, onWakeUpCallback)
108+
{
109+
MockFunction<void()> mock_callback;
110+
111+
EXPECT_CALL(mocki2c, write).Times(AtLeast(1));
112+
EXPECT_CALL(mocki2c, read).Times(AtLeast(1));
113+
EXPECT_CALL(mock_callback, Call).Times(AnyNumber());
114+
115+
coreimu.registerOnWakeUpCallback(mock_callback.AsStdFunction());
116+
117+
auto on_rise_callback = spy_InterruptIn_getRiseCallback();
118+
on_rise_callback();
119+
}
120+
121+
TEST_F(CoreIMUTest, emptyOnWakeUpCallback)
122+
{
123+
coreimu.registerOnWakeUpCallback({});
124+
125+
auto on_rise_callback = spy_InterruptIn_getRiseCallback();
126+
on_rise_callback();
127+
}
128+
129+
TEST_F(CoreIMUTest, enableOnWakeUpInterrupt)
130+
{
131+
EXPECT_CALL(mocki2c, write).Times(AtLeast(1));
132+
EXPECT_CALL(mocki2c, read).Times(AtLeast(1));
133+
134+
coreimu.enableOnWakeUpInterrupt();
135+
}
136+
137+
TEST_F(CoreIMUTest, disableOnWakeUpInterrupt)
138+
{
139+
EXPECT_CALL(mocki2c, write).Times(AtLeast(1));
140+
EXPECT_CALL(mocki2c, read).Times(AtLeast(1));
141+
142+
coreimu.disableOnWakeUpInterrupt();
143+
}
144+
145+
TEST_F(CoreIMUTest, switchCallbacks)
146+
{
147+
auto mock_data_available_callback = MockFunction<void(const leka::interface::IMU::SensorData &data)> {};
148+
auto mock_wake_up_callback = MockFunction<void()> {};
149+
auto on_rise_callback = mbed::Callback<void()> {};
150+
151+
EXPECT_CALL(mocki2c, write).Times(AnyNumber());
152+
EXPECT_CALL(mocki2c, read).Times(AnyNumber());
153+
154+
coreimu.registerOnDataAvailableCallback(mock_data_available_callback.AsStdFunction());
155+
coreimu.registerOnWakeUpCallback(mock_wake_up_callback.AsStdFunction());
156+
157+
{
158+
// Enable Data Available
159+
EXPECT_CALL(mock_data_available_callback, Call).Times(1);
160+
EXPECT_CALL(mock_wake_up_callback, Call).Times(0);
161+
coreimu.enableOnDataAvailable();
162+
on_rise_callback = spy_InterruptIn_getRiseCallback();
163+
on_rise_callback();
164+
}
165+
166+
{
167+
// Enable Wake Up
168+
EXPECT_CALL(mock_data_available_callback, Call).Times(0);
169+
EXPECT_CALL(mock_wake_up_callback, Call).Times(AnyNumber());
170+
coreimu.enableOnWakeUpInterrupt();
171+
on_rise_callback = spy_InterruptIn_getRiseCallback();
172+
on_rise_callback();
173+
}
174+
175+
{
176+
// Enable Data Available
177+
EXPECT_CALL(mock_data_available_callback, Call).Times(1);
178+
EXPECT_CALL(mock_wake_up_callback, Call).Times(0);
179+
coreimu.enableOnDataAvailable();
180+
on_rise_callback = spy_InterruptIn_getRiseCallback();
181+
on_rise_callback();
182+
}
183+
184+
{
185+
// Disable Data Available
186+
EXPECT_CALL(mock_data_available_callback, Call).Times(0);
187+
EXPECT_CALL(mock_wake_up_callback, Call).Times(0);
188+
coreimu.disableOnDataAvailable();
189+
on_rise_callback = spy_InterruptIn_getRiseCallback();
190+
on_rise_callback();
191+
}
192+
}

‎include/interface/drivers/IMU.hpp

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -53,6 +53,10 @@ class IMU
5353
virtual void enableOnDataAvailable() = 0;
5454
virtual void disableOnDataAvailable() = 0;
5555

56+
virtual void registerOnWakeUpCallback(std::function<void()> const &callback) = 0;
57+
virtual void enableOnWakeUpInterrupt() = 0;
58+
virtual void disableOnWakeUpInterrupt() = 0;
59+
5660
virtual void setPowerMode(PowerMode) = 0;
5761
};
5862
} // namespace leka::interface

‎tests/unit/mocks/mocks/leka/IMU.h

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -19,10 +19,16 @@ class IMU : public interface::IMU
1919
MOCK_METHOD(void, enableOnDataAvailable, (), (override));
2020
MOCK_METHOD(void, disableOnDataAvailable, (), (override));
2121

22+
void registerOnWakeUpCallback(std::function<void()> const &cb) override { wake_up_callback = cb; }
23+
MOCK_METHOD(void, enableOnWakeUpInterrupt, (), (override));
24+
MOCK_METHOD(void, disableOnWakeUpInterrupt, (), (override));
25+
2226
void call_data_available_callback(const SensorData &data) { data_available_callback(data); }
27+
void call_wake_up_callback() { wake_up_callback(); }
2328

2429
private:
2530
data_available_callback_t data_available_callback {};
31+
std::function<void()> wake_up_callback {};
2632
};
2733

2834
} // namespace leka::mock

0 commit comments

Comments
 (0)