From 185bcacb7b17ee8694550ce19d7e207a1e9cf44c Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Niccol=C3=B2=20Betto?= <niccolo.betto@skywarder.eu>
Date: Sun, 19 Jan 2025 17:54:38 +0100
Subject: [PATCH] [TARS1] Refactor to async-like syntax for a linear
implementation
Previosly the TARS1 refueling procedure was implemented as various
explicit switch-case statements, with various events being manually
posted to the event queue, which was somewhat hard to read and reason
about.
This commit introduces 'ASYNC_' macros that allow async-like syntax to
be used for waiting on delayed events, allowing a linear implementation
of the code as if it was synchronous.
If these are proven to be useful and work well, we might consider adding
them to the FSM base class.
---
src/RIGv2/Configs/TARS1Config.h | 2 +-
src/RIGv2/StateMachines/TARS1/TARS1.cpp | 194 ++++++++++++------------
src/common/Events.h | 1 +
src/common/Topics.h | 1 +
4 files changed, 98 insertions(+), 100 deletions(-)
diff --git a/src/RIGv2/Configs/TARS1Config.h b/src/RIGv2/Configs/TARS1Config.h
index 6cb04df68..9a99279d3 100644
--- a/src/RIGv2/Configs/TARS1Config.h
+++ b/src/RIGv2/Configs/TARS1Config.h
@@ -44,7 +44,7 @@ constexpr uint32_t WASHING_TIME_DELAY = 1000;
constexpr uint32_t FILLING_OPENING_TIME = 900000;
constexpr uint32_t PRESSURE_STABILIZE_WAIT_TIME = 1000;
-constexpr int NUM_MASS_STABLE_ITERATIONS = 2;
+constexpr int NUM_MASS_STABLE_ITERATIONS = 3;
constexpr float MASS_TOLERANCE = 0.2; // [kg]
constexpr float PRESSURE_TOLERANCE = 0.035; // [bar]
diff --git a/src/RIGv2/StateMachines/TARS1/TARS1.cpp b/src/RIGv2/StateMachines/TARS1/TARS1.cpp
index 5b0f879d6..f7675725b 100644
--- a/src/RIGv2/StateMachines/TARS1/TARS1.cpp
+++ b/src/RIGv2/StateMachines/TARS1/TARS1.cpp
@@ -37,6 +37,7 @@ TARS1::TARS1()
Config::Scheduler::TARS1_PRIORITY)
{
EventBroker::getInstance().subscribe(this, TOPIC_TARS);
+ EventBroker::getInstance().subscribe(this, TOPIC_TARS_ASYNC);
EventBroker::getInstance().subscribe(this, TOPIC_MOTOR);
}
@@ -82,113 +83,112 @@ void TARS1::state_ready(const Event& event)
}
}
+#define ASYNC_BEGIN(topic, event) \
+ constexpr auto _ASYNC_TOPIC = topic; \
+ constexpr auto _ASYNC_CONT_EVENT = event; \
+ static int _lc = 0; \
+ if (event == TARS_ASYNC_CONTINUE || event == EV_ENTRY) \
+ { \
+ switch (_lc) \
+ { \
+ case 0:
+#define ASYNC_END() \
+ } \
+ return; \
+ }
+
+#define ASYNC_WAIT_FOR(t) \
+ _lc = __LINE__; \
+ case __LINE__: \
+ if (event != __LINE__) \
+ { \
+ nextDelayedEventId = EventBroker::getInstance().postDelayed( \
+ _ASYNC_CONT_EVENT, _ASYNC_TOPIC, t); \
+ return; \
+ }
+
void TARS1::state_refueling(const Event& event)
{
Actuators* actuators = getModule<Actuators>();
- switch (event)
- {
- case EV_ENTRY:
- {
- // Reset TARS state
- massStableCounter = 0;
- previousMass = 0.0f;
- currentMass = 0.0f;
- previousPressure = 0.0f;
- currentPressure = 0.0f;
-
- // First close all valves
- actuators->closeAllServos();
+ ASYNC_BEGIN(TOPIC_TARS_ASYNC, TARS_ASYNC_CONTINUE)
- LOG_INFO(logger, "TARS start washing");
- logAction(TarsActionType::WASHING);
+ // Reset TARS state
+ massStableCounter = 0;
+ previousMass = 0.0f;
+ currentMass = 0.0f;
+ previousPressure = 0.0f;
+ currentPressure = 0.0f;
- // Start washing
- actuators->openServoWithTime(ServosList::VENTING_VALVE,
- Config::TARS1::WASHING_OPENING_TIME);
+ // Start with all vavles closed
+ actuators->closeAllServos();
- // Wait a bit so that the servo don't actuate at the same time
- Thread::sleep(Config::TARS1::WASHING_TIME_DELAY);
+ LOG_INFO(logger, "TARS start washing");
+ logAction(TarsActionType::WASHING);
- actuators->openServoWithTime(ServosList::FILLING_VALVE,
- Config::TARS1::WASHING_OPENING_TIME);
+ // Start washing procedure
+ actuators->openServoWithTime(ServosList::VENTING_VALVE,
+ Config::TARS1::WASHING_OPENING_TIME);
+ // Wait a bit so that the servo don't actuate at the same time
+ ASYNC_WAIT_FOR(Config::TARS1::WASHING_TIME_DELAY);
+ actuators->openServoWithTime(ServosList::FILLING_VALVE,
+ Config::TARS1::WASHING_OPENING_TIME);
- // After double the time we opened the valve, move to the next phase
- nextDelayedEventId = EventBroker::getInstance().postDelayed(
- TARS_WASHING_DONE, TOPIC_TARS,
- Config::TARS1::WASHING_OPENING_TIME * 2);
+ // Wait after washing to ensure the valves have closed
+ ASYNC_WAIT_FOR(Config::TARS1::WASHING_OPENING_TIME * 2);
- break;
- }
-
- case TARS_WASHING_DONE:
- {
- LOG_INFO(logger, "TARS washing done");
- logAction(TarsActionType::OPEN_FILLING);
+ // Washing complete
+ LOG_INFO(logger, "TARS washing done");
+ logAction(TarsActionType::OPEN_FILLING);
- // Open the filling for a long time
- actuators->openServoWithTime(ServosList::FILLING_VALVE,
- Config::TARS1::FILLING_OPENING_TIME);
+ // Open the filling valve for the whole refueling process
+ actuators->openServoWithTime(ServosList::FILLING_VALVE,
+ Config::TARS1::FILLING_OPENING_TIME);
+ // Wait for the system to stabilize
+ ASYNC_WAIT_FOR(Config::TARS1::PRESSURE_STABILIZE_WAIT_TIME);
- nextDelayedEventId = EventBroker::getInstance().postDelayed(
- TARS_PRESSURE_STABILIZED, TOPIC_TARS,
- Config::TARS1::PRESSURE_STABILIZE_WAIT_TIME);
+ while (true)
+ {
+ LOG_INFO(logger, "TARS check mass");
+ logAction(TarsActionType::CHECK_MASS);
- break;
+ // Lock in a new mass value
+ {
+ Lock<FastMutex> lock(sampleMutex);
+ previousMass = currentMass;
+ currentMass = massSample;
}
- case TARS_PRESSURE_STABILIZED:
+ if (Config::TARS1::STOP_ON_MASS_STABILIZATION)
{
- LOG_INFO(logger, "TARS check mass");
- logAction(TarsActionType::CHECK_MASS);
-
- // Lock in a new mass value
- {
- Lock<FastMutex> lock(sampleMutex);
- previousMass = currentMass;
- currentMass = massSample;
- }
+ float massDelta = std::abs(currentMass - previousMass);
+ // Update the mass stabilization counter
+ if (massDelta < Config::TARS1::MASS_TOLERANCE)
+ massStableCounter++;
+ else
+ massStableCounter = 0;
- if (Config::TARS1::STOP_ON_MASS_STABILIZATION)
+ // If the mass is stable, stop the refueling
+ if (massStableCounter >= Config::TARS1::NUM_MASS_STABLE_ITERATIONS)
{
- if (std::abs(currentMass - previousMass) <
- Config::TARS1::MASS_TOLERANCE)
- {
- if (massStableCounter >=
- Config::TARS1::NUM_MASS_STABLE_ITERATIONS)
- {
- EventBroker::getInstance().post(TARS_FILLING_DONE,
- TOPIC_TARS);
- break;
- }
- else
- {
- massStableCounter++;
- }
- }
- else
- {
- massStableCounter = 0;
- }
+ massStableCounter = 0;
+ EventBroker::getInstance().post(TARS_FILLING_DONE, TOPIC_TARS);
+ break;
}
+ }
- LOG_INFO(logger, "TARS open venting");
- logAction(TarsActionType::OPEN_VENTING);
-
- // Open the venting and check for pressure stabilization
- actuators->openServo(ServosList::VENTING_VALVE);
-
- // Calculate next check time based on the time the valve stays open
- unsigned int nextCheckTime =
- actuators->getServoOpeningTime(ServosList::VENTING_VALVE) +
- Config::TARS1::PRESSURE_STABILIZE_WAIT_TIME * 5;
+ LOG_INFO(logger, "TARS open venting");
+ logAction(TarsActionType::OPEN_VENTING);
- nextDelayedEventId = EventBroker::getInstance().postDelayed(
- TARS_CHECK_PRESSURE_STABILIZE, TOPIC_TARS, nextCheckTime);
- break;
- }
+ // Open venting valve
+ actuators->openServo(ServosList::VENTING_VALVE);
+ // Wait for system stabilization based on valve opening time
+ ASYNC_WAIT_FOR(
+ actuators->getServoOpeningTime(ServosList::VENTING_VALVE) +
+ Config::TARS1::PRESSURE_STABILIZE_WAIT_TIME * 5) // wait (5000ms)
- case TARS_CHECK_PRESSURE_STABILIZE:
+ // If the pressure is not stable, keep waiting until it is
+ while (true)
{
LOG_INFO(logger, "TARS check pressure");
logAction(TarsActionType::CHECK_PRESSURE);
@@ -199,25 +199,20 @@ void TARS1::state_refueling(const Event& event)
currentPressure = pressureSample;
}
+ // Stop waiting once the pressure is stable
if (std::abs(currentPressure - previousPressure) <
Config::TARS1::PRESSURE_TOLERANCE)
- {
- // The pressure is stable, do another cicle
- nextDelayedEventId = EventBroker::getInstance().postDelayed(
- TARS_PRESSURE_STABILIZED, TOPIC_TARS,
- Config::TARS1::PRESSURE_STABILIZE_WAIT_TIME);
- }
- else
- {
- // Schedule a new check
- nextDelayedEventId = EventBroker::getInstance().postDelayed(
- TARS_CHECK_PRESSURE_STABILIZE, TOPIC_TARS,
- Config::TARS1::PRESSURE_STABILIZE_WAIT_TIME);
- }
+ break;
- break;
+ // Pressure is not stable yet, wait more
+ ASYNC_WAIT_FOR(Config::TARS1::PRESSURE_STABILIZE_WAIT_TIME);
}
+ }
+ ASYNC_END()
+
+ switch (event)
+ {
case TARS_FILLING_DONE:
{
LOG_INFO(logger, "TARS filling done");
@@ -249,6 +244,7 @@ void TARS1::state_refueling(const Event& event)
// Disable next event
EventBroker::getInstance().removeDelayed(nextDelayedEventId);
transition(&TARS1::state_ready);
+ break;
}
}
}
diff --git a/src/common/Events.h b/src/common/Events.h
index fc524997e..1d24ae46f 100644
--- a/src/common/Events.h
+++ b/src/common/Events.h
@@ -163,6 +163,7 @@ enum Events : uint8_t
TARS_PRESSURE_STABILIZED,
TARS_FILLING_DONE,
TARS_REFINING_DONE,
+ TARS_ASYNC_CONTINUE,
ALTITUDE_TRIGGER_ALTITUDE_REACHED,
WING_ALGORITHM_ENDED,
LAST_EVENT
diff --git a/src/common/Topics.h b/src/common/Topics.h
index 37c83378b..b9e7171c6 100644
--- a/src/common/Topics.h
+++ b/src/common/Topics.h
@@ -44,6 +44,7 @@ enum Topics : uint8_t
TOPIC_TMTC,
TOPIC_MOTOR,
TOPIC_TARS,
+ TOPIC_TARS_ASYNC,
TOPIC_ALT,
TOPIC_WING,
};
--
GitLab