From a8a5c1d3322f810b06ae322ac7e75447df396f28 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 19 Sep 2017 16:21:59 +0900 Subject: [PATCH] Copter: command-long DO_SET_HOME check first param is zero Also do not use current location just because lat,lon,alt are all zero --- ArduCopter/GCS_Mavlink.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 2cbdc262d5..edc8e89735 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -973,11 +973,15 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg) // param6 : longitude // param7 : altitude (absolute) result = MAV_RESULT_FAILED; // assume failure - if(is_equal(packet.param1,1.0f) || (is_zero(packet.param5) && is_zero(packet.param6) && is_zero(packet.param7))) { + if (is_equal(packet.param1,1.0f)) { if (copter.set_home_to_current_location(true)) { result = MAV_RESULT_ACCEPTED; } } else { + // ensure param1 is zero + if (!is_zero(packet.param1)) { + break; + } // sanity check location if (!check_latlng(packet.param5, packet.param6)) { break;