From ad51c767bc5ba3e911227746f1636401baa37ca3 Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Fri, 13 Nov 2015 12:06:37 -0800 Subject: [PATCH] Copter: don't take any action on Solo button press events if there is a radio failsafe --- ArduCopter/GCS_Mavlink.pde | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index b6dcfdd877..0575757a92 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -1487,6 +1487,11 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) case MAV_CMD_SOLO_BTN_FLY_CLICK: { result = MAV_RESULT_ACCEPTED; + // don't do anything if there is a radio failsafe + if (failsafe.radio) { + break; + } + if (set_mode(LOITER)) { send_heartbeat_immediately = true; } else if (set_mode(ALT_HOLD)) { @@ -1498,6 +1503,11 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) case MAV_CMD_SOLO_BTN_FLY_HOLD: { result = MAV_RESULT_ACCEPTED; + // don't do anything if there is a radio failsafe + if (failsafe.radio) { + break; + } + if (!motors.armed()) { init_arm_motors(true); } else if (ap.land_complete) { @@ -1516,6 +1526,11 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) case MAV_CMD_SOLO_BTN_PAUSE_CLICK: { result = MAV_RESULT_ACCEPTED; + // don't do anything if there is a radio failsafe + if (failsafe.radio) { + break; + } + if (motors.armed()) { if (ap.land_complete) { // if landed, disarm motors