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
This commit is contained in:
bresch
2022-04-19 15:29:25 +02:00
committed by Mathieu Bresciani
parent d2f2ba59a4
commit 3f3a4ac5c1
8 changed files with 20 additions and 15 deletions
+2 -2
View File
@@ -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;
+3 -3
View File
@@ -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();
@@ -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) {
+2 -2
View File
@@ -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; }
+5 -2
View File
@@ -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);
}
+2 -2
View File
@@ -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 {
+4 -2
View File
@@ -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.
+1 -1
View File
@@ -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();