From 330961b524ad84a5dd577da5c48a0c83fd0c4952 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 2 Oct 2015 17:56:31 +0900 Subject: [PATCH] Copter: send home position when home is set or get-home msg received --- ArduCopter/GCS_Mavlink.cpp | 4 ++++ ArduCopter/commands.cpp | 3 +++ 2 files changed, 7 insertions(+) diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index cef959c6a3..feaa1dd801 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -1386,6 +1386,10 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) } break; + case MAV_CMD_GET_HOME_POSITION: + send_home(copter.ahrs.get_home()); + break; + case MAV_CMD_DO_SET_SERVO: if (copter.ServoRelayEvents.do_set_servo(packet.param1, packet.param2)) { result = MAV_RESULT_ACCEPTED; diff --git a/ArduCopter/commands.cpp b/ArduCopter/commands.cpp index f79c50aef1..f51bb70b4b 100644 --- a/ArduCopter/commands.cpp +++ b/ArduCopter/commands.cpp @@ -104,6 +104,9 @@ bool Copter::set_home(const Location& loc) // log ahrs home and ekf origin dataflash Log_Write_Home_And_Origin(); + // send new home location to GCS + GCS_MAVLINK::send_home_all(loc); + // return success return true; }