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();