dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

navigator: fix home altitude feasibility check 

This includes the following:
- Remove warning/throw_error flag as it is always !home_alt_valid.
- Remove impossible code path when home_alt_valid = false and
  throw_error = false.
- Add home_alt_valid check in second check before using home_alt.
- Only use warning for the second check to allow waypoints below home.
- Don't return early when only warning.
This commit is contained in:
Julian Oes 2021-05-19 16:05:17 +02:00 committed by Daniel Agar
parent 90a33d59e4
commit 518222bae4
2 changed files with 7 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
src/modules/navigator/mission_feasibility_checker.cpp
View File

@ -67,7 +67,6 @@ MissionFeasibilityChecker::checkMissionFeasible(const mission_s &mission,
}
bool failed = false;
bool warned = false;
// first check if we have a valid position
const bool home_valid = _navigator->home_position_valid();
@ -75,7 +74,6 @@ MissionFeasibilityChecker::checkMissionFeasible(const mission_s &mission,
if (!home_alt_valid) {
failed = true;
warned = true;
mavlink_log_info(_navigator->get_mavlink_log_pub(), "Not yet ready for mission, no position lock.");
} else {
@ -88,7 +86,7 @@ MissionFeasibilityChecker::checkMissionFeasible(const mission_s &mission,
failed = failed || !checkMissionItemValidity(mission);
failed = failed || !checkDistancesBetweenWaypoints(mission, max_distance_between_waypoints);
failed = failed || !checkGeofence(mission, home_alt, home_valid);
failed = failed || !checkHomePositionAltitude(mission, home_alt, home_alt_valid, warned);
failed = failed || !checkHomePositionAltitude(mission, home_alt, home_alt_valid);
if (_navigator->get_vstatus()->is_vtol) {
failed = failed || !checkVTOL(mission, home_alt, false);
@ -174,8 +172,7 @@ MissionFeasibilityChecker::checkGeofence(const mission_s &mission, float home_al
}
bool
MissionFeasibilityChecker::checkHomePositionAltitude(const mission_s &mission, float home_alt, bool home_alt_valid,
bool throw_error)
MissionFeasibilityChecker::checkHomePositionAltitude(const mission_s &mission, float home_alt, bool home_alt_valid)
{
/* Check if all waypoints are above the home altitude */
for (size_t i = 0; i < mission.count; i++) {
@ -193,31 +190,19 @@ MissionFeasibilityChecker::checkHomePositionAltitude(const mission_s &mission, f
_navigator->get_mission_result()->warning = true;
if (throw_error) {
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: No home pos, WP %zu uses rel alt", i + 1);
return false;
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: No home pos, WP %zu uses rel alt", i + 1);
return false;
} else {
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Warning: No home pos, WP %zu uses rel alt", i + 1);
return true;
}
}
/* calculate the global waypoint altitude */
float wp_alt = (missionitem.altitude_is_relative) ? missionitem.altitude + home_alt : missionitem.altitude;
if ((home_alt > wp_alt) && MissionBlock::item_contains_position(missionitem)) {
if (home_alt_valid && home_alt > wp_alt && MissionBlock::item_contains_position(missionitem)) {
_navigator->get_mission_result()->warning = true;
if (throw_error) {
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: Waypoint %zu below home", i + 1);
return false;
} else {
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Warning: Waypoint %zu below home", i + 1);
return true;
}
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Warning: Waypoint %zu below home", i + 1);
}
}

2
src/modules/navigator/mission_feasibility_checker.h
View File

@ -56,7 +56,7 @@ private:
/* Checks for all airframes */
bool checkGeofence(const mission_s &mission, float home_alt, bool home_valid);
bool checkHomePositionAltitude(const mission_s &mission, float home_alt, bool home_alt_valid, bool throw_error);
bool checkHomePositionAltitude(const mission_s &mission, float home_alt, bool home_alt_valid);
bool checkMissionItemValidity(const mission_s &mission);