From 836fff21197150d6d4e4224a2b6a2878aaeeeb98 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 2 Oct 2015 20:34:44 +0900 Subject: [PATCH] Rover: send home position when home is set or get-home msg received --- APMrover2/GCS_Mavlink.cpp | 4 ++++ APMrover2/commands.cpp | 1 + APMrover2/commands_logic.cpp | 1 + 3 files changed, 6 insertions(+) diff --git a/APMrover2/GCS_Mavlink.cpp b/APMrover2/GCS_Mavlink.cpp index d200a97a693..4065f8f8a56 100644 --- a/APMrover2/GCS_Mavlink.cpp +++ b/APMrover2/GCS_Mavlink.cpp @@ -1075,6 +1075,10 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) } break; + case MAV_CMD_GET_HOME_POSITION: + send_home(rover.ahrs.get_home()); + break; + case MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES: { if (is_equal(packet.param1,1.0f)) { rover.gcs[chan-MAVLINK_COMM_0].send_autopilot_version(FIRMWARE_VERSION); diff --git a/APMrover2/commands.cpp b/APMrover2/commands.cpp index 55320218276..1d0ee6b4477 100644 --- a/APMrover2/commands.cpp +++ b/APMrover2/commands.cpp @@ -68,6 +68,7 @@ void Rover::init_home() ahrs.set_home(gps.location()); home_is_set = true; Log_Write_Home_And_Origin(); + GCS_MAVLINK::send_home_all(gps.location()); // Save Home to EEPROM mission.write_home_to_storage(); diff --git a/APMrover2/commands_logic.cpp b/APMrover2/commands_logic.cpp index 25edcb5c7b8..d007270ce44 100644 --- a/APMrover2/commands_logic.cpp +++ b/APMrover2/commands_logic.cpp @@ -326,6 +326,7 @@ void Rover::do_set_home(const AP_Mission::Mission_Command& cmd) ahrs.set_home(cmd.content.location); home_is_set = true; Log_Write_Home_And_Origin(); + GCS_MAVLINK::send_home_all(cmd.content.location); } }