From 22dbef59214228f0647c7cbfb1e19bb53da264da Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Niccol=C3=B2=20Betto?= <niccolo.betto@skywarder.eu>
Date: Wed, 27 Nov 2024 11:46:38 +0100
Subject: [PATCH] [RIGv2] Implement chamber valve control logic
---
src/RIGv2/Actuators/Actuators.cpp | 19 +++----
src/RIGv2/Actuators/Actuators.h | 13 ++---
src/RIGv2/Configs/ActuatorsConfig.h | 5 --
src/RIGv2/Configs/GMMConfig.h | 6 ++-
src/RIGv2/Radio/Radio.cpp | 22 +++++++-
src/RIGv2/Registry/Registry.h | 2 +
.../GroundModeManager/GroundModeManager.cpp | 54 ++++++++++++++-----
.../GroundModeManager/GroundModeManager.h | 5 +-
src/common/Events.h | 2 +
9 files changed, 88 insertions(+), 40 deletions(-)
diff --git a/src/RIGv2/Actuators/Actuators.cpp b/src/RIGv2/Actuators/Actuators.cpp
index cb671723e..d2a92c244 100644
--- a/src/RIGv2/Actuators/Actuators.cpp
+++ b/src/RIGv2/Actuators/Actuators.cpp
@@ -408,12 +408,7 @@ bool Actuators::isCanServoOpen(ServosList servo)
return false;
}
-void Actuators::openNitrogen()
-{
- openNitrogenWithTime(Config::Actuators::NITROGEN_OPENING_TIME);
-}
-
-void Actuators::openNitrogenWithTime(uint32_t time)
+void Actuators::openChamberWithTime(uint32_t time)
{
Lock<FastMutex> lock(infosMutex);
long long currentTime = getTime();
@@ -422,13 +417,13 @@ void Actuators::openNitrogenWithTime(uint32_t time)
nitrogenLastActionTs = currentTime;
}
-void Actuators::closeNitrogen()
+void Actuators::closeChamber()
{
Lock<FastMutex> lock(infosMutex);
nitrogenCloseTs = 0;
}
-bool Actuators::isNitrogenOpen()
+bool Actuators::isChamberOpen()
{
Lock<FastMutex> lock(infosMutex);
return nitrogenCloseTs != 0;
@@ -511,9 +506,9 @@ void Actuators::unsafeSetServoPosition(uint8_t idx, float position)
sdLogger.log(data);
}
-void Actuators::unsafeOpenNitrogen() { relays::nitrogen::low(); }
+void Actuators::unsafeOpenChamber() { relays::nitrogen::low(); }
-void Actuators::unsafeCloseNitrogen() { relays::nitrogen::high(); }
+void Actuators::unsafeCloseChamber() { relays::nitrogen::high(); }
void Actuators::updatePositionsTask()
{
@@ -571,12 +566,12 @@ void Actuators::updatePositionsTask()
// Handle nitrogen logic
if (currentTime < nitrogenCloseTs)
{
- unsafeOpenNitrogen();
+ unsafeOpenChamber();
}
else
{
nitrogenCloseTs = 0;
- unsafeCloseNitrogen();
+ unsafeCloseChamber();
}
}
diff --git a/src/RIGv2/Actuators/Actuators.h b/src/RIGv2/Actuators/Actuators.h
index 41cf009ff..08b66c634 100644
--- a/src/RIGv2/Actuators/Actuators.h
+++ b/src/RIGv2/Actuators/Actuators.h
@@ -92,10 +92,11 @@ public:
bool setOpeningTime(ServosList servo, uint32_t time);
bool isServoOpen(ServosList servo);
bool isCanServoOpen(ServosList servo);
- void openNitrogen();
- void openNitrogenWithTime(uint32_t time);
- void closeNitrogen();
- bool isNitrogenOpen();
+
+ // Chamber valve control
+ void openChamberWithTime(uint32_t time);
+ void closeChamber();
+ bool isChamberOpen();
uint32_t getServoOpeningTime(ServosList servo);
float getServoMaxAperture(ServosList servo);
@@ -114,8 +115,8 @@ private:
ServoInfo* getServo(ServosList servo);
void unsafeSetServoPosition(uint8_t idx, float position);
- void unsafeOpenNitrogen();
- void unsafeCloseNitrogen();
+ void unsafeOpenChamber();
+ void unsafeCloseChamber();
void updatePositionsTask();
diff --git a/src/RIGv2/Configs/ActuatorsConfig.h b/src/RIGv2/Configs/ActuatorsConfig.h
index bfa3ddf43..74b5a06ac 100644
--- a/src/RIGv2/Configs/ActuatorsConfig.h
+++ b/src/RIGv2/Configs/ActuatorsConfig.h
@@ -76,10 +76,5 @@ constexpr bool DISCONNECT_FLIPPED = false;
} // namespace Servos
-namespace Actuators
-{
-static constexpr uint32_t NITROGEN_OPENING_TIME = 5000; // 5s
-}
-
} // namespace Config
} // namespace RIGv2
diff --git a/src/RIGv2/Configs/GMMConfig.h b/src/RIGv2/Configs/GMMConfig.h
index cc02af3a0..93e60c045 100644
--- a/src/RIGv2/Configs/GMMConfig.h
+++ b/src/RIGv2/Configs/GMMConfig.h
@@ -33,8 +33,10 @@ namespace GroundModeManager
constexpr uint32_t DEFAULT_IGNITION_WAITING_TIME = 3700; // [ms]
-constexpr uint32_t MOTOR_COOLING_TIME = 1500;
-constexpr uint32_t NITROGEN_TIME = 20000; // 20s
+/// Delay between the main valve opening and the chamber valve opening
+constexpr uint32_t DEFAULT_CHAMBER_VALVE_DELAY = 500;
+// Time the chamber valve stays open
+constexpr uint32_t DEFAULT_CHAMBER_VALVE_TIME = 6000;
} // namespace GroundModeManager
} // namespace Config
diff --git a/src/RIGv2/Radio/Radio.cpp b/src/RIGv2/Radio/Radio.cpp
index f657b77f3..fe44a57c2 100644
--- a/src/RIGv2/Radio/Radio.cpp
+++ b/src/RIGv2/Radio/Radio.cpp
@@ -259,6 +259,26 @@ void Radio::handleMessage(const mavlink_message_t& msg)
break;
}
+ case MAVLINK_MSG_ID_SET_NITROGEN_TIME_TC:
+ {
+ // Chamber valve opening time
+ uint32_t timing = mavlink_msg_set_nitrogen_time_tc_get_timing(&msg);
+ getModule<GroundModeManager>()->setChamberTime(timing);
+
+ enqueueAck(msg);
+ break;
+ }
+
+ case MAVLINK_MSG_ID_SET_COOLING_TIME_TC:
+ {
+ // Chamber valve delay after main valve opening
+ uint32_t timing = mavlink_msg_set_cooling_time_tc_get_timing(&msg);
+ getModule<GroundModeManager>()->setChamberDelay(timing);
+
+ enqueueAck(msg);
+ break;
+ }
+
default:
{
// Unrecognized packet
@@ -569,7 +589,7 @@ bool Radio::enqueueSystemTm(uint8_t tmId)
actuators->isServoOpen(ServosList::MAIN_VALVE);
tm.nitrogen_valve_state =
actuators->isServoOpen(ServosList::NITROGEN_VALVE);
- tm.ignition_state = actuators->isNitrogenOpen();
+ tm.ignition_state = actuators->isChamberOpen();
// Internal states
tm.gmm_state = getModule<GroundModeManager>()->getState();
diff --git a/src/RIGv2/Registry/Registry.h b/src/RIGv2/Registry/Registry.h
index 81164f317..1f9b99b61 100644
--- a/src/RIGv2/Registry/Registry.h
+++ b/src/RIGv2/Registry/Registry.h
@@ -43,6 +43,8 @@ enum ConfigurationKeys
CONFIG_ID_IGNITION_TIME = 11,
CONFIG_ID_DEFAULT_OPENING_TIME = 12,
CONFIG_ID_DEFAULT_MAX_APERTURE = 13,
+ CONFIG_ID_CHAMBER_TIME = 14,
+ CONFIG_ID_CHAMBER_DELAY = 15,
};
const char* configurationIdToName(Boardcore::ConfigurationId id);
diff --git a/src/RIGv2/StateMachines/GroundModeManager/GroundModeManager.cpp b/src/RIGv2/StateMachines/GroundModeManager/GroundModeManager.cpp
index 2b372417e..cfff1aaa0 100644
--- a/src/RIGv2/StateMachines/GroundModeManager/GroundModeManager.cpp
+++ b/src/RIGv2/StateMachines/GroundModeManager/GroundModeManager.cpp
@@ -47,6 +47,16 @@ void GroundModeManager::setIgnitionTime(uint32_t time)
getModule<Registry>()->setUnsafe(CONFIG_ID_IGNITION_TIME, time);
}
+void GroundModeManager::setChamberTime(uint32_t time)
+{
+ getModule<Registry>()->setUnsafe(CONFIG_ID_CHAMBER_TIME, time);
+}
+
+void GroundModeManager::setChamberDelay(uint32_t time)
+{
+ getModule<Registry>()->setUnsafe(CONFIG_ID_CHAMBER_DELAY, time);
+}
+
State GroundModeManager::state_idle(const Event& event)
{
switch (event)
@@ -194,7 +204,11 @@ State GroundModeManager::state_disarmed(const Event& event)
case TMTC_OPEN_NITROGEN:
{
- getModule<Actuators>()->openNitrogen();
+ uint32_t chamberTime = getModule<Registry>()->getOrSetDefaultUnsafe(
+ CONFIG_ID_CHAMBER_TIME,
+ Config::GroundModeManager::DEFAULT_CHAMBER_VALVE_TIME);
+
+ getModule<Actuators>()->openChamberWithTime(chamberTime);
return HANDLED;
}
@@ -227,7 +241,7 @@ State GroundModeManager::state_armed(const Event& event)
getModule<CanHandler>()->sendEvent(CanConfig::EventId::ARM);
getModule<Actuators>()->armLightOn();
getModule<Actuators>()->closeAllServos();
- getModule<Actuators>()->closeNitrogen();
+ getModule<Actuators>()->closeChamber();
return HANDLED;
}
@@ -248,7 +262,10 @@ State GroundModeManager::state_armed(const Event& event)
case TMTC_OPEN_NITROGEN:
{
- getModule<Actuators>()->openNitrogen();
+ uint32_t chamberTime = getModule<Registry>()->getOrSetDefaultUnsafe(
+ CONFIG_ID_CHAMBER_TIME,
+ Config::GroundModeManager::DEFAULT_CHAMBER_VALVE_TIME);
+ getModule<Actuators>()->openChamberWithTime(chamberTime);
// Nitrogen causes automatic disarm
return transition(&GroundModeManager::state_disarmed);
@@ -289,7 +306,6 @@ State GroundModeManager::state_firing(const Event& event)
// Disable all events
EventBroker::getInstance().removeDelayed(openOxidantDelayEventId);
- EventBroker::getInstance().removeDelayed(coolingDelayEventId);
return HANDLED;
}
@@ -307,7 +323,11 @@ State GroundModeManager::state_firing(const Event& event)
case TMTC_OPEN_NITROGEN:
{
// Open nitrogen
- getModule<Actuators>()->openNitrogen();
+ uint32_t chamberTime = getModule<Registry>()->getOrSetDefaultUnsafe(
+ CONFIG_ID_CHAMBER_TIME,
+ Config::GroundModeManager::DEFAULT_CHAMBER_VALVE_TIME);
+
+ getModule<Actuators>()->openChamberWithTime(chamberTime);
return transition(&GroundModeManager::state_disarmed);
}
@@ -315,9 +335,6 @@ State GroundModeManager::state_firing(const Event& event)
case MOTOR_COOLING_TIMEOUT: // Normal firing end
case TMTC_DISARM: // Abort signal
{
- getModule<Actuators>()->openNitrogenWithTime(
- Config::GroundModeManager::NITROGEN_TIME);
-
return transition(&GroundModeManager::state_disarmed);
}
@@ -389,6 +406,14 @@ State GroundModeManager::state_oxidizer(const Event& event)
getModule<Actuators>()->openServo(ServosList::MAIN_VALVE);
+ uint32_t chamberDelay =
+ getModule<Registry>()->getOrSetDefaultUnsafe(
+ CONFIG_ID_CHAMBER_DELAY,
+ Config::GroundModeManager::DEFAULT_CHAMBER_VALVE_DELAY);
+
+ EventBroker::getInstance().postDelayed(MOTOR_OPEN_CHAMBER,
+ TOPIC_MOTOR, chamberDelay);
+
return HANDLED;
}
@@ -407,8 +432,15 @@ State GroundModeManager::state_oxidizer(const Event& event)
return HANDLED;
}
- case MOTOR_CLOSE_FEED_VALVE:
+ case MOTOR_OPEN_CHAMBER:
{
+ uint32_t chamberTime = getModule<Registry>()->getOrSetDefaultUnsafe(
+ CONFIG_ID_CHAMBER_TIME,
+ Config::GroundModeManager::DEFAULT_CHAMBER_VALVE_TIME);
+
+ // Open the chamber valve
+ getModule<Actuators>()->openChamberWithTime(chamberTime);
+
return transition(&GroundModeManager::state_cooling);
}
@@ -427,10 +459,6 @@ State GroundModeManager::state_cooling(const Event& event)
{
updateAndLogStatus(GroundModeManagerState::COOLING);
- coolingDelayEventId = EventBroker::getInstance().postDelayed(
- MOTOR_COOLING_TIMEOUT, TOPIC_MOTOR,
- Config::GroundModeManager::MOTOR_COOLING_TIME);
-
return HANDLED;
}
diff --git a/src/RIGv2/StateMachines/GroundModeManager/GroundModeManager.h b/src/RIGv2/StateMachines/GroundModeManager/GroundModeManager.h
index a4939c0cb..3abbb141c 100644
--- a/src/RIGv2/StateMachines/GroundModeManager/GroundModeManager.h
+++ b/src/RIGv2/StateMachines/GroundModeManager/GroundModeManager.h
@@ -51,6 +51,10 @@ public:
void setIgnitionTime(uint32_t time);
+ void setChamberTime(uint32_t time);
+
+ void setChamberDelay(uint32_t time);
+
private:
Boardcore::State state_idle(const Boardcore::Event& event);
Boardcore::State state_init(const Boardcore::Event& event);
@@ -70,7 +74,6 @@ private:
std::atomic<GroundModeManagerState> state{GroundModeManagerState::IDLE};
uint16_t openOxidantDelayEventId = -1;
- uint16_t coolingDelayEventId = -1;
};
} // namespace RIGv2
diff --git a/src/common/Events.h b/src/common/Events.h
index fc524997e..efff19c8d 100644
--- a/src/common/Events.h
+++ b/src/common/Events.h
@@ -158,6 +158,7 @@ enum Events : uint8_t
MOTOR_OPEN_OXIDANT,
MOTOR_COOLING_TIMEOUT,
MOTOR_SHADOW_MODE_TIMEOUT,
+ MOTOR_OPEN_CHAMBER,
TARS_WASHING_DONE,
TARS_CHECK_PRESSURE_STABILIZE,
TARS_PRESSURE_STABILIZED,
@@ -293,6 +294,7 @@ inline std::string getEventString(uint8_t event)
{MOTOR_OPEN_OXIDANT, "MOTOR_OPEN_OXIDANT"},
{MOTOR_COOLING_TIMEOUT, "MOTOR_COOLING_TIMEOUT"},
{MOTOR_SHADOW_MODE_TIMEOUT, "MOTOR_SHADOW_MODE_TIMEOUT"},
+ {MOTOR_OPEN_CHAMBER, "MOTOR_OPEN_CHAMBER"},
{TARS_WASHING_DONE, "TARS_WASHING_DONE"},
{TARS_CHECK_PRESSURE_STABILIZE, "TARS_CHECK_PRESSURE_STABILIZE"},
{TARS_PRESSURE_STABILIZED, "TARS_PRESSURE_STABILIZED"},
--
GitLab