Browse Source

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
v1.13.0-BW
bresch 3 years ago committed by Mathieu Bresciani
parent
commit
3f3a4ac5c1
  1. 4
      src/modules/navigator/geofence.cpp
  2. 6
      src/modules/navigator/mission.cpp
  3. 2
      src/modules/navigator/mission_feasibility_checker.cpp
  4. 4
      src/modules/navigator/navigator.h
  5. 7
      src/modules/navigator/navigator_main.cpp
  6. 4
      src/modules/navigator/rtl.cpp
  7. 6
      src/modules/navigator/takeoff.cpp
  8. 2
      src/modules/navigator/vtol_takeoff.cpp

4
src/modules/navigator/geofence.cpp

@ -247,7 +247,7 @@ bool Geofence::isCloserThanMaxDistToHome(double lat, double lon, float altitude) @@ -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) @@ -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;

6
src/modules/navigator/mission.cpp

@ -103,7 +103,7 @@ Mission::on_inactive() @@ -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() @@ -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) @@ -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();

2
src/modules/navigator/mission_feasibility_checker.cpp

@ -70,7 +70,7 @@ MissionFeasibilityChecker::checkMissionFeasible(const mission_s &mission, @@ -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) {

4
src/modules/navigator/navigator.h

@ -172,9 +172,9 @@ public: @@ -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; }

7
src/modules/navigator/navigator_main.cpp

@ -405,7 +405,10 @@ void Navigator::run() @@ -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) @@ -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);
}

4
src/modules/navigator/rtl.cpp

@ -83,7 +83,7 @@ void RTL::on_inactive() @@ -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() @@ -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 {

6
src/modules/navigator/takeoff.cpp

@ -98,7 +98,9 @@ Takeoff::set_takeoff_position() @@ -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() @@ -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.

2
src/modules/navigator/vtol_takeoff.cpp

@ -51,7 +51,7 @@ VtolTakeoff::VtolTakeoff(Navigator *navigator) : @@ -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();

Loading…
Cancel
Save