diff --git a/APMrover2/commands_logic.cpp b/APMrover2/commands_logic.cpp index 6f568a6824..755b35e043 100644 --- a/APMrover2/commands_logic.cpp +++ b/APMrover2/commands_logic.cpp @@ -4,7 +4,7 @@ void Rover::update_mission(void) { if (control_mode == &mode_auto) { - if (home_is_set != HOME_UNSET && mission.num_commands() > 1) { + if (ahrs.home_is_set() && mission.num_commands() > 1) { mission.update(); } } diff --git a/APMrover2/mode_rtl.cpp b/APMrover2/mode_rtl.cpp index 8ed0da7926..671cea37c3 100644 --- a/APMrover2/mode_rtl.cpp +++ b/APMrover2/mode_rtl.cpp @@ -4,7 +4,7 @@ bool ModeRTL::_enter() { // refuse RTL if home has not been set - if (rover.home_is_set == HOME_UNSET) { + if (!AP::ahrs().home_is_set()) { return false; } diff --git a/ArduPlane/AP_Arming.cpp b/ArduPlane/AP_Arming.cpp index 12cbba1ec2..a52a86ad19 100644 --- a/ArduPlane/AP_Arming.cpp +++ b/ArduPlane/AP_Arming.cpp @@ -18,11 +18,6 @@ const AP_Param::GroupInfo AP_Arming_Plane::var_info[] = { AP_GROUPEND }; -enum HomeState AP_Arming_Plane::home_status() const -{ - return plane.home_is_set; -} - /* additional arming checks for plane diff --git a/ArduPlane/AP_Arming.h b/ArduPlane/AP_Arming.h index 8ea1d644be..172354286d 100644 --- a/ArduPlane/AP_Arming.h +++ b/ArduPlane/AP_Arming.h @@ -34,7 +34,6 @@ public: protected: bool ins_checks(bool report); - enum HomeState home_status() const override; // parameters AP_Int8 rudder_arming_value; diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 301dba44aa..8a2d6fd9d7 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -756,7 +756,7 @@ void Plane::update_navigation() switch(control_mode) { case AUTO: - if (home_is_set != HOME_UNSET) { + if (ahrs.home_is_set()) { mission.update(); } break; diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 5782905145..4a75cdbf4f 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -900,14 +900,14 @@ void GCS_MAVLINK_Plane::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 (plane.home_is_set == HOME_UNSET) { + if (!AP::ahrs().home_is_set()) { // cannot use relative altitude if home is not set break; } new_home_loc.alt += plane.ahrs.get_home().alt; } plane.ahrs.set_home(new_home_loc); - plane.home_is_set = HOME_SET_NOT_LOCKED; + AP::ahrs().set_home_status(HOME_SET_NOT_LOCKED); plane.Log_Write_Home_And_Origin(); gcs().send_home(new_home_loc); result = MAV_RESULT_ACCEPTED; @@ -1194,7 +1194,7 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg) break; case MAV_CMD_GET_HOME_POSITION: - if (plane.home_is_set != HOME_UNSET) { + if (AP::ahrs().home_is_set()) { send_home(plane.ahrs.get_home()); Location ekf_origin; if (plane.ahrs.get_origin(ekf_origin)) { @@ -1300,7 +1300,7 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg) 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); - plane.home_is_set = HOME_SET_NOT_LOCKED; + AP::ahrs().set_home_status(HOME_SET_NOT_LOCKED); plane.Log_Write_Home_And_Origin(); gcs().send_home(new_home_loc); result = MAV_RESULT_ACCEPTED; @@ -1712,7 +1712,7 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg) new_home_loc.lng = packet.longitude; new_home_loc.alt = packet.altitude / 10; plane.ahrs.set_home(new_home_loc); - plane.home_is_set = HOME_SET_NOT_LOCKED; + plane.ahrs.set_home_status(HOME_SET_NOT_LOCKED); plane.Log_Write_Home_And_Origin(); gcs().send_home(new_home_loc); gcs().send_text(MAV_SEVERITY_INFO, "Set HOME to %.6f %.6f at %um", diff --git a/ArduPlane/Log.cpp b/ArduPlane/Log.cpp index 13e652435f..67876e3cc5 100644 --- a/ArduPlane/Log.cpp +++ b/ArduPlane/Log.cpp @@ -325,7 +325,7 @@ void Plane::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/ArduPlane/Plane.h b/ArduPlane/Plane.h index c3abf86eaf..46c1b9c0fd 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -684,9 +684,6 @@ private: // Location structure defined in AP_Common const struct Location &home = ahrs.get_home(); - // Flag for if we have g_gps lock and have set the home location in AHRS - enum HomeState home_is_set = HOME_UNSET; - // The location of the previous waypoint. Used for track following and altitude ramp calculations Location prev_WP_loc {}; diff --git a/ArduPlane/altitude.cpp b/ArduPlane/altitude.cpp index 5113834418..dc8fbe00d2 100644 --- a/ArduPlane/altitude.cpp +++ b/ArduPlane/altitude.cpp @@ -574,7 +574,7 @@ void Plane::rangefinder_height_update(void) { float distance = rangefinder.distance_cm_orient(ROTATION_PITCH_270)*0.01f; - if ((rangefinder.status_orient(ROTATION_PITCH_270) == RangeFinder::RangeFinder_Good) && home_is_set != HOME_UNSET) { + if ((rangefinder.status_orient(ROTATION_PITCH_270) == RangeFinder::RangeFinder_Good) && ahrs.home_is_set()) { if (!rangefinder_state.have_initial_reading) { rangefinder_state.have_initial_reading = true; rangefinder_state.initial_range = distance; diff --git a/ArduPlane/commands.cpp b/ArduPlane/commands.cpp index 1cbacd3968..bc46fa0b01 100644 --- a/ArduPlane/commands.cpp +++ b/ArduPlane/commands.cpp @@ -110,7 +110,7 @@ void Plane::init_home() gcs().send_text(MAV_SEVERITY_INFO, "Init HOME"); ahrs.set_home(gps.location()); - home_is_set = HOME_SET_NOT_LOCKED; + ahrs.set_home_status(HOME_SET_NOT_LOCKED); Log_Write_Home_And_Origin(); gcs().send_home(gps.location()); @@ -138,7 +138,7 @@ void Plane::update_home() // significantly return; } - if (home_is_set == HOME_SET_NOT_LOCKED) { + if (ahrs.home_status() == HOME_SET_NOT_LOCKED) { Location loc; if(ahrs.get_position(loc)) { ahrs.set_home(loc); diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index e315b78d61..a43ad58691 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -937,7 +937,7 @@ void Plane::do_set_home(const AP_Mission::Mission_Command& cmd) init_home(); } else { ahrs.set_home(cmd.content.location); - home_is_set = HOME_SET_NOT_LOCKED; + ahrs.set_home_status(HOME_SET_NOT_LOCKED); Log_Write_Home_And_Origin(); gcs().send_home(cmd.content.location); // send ekf origin if set