Sub: move home state into AP_AHRS

ArduSub/AP_Arming_Sub.cpp

@@ -1,11 +1,6 @@
 #include "AP_Arming_Sub.h"
 #include "Sub.h"
 
-enum HomeState AP_Arming_Sub::home_status() const
-{
-    return sub.ap.home_state;
-}
-
 bool AP_Arming_Sub::rc_calibration_checks(bool display_failure)
 {
     const RC_Channel *channels[] = {

ArduSub/AP_Arming_Sub.h

@@ -19,5 +19,4 @@ class AP_Arming_Sub : public AP_Arming {
 
 protected:
     bool ins_checks(bool report) override;
-    enum HomeState home_status() const override;
 };

ArduSub/AP_State.cpp

@@ -1,24 +1 @@
 #include "Sub.h"
-
-// set_home_state - update home state
-void Sub::set_home_state(enum HomeState new_home_state)
-{
-    // if no change, exit immediately
-    if (ap.home_state == new_home_state) {
-        return;
-    }
-
-    // update state
-    ap.home_state = new_home_state;
-
-    // log if home has been set
-    if (new_home_state == HOME_SET_NOT_LOCKED || new_home_state == HOME_SET_AND_LOCKED) {
-        Log_Write_Event(DATA_SET_HOME);
-    }
-}
-
-// home_is_set - returns true if home positions has been set (to GPS location, armed location or EKF origin)
-bool Sub::home_is_set()
-{
-    return (ap.home_state == HOME_SET_NOT_LOCKED || ap.home_state == HOME_SET_AND_LOCKED);
-}

ArduSub/GCS_Mavlink.cpp

@@ -968,7 +968,7 @@ void GCS_MAVLINK_Sub::handleMessage(mavlink_message_t* msg)
             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 (sub.ap.home_state == HOME_UNSET) {
+                if (!AP::ahrs().home_is_set()) {
                     // cannot use relative altitude if home is not set
                     break;
                 }
@@ -1194,7 +1194,7 @@ void GCS_MAVLINK_Sub::handleMessage(mavlink_message_t* msg)
             break;
 
         case MAV_CMD_GET_HOME_POSITION:
-            if (sub.ap.home_state != HOME_UNSET) {
+            if (AP::ahrs().home_is_set()) {
                 send_home(sub.ahrs.get_home());
                 Location ekf_origin;
                 if (sub.ahrs.get_origin(ekf_origin)) {

ArduSub/Log.cpp

@@ -291,7 +291,7 @@ void Sub::Log_Write_Home_And_Origin()
     }
 
     // log ahrs home if set
-    if (ap.home_state != HOME_UNSET) {
+    if (ahrs.home_is_set()) {
         DataFlash.Log_Write_Origin(LogOriginType::ahrs_home, ahrs.get_home());
     }
 }

ArduSub/Sub.h

@@ -235,7 +235,6 @@ class Sub : public AP_HAL::HAL::Callbacks {
             uint8_t initialised         : 1; // true once the init_ardupilot function has completed.  Extended status to GCS is not sent until this completes
             uint8_t system_time_set     : 1; // true if the system time has been set from the GPS
             uint8_t gps_base_pos_set    : 1; // true when the gps base position has been set (used for RTK gps only)
-            enum HomeState home_state   : 2; // home status (unset, set, locked)
             uint8_t at_bottom           : 1; // true if we are at the bottom
             uint8_t at_surface          : 1; // true if we are at the surface
             uint8_t depth_sensor_present: 1; // true if there is a depth sensor detected at boot
@@ -459,8 +458,6 @@ class Sub : public AP_HAL::HAL::Callbacks {
     void update_turn_counter();
     void read_AHRS(void);
     void update_altitude();
-    void set_home_state(enum HomeState new_home_state);
-    bool home_is_set();
     float get_smoothing_gain();
     void get_pilot_desired_lean_angles(float roll_in, float pitch_in, float &roll_out, float &pitch_out, float angle_max);
     float get_pilot_desired_yaw_rate(int16_t stick_angle);

ArduSub/commands.cpp

@@ -11,7 +11,7 @@
 void Sub::update_home_from_EKF()
 {
     // exit immediately if home already set
-    if (ap.home_state != HOME_UNSET) {
+    if (ahrs.home_is_set()) {
         return;
     }
 
@@ -73,11 +73,12 @@ bool Sub::set_home(const Location& loc, bool lock)
     ahrs.set_home(loc);
 
     // init inav and compass declination
-    if (ap.home_state == HOME_UNSET) {
+    if (!ahrs.home_is_set()) {
         // update navigation scalers.  used to offset the shrinking longitude as we go towards the poles
         scaleLongDown = longitude_scale(loc);
         // record home is set
-        set_home_state(HOME_SET_NOT_LOCKED);
+        ahrs.set_home_status(HOME_SET_NOT_LOCKED);
+        Log_Write_Event(DATA_SET_HOME);
 
         // log new home position which mission library will pull from ahrs
         if (should_log(MASK_LOG_CMD)) {
@@ -90,7 +91,7 @@ bool Sub::set_home(const Location& loc, bool lock)
 
     // lock home position
     if (lock) {
-        set_home_state(HOME_SET_AND_LOCKED);
+        ahrs.set_home_status(HOME_SET_AND_LOCKED);
     }
 
     // log ahrs home and ekf origin dataflash

ArduSub/motors.cpp

@@ -44,13 +44,13 @@ bool Sub::init_arm_motors(bool arming_from_gcs)
 
     initial_armed_bearing = ahrs.yaw_sensor;
 
-    if (ap.home_state == HOME_UNSET) {
+    if (!ahrs.home_is_set()) {
         // Reset EKF altitude if home hasn't been set yet (we use EKF altitude as substitute for alt above home)
 
         // Always use absolute altitude for ROV
         // ahrs.resetHeightDatum();
         // Log_Write_Event(DATA_EKF_ALT_RESET);
-    } else if (ap.home_state == HOME_SET_NOT_LOCKED) {
+    } else if (ahrs.home_status() == HOME_SET_NOT_LOCKED) {
         // Reset home position if it has already been set before (but not locked)
         set_home_to_current_location(false);
     }