🔀️ Merge branch 'yann/feature/rc/deep-sleep-use-magic-card-to-wakeup'…

… into develop
leka · Aug 15, 2024 · 28b2205 · 28b2205
2 parents b7867ed + e191a5d
commit 28b2205
Show file tree

Hide file tree

Showing 8 changed files with 59 additions and 12 deletions.
diff --git a/app/os/main.cpp b/app/os/main.cpp
@@ -539,7 +539,7 @@ namespace deep_sleep {
 		&motors::left::motor,
 		&motors::right::motor,
 		&display::internal::corelcd,
-		&rfid::reader,
+		&rfidkit,
 		&imu::coreimu,
 	});
 }

diff --git a/libs/RFIDKit/include/RFIDKit.h b/libs/RFIDKit/include/RFIDKit.h
@@ -4,6 +4,7 @@
 
 #pragma once
 
+#include "CoreEventQueue.h"
 #include "ISO14443A.h"
 #include "MagicCard.h"
 #include "interface/drivers/DeepSleepEnabled.h"
@@ -34,6 +35,9 @@ class RFIDKit : public interface::DeepSleepEnabled
 	boost::sml::sm<rfid::ISO14443A> state_machine {_rfid_reader};
 
 	static constexpr std::array<uint8_t, 4> leka_tag_header = {0x4C, 0x45, 0x4B, 0x41};
+
+	CoreEventQueue _event_queue {};
+	int _event_queue_id {};
 };
 
 }	// namespace leka
diff --git a/libs/RFIDKit/source/RFIDKit.cpp b/libs/RFIDKit/source/RFIDKit.cpp
@@ -4,6 +4,10 @@
 
 #include "RFIDKit.h"
 
