diff --git a/src/boards/RIGv2/Configs/RadioConfig.h b/src/boards/RIGv2/Configs/RadioConfig.h index 23801aae1e71fa0e1e3dee35f5c2814170347087..d3f761eb24295c31d24328adf985da15351e9c35 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 b0f341f7e68ca98390ac0a6ea7bbcab390e46289..a4f98607e3a86b1b425ef943e4ca0a25723a517e 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 4796e7f838220948b34ffee776d0fff7642e771d..83168fbcd9ec7271bf39bd9e5e12726b4fbec379 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