diff --git a/cmake/dependencies.cmake b/cmake/dependencies.cmake
index ac0c876bc981f542ded4bf4bcef87ce48af68af8..54daedb49676e8235e0b7bcdf9518680ceec0a33 100644
--- a/cmake/dependencies.cmake
+++ b/cmake/dependencies.cmake
@@ -73,6 +73,7 @@ set(RIG_V2_COMPUTER
src/boards/RIGv2/Sensors/Sensors.cpp
src/boards/RIGv2/Actuators/Actuators.cpp
src/boards/RIGv2/StateMachines/GroundModeManager/GroundModeManager.cpp
+ src/boards/RIGv2/StateMachines/TARS1/TARS1.cpp
)
set(CON_RIG_COMPUTER
diff --git a/src/boards/RIGv2/Actuators/Actuators.cpp b/src/boards/RIGv2/Actuators/Actuators.cpp
index 2483af3be6c0307d6e86d036b94c807af82968ea..3464c7aa2d94082280efd49c9732dc0d6728c602 100644
--- a/src/boards/RIGv2/Actuators/Actuators.cpp
+++ b/src/boards/RIGv2/Actuators/Actuators.cpp
@@ -93,7 +93,7 @@ float Actuators::ServoInfo::getServoPosition()
}
}
-Actuators::Actuators(Boardcore::TaskScheduler &scheduler) : scheduler{scheduler}
+Actuators::Actuators(TaskScheduler &scheduler) : scheduler{scheduler}
{
// Initialize servos
infos[0].servo = std::make_unique<Servo>(
@@ -312,6 +312,28 @@ bool Actuators::isServoOpen(ServosList servo)
return info->getServoPosition() > Config::Servos::SERVO_OPEN_THRESHOLD;
}
+uint64_t Actuators::getServoOpeningTime(ServosList servo) {
+ Lock<FastMutex> lock(infosMutex);
+ ServoInfo *info = getServo(servo);
+ if(info == nullptr)
+ {
+ return 0;
+ }
+
+ return info->openingTime;
+}
+
+float Actuators::getServoMaxAperture(ServosList servo) {
+ Lock<FastMutex> lock(infosMutex);
+ ServoInfo *info = getServo(servo);
+ if(info == nullptr)
+ {
+ return 0;
+ }
+
+ return info->maxAperture;
+}
+
Actuators::ServoInfo *Actuators::getServo(ServosList servo)
{
switch (servo)
diff --git a/src/boards/RIGv2/Actuators/Actuators.h b/src/boards/RIGv2/Actuators/Actuators.h
index 75aa4920e32a70c62dbd18b8d2a2de320baa588b..f824329a29b7e66c62dd5773e68d28e1e69b4581 100644
--- a/src/boards/RIGv2/Actuators/Actuators.h
+++ b/src/boards/RIGv2/Actuators/Actuators.h
@@ -77,6 +77,9 @@ public:
bool setOpeningTime(ServosList servo, uint64_t time);
bool isServoOpen(ServosList servo);
+ uint64_t getServoOpeningTime(ServosList servo);
+ float getServoMaxAperture(ServosList servo);
+
private:
ServoInfo *getServo(ServosList servo);
diff --git a/src/boards/RIGv2/Configs/TARS1Config.h b/src/boards/RIGv2/Configs/TARS1Config.h
new file mode 100644
index 0000000000000000000000000000000000000000..f9d372f67f8f9d2a6b8f5fa72dac993d1f87d71b
--- /dev/null
+++ b/src/boards/RIGv2/Configs/TARS1Config.h
@@ -0,0 +1,46 @@
+/* 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 <cstdint>
+
+namespace RIGv2
+{
+namespace Config
+{
+namespace TARS1
+{
+static constexpr uint32_t SAMPLE_PERIOD = 10;
+
+static constexpr uint32_t WASHING_OPENING_TIME = 5000;
+static constexpr uint32_t WASHING_TIME_DELAY = 1000;
+static constexpr uint32_t FILLING_OPENING_TIME = 900000;
+static constexpr uint32_t PRESSURE_STABILIZE_WAIT_TIME = 1000;
+
+static constexpr int NUM_MASS_STABLE_ITERATIONS = 2;
+
+static constexpr float MASS_TOLERANCE = 0.2; // [kg]
+static constexpr float PRESSURE_TOLERANCE = 0.035; // [bar]
+} // namespace TARS1
+} // namespace Config
+} // namespace RIGv2
\ No newline at end of file
diff --git a/src/boards/RIGv2/Radio/Radio.cpp b/src/boards/RIGv2/Radio/Radio.cpp
index bd978c62364d8941098fc1e4b70aa4ad311291f9..7bb15b073c595950099acf740145d8fb21531361 100644
--- a/src/boards/RIGv2/Radio/Radio.cpp
+++ b/src/boards/RIGv2/Radio/Radio.cpp
@@ -26,6 +26,7 @@
#include <RIGv2/Buses.h>
#include <RIGv2/Sensors/Sensors.h>
#include <RIGv2/StateMachines/GroundModeManager/GroundModeManager.h>
+#include <RIGv2/StateMachines/TARS1/TARS1.h>
#include <common/Events.h>
#include <common/Radio.h>
#include <events/EventBroker.h>
@@ -413,12 +414,16 @@ bool Radio::packSystemTm(uint8_t tmId, mavlink_message_t& msg)
actuators->isServoOpen(ServosList::RELEASE_VALVE) ? 1 : 0;
tm.main_valve_state =
actuators->isServoOpen(ServosList::MAIN_VALVE) ? 1 : 0;
- tm.arming_state = modules.get<GroundModeManager>()->isArmed();
- tm.ignition_state = modules.get<GroundModeManager>()->isIgniting();
+ tm.arming_state =
+ modules.get<GroundModeManager>()->isArmed() ? 1 : 0;
+ tm.ignition_state =
+ modules.get<GroundModeManager>()->isIgniting() ? 1 : 0;
+ tm.tars_state = modules.get<TARS1>()->isRefueling() ? 1 : 0;
// TODO(davide.mor): Add the rest of these
// Temporary hack to tell if the board initialized or not
- tm.main_board_status = modules.get<GroundModeManager>()->isDisarmed();
+ tm.main_board_status =
+ modules.get<GroundModeManager>()->isDisarmed();
tm.battery_voltage = sensors->getBatteryVoltage().voltage;
tm.current_consumption = sensors->getServoCurrent().current;
@@ -487,8 +492,7 @@ void Radio::handleConrigState(const mavlink_message_t& msg)
if (oldConrigState.arm_switch == 0 && state.arm_switch == 1)
{
// The ARM switch was pressed
- // TODO(davide.mor): Notify everybody of a manual actuation
-
+ EventBroker::getInstance().post(MOTOR_MANUAL_ACTION, TOPIC_TARS);
EventBroker::getInstance().post(TMTC_ARM, TOPIC_MOTOR);
lastManualActuation = currentTime;
@@ -497,8 +501,7 @@ void Radio::handleConrigState(const mavlink_message_t& msg)
if (oldConrigState.ignition_btn == 0 && state.ignition_btn == 1)
{
// The ignition switch was pressed
- // TODO(davide.mor): Notify everybody of a manual actuation
-
+ EventBroker::getInstance().post(MOTOR_MANUAL_ACTION, TOPIC_TARS);
EventBroker::getInstance().post(MOTOR_IGNITION, TOPIC_MOTOR);
lastManualActuation = currentTime;
@@ -508,8 +511,7 @@ void Radio::handleConrigState(const mavlink_message_t& msg)
state.filling_valve_btn == 1)
{
// The filling switch was pressed
- // TODO(davide.mor): Notify everybody of a manual actuation
-
+ EventBroker::getInstance().post(MOTOR_MANUAL_ACTION, TOPIC_TARS);
modules.get<Actuators>()->toggleServo(ServosList::FILLING_VALVE);
lastManualActuation = currentTime;
@@ -519,8 +521,7 @@ void Radio::handleConrigState(const mavlink_message_t& msg)
state.quick_connector_btn == 1)
{
// The quick conector switch was pressed
- // TODO(davide.mor): Notify everybody of a manual actuation
-
+ EventBroker::getInstance().post(MOTOR_MANUAL_ACTION, TOPIC_TARS);
modules.get<Actuators>()->toggleServo(ServosList::DISCONNECT_SERVO);
lastManualActuation = currentTime;
@@ -530,8 +531,7 @@ void Radio::handleConrigState(const mavlink_message_t& msg)
state.release_pressure_btn == 1)
{
// The release switch was pressed
- // TODO(davide.mor): Notify everybody of a manual actuation
-
+ EventBroker::getInstance().post(MOTOR_MANUAL_ACTION, TOPIC_TARS);
modules.get<Actuators>()->toggleServo(ServosList::RELEASE_VALVE);
lastManualActuation = currentTime;
@@ -541,19 +541,25 @@ void Radio::handleConrigState(const mavlink_message_t& msg)
state.venting_valve_btn == 1)
{
// The venting switch was pressed
- // TODO(davide.mor): Notify everybody of a manual actuation
-
+ EventBroker::getInstance().post(MOTOR_MANUAL_ACTION, TOPIC_TARS);
modules.get<Actuators>()->toggleServo(ServosList::VENTING_VALVE);
lastManualActuation = currentTime;
}
+
+ if (oldConrigState.start_tars_btn == 0 && state.start_tars_btn == 1)
+ {
+ // The TARS switch was pressed
+ EventBroker::getInstance().post(MOTOR_START_TARS, TOPIC_TARS);
+
+ lastManualActuation = currentTime;
+ }
}
// Special case for disarming, that can be done bypassing the timeout
if (oldConrigState.arm_switch == 1 && state.arm_switch == 0)
{
- // TODO(davide.mor): Notify everybody of a manual actuation
-
+ EventBroker::getInstance().post(MOTOR_MANUAL_ACTION, TOPIC_TARS);
EventBroker::getInstance().post(TMTC_DISARM, TOPIC_MOTOR);
lastManualActuation = currentTime;
diff --git a/src/boards/RIGv2/StateMachines/GroundModeManager/GroundModeManager.cpp b/src/boards/RIGv2/StateMachines/GroundModeManager/GroundModeManager.cpp
index a48feec0a3e41e328e1732dc5ee6b1b646a3f1d6..b7ae89aff0b4ea8b40bc146ac35a059fa87c899e 100644
--- a/src/boards/RIGv2/StateMachines/GroundModeManager/GroundModeManager.cpp
+++ b/src/boards/RIGv2/StateMachines/GroundModeManager/GroundModeManager.cpp
@@ -69,7 +69,7 @@ void GroundModeManager::state_idle(const Boardcore::Event &event)
{
case EV_ENTRY:
{
- logStatus(GroundModeManagerState::STATE_IDLE);
+ logStatus(GMM_STATE_IDLE);
break;
}
@@ -93,7 +93,7 @@ void GroundModeManager::state_init_err(const Boardcore::Event &event)
{
case EV_ENTRY:
{
- logStatus(GroundModeManagerState::STATE_INIT_ERR);
+ logStatus(GMM_STATE_INIT_ERR);
break;
}
@@ -113,7 +113,7 @@ void GroundModeManager::state_disarmed(const Boardcore::Event &event)
case EV_ENTRY:
{
armLightOff();
- logStatus(GroundModeManagerState::STATE_DISARMED);
+ logStatus(GMM_STATE_DISARMED);
break;
}
@@ -142,7 +142,7 @@ void GroundModeManager::state_armed(const Boardcore::Event &event)
{
armLightOn();
modules.get<Actuators>()->closeAllServos();
- logStatus(GroundModeManagerState::STATE_ARMED);
+ logStatus(GMM_STATE_ARMED);
break;
}
@@ -174,7 +174,7 @@ void GroundModeManager::state_igniting(const Boardcore::Event &event)
openOxidantDelayEventId = EventBroker::getInstance().postDelayed(
MOTOR_OPEN_OXIDANT, TOPIC_MOTOR, ignitionTime);
- logStatus(GroundModeManagerState::STATE_IGNITING);
+ logStatus(GMM_STATE_IGNITING);
break;
}
diff --git a/src/boards/RIGv2/StateMachines/GroundModeManager/GroundModeManager.h b/src/boards/RIGv2/StateMachines/GroundModeManager/GroundModeManager.h
index 79699bcac1c0fc92fa3a16c8214a4063348e4bee..07276ebe896c01f94b4439879c58fa152a1ae531 100644
--- a/src/boards/RIGv2/StateMachines/GroundModeManager/GroundModeManager.h
+++ b/src/boards/RIGv2/StateMachines/GroundModeManager/GroundModeManager.h
@@ -56,7 +56,7 @@ private:
void logStatus(GroundModeManagerState newState);
Boardcore::Logger &sdLogger = Boardcore::Logger::getInstance();
- Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("sensors");
+ Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("gmm");
uint16_t openOxidantDelayEventId = -1;
std::atomic<uint32_t> ignitionTime{
diff --git a/src/boards/RIGv2/StateMachines/GroundModeManager/GroundModeManagerData.h b/src/boards/RIGv2/StateMachines/GroundModeManager/GroundModeManagerData.h
index eb83cd754fc9e244046c992cf18cde3702d24621..d2d227b043aa7c85abbec7f22fb2545b155be261 100644
--- a/src/boards/RIGv2/StateMachines/GroundModeManager/GroundModeManagerData.h
+++ b/src/boards/RIGv2/StateMachines/GroundModeManager/GroundModeManagerData.h
@@ -31,12 +31,11 @@ namespace RIGv2
enum GroundModeManagerState : uint8_t
{
- UNINIT = 0,
- STATE_IDLE = 1,
- STATE_INIT_ERR = 2,
- STATE_DISARMED = 3,
- STATE_ARMED = 4,
- STATE_IGNITING = 5,
+ GMM_STATE_IDLE = 0,
+ GMM_STATE_INIT_ERR,
+ GMM_STATE_DISARMED,
+ GMM_STATE_ARMED,
+ GMM_STATE_IGNITING,
};
struct GroundModeManagerData
@@ -45,7 +44,7 @@ struct GroundModeManagerData
GroundModeManagerState state;
GroundModeManagerData()
- : timestamp{0}, state{GroundModeManagerState::UNINIT}
+ : timestamp{0}, state{GMM_STATE_IDLE}
{
}
diff --git a/src/boards/RIGv2/StateMachines/TARS1/TARS1.cpp b/src/boards/RIGv2/StateMachines/TARS1/TARS1.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ccd34de6d598fdeb4cb7d0ea4bc2d4c3b4d98194
--- /dev/null
+++ b/src/boards/RIGv2/StateMachines/TARS1/TARS1.cpp
@@ -0,0 +1,287 @@
+/* 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.
+ */
+
+#include "TARS1.h"
+
+#include <RIGv2/Actuators/Actuators.h>
+#include <RIGv2/Configs/TARS1Config.h>
+#include <RIGv2/Sensors/Sensors.h>
+#include <common/Events.h>
+#include <events/EventBroker.h>
+// TODO(davide.mor): Remove TimestampTimer
+#include <drivers/timer/TimestampTimer.h>
+
+using namespace Boardcore;
+using namespace RIGv2;
+using namespace Common;
+using namespace miosix;
+
+TARS1::TARS1(TaskScheduler& scheduler)
+ : FSM(&TARS1::state_ready), scheduler{scheduler}
+{
+ EventBroker::getInstance().subscribe(this, TOPIC_TARS);
+ EventBroker::getInstance().subscribe(this, TOPIC_MOTOR);
+}
+
+bool TARS1::start()
+{
+ uint8_t result =
+ scheduler.addTask([this]() { sample(); }, Config::TARS1::SAMPLE_PERIOD);
+
+ if (result == 0)
+ {
+ LOG_ERR(logger, "Failed to add TARS1 sample task");
+ return false;
+ }
+
+ if (!ActiveObject::start())
+ {
+ LOG_ERR(logger, "Failed to activate TARS1 thread");
+ return false;
+ }
+
+ return true;
+}
+
+bool TARS1::isRefueling()
+{
+ return testState(&TARS1::state_refueling);
+}
+
+void TARS1::state_ready(const Event& event)
+{
+ switch (event)
+ {
+ case EV_ENTRY:
+ {
+ logAction(TARS_ACTION_READY);
+ break;
+ }
+
+ case MOTOR_START_TARS:
+ {
+ transition(&TARS1::state_refueling);
+ break;
+ }
+ }
+}
+
+void TARS1::state_refueling(const Event& event)
+{
+ ModuleManager& modules = ModuleManager::getInstance();
+ Actuators* actuators = modules.get<Actuators>();
+
+ switch (event)
+ {
+ case EV_ENTRY:
+ {
+ // Reset TARS state
+ massStableCounter = 0;
+ previousMass = 0.0f;
+ currentMass = 0.0f;
+ previousPressure = 0.0f;
+ currentPressure = 0.0f;
+ // TODO(davide.mor): Add the rest
+
+ // First close all valves
+ actuators->closeAllServos();
+
+ LOG_INFO(logger, "TARS start washing");
+ logAction(TARS_ACTION_WASHING);
+
+ // Start washing
+ actuators->openServoWithTime(ServosList::VENTING_VALVE,
+ Config::TARS1::WASHING_OPENING_TIME);
+
+ // Wait a bit so that the servo don't actuate at the same time
+ Thread::sleep(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);
+
+ break;
+ }
+
+ case TARS_WASHING_DONE:
+ {
+ LOG_INFO(logger, "TARS washing done");
+ logAction(TARS_ACTION_OPEN_FILLING);
+
+ // Open the filling for a long time
+ actuators->openServoWithTime(ServosList::FILLING_VALVE,
+ Config::TARS1::FILLING_OPENING_TIME);
+
+ nextDelayedEventId = EventBroker::getInstance().postDelayed(
+ TARS_PRESSURE_STABILIZED, TOPIC_TARS,
+ Config::TARS1::PRESSURE_STABILIZE_WAIT_TIME);
+
+ break;
+ }
+
+ case TARS_PRESSURE_STABILIZED:
+ {
+ LOG_INFO(logger, "TARS check mass");
+ logAction(TARS_ACTION_CHECK_MASS);
+
+ // Lock in a new mass value
+ {
+ Lock<FastMutex> lock(sampleMutex);
+ previousMass = currentMass;
+ currentMass = massSample;
+ }
+
+ 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;
+ }
+
+ LOG_INFO(logger, "TARS open venting");
+ logAction(TARS_ACTION_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;
+
+ nextDelayedEventId = EventBroker::getInstance().postDelayed(
+ TARS_CHECK_PRESSURE_STABILIZE, TOPIC_TARS, nextCheckTime);
+ break;
+ }
+
+ case TARS_CHECK_PRESSURE_STABILIZE:
+ {
+ LOG_INFO(logger, "TARS check pressure");
+ logAction(TARS_ACTION_CHECK_PRESSURE);
+
+ {
+ Lock<FastMutex> lock(sampleMutex);
+ previousPressure = currentPressure;
+ currentPressure = pressureSample;
+ }
+
+ 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;
+ }
+
+ case TARS_FILLING_DONE:
+ {
+ LOG_INFO(logger, "TARS filling done");
+ logAction(TARS_ACTION_AUTOMATIC_STOP);
+
+ actuators->closeAllServos();
+ transition(&TARS1::state_ready);
+ break;
+ }
+
+ case MOTOR_MANUAL_ACTION:
+ {
+ LOG_INFO(logger, "TARS manual stop");
+ logAction(TARS_ACTION_MANUAL_STOP);
+
+ // Disable next event
+ EventBroker::getInstance().removeDelayed(nextDelayedEventId);
+ transition(&TARS1::state_ready);
+ break;
+ }
+
+ case MOTOR_START_TARS:
+ {
+ LOG_INFO(logger, "TARS manual stop");
+ logAction(TARS_ACTION_MANUAL_STOP);
+
+ // The user requested that we stop
+ modules.get<Actuators>()->closeAllServos();
+ // Disable next event
+ EventBroker::getInstance().removeDelayed(nextDelayedEventId);
+ transition(&TARS1::state_ready);
+ }
+ }
+}
+
+void TARS1::sample()
+{
+ ModuleManager& modules = ModuleManager::getInstance();
+ Sensors* sensors = modules.get<Sensors>();
+
+ float pressure = sensors->getTankBottomPress().pressure;
+ float mass = sensors->getTankWeight().load;
+
+ // TODO(davide.mor): Perform filtering on input data
+
+ logSample(pressure, mass);
+
+ {
+ Lock<FastMutex> lock(sampleMutex);
+ pressureSample = pressure;
+ massSample = mass;
+ }
+}
+
+void TARS1::logAction(TarsActionType action)
+{
+ TarsActionData data = {TimestampTimer::getTimestamp(), action};
+ sdLogger.log(data);
+}
+
+void TARS1::logSample(float pressure, float mass)
+{
+ TarsSampleData data = {TimestampTimer::getTimestamp(), pressure, mass};
+ sdLogger.log(data);
+}
\ No newline at end of file
diff --git a/src/boards/RIGv2/StateMachines/TARS1/TARS1.h b/src/boards/RIGv2/StateMachines/TARS1/TARS1.h
new file mode 100644
index 0000000000000000000000000000000000000000..7422ba1f856839fc55f179d2f5b65bca562a817f
--- /dev/null
+++ b/src/boards/RIGv2/StateMachines/TARS1/TARS1.h
@@ -0,0 +1,72 @@
+/* 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 <events/FSM.h>
+#include <miosix.h>
+#include <scheduler/TaskScheduler.h>
+#include <RIGv2/StateMachines/TARS1/TARS1Data.h>
+
+#include <utils/ModuleManager/ModuleManager.hpp>
+
+namespace RIGv2
+{
+
+class TARS1 : public Boardcore::Module, public Boardcore::FSM<TARS1>
+{
+public:
+ TARS1(Boardcore::TaskScheduler &scheduler);
+
+ [[nodiscard]] bool start();
+
+ bool isRefueling();
+
+private:
+ void sample();
+
+ void state_ready(const Boardcore::Event &event);
+ void state_refueling(const Boardcore::Event &event);
+
+ void logAction(TarsActionType action);
+ void logSample(float pressure, float mass);
+
+ Boardcore::Logger &sdLogger = Boardcore::Logger::getInstance();
+ Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("tars1");
+
+ Boardcore::TaskScheduler &scheduler;
+
+ float previousMass = 0;
+ float currentMass = 0;
+
+ float previousPressure = 0;
+ float currentPressure = 0;
+
+ miosix::FastMutex sampleMutex;
+ float massSample = 0;
+ float pressureSample = 0;
+
+ int massStableCounter = 0;
+ uint16_t nextDelayedEventId = 0;
+};
+
+} // namespace RIGv2
\ No newline at end of file
diff --git a/src/boards/RIGv2/StateMachines/TARS1/TARS1Data.h b/src/boards/RIGv2/StateMachines/TARS1/TARS1Data.h
new file mode 100644
index 0000000000000000000000000000000000000000..2842bea144478276e17f665598cc2b29285fd5f3
--- /dev/null
+++ b/src/boards/RIGv2/StateMachines/TARS1/TARS1Data.h
@@ -0,0 +1,85 @@
+/* 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 <cstdint>
+#include <iostream>
+#include <string>
+
+namespace RIGv2
+{
+
+enum TarsActionType : uint8_t
+{
+ TARS_ACTION_READY = 0,
+ TARS_ACTION_WASHING,
+ TARS_ACTION_OPEN_FILLING,
+ TARS_ACTION_OPEN_VENTING,
+ TARS_ACTION_CHECK_PRESSURE,
+ TARS_ACTION_CHECK_MASS,
+ TARS_ACTION_AUTOMATIC_STOP,
+ TARS_ACTION_MANUAL_STOP,
+};
+
+struct TarsActionData
+{
+ uint64_t timestamp;
+ TarsActionType action;
+
+ TarsActionData() : timestamp{0}, action{TARS_ACTION_READY} {}
+
+ TarsActionData(uint64_t timestamp, TarsActionType action)
+ : timestamp{timestamp}, action{action}
+ {
+ }
+
+ static std::string header() { return "timestamp,action\n"; }
+
+ void print(std::ostream &os) const
+ {
+ os << timestamp << "," << (int)action << "\n";
+ }
+};
+
+struct TarsSampleData
+{
+ uint64_t timestamp;
+ float pressure;
+ float mass;
+
+ TarsSampleData() : timestamp{0}, pressure{0}, mass{0} {}
+
+ TarsSampleData(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
\ No newline at end of file
diff --git a/src/entrypoints/RIGv2/rig-v2-entry.cpp b/src/entrypoints/RIGv2/rig-v2-entry.cpp
index 4825a5a7cf65665a36135eed5faa25304474f2e1..b93bbab2329a5c323510b8e5ad53289ad1a04843 100644
--- a/src/entrypoints/RIGv2/rig-v2-entry.cpp
+++ b/src/entrypoints/RIGv2/rig-v2-entry.cpp
@@ -25,6 +25,7 @@
#include <RIGv2/Radio/Radio.h>
#include <RIGv2/Sensors/Sensors.h>
#include <RIGv2/StateMachines/GroundModeManager/GroundModeManager.h>
+#include <RIGv2/StateMachines/TARS1/TARS1.h>
#include <common/Events.h>
#include <diagnostic/CpuMeter/CpuMeter.h>
#include <diagnostic/StackLogger.h>
@@ -52,6 +53,7 @@ int main()
Sensors *sensors = new Sensors(*scheduler1);
Actuators *actuators = new Actuators(*scheduler2);
GroundModeManager *gmm = new GroundModeManager();
+ TARS1 *tars1 = new TARS1(*scheduler2);
Radio *radio = new Radio();
Logger &sdLogger = Logger::getInstance();
@@ -99,6 +101,12 @@ int main()
LOG_ERR(logger, "Error failed to insert GroundModeManager");
}
+ if (!modules.insert<TARS1>(tars1))
+ {
+ initResult = false;
+ LOG_ERR(logger, "Error failed to insert TARS1");
+ }
+
// Start modules
if (!sdLogger.testSDCard())
{
@@ -136,6 +144,12 @@ int main()
LOG_ERR(logger, "Error failed to start GroundModeManager module");
}
+ if (!tars1->start())
+ {
+ initResult = false;
+ LOG_ERR(logger, "Error failed to start TARS1 module");
+ }
+
if (!scheduler1->start() || !scheduler2->start())
{
initResult = false;
diff --git a/src/scripts/logdecoder/RIGv2/logdecoder.cpp b/src/scripts/logdecoder/RIGv2/logdecoder.cpp
index 75bbe6dc64885e7e6a9c6dd99a6212a1dae4fc6f..1d63f0bec03bcd54df04b415ac41d1ea6bf13658 100644
--- a/src/scripts/logdecoder/RIGv2/logdecoder.cpp
+++ b/src/scripts/logdecoder/RIGv2/logdecoder.cpp
@@ -23,6 +23,7 @@
#include <RIGv2/Sensors/SensorsData.h>
#include <RIGv2/Actuators/ActuatorsData.h>
#include <RIGv2/StateMachines/GroundModeManager/GroundModeManagerData.h>
+#include <RIGv2/StateMachines/TARS1/TARS1Data.h>
#include <logger/Deserializer.h>
#include <logger/LogTypes.h>
#include <tscpp/stream.h>
@@ -56,6 +57,8 @@ void registerTypes(Deserializer& ds)
ds.registerType<TCsData>();
ds.registerType<ActuatorsData>();
ds.registerType<GroundModeManagerData>();
+ ds.registerType<TarsActionData>();
+ ds.registerType<TarsSampleData>();
}
void showUsage(const string& cmdName)