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

Mission feasibility: Geofence check fix

Browse Source 
Current implementation checks against geofence only if it is in polygon form.
When it's created via params, it accepts all the waypoints as the number of vertices = 0.
Thus, changed the function to the one that is used to check whether geofence is breached in flight.
This commit is contained in:
Michal Stasiak
2016-11-05 11:07:23 +01:00
committed by Lorenz Meier
parent bf9a1c5a18
commit 2bfac7ff4f
4 changed files with 18 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/mission_feasibility_checker.cpp
+10 -5
View File
@@ -87,7 +87,7 @@ bool MissionFeasibilityChecker::checkMissionFeasible(orb_advert_t *mavlink_log_p
// check if all mission item commands are supported
failed = failed || !checkMissionItemValidity(dm_current, nMissionItems, condition_landed);
failed = failed || !checkGeofence(dm_current, nMissionItems, geofence);
failed = failed || !checkGeofence(dm_current, nMissionItems, geofence, home_alt);
failed = failed || !checkHomePositionAltitude(dm_current, nMissionItems, home_alt, home_valid, warned);
if (isRotarywing) {
@@ -149,7 +149,7 @@ bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_curre
return resLanding;
}
bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence)
bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt)
{
/* Check if all mission items are inside the geofence (if we have a valid geofence) */
if (geofence.valid()) {
@@ -162,10 +162,15 @@ bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMiss
return false;
}
if (MissionBlock::item_contains_position(&missionitem) &&
!geofence.inside_polygon(missionitem.lat, missionitem.lon, missionitem.altitude)) {
// Geofence function checks against home altitude amsl
missionitem.altitude = missionitem.altitude_is_relative
? missionitem.altitude + home_alt
: missionitem.altitude;
mavlink_log_critical(_mavlink_log_pub, "Geofence violation for waypoint %d", i);
if (MissionBlock::item_contains_position(&missionitem) &&
!geofence.inside(missionitem)) {
mavlink_log_critical(_mavlink_log_pub, "Geofence violation for waypoint %d", i + 1);
return false;
}
}