From eee69bdccec3393057219323c8318456de7d8ab5 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Nicol=C3=B2=20Caruso?= <niccolo.caruso@skywarder.eu>
Date: Sat, 15 Jun 2024 15:33:55 +0000
Subject: [PATCH] [ARP] SMA added ARM_READY state for no-feedback
---
.../Groundstation/Automated/BoardStatus.cpp | 3 +-
.../Groundstation/Automated/SMA/SMA.cpp | 37 ++++++++++++++++++-
src/boards/Groundstation/Automated/SMA/SMA.h | 1 +
.../Groundstation/Automated/SMA/SMAData.h | 1 +
.../Automated/test-smcontroller.cpp | 16 +++++---
5 files changed, 49 insertions(+), 9 deletions(-)
diff --git a/src/boards/Groundstation/Automated/BoardStatus.cpp b/src/boards/Groundstation/Automated/BoardStatus.cpp
index 3ff0f2cd9..b937aebc3 100644
--- a/src/boards/Groundstation/Automated/BoardStatus.cpp
+++ b/src/boards/Groundstation/Automated/BoardStatus.cpp
@@ -97,7 +97,8 @@ void BoardStatus::run()
tm.gps_longitude = vn300.longitude; /*< [deg] Longitude*/
tm.gps_height = vn300.altitude; /*< [m] Altitude*/
tm.gps_fix = vn300.fix_ins; /*< Wether the GPS has a FIX*/
- tm.log_number = Logger::getInstance().getCurrentLogNumber(); /*< Log number*/
+ tm.log_number =
+ Logger::getInstance().getCurrentLogNumber(); /*< Log number*/
tm.battery_voltage = -420.0;
diff --git a/src/boards/Groundstation/Automated/SMA/SMA.cpp b/src/boards/Groundstation/Automated/SMA/SMA.cpp
index 4ce7e2b95..b9e9bc9fc 100644
--- a/src/boards/Groundstation/Automated/SMA/SMA.cpp
+++ b/src/boards/Groundstation/Automated/SMA/SMA.cpp
@@ -63,11 +63,12 @@ bool SMA::start()
void SMA::setAntennaCoordinates(const Boardcore::GPSData& antennaCoordinates)
{
if (!testState(&SMA::state_insert_info) &&
+ !testState(&SMA::state_arm_ready) &&
!testState(&SMA::state_fix_antennas))
{
LOG_ERR(logger,
"Antenna coordinates can only be set in states: "
- "FIX_ANTENNAS, INSERT_INFO");
+ "FIX_ANTENNAS, ARM_READY, INSERT_INFO");
}
else
{
@@ -336,7 +337,7 @@ State SMA::state_no_feedback(const Event& event)
}
case TMTC_ARP_DISARM:
{
- return transition(&SMA::state_insert_info);
+ return transition(&SMA::state_arm_ready);
}
default:
{
@@ -478,6 +479,38 @@ State SMA::state_insert_info(const Event& event)
{
return HANDLED;
}
+ case ARP_FIX_ANTENNAS:
+ {
+ return transition(&SMA::state_arm_ready);
+ }
+ default:
+ {
+ return UNHANDLED;
+ }
+ }
+}
+
+State SMA::state_arm_ready(const Event& event)
+{
+ switch (event)
+ {
+ case EV_ENTRY:
+ {
+ logStatus(SMAState::ARM_READY);
+ return HANDLED;
+ }
+ case EV_EXIT:
+ {
+ return HANDLED;
+ }
+ case EV_EMPTY:
+ {
+ return tranSuper(&SMA::state_config);
+ }
+ case EV_INIT:
+ {
+ return HANDLED;
+ }
case TMTC_ARP_ARM:
{
return transition(&SMA::state_no_feedback);
diff --git a/src/boards/Groundstation/Automated/SMA/SMA.h b/src/boards/Groundstation/Automated/SMA/SMA.h
index e740a5e59..f4a3a5d9e 100644
--- a/src/boards/Groundstation/Automated/SMA/SMA.h
+++ b/src/boards/Groundstation/Automated/SMA/SMA.h
@@ -56,6 +56,7 @@ public:
Boardcore::State state_init_error(const Boardcore::Event& event);
Boardcore::State state_init_done(const Boardcore::Event& event);
Boardcore::State state_insert_info(const Boardcore::Event& event);
+ Boardcore::State state_arm_ready(const Boardcore::Event& event);
Boardcore::State state_armed(const Boardcore::Event& event);
Boardcore::State state_test(const Boardcore::Event& event);
Boardcore::State state_calibrate(const Boardcore::Event& event);
diff --git a/src/boards/Groundstation/Automated/SMA/SMAData.h b/src/boards/Groundstation/Automated/SMA/SMAData.h
index 82f7a8053..bd9382df2 100644
--- a/src/boards/Groundstation/Automated/SMA/SMAData.h
+++ b/src/boards/Groundstation/Automated/SMA/SMAData.h
@@ -44,6 +44,7 @@ enum class SMAState : uint8_t
FIX_ANTENNAS,
FIX_ROCKET,
FIX_ROCKET_NF,
+ ARM_READY,
ACTIVE,
ACTIVE_NF,
/**
diff --git a/src/entrypoints/Groundstation/Automated/test-smcontroller.cpp b/src/entrypoints/Groundstation/Automated/test-smcontroller.cpp
index 06320cdf1..500e82291 100644
--- a/src/entrypoints/Groundstation/Automated/test-smcontroller.cpp
+++ b/src/entrypoints/Groundstation/Automated/test-smcontroller.cpp
@@ -119,8 +119,12 @@ int main()
TMTC_ARP_FORCE_NO_FEEDBACK);
// TEST STATE: INSERT_INFO
- TEST_STATE(state_insert_info, TMTC_ARP_ARM, TOPIC_TMTC, state_armed_nf);
- TEST_ALL_OTHER(state_insert_info, TMTC_ARP_ARM);
+ TEST_STATE(state_insert_info, ARP_FIX_ANTENNAS, TOPIC_ARP, state_arm_ready);
+ TEST_ALL_OTHER(state_insert_info, ARP_FIX_ANTENNAS);
+
+ // TEST STATE: ARM READY
+ TEST_STATE(state_arm_ready, TMTC_ARP_ARM, TOPIC_TMTC, state_armed_nf);
+ TEST_ALL_OTHER(state_arm_ready, TMTC_ARP_ARM);
// TEST STATE: ARMED
TEST_STATE(state_armed, TMTC_ARP_DISARM, TOPIC_TMTC, state_init_done);
@@ -166,7 +170,7 @@ int main()
TEST_ALL_OTHER(state_active, TMTC_ARP_DISARM, TMTC_ARP_RESET_ALGORITHM);
// TEST STATE: ARMED_NO_FEEDBACK
- TEST_STATE(state_armed_nf, TMTC_ARP_DISARM, TOPIC_TMTC, state_insert_info);
+ TEST_STATE(state_armed_nf, TMTC_ARP_DISARM, TOPIC_TMTC, state_arm_ready);
TEST_STATE(state_armed_nf, TMTC_ARP_CALIBRATE, TOPIC_TMTC,
state_fix_rocket_nf);
TEST_STATE(state_armed_nf, TMTC_ARP_ENTER_TEST_MODE, TOPIC_TMTC,
@@ -177,20 +181,20 @@ int main()
// TEST STATE: TEST_NO_FEEDBACK
TEST_STATE(state_test_nf, TMTC_ARP_EXIT_TEST_MODE, TOPIC_TMTC,
state_armed_nf);
- TEST_STATE(state_test_nf, TMTC_ARP_DISARM, TOPIC_TMTC, state_insert_info);
+ TEST_STATE(state_test_nf, TMTC_ARP_DISARM, TOPIC_TMTC, state_arm_ready);
TEST_ALL_OTHER(state_test_nf, TMTC_ARP_EXIT_TEST_MODE, TMTC_ARP_DISARM);
// TEST STATE: FIX_ROCKET_NO_FEEDBACK
TEST_STATE(state_fix_rocket_nf, ARP_FIX_ROCKET, TOPIC_ARP, state_active_nf);
TEST_STATE(state_fix_rocket_nf, TMTC_ARP_DISARM, TOPIC_TMTC,
- state_insert_info);
+ state_arm_ready);
TEST_STATE(state_fix_rocket_nf, TMTC_ARP_RESET_ALGORITHM, TOPIC_TMTC,
state_armed_nf);
TEST_ALL_OTHER(state_fix_rocket_nf, ARP_FIX_ROCKET, TMTC_ARP_DISARM,
TMTC_ARP_RESET_ALGORITHM);
// TEST STATE: ACTIVE_NO_FEEDBACK
- TEST_STATE(state_active_nf, TMTC_ARP_DISARM, TOPIC_TMTC, state_insert_info);
+ TEST_STATE(state_active_nf, TMTC_ARP_DISARM, TOPIC_TMTC, state_arm_ready);
TEST_STATE(state_active_nf, TMTC_ARP_RESET_ALGORITHM, TOPIC_TMTC,
state_armed_nf);
TEST_ALL_OTHER(state_active_nf, TMTC_ARP_DISARM, TMTC_ARP_RESET_ALGORITHM);
--
GitLab