From 58f6ac4b39cb470a0d223620302127a54a6947ce Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Thu, 12 Nov 2020 06:43:46 +0300 Subject: [PATCH] simple_offboard: fix checking kill switch state --- clover/src/simple_offboard.cpp | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/clover/src/simple_offboard.cpp b/clover/src/simple_offboard.cpp index 08c6c6b1..bc66d511 100644 --- a/clover/src/simple_offboard.cpp +++ b/clover/src/simple_offboard.cpp @@ -496,10 +496,16 @@ inline void checkManualControl() } if (check_kill_switch) { - const int KILL_SWITCH_BIT = 12; // https://github.com/PX4/Firmware/blob/c302514a0809b1765fafd13c014d705446ae1113/src/modules/mavlink/mavlink_messages.cpp#L3975 - bool kill_switch = manual_control.buttons & (1 << KILL_SWITCH_BIT); + // switch values: https://github.com/PX4/PX4-Autopilot/blob/c302514a0809b1765fafd13c014d705446ae1113/msg/manual_control_setpoint.msg#L3 + const uint8_t SWITCH_POS_NONE = 0; // switch is not mapped + const uint8_t SWITCH_POS_ON = 1; // switch activated + const uint8_t SWITCH_POS_MIDDLE = 2; // middle position + const uint8_t SWITCH_POS_OFF = 3; // switch not activated - if (kill_switch) + const int KILL_SWITCH_BIT = 12; // https://github.com/PX4/Firmware/blob/c302514a0809b1765fafd13c014d705446ae1113/src/modules/mavlink/mavlink_messages.cpp#L3975 + uint8_t kill_switch = (manual_control.buttons & (0b11 << KILL_SWITCH_BIT)) >> KILL_SWITCH_BIT; + + if (kill_switch == SWITCH_POS_ON) throw std::runtime_error("Kill switch is on"); } }