Plane: factor out a Plane::set_home

ArduPlane/ArduPlane.cpp

@@ -436,7 +436,11 @@ void Plane::update_GPS_10Hz(void)
                 ground_start_count = 5;
 
             } else {
-                init_home();
+                gcs().send_text(MAV_SEVERITY_INFO, "Init HOME");
+
+                set_home_persistently(gps.location());
+
+                next_WP_loc = prev_WP_loc = home;
 
                 // set system clock for log timestamps
                 uint64_t gps_timestamp = gps.time_epoch_usec();

ArduPlane/GCS_Mavlink.cpp

@@ -776,7 +776,7 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg)
         case MAV_CMD_DO_SET_HOME: {
             result = MAV_RESULT_FAILED; // assume failure
             if (is_equal(packet.param1, 1.0f)) {
-                plane.init_home();
+                plane.set_home_persistently(AP::gps().location());
             } else {
                 // ensure param1 is zero
                 if (!is_zero(packet.param1)) {
@@ -809,9 +809,7 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg)
                     }
                     new_home_loc.alt += plane.ahrs.get_home().alt;
                 }
-                plane.ahrs.set_home(new_home_loc);
-                AP::ahrs().Log_Write_Home_And_Origin();
-                gcs().send_home();
+                plane.set_home(new_home_loc);
                 result = MAV_RESULT_ACCEPTED;
                 gcs().send_text(MAV_SEVERITY_INFO, "Set HOME to %.6f %.6f at %um",
                                         (double)(new_home_loc.lat*1.0e-7f),
@@ -1076,7 +1074,7 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg)
             // param7 : altitude (absolute)
             result = MAV_RESULT_FAILED; // assume failure
             if (is_equal(packet.param1,1.0f)) {
-                plane.init_home();
+                plane.set_home_persistently(AP::gps().location());
             } else {
                 // ensure param1 is zero
                 if (!is_zero(packet.param1)) {
@@ -1094,9 +1092,7 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg)
                 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);
-                plane.ahrs.set_home(new_home_loc);
-                AP::ahrs().Log_Write_Home_And_Origin();
-                gcs().send_home();
+                plane.set_home(new_home_loc);
                 result = MAV_RESULT_ACCEPTED;
                 gcs().send_text(MAV_SEVERITY_INFO, "Set HOME to %.6f %.6f at %um",
                                         (double)(new_home_loc.lat*1.0e-7f),
@@ -1511,9 +1507,7 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg)
         new_home_loc.lat = packet.latitude;
         new_home_loc.lng = packet.longitude;
         new_home_loc.alt = packet.altitude / 10;
-        plane.ahrs.set_home(new_home_loc);
-        plane.ahrs.Log_Write_Home_And_Origin();
-        gcs().send_home();
+        plane.set_home(new_home_loc);
         gcs().send_text(MAV_SEVERITY_INFO, "Set HOME to %.6f %.6f at %um",
                                 (double)(new_home_loc.lat*1.0e-7f),
                                 (double)(new_home_loc.lng*1.0e-7f),

ArduPlane/Plane.h

@@ -828,8 +828,11 @@ private:
     void rangefinder_height_update(void);
     void set_next_WP(const struct Location &loc);
     void set_guided_WP(void);
-    void init_home();
     void update_home();
+    // set home location and store it persistently:
+    void set_home_persistently(const Location &loc);
+    // set home location:
+    void set_home(const Location &loc);
     void do_RTL(int32_t alt);
     bool verify_takeoff();
     bool verify_loiter_unlim();

ArduPlane/commands.cpp

@@ -103,24 +103,6 @@ void Plane::set_guided_WP(void)
     loiter_angle_reset();
 }
 
-// run this at setup on the ground
-// -------------------------------
-void Plane::init_home()
-{
-    gcs().send_text(MAV_SEVERITY_INFO, "Init HOME");
-
-    ahrs.set_home(gps.location());
-    ahrs.Log_Write_Home_And_Origin();
-    gcs().send_home();
-
-    // Save Home to EEPROM
-    mission.write_home_to_storage();
-
-    // Save prev loc
-    // -------------
-    next_WP_loc = prev_WP_loc = home;
-}
-
 /*
   update home location from GPS
   this is called as long as we have 3D lock and the arming switch is
@@ -140,10 +122,23 @@ void Plane::update_home()
     if (ahrs.home_is_set() && !ahrs.home_is_locked()) {
         Location loc;
         if(ahrs.get_position(loc)) {
-            ahrs.set_home(loc);
-            ahrs.Log_Write_Home_And_Origin();
-            gcs().send_home();
+            plane.set_home(loc);
         }
     }
     barometer.update_calibration();
 }
+
+void Plane::set_home_persistently(const Location &loc)
+{
+    set_home(loc);
+
+    // Save Home to EEPROM
+    mission.write_home_to_storage();
+}
+
+void Plane::set_home(const Location &loc)
+{
+    ahrs.set_home(loc);
+    ahrs.Log_Write_Home_And_Origin();
+    gcs().send_home();
+}

ArduPlane/commands_logic.cpp

@@ -934,11 +934,9 @@ void Plane::do_change_speed(const AP_Mission::Mission_Command& cmd)
 void Plane::do_set_home(const AP_Mission::Mission_Command& cmd)
 {
     if (cmd.p1 == 1 && gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
-        init_home();
+        set_home_persistently(gps.location());
     } else {
-        ahrs.set_home(cmd.content.location);
-        ahrs.Log_Write_Home_And_Origin();
-        gcs().send_home();
+        plane.set_home(cmd.content.location);
         gcs().send_ekf_origin();
     }
 }