From 236056ffbd321dea7377d2c012d5f19d56516a1b Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 19 Mar 2019 16:34:07 +1100 Subject: [PATCH] GCS_MAVLink: take 0,0 as meaning set-home-to-current-location This isn't in spec, but is what Sub used to do, and what other vehicles also used to do. We treat 0,0 as "current location" in various other places, so it kind of makes sense here too. --- libraries/GCS_MAVLink/GCS_Common.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index f6067f11f3..1f9ad0bb2c 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -3523,8 +3523,8 @@ MAV_RESULT GCS_MAVLINK::handle_command_mount(const mavlink_command_long_t &packe MAV_RESULT GCS_MAVLINK::handle_command_do_set_home(const mavlink_command_long_t &packet) { - // if param1 is 1 use current location: - if (is_equal(packet.param1, 1.0f)) { + if (is_equal(packet.param1, 1.0f) || (is_zero(packet.param5) && is_zero(packet.param6))) { + // param1 is 1 (or both lat and lon are zero); use current location if (!set_home_to_current_location(true)) { return MAV_RESULT_FAILED; } @@ -3707,8 +3707,8 @@ MAV_RESULT GCS_MAVLINK::handle_command_do_set_roi(const Location &roi_loc) MAV_RESULT GCS_MAVLINK::handle_command_int_do_set_home(const mavlink_command_int_t &packet) { - if (is_equal(packet.param1, 1.0f)) { - // if param1 is 1, use current location + if (is_equal(packet.param1, 1.0f) || (packet.x == 0 && packet.y == 0)) { + // param1 is 1 (or both lat and lon are zero); use current location if (!set_home_to_current_location(true)) { return MAV_RESULT_FAILED; }