From b0f9f9993fb35f523e8e8a90118015452e194574 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 27 Aug 2020 11:20:10 +0900 Subject: [PATCH] Rover: guided consumes set-pos-target-local-ned as offset from origin --- Rover/GCS_Mavlink.cpp | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/Rover/GCS_Mavlink.cpp b/Rover/GCS_Mavlink.cpp index 9df11b572d..40bf8e383b 100644 --- a/Rover/GCS_Mavlink.cpp +++ b/Rover/GCS_Mavlink.cpp @@ -766,6 +766,12 @@ void GCS_MAVLINK_Rover::handleMessage(const mavlink_message_t &msg) break; } + // need ekf origin + Location ekf_origin; + if (!rover.ahrs.get_origin(ekf_origin)) { + break; + } + // check for supported coordinate frames if (packet.coordinate_frame != MAV_FRAME_LOCAL_NED && packet.coordinate_frame != MAV_FRAME_LOCAL_OFFSET_NED && @@ -799,9 +805,10 @@ void GCS_MAVLINK_Rover::handleMessage(const mavlink_message_t &msg) target_loc.offset(packet.x, packet.y); break; + case MAV_FRAME_LOCAL_NED: default: - // MAV_FRAME_LOCAL_NED interpret as an offset from home - target_loc = rover.ahrs.get_home(); + // MAV_FRAME_LOCAL_NED is interpreted as an offset from EKF origin + target_loc = ekf_origin; target_loc.offset(packet.x, packet.y); break; }