From 76f8b9b07c89c8cd086142cd8621d8c6fee8b979 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Sat, 21 Feb 2015 10:14:18 +1100
Subject: [PATCH] Plane: support DO_SET_HOME MAVLink command

---
 ArduPlane/ArduPlane.pde        |  2 +-
 ArduPlane/GCS_Mavlink.pde      | 23 +++++++++++++++++++++++
 ArduPlane/altitude.pde         |  2 +-
 ArduPlane/commands.pde         |  6 ++++--
 ArduPlane/commands_logic.pde   |  2 +-
 ArduPlane/commands_process.pde |  2 +-
 6 files changed, 31 insertions(+), 6 deletions(-)

diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde
index ca7152ca77..9dad021f48 100644
--- a/ArduPlane/ArduPlane.pde
+++ b/ArduPlane/ArduPlane.pde
@@ -672,7 +672,7 @@ static int16_t condition_rate;
 static const struct Location &home = ahrs.get_home();
 
 // Flag for if we have g_gps lock and have set the home location in AHRS
-static bool home_is_set;
+static enum HomeState home_is_set;
 // The location of the previous waypoint.  Used for track following and altitude ramp calculations
 static Location prev_WP_loc;
 // The plane's current location
diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde
index b7a7b5716c..e53faac13d 100644
--- a/ArduPlane/GCS_Mavlink.pde
+++ b/ArduPlane/GCS_Mavlink.pde
@@ -1225,6 +1225,29 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
                 result = MAV_RESULT_ACCEPTED;
             }
             break;
+
+        case MAV_CMD_DO_SET_HOME:
+            // param1 : use current (1=use current location, 0=use specified location)
+            // param5 : latitude
+            // param6 : longitude
+            // param7 : altitude (absolute)
+            result = MAV_RESULT_FAILED; // assume failure
+            if (packet.param1 == 1) {
+                init_home();
+            } else {
+                if (packet.param5 == 0 && packet.param6 == 0 && packet.param7 == 0) {
+                    // don't allow the 0,0 position
+                    break;
+                }
+                Location new_home_loc;
+                new_home_loc.lat = (int32_t)(packet.param5 * 1.0e7f);
+                new_home_loc.lng = (int32_t)(packet.param6 * 1.0e7f);
+                new_home_loc.alt = (int32_t)(packet.param7 * 100.0f);
+                ahrs.set_home(new_home_loc);
+                home_is_set = HOME_SET_NOT_LOCKED;
+                result = MAV_RESULT_ACCEPTED;
+            }
+            break;
         }
 
         default:
diff --git a/ArduPlane/altitude.pde b/ArduPlane/altitude.pde
index 5badd29154..464533bc6a 100644
--- a/ArduPlane/altitude.pde
+++ b/ArduPlane/altitude.pde
@@ -528,7 +528,7 @@ static void rangefinder_height_update(void)
     uint16_t distance_cm = rangefinder.distance_cm();
     int16_t max_distance_cm = rangefinder.max_distance_cm();
     float height_estimate = 0;
-    if (rangefinder.healthy() && distance_cm < max_distance_cm && home_is_set) {
+    if (rangefinder.healthy() && distance_cm < max_distance_cm && home_is_set != HOME_UNSET) {
         // correct the range for attitude (multiply by DCM.c.z, which
         // is cos(roll)*cos(pitch))
         height_estimate = distance_cm * 0.01f * ahrs.get_dcm_matrix().c.z;
diff --git a/ArduPlane/commands.pde b/ArduPlane/commands.pde
index 70ca155ef3..9f2cc24554 100644
--- a/ArduPlane/commands.pde
+++ b/ArduPlane/commands.pde
@@ -101,7 +101,7 @@ static void init_home()
     gcs_send_text_P(SEVERITY_LOW, PSTR("init home"));
 
     ahrs.set_home(gps.location());
-    home_is_set = true;
+    home_is_set = HOME_SET_NOT_LOCKED;
 
     gcs_send_text_fmt(PSTR("gps alt: %lu"), (unsigned long)home.alt);
 
@@ -125,6 +125,8 @@ static void init_home()
 */
 static void update_home()
 {
-    ahrs.set_home(gps.location());
+    if (home_is_set == HOME_SET_NOT_LOCKED) {
+        ahrs.set_home(gps.location());
+    }
     barometer.update_calibration();
 }
diff --git a/ArduPlane/commands_logic.pde b/ArduPlane/commands_logic.pde
index d76a6b14c3..6dc17b15e9 100644
--- a/ArduPlane/commands_logic.pde
+++ b/ArduPlane/commands_logic.pde
@@ -643,7 +643,7 @@ static void do_set_home(const AP_Mission::Mission_Command& cmd)
         init_home();
     } else {
         ahrs.set_home(cmd.content.location);
-        home_is_set = true;
+        home_is_set = HOME_SET_NOT_LOCKED;
     }
 }
 
diff --git a/ArduPlane/commands_process.pde b/ArduPlane/commands_process.pde
index 570baf9b6c..0051661868 100644
--- a/ArduPlane/commands_process.pde
+++ b/ArduPlane/commands_process.pde
@@ -8,7 +8,7 @@ static bool verify_command(const AP_Mission::Mission_Command& cmd);
 static void update_commands(void)
 {
     if(control_mode == AUTO) {
-        if (home_is_set) {
+        if (home_is_set != HOME_UNSET) {
             if(mission.state() == AP_Mission::MISSION_RUNNING) {
                 mission.update();
             } else {