Plane: move handling of command-int MAV_CMD_DO_SET_HOME up

Plane: eliminate set_home shim around AP_AHRS::set_home

ArduPlane/GCS_Mavlink.cpp

@@ -746,53 +746,27 @@ void GCS_MAVLINK_Plane::packetReceived(const mavlink_status_t &status,
 }
 
 
+bool GCS_MAVLINK_Plane::set_home_to_current_location(bool lock)
+{
+    plane.set_home_persistently(AP::gps().location());
+    if (lock) {
+        AP::ahrs().lock_home();
+    }
+    return true;
+}
+bool GCS_MAVLINK_Plane::set_home(const Location& loc, bool lock)
+{
+    AP::ahrs().set_home(loc);
+    if (lock) {
+        AP::ahrs().lock_home();
+    }
+    return true;
+}
+
 MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_packet(const mavlink_command_int_t &packet)
 {
     switch(packet.command) {
 
-    case MAV_CMD_DO_SET_HOME:
-        if (is_equal(packet.param1, 1.0f)) {
-            plane.set_home_persistently(AP::gps().location());
-            AP::ahrs().lock_home();
-            return MAV_RESULT_ACCEPTED;
-        } else {
-            // ensure param1 is zero
-            if (!is_zero(packet.param1)) {
-                return MAV_RESULT_FAILED;
-            }
-            if ((packet.x == 0) && (packet.y == 0) && is_zero(packet.z)) {
-                // don't allow the 0,0 position
-                return MAV_RESULT_FAILED;
-            }
-            // check frame type is supported
-            if (packet.frame != MAV_FRAME_GLOBAL &&
-                packet.frame != MAV_FRAME_GLOBAL_INT &&
-                packet.frame != MAV_FRAME_GLOBAL_RELATIVE_ALT &&
-                packet.frame != MAV_FRAME_GLOBAL_RELATIVE_ALT_INT) {
-                return MAV_RESULT_FAILED;
-            }
-            // sanity check location
-            if (!check_latlng(packet.x, packet.y)) {
-                return MAV_RESULT_FAILED;
-            }
-            Location new_home_loc {};
-            new_home_loc.lat = packet.x;
-            new_home_loc.lng = packet.y;
-            new_home_loc.alt = packet.z * 100;
-            // handle relative altitude
-            if (packet.frame == MAV_FRAME_GLOBAL_RELATIVE_ALT || packet.frame == MAV_FRAME_GLOBAL_RELATIVE_ALT_INT) {
-                if (!AP::ahrs().home_is_set()) {
-                    // cannot use relative altitude if home is not set
-                    return MAV_RESULT_FAILED;
-                }
-                new_home_loc.alt += plane.ahrs.get_home().alt;
-            }
-            plane.set_home(new_home_loc);
-            AP::ahrs().lock_home();
-            return MAV_RESULT_ACCEPTED;
-        }
-        return MAV_RESULT_FAILED;
-
     case MAV_CMD_DO_REPOSITION: {
         // sanity check location
         if (!check_latlng(packet.x, packet.y)) {
@@ -1014,8 +988,7 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_long_packet(const mavlink_command_l
             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.set_home(new_home_loc);
-            AP::ahrs().lock_home();
+            set_home(new_home_loc, true);
             return MAV_RESULT_ACCEPTED;
         }
         break;
@@ -1325,7 +1298,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.set_home(new_home_loc);
+        set_home(new_home_loc, false);
         break;
     }
 

ArduPlane/GCS_Mavlink.h

@@ -37,6 +37,9 @@ protected:
 
     bool persist_streamrates() const override { return true; }
 
+    bool set_home_to_current_location(bool lock) override;
+    bool set_home(const Location& loc, bool lock) override;
+
 private:
 
     void handleMessage(mavlink_message_t * msg) override;

ArduPlane/Plane.h

@@ -855,8 +855,6 @@ private:
     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(const AP_Mission::Mission_Command &cmd);

ArduPlane/commands.cpp

@@ -120,7 +120,7 @@ void Plane::update_home()
     if (ahrs.home_is_set() && !ahrs.home_is_locked()) {
         Location loc;
         if(ahrs.get_position(loc)) {
-            plane.set_home(loc);
+            AP::ahrs().set_home(loc);
         }
     }
     barometer.update_calibration();
@@ -129,13 +129,8 @@ void Plane::update_home()
 
 void Plane::set_home_persistently(const Location &loc)
 {
-    set_home(loc);
+    AP::ahrs().set_home(loc);
 
     // Save Home to EEPROM
     mission.write_home_to_storage();
 }
-
-void Plane::set_home(const Location &loc)
-{
-    ahrs.set_home(loc);
-}

ArduPlane/commands_logic.cpp

@@ -915,7 +915,7 @@ void Plane::do_set_home(const AP_Mission::Mission_Command& cmd)
     if (cmd.p1 == 1 && gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
         set_home_persistently(gps.location());
     } else {
-        plane.set_home(cmd.content.location);
+        AP::ahrs().set_home(cmd.content.location);
         gcs().send_ekf_origin();
     }
 }