diff --git a/scripts/logdecoder/General/logdecoder.cpp b/scripts/logdecoder/General/logdecoder.cpp
index a237a6efc86fc7c05e94f49c7dc478fe92f2bd63..2de381cf780757a66c1dd754ac20d448f3981aa0 100644
--- a/scripts/logdecoder/General/logdecoder.cpp
+++ b/scripts/logdecoder/General/logdecoder.cpp
@@ -43,6 +43,7 @@
#include <RIGv2/Sensors/SensorsData.h>
#include <RIGv2/StateMachines/GroundModeManager/GroundModeManagerData.h>
#include <RIGv2/StateMachines/TARS1/TARS1Data.h>
+#include <RIGv2/StateMachines/TARS3/TARS3Data.h>
#include <algorithms/MEA/MEAData.h>
#include <logger/Deserializer.h>
#include <logger/LogTypes.h>
@@ -124,8 +125,10 @@ void registerTypes(Deserializer& ds)
ds.registerType<RIGv2::N2TankPressureData>();
ds.registerType<RIGv2::ActuatorsData>();
ds.registerType<RIGv2::GroundModeManagerData>();
- ds.registerType<RIGv2::TarsActionData>();
- ds.registerType<RIGv2::TarsSampleData>();
+ ds.registerType<RIGv2::Tars1ActionData>();
+ ds.registerType<RIGv2::Tars1SampleData>();
+ ds.registerType<RIGv2::Tars3ActionData>();
+ ds.registerType<RIGv2::Tars3SampleData>();
// Groundstation (ARP)
ds.registerType<Antennas::StepperXData>();
diff --git a/scripts/logdecoder/RIGv2/logdecoder.cpp b/scripts/logdecoder/RIGv2/logdecoder.cpp
index 6062e9ff7a2a7f5c962c439b4cd167fc406ea0e3..78ad363572439805c264dbfdab9a3972f71ee74e 100644
--- a/scripts/logdecoder/RIGv2/logdecoder.cpp
+++ b/scripts/logdecoder/RIGv2/logdecoder.cpp
@@ -24,6 +24,7 @@
#include <RIGv2/Sensors/SensorsData.h>
#include <RIGv2/StateMachines/GroundModeManager/GroundModeManagerData.h>
#include <RIGv2/StateMachines/TARS1/TARS1Data.h>
+#include <RIGv2/StateMachines/TARS3/TARS3Data.h>
#include <logger/Deserializer.h>
#include <logger/LogTypes.h>
#include <tscpp/stream.h>
@@ -68,8 +69,10 @@ void registerTypes(Deserializer& ds)
ds.registerType<N2TankPressureData>();
ds.registerType<ActuatorsData>();
ds.registerType<GroundModeManagerData>();
- ds.registerType<TarsActionData>();
- ds.registerType<TarsSampleData>();
+ ds.registerType<RIGv2::Tars1ActionData>();
+ ds.registerType<RIGv2::Tars1SampleData>();
+ ds.registerType<RIGv2::Tars3ActionData>();
+ ds.registerType<RIGv2::Tars3SampleData>();
ds.registerType<VoltageData>();
}
diff --git a/src/RIGv2/BoardScheduler.h b/src/RIGv2/BoardScheduler.h
index 11556aa7f0bc041d1079f7e4883f70f2addcee78..3d29aaa94d303cf047d33b72473f6fb09c40f9fc 100644
--- a/src/RIGv2/BoardScheduler.h
+++ b/src/RIGv2/BoardScheduler.h
@@ -33,16 +33,16 @@ class BoardScheduler : public Boardcore::Injectable
{
public:
BoardScheduler()
- : tars1(Config::Scheduler::TARS1_PRIORITY),
+ : tars(Config::Scheduler::TARS_PRIORITY),
sensors(Config::Scheduler::SENSORS_PRIORITY)
{
}
[[nodiscard]] bool start()
{
- if (!tars1.start())
+ if (!tars.start())
{
- LOG_ERR(logger, "Failed to start TARS1 scheduler");
+ LOG_ERR(logger, "Failed to start TARS scheduler");
return false;
}
@@ -58,7 +58,9 @@ public:
bool isStarted() { return started; }
- Boardcore::TaskScheduler& getTars1Scheduler() { return tars1; }
+ Boardcore::TaskScheduler& getTars1Scheduler() { return tars; }
+
+ Boardcore::TaskScheduler& getTars3Scheduler() { return tars; }
Boardcore::TaskScheduler& getSensorsScheduler() { return sensors; }
@@ -72,7 +74,7 @@ private:
std::atomic<bool> started{false};
- Boardcore::TaskScheduler tars1;
+ Boardcore::TaskScheduler tars;
Boardcore::TaskScheduler sensors;
};
diff --git a/src/RIGv2/Configs/SchedulerConfig.h b/src/RIGv2/Configs/SchedulerConfig.h
index be37f3cf651b7c1e4e89c628ba9e6e5122bf6b7d..129a7f13b5593750e61e3265f712d62754611bfc 100644
--- a/src/RIGv2/Configs/SchedulerConfig.h
+++ b/src/RIGv2/Configs/SchedulerConfig.h
@@ -33,8 +33,8 @@ namespace Config
namespace Scheduler
{
-// Used for TARS1 task scheduler/FSM
-static const miosix::Priority TARS1_PRIORITY = miosix::PRIORITY_MAX - 1;
+// Used for TARS1/TARS3 task scheduler/FSM
+static const miosix::Priority TARS_PRIORITY = miosix::PRIORITY_MAX - 1;
// Used for Sensors TaskScheduler
static const miosix::Priority SENSORS_PRIORITY = miosix::PRIORITY_MAX - 2;
diff --git a/src/RIGv2/Configs/TARS3Config.h b/src/RIGv2/Configs/TARS3Config.h
new file mode 100644
index 0000000000000000000000000000000000000000..3c9011441df83bce8b43c2024c567c9f552e678c
--- /dev/null
+++ b/src/RIGv2/Configs/TARS3Config.h
@@ -0,0 +1,57 @@
+/* Copyright (c) 2025 Skyward Experimental Rocketry
+ * Author: Niccolò Betto
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+#include <units/Frequency.h>
+
+#include <chrono>
+#include <cstddef>
+#include <cstdint>
+
+namespace RIGv2
+{
+namespace Config
+{
+namespace TARS3
+{
+/* linter off */ using namespace std::chrono;
+/* linter off */ using namespace Boardcore::Units::Frequency;
+
+constexpr Hertz SAMPLE_PERIOD = 100_hz;
+constexpr size_t MEDIAN_SAMPLE_NUMBER = 10;
+
+constexpr auto WAIT_BETWEEN_CYCLES = 1000ms;
+
+constexpr float PRESSURE_CHANGE_TOLERANCE = 0.035; // [bar]
+constexpr auto PRESSURE_STABILIZE_WAIT_TIME = 1000ms;
+
+// Cold refueling parameters
+constexpr float PRESSURE_LOWER_RANGE = 17; // [bar]
+constexpr float PRESSURE_UPPER_RANGE = 20; // [bar]
+
+constexpr auto FILLING_TIME = 3000ms;
+constexpr auto VENTING_TIME = 3000ms;
+
+} // namespace TARS3
+} // namespace Config
+} // namespace RIGv2
diff --git a/src/RIGv2/StateMachines/TARS1/TARS1.cpp b/src/RIGv2/StateMachines/TARS1/TARS1.cpp
index 2d1e27c004ddcf5845b3cba132cb95c99ae3f576..d4c66dca7b78e4aff6f3b1799d31d18fc62fd5ae 100644
--- a/src/RIGv2/StateMachines/TARS1/TARS1.cpp
+++ b/src/RIGv2/StateMachines/TARS1/TARS1.cpp
@@ -34,7 +34,7 @@ using namespace miosix;
TARS1::TARS1()
: FSM(&TARS1::state_ready, miosix::STACK_DEFAULT_FOR_PTHREAD,
- Config::Scheduler::TARS1_PRIORITY)
+ Config::Scheduler::TARS_PRIORITY)
{
EventBroker::getInstance().subscribe(this, TOPIC_TARS);
EventBroker::getInstance().subscribe(this, TOPIC_MOTOR);
@@ -70,7 +70,7 @@ void TARS1::state_ready(const Event& event)
{
case EV_ENTRY:
{
- logAction(TarsActionType::READY);
+ logAction(Tars1ActionType::READY);
break;
}
@@ -101,7 +101,7 @@ void TARS1::state_refueling(const Event& event)
actuators->closeAllServos();
LOG_INFO(logger, "TARS start washing");
- logAction(TarsActionType::WASHING);
+ logAction(Tars1ActionType::WASHING);
// Start washing
actuators->openServoWithTime(ServosList::OX_VENTING_VALVE,
@@ -124,7 +124,7 @@ void TARS1::state_refueling(const Event& event)
case TARS_WASHING_DONE:
{
LOG_INFO(logger, "TARS washing done");
- logAction(TarsActionType::OPEN_FILLING);
+ logAction(Tars1ActionType::OPEN_FILLING);
// Open the filling for a long time
actuators->openServoWithTime(ServosList::OX_FILLING_VALVE,
@@ -140,7 +140,7 @@ void TARS1::state_refueling(const Event& event)
case TARS_PRESSURE_STABILIZED:
{
LOG_INFO(logger, "TARS check mass");
- logAction(TarsActionType::CHECK_MASS);
+ logAction(Tars1ActionType::CHECK_MASS);
// Lock in a new mass value
{
@@ -173,7 +173,7 @@ void TARS1::state_refueling(const Event& event)
}
LOG_INFO(logger, "TARS open venting");
- logAction(TarsActionType::OPEN_VENTING);
+ logAction(Tars1ActionType::OPEN_VENTING);
// Open the venting and check for pressure stabilization
actuators->openServo(ServosList::OX_VENTING_VALVE);
@@ -191,7 +191,7 @@ void TARS1::state_refueling(const Event& event)
case TARS_CHECK_PRESSURE_STABILIZE:
{
LOG_INFO(logger, "TARS check pressure");
- logAction(TarsActionType::CHECK_PRESSURE);
+ logAction(Tars1ActionType::CHECK_PRESSURE);
{
Lock<FastMutex> lock(sampleMutex);
@@ -221,7 +221,7 @@ void TARS1::state_refueling(const Event& event)
case TARS_FILLING_DONE:
{
LOG_INFO(logger, "TARS filling done");
- logAction(TarsActionType::AUTOMATIC_STOP);
+ logAction(Tars1ActionType::AUTOMATIC_STOP);
actuators->closeAllServos();
transition(&TARS1::state_ready);
@@ -231,7 +231,7 @@ void TARS1::state_refueling(const Event& event)
case MOTOR_MANUAL_ACTION:
{
LOG_INFO(logger, "TARS manual stop");
- logAction(TarsActionType::MANUAL_STOP);
+ logAction(Tars1ActionType::MANUAL_STOP);
// Disable next event
EventBroker::getInstance().removeDelayed(nextDelayedEventId);
@@ -242,7 +242,7 @@ void TARS1::state_refueling(const Event& event)
case MOTOR_START_TARS:
{
LOG_INFO(logger, "TARS manual stop");
- logAction(TarsActionType::MANUAL_STOP);
+ logAction(Tars1ActionType::MANUAL_STOP);
// The user requested that we stop
getModule<Actuators>()->closeAllServos();
@@ -277,14 +277,14 @@ void TARS1::sample()
}
}
-void TARS1::logAction(TarsActionType action)
+void TARS1::logAction(Tars1ActionType action)
{
- TarsActionData data = {TimestampTimer::getTimestamp(), action};
+ Tars1ActionData data = {TimestampTimer::getTimestamp(), action};
sdLogger.log(data);
}
void TARS1::logSample(float pressure, float mass)
{
- TarsSampleData data = {TimestampTimer::getTimestamp(), pressure, mass};
+ Tars1SampleData data = {TimestampTimer::getTimestamp(), pressure, mass};
sdLogger.log(data);
}
diff --git a/src/RIGv2/StateMachines/TARS1/TARS1.h b/src/RIGv2/StateMachines/TARS1/TARS1.h
index eba7653f2d985ada2ef80897f3b798946e4254a9..cac66d3696db63a917c02fe5643a972d7ba80700 100644
--- a/src/RIGv2/StateMachines/TARS1/TARS1.h
+++ b/src/RIGv2/StateMachines/TARS1/TARS1.h
@@ -53,7 +53,7 @@ private:
void state_ready(const Boardcore::Event& event);
void state_refueling(const Boardcore::Event& event);
- void logAction(TarsActionType action);
+ void logAction(Tars1ActionType action);
void logSample(float pressure, float mass);
Boardcore::Logger& sdLogger = Boardcore::Logger::getInstance();
diff --git a/src/RIGv2/StateMachines/TARS1/TARS1Data.h b/src/RIGv2/StateMachines/TARS1/TARS1Data.h
index bf1fc75e8c92389dfef10f7a79c76de8a81c59cb..8cf7d67f0183195173b220976364462a093c569c 100644
--- a/src/RIGv2/StateMachines/TARS1/TARS1Data.h
+++ b/src/RIGv2/StateMachines/TARS1/TARS1Data.h
@@ -29,7 +29,7 @@
namespace RIGv2
{
-enum class TarsActionType : uint8_t
+enum class Tars1ActionType : uint8_t
{
READY = 0,
WASHING,
@@ -41,14 +41,14 @@ enum class TarsActionType : uint8_t
MANUAL_STOP,
};
-struct TarsActionData
+struct Tars1ActionData
{
uint64_t timestamp;
- TarsActionType action;
+ Tars1ActionType action;
- TarsActionData() : timestamp{0}, action{TarsActionType::READY} {}
+ Tars1ActionData() : timestamp{0}, action{Tars1ActionType::READY} {}
- TarsActionData(uint64_t timestamp, TarsActionType action)
+ Tars1ActionData(uint64_t timestamp, Tars1ActionType action)
: timestamp{timestamp}, action{action}
{
}
@@ -61,15 +61,15 @@ struct TarsActionData
}
};
-struct TarsSampleData
+struct Tars1SampleData
{
uint64_t timestamp;
float pressure;
float mass;
- TarsSampleData() : timestamp{0}, pressure{0}, mass{0} {}
+ Tars1SampleData() : timestamp{0}, pressure{0}, mass{0} {}
- TarsSampleData(uint64_t timestamp, float pressure, float mass)
+ Tars1SampleData(uint64_t timestamp, float pressure, float mass)
: timestamp{timestamp}, pressure{pressure}, mass{mass}
{
}
diff --git a/src/RIGv2/StateMachines/TARS3/MedianFilter.h b/src/RIGv2/StateMachines/TARS3/MedianFilter.h
new file mode 100644
index 0000000000000000000000000000000000000000..0d14f3f1b12f4cd83a2ecb0a3d01fbc5623fe371
--- /dev/null
+++ b/src/RIGv2/StateMachines/TARS3/MedianFilter.h
@@ -0,0 +1,57 @@
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Authors: Davide Mor
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+#include <algorithm>
+#include <array>
+#include <memory>
+
+namespace RIGv2
+{
+
+template <typename T, size_t Max>
+class MedianFilter
+{
+public:
+ MedianFilter() {}
+
+ void reset() { idx = 0; }
+
+ void add(T value)
+ {
+ values[idx] = value;
+ idx = (idx + 1) % Max;
+ }
+
+ T calcMedian()
+ {
+ std::sort(values.begin(), values.end());
+ return values[idx / 2];
+ }
+
+private:
+ size_t idx = 0;
+ std::array<T, Max> values = {0};
+};
+
+} // namespace RIGv2
diff --git a/src/RIGv2/StateMachines/TARS3/TARS3.cpp b/src/RIGv2/StateMachines/TARS3/TARS3.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5fe64473103bb6e53a3d7ef7fca74f88f9166ef8
--- /dev/null
+++ b/src/RIGv2/StateMachines/TARS3/TARS3.cpp
@@ -0,0 +1,410 @@
+/* Copyright (c) 2025 Skyward Experimental Rocketry
+ * Author: Niccolò Betto
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include "TARS3.h"
+
+#include <RIGv2/Actuators/Actuators.h>
+#include <RIGv2/BoardScheduler.h>
+#include <RIGv2/Configs/TARS3Config.h>
+#include <RIGv2/Sensors/Sensors.h>
+#include <common/Events.h>
+#include <common/Topics.h>
+#include <drivers/timer/TimestampTimer.h>
+#include <events/EventBroker.h>
+
+using namespace std::chrono;
+using namespace Boardcore;
+using namespace Common;
+
+namespace RIGv2
+{
+
+TARS3::TARS3()
+ : HSM(&TARS3::Ready, miosix::STACK_DEFAULT_FOR_PTHREAD,
+ Config::Scheduler::TARS_PRIORITY)
+{
+ EventBroker::getInstance().subscribe(this, TOPIC_TARS);
+ EventBroker::getInstance().subscribe(this, TOPIC_MOTOR);
+}
+
+bool TARS3::start()
+{
+ TaskScheduler& scheduler = getModule<BoardScheduler>()->getTars1Scheduler();
+
+ uint8_t result =
+ scheduler.addTask([this]() { sample(); }, Config::TARS3::SAMPLE_PERIOD);
+
+ if (result == 0)
+ {
+ LOG_ERR(logger, "Failed to add TARS3 sample task");
+ return false;
+ }
+
+ if (!HSM::start())
+ {
+ LOG_ERR(logger, "Failed to activate TARS3 thread");
+ return false;
+ }
+
+ return true;
+}
+
+void TARS3::sample()
+{
+ Sensors* sensors = getModule<Sensors>();
+
+ pressureFilter.add(sensors->getOxTankBottomPressure().pressure);
+ massFilter.add(sensors->getOxTankWeight().load);
+ medianSamples++;
+
+ if (medianSamples == Config::TARS3::MEDIAN_SAMPLE_NUMBER)
+ {
+ float pressure = pressureFilter.calcMedian();
+ float mass = massFilter.calcMedian();
+ medianSamples = 0;
+
+ logSample(pressure, mass);
+
+ {
+ Lock<FastMutex> lock(sampleMutex);
+ pressureSample = pressure;
+ massSample = mass;
+ }
+ }
+}
+
+void TARS3::logAction(Tars3Action action, float data)
+{
+ auto data = Tars3ActionData{.timestamp = TimestampTimer::getTimestamp(),
+ .action = action,
+ .data = data};
+ sdLogger.log(data);
+}
+
+void TARS3::logSample(float pressure, float mass)
+{
+ Tars3SampleData data = {TimestampTimer::getTimestamp(), pressure, mass};
+ sdLogger.log(data);
+}
+
+State TARS3::Ready(const Event& event)
+{
+ switch (event)
+ {
+ case EV_ENTRY:
+ {
+ logAction(Tars3Action::READY);
+ return HANDLED;
+ }
+
+ case MOTOR_START_TARS:
+ {
+ return transition(&TARS3::Refueling);
+ }
+
+ case EV_EMPTY:
+ {
+ return tranSuper(&TARS3::state_top);
+ }
+
+ default:
+ {
+ return UNHANDLED;
+ }
+ }
+}
+
+State TARS3::Refueling(const Event& event)
+{
+ Actuators* actuators = getModule<Actuators>();
+
+ switch (event)
+ {
+ case EV_INIT:
+ {
+ previousPressure = 0.0f;
+ currentPressure = 0.0f;
+ fillingTime = Config::TARS3::FILLING_TIME;
+ ventingTime = Config::TARS3::VENTING_TIME;
+
+ // Initialize the valves to a known closed state
+ actuators->closeAllServos();
+
+ LOG_INFO(logger, "TARS3 cold refueling start");
+ logAction(Tars3Action::START);
+
+ return transition(&TARS3::RefuelingWaitAfterCycle);
+ }
+
+ case EV_ENTRY:
+ {
+ return HANDLED;
+ }
+
+ case MOTOR_MANUAL_ACTION:
+ {
+ LOG_INFO(logger, "TARS3 stopped because of manual valve action");
+ logAction(Tars3Action::MANUAL_ACTION_STOP);
+
+ return transition(&TARS3::Ready);
+ }
+
+ case MOTOR_STOP_TARS:
+ {
+ LOG_INFO(logger, "TARS3 stopped because of manual stop");
+ logAction(Tars3Action::MANUAL_STOP);
+
+ return transition(&TARS3::Ready);
+ }
+
+ case EV_EXIT:
+ {
+ actuators->closeAllServos();
+ EventBroker::getInstance().removeDelayed(delayedEventId);
+
+ return HANDLED;
+ }
+
+ case EV_EMPTY:
+ {
+ return tranSuper(&TARS3::state_top);
+ }
+
+ default:
+ {
+ return UNHANDLED;
+ }
+ }
+}
+
+State TARS3::RefuelingWaitAfterCycle(const Event& event)
+{
+ {
+ Lock<FastMutex> lock(sampleMutex);
+ previousPressure = currentPressure;
+ currentPressure = pressureSample;
+ }
+
+ switch (event)
+ {
+ case EV_ENTRY:
+ {
+ LOG_INFO(logger, "Waiting for system to stabilize after cycle");
+ logAction(Tars3Action::WAITING_CYCLE);
+
+ EventBroker::getInstance().postDelayed(
+ TARS_CHECK_PRESSURE_STABILIZE, TOPIC_TARS,
+ milliseconds{Config::TARS3::WAIT_BETWEEN_CYCLES}.count());
+
+ return HANDLED;
+ }
+
+ case TARS_CHECK_PRESSURE_STABILIZE:
+ {
+ float pressureChange = std::abs(currentPressure - previousPressure);
+
+ if (pressureChange < Config::TARS3::PRESSURE_CHANGE_TOLERANCE)
+ {
+ // The pressure is stable
+ LOG_INFO(logger,
+ "Pressure is stable, proceeding with the cycle");
+ logAction(Tars3Action::PRESSURE_STABLE_CYCLE, pressureChange);
+
+ EventBroker::getInstance().post(TARS_PRESSURE_STABILIZED,
+ TOPIC_TARS);
+ }
+ else
+ {
+ // Pressure is not stable yet, schedule a new check
+ LOG_INFO(logger,
+ "Waiting for pressure to stabilize before cycle");
+ logAction(Tars3Action::WAITING_CYCLE, pressureChange);
+
+ delayedEventId = EventBroker::getInstance().postDelayed(
+ TARS_CHECK_PRESSURE_STABILIZE, TOPIC_TARS,
+ milliseconds{Config::TARS3::PRESSURE_STABILIZE_WAIT_TIME}
+ .count());
+ }
+
+ return HANDLED;
+ }
+
+ case TARS_PRESSURE_STABILIZED:
+ {
+ if (currentPressure < Config::TARS3::PRESSURE_LOWER_RANGE)
+ {
+ // If OX is cold enough, start a new filling-venting cycle
+ return transition(&TARS3::RefuelingFilling);
+ }
+ else
+ {
+ // OX is too hot, vent to lower the temperature
+ return transition(&TARS3::RefuelingVenting);
+ }
+ }
+
+ case EV_EMPTY:
+ {
+ return tranSuper(&TARS3::Refueling);
+ }
+
+ default:
+ {
+ return UNHANDLED;
+ }
+ }
+}
+
+State TARS3::RefuelingFilling(const Event& event)
+{
+ switch (event)
+ {
+ case EV_ENTRY:
+ {
+ LOG_INFO(logger, "Filling");
+ logAction(Tars3Action::FILLING, fillingTime.count());
+
+ getModule<Actuators>()->openServoWithTime(
+ ServosList::OX_FILLING_VALVE, fillingTime.count());
+
+ return HANDLED;
+ }
+
+ case MOTOR_OX_FIL_CLOSE:
+ {
+ LOG_INFO(logger, "Filling done");
+
+ return transition(&TARS3::RefuelingWaitAfterFilling);
+ }
+
+ case EV_EMPTY:
+ {
+ return tranSuper(&TARS3::Refueling);
+ }
+
+ default:
+ {
+ return UNHANDLED;
+ }
+ }
+}
+
+State TARS3::RefuelingWaitAfterFilling(const Event& event)
+{
+ {
+ Lock<FastMutex> lock(sampleMutex);
+ previousPressure = currentPressure;
+ currentPressure = pressureSample;
+ }
+
+ switch (event)
+ {
+ case EV_ENTRY:
+ case TARS_CHECK_PRESSURE_STABILIZE:
+ {
+ float pressureChange = std::abs(currentPressure - previousPressure);
+
+ if (pressureChange < Config::TARS3::PRESSURE_CHANGE_TOLERANCE)
+ {
+ // The pressure is stable
+ LOG_INFO(logger,
+ "Pressure is stable after filling, proceeding");
+ logAction(Tars3Action::PRESSURE_STABLE_FILLING, pressureChange);
+
+ EventBroker::getInstance().post(TARS_PRESSURE_STABILIZED,
+ TOPIC_TARS);
+ }
+ else
+ {
+ // Pressure is not stable yet, schedule a new check
+ LOG_INFO(logger,
+ "Waiting for pressure to stabilize after filling");
+ logAction(Tars3Action::WAITING_FILLING, pressureChange);
+
+ delayedEventId = EventBroker::getInstance().postDelayed(
+ TARS_CHECK_PRESSURE_STABILIZE, TOPIC_TARS,
+ milliseconds{Config::TARS3::PRESSURE_STABILIZE_WAIT_TIME}
+ .count());
+ }
+
+ return HANDLED;
+ }
+
+ case TARS_PRESSURE_STABILIZED:
+ {
+ if (currentPressure > Config::TARS3::PRESSURE_UPPER_RANGE)
+ {
+ // OX is hot, vent to lower the temperature and end the cycle
+ return transition(&TARS3::RefuelingVenting);
+ }
+ else
+ {
+ // OX is still cold enough, keep filling
+ return transition(&TARS3::RefuelingFilling);
+ }
+ }
+
+ case EV_EMPTY:
+ {
+ return tranSuper(&TARS3::Refueling);
+ }
+
+ default:
+ {
+ return UNHANDLED;
+ }
+ }
+}
+
+State TARS3::RefuelingVenting(const Event& event)
+{
+ switch (event)
+ {
+ case EV_ENTRY:
+ {
+ LOG_INFO(logger, "Venting");
+ logAction(Tars3Action::VENTING, ventingTime.count());
+
+ getModule<Actuators>()->openServoWithTime(
+ ServosList::OX_VENTING_VALVE, ventingTime.count());
+
+ return HANDLED;
+ }
+
+ case MOTOR_OX_VEN_CLOSE:
+ {
+ LOG_INFO(logger, "Venting done");
+
+ return transition(&TARS3::RefuelingWaitAfterCycle);
+ }
+
+ case EV_EMPTY:
+ {
+ return tranSuper(&TARS3::Refueling);
+ }
+
+ default:
+ {
+ return UNHANDLED;
+ }
+ }
+}
diff --git a/src/RIGv2/StateMachines/TARS3/TARS3.h b/src/RIGv2/StateMachines/TARS3/TARS3.h
new file mode 100644
index 0000000000000000000000000000000000000000..6d81274b8b685983bcf04c307d99d0014afcdff8
--- /dev/null
+++ b/src/RIGv2/StateMachines/TARS3/TARS3.h
@@ -0,0 +1,109 @@
+/* Copyright (c) 2025 Skyward Experimental Rocketry
+ * Author: Niccolò Betto
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+#include <events/HSM.h>
+#include <utils/DependencyManager/DependencyManager.h>
+
+#include "MedianFilter.h"
+#include "TARS3Data.h"
+
+namespace RIGv2
+{
+
+class Sensors;
+class Actuators;
+class BoardScheduler;
+
+class TARS3
+ : public Boardcore::InjectableWithDeps<BoardScheduler, Sensors, Actuators>,
+ public Boardcore::HSM<TARS3>
+{
+public:
+ TARS3();
+
+ [[nodiscard]] bool start();
+
+private:
+ void sample();
+
+ // HSM states
+
+ /**
+ * @brief TARS3 is ready and waiting for the user to start cold refueling.
+ */
+ Boardcore::State Ready(const Boardcore::Event& event);
+
+ /**
+ * @brief Super state for when TARS3 is refueling.
+ */
+ Boardcore::State Refueling(const Boardcore::Event& event);
+
+ /**
+ * @brief TARS3 is waiting for the pressure to stabilize after completing a
+ * filling-venting cycle.
+ * Super state: Refueling
+ */
+ Boardcore::State RefuelingWaitAfterCycle(const Boardcore::Event& event);
+
+ /**
+ * @brief TARS3 is filling the OX tank.
+ * Super state: Refueling
+ */
+ Boardcore::State RefuelingFilling(const Boardcore::Event& event);
+
+ /**
+ * @brief TARS3 is waiting for the pressure to stabilize after filling.
+ * Super state: Refueling
+ */
+ Boardcore::State RefuelingWaitAfterFilling(const Boardcore::Event& event);
+
+ /**
+ * @brief TARS3 is venting the OX tank.
+ * Super state: Refueling
+ */
+ Boardcore::State RefuelingVenting(const Boardcore::Event& event);
+
+ void logAction(Tars3Action action, float data = 0);
+ void logSample(float pressure, float mass);
+
+ std::chrono::milliseconds fillingTime = 0ms;
+ std::chrono::milliseconds ventingTime = 0ms;
+
+ float previousPressure = 0;
+ float currentPressure = 0;
+
+ int medianSamples = 0;
+ MedianFilter<float, Config::TARS3::MEDIAN_SAMPLE_NUMBER> massFilter;
+ MedianFilter<float, Config::TARS3::MEDIAN_SAMPLE_NUMBER> pressureFilter;
+
+ miosix::FastMutex sampleMutex;
+ float massSample = 0;
+ float pressureSample = 0;
+
+ uint16_t delayedEventId = 0;
+
+ Boardcore::Logger& sdLogger = Boardcore::Logger::getInstance();
+ Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("tars3");
+};
+} // namespace RIGv2
diff --git a/src/RIGv2/StateMachines/TARS3/TARS3Data.h b/src/RIGv2/StateMachines/TARS3/TARS3Data.h
new file mode 100644
index 0000000000000000000000000000000000000000..36a1e3b025c3a5000aee22ed49ec2905637918d8
--- /dev/null
+++ b/src/RIGv2/StateMachines/TARS3/TARS3Data.h
@@ -0,0 +1,118 @@
+/* Copyright (c) 2025 Skyward Experimental Rocketry
+ * Author: Niccolò Betto
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+#include <cstdint>
+#include <iostream>
+#include <string>
+
+namespace RIGv2
+{
+
+enum class Tars3Action : uint32_t
+{
+ READY = 0,
+ START,
+ WAITING_CYCLE,
+ WAITING_FILLING,
+ PRESSURE_STABLE_CYCLE,
+ PRESSURE_STABLE_FILLING,
+ FILLING,
+ VENTING,
+ MANUAL_STOP,
+ MANUAL_ACTION_STOP,
+};
+
+static std::array<const char*, 10> TARS3_ACTION_STRINGS = {
+ "READY",
+ "START",
+ "WAITING_CYCLE",
+ "WAITING_FILLING",
+ "PRESSURE_STABLE_CYCLE",
+ "PRESSURE_STABLE_FILLING",
+ "FILLING",
+ "VENTING",
+ "MANUAL_STOP",
+ "MANUAL_ACTION_STOP",
+};
+
+static std::array<const char*, 10> TARS3_ACTION_DATA_TYPE = {
+ "", // READY
+ "", // START
+ "PRESSURE_DELTA", // WAITING_CYCLE
+ "PRESSURE_DELTA", // WAITING_FILLING
+ "PRESSURE_DELTA", // PRESSURE_STABLE_CYCLE
+ "PRESSURE_DELTA", // PRESSURE_STABLE_FILLING
+ "OPEN_TIME", // FILLING
+ "OPEN_TIME", // VENTING
+ "", // MANUAL_STOP
+ "", // MANUAL_ACTION_STOP
+};
+
+inline std::ostream& operator<<(std::ostream& os, Tars3Action action)
+{
+ os << TARS3_ACTION_STRINGS[static_cast<uint32_t>(action)];
+ return os;
+}
+
+struct Tars3ActionData
+{
+ uint64_t timestamp = 0;
+ Tars3Action action = Tars3Action::READY;
+ float data = 0; // Additional data attached to the action
+
+ static std::string header()
+ {
+ return "timestamp,action,actionName,data,dataType\n";
+ }
+
+ void print(std::ostream& os) const
+ {
+ os << timestamp << "," << (int)action << "," << action << "," << data
+ << "," << TARS3_ACTION_DATA_TYPE[static_cast<uint32_t>(action)]
+ << "\n";
+ }
+};
+
+struct Tars3SampleData
+{
+ uint64_t timestamp;
+ float pressure;
+ float mass;
+
+ Tars3SampleData() : timestamp{0}, pressure{0}, mass{0} {}
+
+ Tars3SampleData(uint64_t timestamp, float pressure, float mass)
+ : timestamp{timestamp}, pressure{pressure}, mass{mass}
+ {
+ }
+
+ static std::string header() { return "timestamp,pressure,mass\n"; }
+
+ void print(std::ostream& os) const
+ {
+ os << timestamp << "," << pressure << "," << mass << "\n";
+ }
+};
+
+} // namespace RIGv2
diff --git a/src/RIGv2/rig-v2-entry.cpp b/src/RIGv2/rig-v2-entry.cpp
index 6548c191f8c6863add690115be2c3471a51d389e..5a61a17d7daeb4e9498dbd50ff650192b9c99681 100644
--- a/src/RIGv2/rig-v2-entry.cpp
+++ b/src/RIGv2/rig-v2-entry.cpp
@@ -29,6 +29,7 @@
#include <RIGv2/Sensors/Sensors.h>
#include <RIGv2/StateMachines/GroundModeManager/GroundModeManager.h>
#include <RIGv2/StateMachines/TARS1/TARS1.h>
+#include <RIGv2/StateMachines/TARS3/TARS3.h>
#include <common/Events.h>
#include <diagnostic/CpuMeter/CpuMeter.h>
#include <diagnostic/StackLogger.h>
@@ -60,6 +61,7 @@ int main()
CanHandler* canHandler = new CanHandler();
GroundModeManager* gmm = new GroundModeManager();
TARS1* tars1 = new TARS1();
+ TARS3* tars3 = new TARS3();
Radio* radio = new Radio();
Logger& sdLogger = Logger::getInstance();
@@ -83,7 +85,8 @@ int main()
manager.insert<CanHandler>(canHandler) &&
manager.insert<Registry>(registry) &&
manager.insert<GroundModeManager>(gmm) &&
- manager.insert<TARS1>(tars1) && manager.inject();
+ manager.insert<TARS1>(tars1) &&
+ manager.insert<TARS3>(tars3) && manager.inject();
if (!initResult)
{
@@ -172,6 +175,13 @@ int main()
std::cout << "*** Failed to start TARS1 ***" << std::endl;
}
+ std::cout << "Starting TARS3" << std::endl;
+ if (!tars3->start())
+ {
+ initResult = false;
+ std::cout << "*** Failed to start TARS3 ***" << std::endl;
+ }
+
std::cout << "Starting BoardScheduler" << std::endl;
if (!scheduler->start())
{