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

Geofence: rework the way to check all GF at once

Browse Source 
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2023-11-22 21:45:02 -05:00
committed by Daniel Agar
parent a41f24a2e2
commit 3c194d552a
4 changed files with 8 additions and 59 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/geofence.cpp
+4 -30
View File
@@ -244,39 +244,13 @@ void Geofence::_updateFence()
}
}
bool Geofence::checkAll(const struct vehicle_global_position_s &global_position)
bool Geofence::checkPointAgainstAllGeofences(double lat, double lon, float altitude)
{
return checkAll(global_position.lat, global_position.lon, global_position.alt);
}
bool Geofence::checkAll(double lat, double lon, float altitude)
{
bool inside_fence = isCloserThanMaxDistToHome(lat, lon, altitude);
inside_fence = inside_fence && isBelowMaxAltitude(altitude);
// to be inside the geofence both fences have to report being inside
// as they both report being inside when not enabled
inside_fence = inside_fence && isInsidePolygonOrCircle(lat, lon, altitude);
const bool inside_fence = isCloserThanMaxDistToHome(lat, lon, altitude) && isBelowMaxAltitude(altitude)
&& isInsidePolygonOrCircle(lat, lon, altitude);
return inside_fence;
}
}
bool Geofence::check(const vehicle_global_position_s &global_position, const sensor_gps_s &gps_position)
{
if (getSource() == Geofence::GF_SOURCE_GLOBALPOS) {
return checkAll(global_position);
} else {
return checkAll(gps_position.latitude_deg, gps_position.longitude_deg, gps_position.altitude_msl_m);
}
}
bool Geofence::check(const struct mission_item_s &mission_item)
{
return checkAll(mission_item.lat, mission_item.lon, mission_item.altitude);
}
bool Geofence::isCloserThanMaxDistToHome(double lat, double lon, float altitude)
{