+#include "rtos/ThisThread.h"
+
+using namespace std::chrono_literals;
+
 namespace leka {
 
 void RFIDKit::init()
@@ -55,11 +59,23 @@ void RFIDKit::onTagActivated(std::function<void(const MagicCard &_card)> const &
 
 void RFIDKit::enableDeepSleep()
 {
+	_event_queue.dispatch_forever();
+
+	auto on_tick = [this] {
+		_rfid_reader.disableDeepSleep();
+		rtos::ThisThread::sleep_for(40ms);
+		_rfid_reader.enableDeepSleep();
+	};
+
+	_event_queue_id = _event_queue.call_every(3s, on_tick);
+
 	_rfid_reader.enableDeepSleep();
 }
 
 void RFIDKit::disableDeepSleep()
 {
+	_event_queue.cancel(_event_queue_id);
+
 	_rfid_reader.disableDeepSleep();
 }
 

diff --git a/libs/RFIDKit/tests/RFIDKit_test.cpp b/libs/RFIDKit/tests/RFIDKit_test.cpp
@@ -6,11 +6,13 @@
 
 #include "gtest/gtest.h"
 #include "mocks/leka/CoreRFIDReader.h"
+#include "mocks/leka/EventQueue.h"
 
 using namespace leka;
 
+using ::testing::AnyNumber;
+using ::testing::InSequence;
 using ::testing::MockFunction;
-using ::testing::ReturnRef;
 using ::testing::SaveArg;
 
 class RFIDKitTest : public ::testing::Test
@@ -21,6 +23,8 @@ class RFIDKitTest : public ::testing::Test
 	// void SetUp() override {}
 	// void TearDown() override {}
 
+	mock::EventQueue event_queue {};
+
 	RFIDKit rfid_kit;
 	mock::CoreRFIDReader mock_reader {};
 	MockFunction<void(const MagicCard &)> mock_callback;
@@ -184,14 +188,18 @@ TEST_F(RFIDKitTest, getLastMagicCardActivated)
 
 TEST_F(RFIDKitTest, enableDeepSleep)
 {
-	EXPECT_CALL(mock_reader, enableDeepSleep());
+	{
+		InSequence seq;
 
+		EXPECT_CALL(mock_reader, disableDeepSleep);
+		EXPECT_CALL(mock_reader, enableDeepSleep).Times(AnyNumber());
+	}
 	rfid_kit.enableDeepSleep();
 }
 
 TEST_F(RFIDKitTest, disableDeepSleep)
 {
-	EXPECT_CALL(mock_reader, disableDeepSleep());
+	EXPECT_CALL(mock_reader, disableDeepSleep);
 
 	rfid_kit.disableDeepSleep();
 }
diff --git a/libs/RobotKit/include/RobotController.h b/libs/RobotKit/include/RobotController.h
@@ -451,10 +451,15 @@ class RobotController : public interface::RobotController
 		// Setup callbacks for monitoring
 
 		_rfidkit.onTagActivated([this](const MagicCard &card) {
-			// ! IMPORTANT NOTE
-			// ! The order of the following functions MUST NOT
-			// ! be changed. It is a temporary fix for #1311
-			// TODO(@leka/dev-embedded): remove when fixed
+		// ! IMPORTANT NOTE
+		// ! The order of the following functions MUST NOT
+		// ! be changed. It is a temporary fix for #1311
+		// TODO(@leka/dev-embedded): remove when fixed
+
+#if defined(ENABLE_FAST_SLEEP)
+#else
+			raise(event::magic_card_detected {});
+#endif
 			_service_magic_card.setMagicCard(card);
 			onMagicCardAvailable(card);
 		});

diff --git a/libs/RobotKit/include/StateMachine.h b/libs/RobotKit/include/StateMachine.h
@@ -43,6 +43,9 @@ namespace sm::event {
 	struct autonomous_activities_mode_exited {
 	};
 
+	struct magic_card_detected {
+	};
+
 }	// namespace sm::event
 
 namespace sm::state {
@@ -243,8 +246,8 @@ struct StateMachine {
 
 			, sm::state::deep_sleeping + event<sm::event::charge_did_start> = X
 			, sm::state::deep_sleeping + event<sm::event::charge_did_stop>  = X
-
-			, sm::state::deep_sleeping + event<sm::event::ble_connection>    = X
+			, sm::state::deep_sleeping + event<sm::event::ble_connection>   = X
+			, sm::state::deep_sleeping + event<sm::event::magic_card_detected> = X
 
 			, sm::state::charging + boost::sml::on_entry<_> / (sm::action::start_deep_sleep_timeout {}, sm::action::start_charging_behavior {} )
 			, sm::state::charging + boost::sml::on_exit<_>  / (sm::action::stop_deep_sleep_timeout  {}, sm::action::stop_charging_behavior  {} )

diff --git a/libs/RobotKit/tests/StateMachine_test.cpp b/libs/RobotKit/tests/StateMachine_test.cpp
@@ -278,6 +278,17 @@ TEST_F(StateMachineTest, stateDeepSleepingEventBleConnection)
 	EXPECT_TRUE(sm.is(X));
 }
 
+TEST_F(StateMachineTest, stateDeepSleepingEventMagicCardDetected)
+{
+	sm.set_current_states(lksm::state::deep_sleeping);
+
+	EXPECT_CALL(mock_rc, wakeUp);
+
+	sm.process_event(lksm::event::magic_card_detected {});
+
+	EXPECT_TRUE(sm.is(X));
+}
+
 TEST_F(StateMachineTest, stateIdleEventChargeDidStart)
 {
 	sm.set_current_states(lksm::state::idle);

diff --git a/tests/functional/tests/deep_sleep_rfid_kit/suite_rfid_kit.cpp b/tests/functional/tests/deep_sleep_rfid_kit/suite_rfid_kit.cpp
@@ -114,9 +114,9 @@ suite suite_rfid_kit = [] {
 
 			when("I wait") = [&] {
 				cardDetected = false;
-				rtos::ThisThread::sleep_for(1s);
+				rtos::ThisThread::sleep_for(10s);
 
-				then("I expect card TO NOT BE detected") = [&cardDetected] { expect(not cardDetected); };
+				then("I expect card TO BE detected") = [&cardDetected] { expect(cardDetected); };
 			};
 
 			when("I disable rfid deep sleep") = [&] {