Skip to content

Commit d2f09ee

Browse files
YannLocatelliYannLocatelli
committed
✨ (imu): Add wake up interrupt
1 parent d47f4db commit d2f09ee

5 files changed

Lines changed: 158 additions & 0 deletions

File tree

‎drivers/CoreIMU/include/CoreIMU.hpp‎

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

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

3135
void enableDeepSleep() final;
@@ -59,6 +63,9 @@ class CoreIMU : public interface::IMU, public interface::DeepSleepEnabled
5963

6064
static constexpr uint8_t kMaxBufferLength = 32;
6165
std::array<uint8_t, kMaxBufferLength> _rx_buffer {};
66+
67+
std::function<void()> _on_wake_up_callback {};
68+
std::function<void()> _on_wake_up_wrapper_callback {};
6269
};
6370

6471
} // namespace leka

‎drivers/CoreIMU/source/CoreIMU.cpp‎

Lines changed: 53 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -139,6 +139,57 @@ void CoreIMU::disableDeepSleep()
139139
setPowerMode(interface::IMU::PowerMode::Normal);
140140
}
141141

142+
void CoreIMU::registerOnWakeUpCallback(std::function<void()> const &callback)
143+
{
144+
_on_wake_up_callback = callback;
145+
146+
_on_wake_up_wrapper_callback = [this] {
147+
_event_queue.call([this] {
148+
lsm6dsox_all_sources_t all_source;
149+
lsm6dsox_all_sources_get(&_register_io_function, &all_source);
150+
151+
if (all_source.sleep_change && all_source.sleep_state == 0 && _on_wake_up_callback != nullptr) {
152+
_on_wake_up_callback();
153+
}
154+
});
155+
};
156+
157+
setInterruptCallback(_on_wake_up_wrapper_callback);
158+
}
159+
160+
void CoreIMU::enableOnWakeUpInterrupt()
161+
{
162+
// ? Set filter and disable user offset
163+
lsm6dsox_xl_hp_path_internal_set(&_register_io_function, LSM6DSOX_USE_SLOPE);
164+
lsm6dsox_xl_usr_offset_on_wkup_set(&_register_io_function, 0);
165+
166+
// ? Set Wakeup config
167+
lsm6dsox_wkup_threshold_set(&_register_io_function, 2);
168+
lsm6dsox_wkup_ths_weight_set(&_register_io_function, LSM6DSOX_LSb_FS_DIV_64);
169+
lsm6dsox_wkup_dur_set(&_register_io_function, 0x02);
170+
171+
// ? Set Activity config
172+
lsm6dsox_act_sleep_dur_set(&_register_io_function, 0x02);
173+
lsm6dsox_act_mode_set(&_register_io_function, LSM6DSOX_XL_AND_GY_NOT_AFFECTED);
174+
175+
lsm6dsox_pin_int1_route_t lsm6dsox_int1 {
176+
.sleep_change = PROPERTY_ENABLE,
177+
};
178+
lsm6dsox_pin_int1_route_set(&_register_io_function, lsm6dsox_int1);
179+
180+
setInterruptCallback(_on_wake_up_wrapper_callback);
181+
}
182+
183+
void CoreIMU::disableOnWakeUpInterrupt()
184+
{
185+
lsm6dsox_pin_int1_route_t lsm6dsox_int1 {
186+
.sleep_change = PROPERTY_DISABLE,
187+
};
188+
lsm6dsox_pin_int1_route_set(&_register_io_function, lsm6dsox_int1);
189+
190+
setInterruptCallback({});
191+
}
192+
142193
auto CoreIMU::read(uint8_t register_address, uint16_t number_bytes_to_read, uint8_t *p_buffer) -> int32_t
143194
{
144195
// Send component address, without STOP condition
@@ -179,6 +230,8 @@ void CoreIMU::setInterruptCallback(std::function<void()> const &callback)
179230
{
180231
if (callback != nullptr) {
181232
_irq.onRise(callback);
233+
} else {
234+
_irq.onRise([] {});
182235
}
183236
}
184237

‎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

@@ -120,3 +121,90 @@ TEST_F(CoreIMUTest, disableDeepSleep)
120121

121122
coreimu.disableDeepSleep();
122123
}
124+
125+
TEST_F(CoreIMUTest, onWakeUpCallback)
126+
{
127+
MockFunction<void()> mock_callback;
128+
129+
EXPECT_CALL(mocki2c, write).Times(AtLeast(1));
130+
EXPECT_CALL(mocki2c, read).Times(AtLeast(1));
131+
EXPECT_CALL(mock_callback, Call).Times(AnyNumber());
132+
133+
coreimu.registerOnWakeUpCallback(mock_callback.AsStdFunction());
134+
135+
auto on_rise_callback = spy_InterruptIn_getRiseCallback();
136+
on_rise_callback();
137+
}
138+
139+
TEST_F(CoreIMUTest, emptyOnWakeUpCallback)
140+
{
141+
coreimu.registerOnWakeUpCallback({});
142+
143+
auto on_rise_callback = spy_InterruptIn_getRiseCallback();
144+
on_rise_callback();
145+
}
146+
147+
TEST_F(CoreIMUTest, enableOnWakeUpInterrupt)
148+
{
149+
EXPECT_CALL(mocki2c, write).Times(AtLeast(1));
150+
EXPECT_CALL(mocki2c, read).Times(AtLeast(1));
151+
152+
coreimu.enableOnWakeUpInterrupt();
153+
}
154+
155+
TEST_F(CoreIMUTest, disableOnWakeUpInterrupt)
156+
{
157+
EXPECT_CALL(mocki2c, write).Times(AtLeast(1));
158+
EXPECT_CALL(mocki2c, read).Times(AtLeast(1));
159+
160+
coreimu.disableOnWakeUpInterrupt();
161+
}
162+
163+
TEST_F(CoreIMUTest, switchCallbacks)
164+
{
165+
auto mock_data_available_callback = MockFunction<void(const leka::interface::IMU::SensorData &data)> {};
166+
auto mock_wake_up_callback = MockFunction<void()> {};
167+
auto on_rise_callback = mbed::Callback<void()> {};
168+
169+
EXPECT_CALL(mocki2c, write).Times(AnyNumber());
170+
EXPECT_CALL(mocki2c, read).Times(AnyNumber());
171+
172+
coreimu.registerOnDataAvailableCallback(mock_data_available_callback.AsStdFunction());
173+
coreimu.registerOnWakeUpCallback(mock_wake_up_callback.AsStdFunction());
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+
// Enable Wake Up
186+
EXPECT_CALL(mock_data_available_callback, Call).Times(0);
187+
EXPECT_CALL(mock_wake_up_callback, Call).Times(AnyNumber());
188+
coreimu.enableOnWakeUpInterrupt();
189+
on_rise_callback = spy_InterruptIn_getRiseCallback();
190+
on_rise_callback();
191+
}
192+
193+
{
194+
// Enable Data Available
195+
EXPECT_CALL(mock_data_available_callback, Call).Times(1);
196+
EXPECT_CALL(mock_wake_up_callback, Call).Times(0);
197+
coreimu.enableOnDataAvailable();
198+
on_rise_callback = spy_InterruptIn_getRiseCallback();
199+
on_rise_callback();
200+
}
201+
202+
{
203+
// Disable Data Available
204+
EXPECT_CALL(mock_data_available_callback, Call).Times(0);
205+
EXPECT_CALL(mock_wake_up_callback, Call).Times(0);
206+
coreimu.disableOnDataAvailable();
207+
on_rise_callback = spy_InterruptIn_getRiseCallback();
208+
on_rise_callback();
209+
}
210+
}

‎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)