From 3f3a4ac5c11afb2bd03851a3c2c83c135987ebc0 Mon Sep 17 00:00:00 2001 From: bresch Date: Wed, 6 Apr 2022 14:53:52 +0200 Subject: [PATCH] navigator: home_positionvalid -> global_home_position_valid This is to make clear that the relevant part of the home position message for navigator is the global one. Local home position isn't required as everything is done in global coordinates. The specific home_alt_valid is used when lat/lon are not used --- src/modules/navigator/geofence.cpp | 4 ++-- src/modules/navigator/mission.cpp | 6 +++--- src/modules/navigator/mission_feasibility_checker.cpp | 2 +- src/modules/navigator/navigator.h | 4 ++-- src/modules/navigator/navigator_main.cpp | 7 +++++-- src/modules/navigator/rtl.cpp | 4 ++-- src/modules/navigator/takeoff.cpp | 6 ++++-- src/modules/navigator/vtol_takeoff.cpp | 2 +- 8 files changed, 20 insertions(+), 15 deletions(-) diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index b941e9ee7d..f0a359f930 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -247,7 +247,7 @@ bool Geofence::isCloserThanMaxDistToHome(double lat, double lon, float altitude) { bool inside_fence = true; - if (isHomeRequired() && _navigator->home_position_valid()) { + if (isHomeRequired() && _navigator->home_global_position_valid()) { const float max_horizontal_distance = _param_gf_max_hor_dist.get(); @@ -281,7 +281,7 @@ bool Geofence::isBelowMaxAltitude(float altitude) { bool inside_fence = true; - if (isHomeRequired() && _navigator->home_position_valid()) { + if (isHomeRequired() && _navigator->home_alt_valid()) { const float max_vertical_distance = _param_gf_max_ver_dist.get(); const float home_alt = _navigator->get_home_position()->alt; diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 77186d89f9..5393dbc56d 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -103,7 +103,7 @@ Mission::on_inactive() } /* Without home a mission can't be valid yet anyway, let's wait. */ - if (!_navigator->home_position_valid()) { + if (!_navigator->home_global_position_valid()) { return; } @@ -1736,7 +1736,7 @@ Mission::set_current_mission_item() void Mission::check_mission_valid(bool force) { - if ((!_home_inited && _navigator->home_position_valid()) || force) { + if ((!_home_inited && _navigator->home_global_position_valid()) || force) { MissionFeasibilityChecker _missionFeasibilityChecker(_navigator); @@ -1749,7 +1749,7 @@ Mission::check_mission_valid(bool force) _navigator->get_mission_result()->seq_total = _mission.count; _navigator->increment_mission_instance_count(); _navigator->set_mission_result_updated(); - _home_inited = _navigator->home_position_valid(); + _home_inited = _navigator->home_global_position_valid(); // find and store landing start marker (if available) find_mission_land_start(); diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index 2df3f922d8..678b133e04 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -70,7 +70,7 @@ MissionFeasibilityChecker::checkMissionFeasible(const mission_s &mission, bool failed = false; // first check if we have a valid position - const bool home_valid = _navigator->home_position_valid(); + const bool home_valid = _navigator->home_global_position_valid(); const bool home_alt_valid = _navigator->home_alt_valid(); if (!home_alt_valid) { diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 3cd0bb5cc3..8553f13146 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -172,9 +172,9 @@ public: void reset_vroi() { _vroi = {}; } - bool home_alt_valid() { return (_home_pos.timestamp > 0 && _home_pos.valid_alt); } + bool home_alt_valid() { return (_home_pos.valid_alt); } - bool home_position_valid() { return (_home_pos.timestamp > 0 && _home_pos.valid_alt && _home_pos.valid_hpos && _home_pos.valid_lpos); } + bool home_global_position_valid() { return (_home_pos.valid_alt && _home_pos.valid_hpos); } Geofence &get_geofence() { return _geofence; } diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index db6d5fc340..ce0092f787 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -405,7 +405,10 @@ void Navigator::run() rep->current.loiter_direction = 1; rep->current.type = position_setpoint_s::SETPOINT_TYPE_TAKEOFF; - if (home_position_valid()) { + if (home_global_position_valid()) { + // Only set yaw if we know the true heading + // We assume that the heading is valid when the global position is valid because true heading + // is required to fuse NE (e.g.: GNSS) data. // TODO: we should be more explicit here rep->current.yaw = cmd.param4; rep->previous.valid = true; @@ -836,7 +839,7 @@ void Navigator::geofence_breach_check(bool &have_geofence_position_data) _gf_breach_avoidance.setMaxHorDistHome(_geofence.getMaxHorDistanceHome()); _gf_breach_avoidance.setMaxVerDistHome(_geofence.getMaxVerDistanceHome()); - if (home_position_valid()) { + if (home_global_position_valid()) { _gf_breach_avoidance.setHomePosition(_home_pos.lat, _home_pos.lon, _home_pos.alt); } diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 54978d8a1f..5b40b01ca0 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -83,7 +83,7 @@ void RTL::on_inactive() if ((hrt_absolute_time() - _destination_check_time) > 1_s) { _destination_check_time = hrt_absolute_time(); - if (_navigator->home_position_valid()) { + if (_navigator->home_global_position_valid()) { find_RTL_destination(); } @@ -723,7 +723,7 @@ void RTL::calc_and_pub_rtl_time_estimate() // Calculate RTL time estimate only when there is a valid home position // TODO: Also check if vehicle position is valid - if (!_navigator->home_position_valid()) { + if (!_navigator->home_global_position_valid()) { rtl_time_estimate.valid = false; } else { diff --git a/src/modules/navigator/takeoff.cpp b/src/modules/navigator/takeoff.cpp index 1112f61595..0f6e9bdc2b 100644 --- a/src/modules/navigator/takeoff.cpp +++ b/src/modules/navigator/takeoff.cpp @@ -98,7 +98,9 @@ Takeoff::set_takeoff_position() float min_abs_altitude; - if (_navigator->home_position_valid()) { //only use home position if it is valid + // TODO: review this, comments are talking about home pos, the validity is checked but the + // current altitude is used instead. Also, the "else" case does not consider the current altitude at all. + if (_navigator->home_alt_valid()) { //only use home position if it is valid min_abs_altitude = _navigator->get_global_position()->alt + _navigator->get_takeoff_min_alt(); } else { //e.g. flow @@ -108,7 +110,7 @@ Takeoff::set_takeoff_position() // Use altitude if it has been set. If home position is invalid use min_abs_altitude events::LogLevel log_level = events::LogLevel::Disabled; - if (rep->current.valid && PX4_ISFINITE(rep->current.alt) && _navigator->home_position_valid()) { + if (rep->current.valid && PX4_ISFINITE(rep->current.alt) && _navigator->home_alt_valid()) { abs_altitude = rep->current.alt; // If the altitude suggestion is lower than home + minimum clearance, raise it and complain. diff --git a/src/modules/navigator/vtol_takeoff.cpp b/src/modules/navigator/vtol_takeoff.cpp index 1cc8cfcbb3..9f642d1acf 100644 --- a/src/modules/navigator/vtol_takeoff.cpp +++ b/src/modules/navigator/vtol_takeoff.cpp @@ -51,7 +51,7 @@ VtolTakeoff::VtolTakeoff(Navigator *navigator) : void VtolTakeoff::on_activation() { - if (_navigator->home_position_valid()) { + if (_navigator->home_global_position_valid()) { set_takeoff_position(); _takeoff_state = vtol_takeoff_state::TAKEOFF_HOVER; _navigator->reset_cruising_speed();