From c9606b2933b2700c60c01baa787868f9454def9d Mon Sep 17 00:00:00 2001
From: Davide Mor <davide.mor@skywarder.eu>
Date: Mon, 26 Feb 2024 10:58:31 +0100
Subject: [PATCH] [RIGv2] Added initial actuation code to Radio
---
src/boards/RIGv2/Configs/RadioConfig.h | 2 +
src/boards/RIGv2/Radio/Radio.cpp | 71 +++++++++++++++++++++++++-
src/boards/RIGv2/Radio/Radio.h | 4 ++
3 files changed, 76 insertions(+), 1 deletion(-)
diff --git a/src/boards/RIGv2/Configs/RadioConfig.h b/src/boards/RIGv2/Configs/RadioConfig.h
index 23801aae1..d3f761eb2 100644
--- a/src/boards/RIGv2/Configs/RadioConfig.h
+++ b/src/boards/RIGv2/Configs/RadioConfig.h
@@ -44,6 +44,8 @@ static constexpr unsigned int CIRCULAR_BUFFER_SIZE = 8;
static constexpr uint8_t MAV_SYSTEM_ID = 171;
static constexpr uint8_t MAV_COMPONENT_ID = 96;
+static constexpr long long LAST_COMMAND_THRESHOLD = 1000;
+
} // namespace Radio
} // namespace Config
diff --git a/src/boards/RIGv2/Radio/Radio.cpp b/src/boards/RIGv2/Radio/Radio.cpp
index b0f341f7e..a4f98607e 100644
--- a/src/boards/RIGv2/Radio/Radio.cpp
+++ b/src/boards/RIGv2/Radio/Radio.cpp
@@ -415,6 +415,8 @@ bool Radio::packSystemTm(uint8_t tmId, mavlink_message_t& msg)
void Radio::handleConrigState(const mavlink_message_t& msg)
{
+ ModuleManager& modules = ModuleManager::getInstance();
+
// Acknowledge the state
sendAck(msg);
@@ -428,5 +430,72 @@ void Radio::handleConrigState(const mavlink_message_t& msg)
packSystemTm(MAV_MOTOR_ID, tm);
enqueuePacket(tm);
- // TODO: Handle buttons
+ mavlink_conrig_state_tc_t state;
+ mavlink_msg_conrig_state_tc_decode(&msg, &state);
+
+ long long currentTime = getTime();
+ if (currentTime >
+ lastManualActuation +
+ (Config::Radio::LAST_COMMAND_THRESHOLD * Constants::NS_IN_MS))
+ {
+ // Ok we can accept new commands
+ if(oldConrigState.arm_switch == 0 && state.arm_switch == 1) {
+ // The ARM switch was pressed
+ // TODO(davide.mor): Arm the system
+
+ lastManualActuation = currentTime;
+ }
+
+ if(oldConrigState.ignition_btn == 0 && state.ignition_btn == 1) {
+ // The ignition switch was pressed
+ // TODO(davide.mor): Perform ignition
+
+ lastManualActuation = currentTime;
+ }
+
+ if(oldConrigState.filling_valve_btn == 0 && state.filling_valve_btn == 1) {
+ // The filling switch was pressed
+ // TODO(davide.mor): Notify everybody of a manual actuation
+
+ modules.get<Actuators>()->toggleServo(ServosList::FILLING_VALVE);
+
+ lastManualActuation = currentTime;
+ }
+
+ if(oldConrigState.quick_connector_btn == 0 && state.quick_connector_btn == 1) {
+ // The quick conector switch was pressed
+ // TODO(davide.mor): Notify everybody of a manual actuation
+
+ modules.get<Actuators>()->toggleServo(ServosList::DISCONNECT_SERVO);
+
+ lastManualActuation = currentTime;
+ }
+
+ if(oldConrigState.release_pressure_btn == 0 && state.release_pressure_btn == 1) {
+ // The release switch was pressed
+ // TODO(davide.mor): Notify everybody of a manual actuation
+
+ modules.get<Actuators>()->toggleServo(ServosList::RELEASE_VALVE);
+
+ lastManualActuation = currentTime;
+ }
+
+ if(oldConrigState.venting_valve_btn == 0 && state.venting_valve_btn == 1) {
+ // The venting switch was pressed
+ // TODO(davide.mor): Notify everybody of a manual actuation
+
+ modules.get<Actuators>()->toggleServo(ServosList::VENTING_VALVE);
+
+ 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): Disarm the system
+
+ lastManualActuation = currentTime;
+ }
+
+ oldConrigState = state;
}
diff --git a/src/boards/RIGv2/Radio/Radio.h b/src/boards/RIGv2/Radio/Radio.h
index 4796e7f83..83168fbcd 100644
--- a/src/boards/RIGv2/Radio/Radio.h
+++ b/src/boards/RIGv2/Radio/Radio.h
@@ -71,6 +71,10 @@ private:
std::atomic<bool> started{false};
std::unique_ptr<Boardcore::SX1278Lora> radio;
std::unique_ptr<MavDriver> mavDriver;
+
+ // Last time a ConRIG state triggered an actuation [ns]
+ long long lastManualActuation = 0;
+ mavlink_conrig_state_tc_t oldConrigState;
};
} // namespace RIGv2
\ No newline at end of file
--
GitLab