diff --git a/APMrover2/AP_Arming.cpp b/APMrover2/AP_Arming.cpp index 3220b55452..90800f67f9 100644 --- a/APMrover2/AP_Arming.cpp +++ b/APMrover2/AP_Arming.cpp @@ -1,11 +1,6 @@ #include "AP_Arming.h" #include "Rover.h" -enum HomeState AP_Arming_Rover::home_status() const -{ - return rover.home_is_set; -} - // perform pre_arm_rc_checks checks bool AP_Arming_Rover::pre_arm_rc_checks(const bool display_failure) { diff --git a/APMrover2/AP_Arming.h b/APMrover2/AP_Arming.h index 0b1af5e93b..a49299c798 100644 --- a/APMrover2/AP_Arming.h +++ b/APMrover2/AP_Arming.h @@ -25,7 +25,6 @@ public: bool gps_checks(bool display_failure) override; protected: - enum HomeState home_status() const override; bool fence_checks(bool report); bool proximity_check(bool report); diff --git a/APMrover2/GCS_Mavlink.cpp b/APMrover2/GCS_Mavlink.cpp index a7c1e68648..3f9105f6e6 100644 --- a/APMrover2/GCS_Mavlink.cpp +++ b/APMrover2/GCS_Mavlink.cpp @@ -691,7 +691,7 @@ void GCS_MAVLINK_Rover::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 (rover.home_is_set == HOME_UNSET) { + if (!rover.ahrs.home_is_set()) { // cannot use relative altitude if home is not set break; } @@ -892,7 +892,7 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg) break; case MAV_CMD_GET_HOME_POSITION: - if (rover.home_is_set != HOME_UNSET) { + if (AP::ahrs().home_is_set()) { send_home(rover.ahrs.get_home()); Location ekf_origin; if (rover.ahrs.get_origin(ekf_origin)) { diff --git a/APMrover2/Log.cpp b/APMrover2/Log.cpp index 1014eb4043..9c63963b95 100644 --- a/APMrover2/Log.cpp +++ b/APMrover2/Log.cpp @@ -231,7 +231,7 @@ void Rover::Log_Write_Home_And_Origin() #endif // log ahrs home if set - if (home_is_set != HOME_UNSET) { + if (ahrs.home_is_set()) { DataFlash.Log_Write_Origin(LogOriginType::ahrs_home, ahrs.get_home()); } } diff --git a/APMrover2/Rover.h b/APMrover2/Rover.h index de31e13cde..5f1b61b983 100644 --- a/APMrover2/Rover.h +++ b/APMrover2/Rover.h @@ -316,9 +316,6 @@ private: // The home location used for RTL. The location is set when we first get stable GPS lock const struct Location &home; - // Flag for if we have g_gps lock and have set the home location in AHRS - enum HomeState home_is_set = HOME_UNSET; - // true if the system time has been set from the GPS bool system_time_set; diff --git a/APMrover2/commands.cpp b/APMrover2/commands.cpp index 2a48527670..03b968d321 100644 --- a/APMrover2/commands.cpp +++ b/APMrover2/commands.cpp @@ -4,7 +4,7 @@ void Rover::update_home_from_EKF() { // exit immediately if home already set - if (home_is_set != HOME_UNSET) { + if (ahrs.home_is_set()) { return; } @@ -50,9 +50,9 @@ bool Rover::set_home(const Location& loc, bool lock) ahrs.set_home(loc); // init compass declination - if (home_is_set == HOME_UNSET) { + if (!ahrs.home_is_set()) { // record home is set - home_is_set = HOME_SET_NOT_LOCKED; + ahrs.set_home_status(HOME_SET_NOT_LOCKED); // log new home position which mission library will pull from ahrs if (should_log(MASK_LOG_CMD)) { @@ -65,7 +65,7 @@ bool Rover::set_home(const Location& loc, bool lock) // lock home position if (lock) { - home_is_set = HOME_SET_AND_LOCKED; + ahrs.set_home_status(HOME_SET_AND_LOCKED); } // Save Home to EEPROM @@ -143,7 +143,7 @@ void Rover::set_system_time_from_GPS() */ void Rover::update_home() { - if (home_is_set == HOME_SET_NOT_LOCKED) { + if (ahrs.home_status() == HOME_SET_NOT_LOCKED) { Location loc; if (ahrs.get_position(loc)) { if (get_distance(loc, ahrs.get_home()) > DISTANCE_HOME_MAX) {