Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-01 02:55:07 +08:00
Code Issues Actions 19 Packages Projects Releases Wiki Activity

Navigator: MissionFeasibilityCheck: check if items fit to the current vehicle type (#21602)

Browse Source 
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2023-05-31 19:27:27 +02:00
committed by GitHub
parent d6413a6a90
commit 4b5e14aeec
2 changed files with 29 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp
+20 -2
View File
@@ -206,6 +206,10 @@ void FeasibilityChecker::doCommonChecks(mission_item_s &mission_item, const int
if (!_takeoff_failed) {
_takeoff_failed = !checkTakeoff(mission_item);
}
if (!_items_fit_to_vehicle_type_failed) {
_items_fit_to_vehicle_type_failed = !checkItemsFitToVehicleType(mission_item);
}
}
void FeasibilityChecker::doVtolChecks(mission_item_s &mission_item, const int current_index, const int last_index)
@@ -686,8 +690,6 @@ bool FeasibilityChecker::checkIfBelowHomeAltitude(const mission_item_s &mission_
if (PX4_ISFINITE(_home_alt_msl) && _home_alt_msl > wp_alt && MissionBlock::item_contains_position(mission_item)) {
mavlink_log_critical(_mavlink_log_pub, "Warning: Waypoint %d below home\t", current_index + 1);
events::send<int16_t>(events::ID("navigator_mis_wp_below_home"), {events::Log::Warning, events::LogInternal::Info},
"Waypoint {1} below home", current_index + 1);
@@ -695,3 +697,19 @@ bool FeasibilityChecker::checkIfBelowHomeAltitude(const mission_item_s &mission_
return true;
}
bool FeasibilityChecker::checkItemsFitToVehicleType(const mission_item_s &mission_item)
{
if (_vehicle_type != VehicleType::Vtol &&
(mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF || mission_item.nav_cmd == NAV_CMD_VTOL_LAND
|| mission_item.nav_cmd == NAV_CMD_DO_VTOL_TRANSITION)) {
mavlink_log_critical(_mavlink_log_pub, "Mission rejected: Mission contains VTOL items but vehicle is not a VTOL\t");
events::send(events::ID("navigator_mis_vtol_items"), {events::Log::Error, events::LogInternal::Info},
"Mission rejected: Mission contains VTOL items but vehicle is not a VTOL");
return false;
}
return true;
}
src/modules/navigator/MissionFeasibility/FeasibilityChecker.hpp
+9
View File
@@ -120,6 +120,7 @@ private:
bool _below_home_alt_failed{false};
bool _fixed_wing_land_approach_failed{false};
bool _takeoff_land_available_failed{false};
bool _items_fit_to_vehicle_type_failed{false};
// internal checkTakeoff related variables
bool _found_item_with_position{false};
@@ -163,6 +164,14 @@ private:
*/
bool checkTakeoff(mission_item_s &mission_item);
/**
* @brief Check if the mission items fit to the vehicle type
*
* @param mission_item The current mission item
* @return False if the check failed.
*/
bool checkItemsFitToVehicleType(const mission_item_s &mission_item);
/**
* @brief Check validity of landing pattern (fixed wing & vtol)
*