Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

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

Merged
merged 1 commit into from
May 31, 2023
Merged

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

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
22 changes: 20 additions & 2 deletions src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down Expand Up @@ -686,12 +690,26 @@ 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);
}

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;
}
9 changes: 9 additions & 0 deletions src/modules/navigator/MissionFeasibility/FeasibilityChecker.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -120,6 +120,7 @@ class FeasibilityChecker : public ModuleParams
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};
Expand Down Expand Up @@ -163,6 +164,14 @@ class FeasibilityChecker : public ModuleParams
*/
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)
*
Expand Down