fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-12 22:23:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

MissionFeasibilityChecker: for MC only find if landing is present, but don't do any validation

Browse Source 
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2022-11-21 18:03:21 +01:00
parent f2f094c33d
commit e68ba81d6d
2 changed files with 24 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/mission_feasibility_checker.cpp
+21 -15
View File
@@ -76,7 +76,7 @@ MissionFeasibilityChecker::checkMissionFeasible(const mission_s &mission,
events::send(events::ID("navigator_mis_no_pos_lock"), events::Log::Info, "Not yet ready for mission, no position lock");
} else {
failed = failed || !checkDistanceToFirstWaypoint(mission, max_distance_to_1st_waypoint);
failed |= !checkDistanceToFirstWaypoint(mission, max_distance_to_1st_waypoint);
}
const float home_alt = _navigator->get_home_position()->alt;
@@ -85,23 +85,27 @@ MissionFeasibilityChecker::checkMissionFeasible(const mission_s &mission,
_has_takeoff = false;
_has_landing = false;
// check if all mission item commands are supported
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);
failed = failed || !checkTakeoff(mission, home_alt);
// run generic (for all vehicle types) checks
failed |= !checkMissionItemValidity(mission);
failed |= !checkDistancesBetweenWaypoints(mission, max_distance_between_waypoints);
failed |= !checkGeofence(mission, home_alt, home_valid);
failed |= !checkHomePositionAltitude(mission, home_alt, home_alt_valid);
failed |= !checkTakeoff(mission, home_alt);
// run type-specifc landing checks, which also include seting _has_landing that is used in checkTakeoffLandAvailable()
if (_navigator->get_vstatus()->is_vtol) {
failed |= (!checkTakeoff(mission, home_alt) || !checkVTOLLanding(mission) || !checkTakeoffLandAvailable());
} else if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
failed |= (!checkTakeoff(mission, home_alt) || !checkRotaryWingLanding(mission) || !checkTakeoffLandAvailable());
failed |= !checkVTOLLanding(mission);
} else if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
failed |= (!checkTakeoff(mission, home_alt) || !checkFixedWingLanding(mission) || !checkTakeoffLandAvailable());
failed |= !checkFixedWingLanding(mission);
} else {
// if neither VTOL nor FW, only check if mission has landing but don't check that one for validity
_has_landing = hasMissionLanding(mission);
}
failed |= !checkTakeoffLandAvailable();
return !failed;
}
@@ -413,11 +417,13 @@ MissionFeasibilityChecker::checkTakeoff(const mission_s &mission, float home_alt
}
bool
MissionFeasibilityChecker::checkRotaryWingLanding(const mission_s &mission)
MissionFeasibilityChecker::hasMissionLanding(const mission_s &mission)
{
// Go through all mission items and search for a landing waypoint.
// For MC we currently do not run any checks on the validity of the planned landing.
bool mission_landing_found = false;
for (size_t i = 0; i < mission.count; i++) {
struct mission_item_s missionitem;
const ssize_t len = sizeof(missionitem);
@@ -428,11 +434,11 @@ MissionFeasibilityChecker::checkRotaryWingLanding(const mission_s &mission)
}
if (missionitem.nav_cmd == NAV_CMD_LAND) {
_has_landing = true;
mission_landing_found = true;
}
}
return true;
return mission_landing_found;
}
bool
src/modules/navigator/mission_feasibility_checker.h
+3 -3
View File
@@ -125,12 +125,12 @@ private:
bool checkTakeoffLandAvailable();
/**
* @brief Rotary-wing vehicle: Check if mission contains landing (safe in _has_landing), and if that landing is valid
* @brief Check if mission contains landing
*
* @param mission Mission struct
* @return True if the planned landing is valid, or no landing planned, False otherwise
* @return True if mission contains a landing, False if otherwise
*/
bool checkRotaryWingLanding(const mission_s &mission);
bool hasMissionLanding(const mission_s &mission);
/**
* @brief Fixed-wing vehicle: Check if mission contains landing (safe in _has_landing), and if that landing is valid