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

Geofence: add checks for custom GF validity when uploading it

Browse Source 
- check if Home would breach it (we do not want to upload GF that
do not allow to return to Home)
- check if the current vehicle position isn't contained in the GF
if armed (as then it would immediately trigger)

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer 2023-11-16 14:41:23 +01:00 committed by Daniel Agar
parent 2865bbb8ab
commit f2aabe968e
2 changed files with 92 additions and 48 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

110
src/modules/navigator/geofence.cpp
View File

@ -229,7 +229,17 @@ void Geofence::_updateFence()
current_seq += mission_fence_point.vertex_count;
}
++_num_polygons;
// check if requiremetns for Home location are met
const bool home_check_okay = checkHomeRequirementsForGeofence(polygon);
// check if current position is inside the fence and vehicle is armed
const bool current_position_check_okay = checkCurrentPositionRequirementsForGeofence(polygon);
// discard the polygon if at least one check fails by not incrementing the counter in that case
if (home_check_okay && current_position_check_okay) {
++_num_polygons;
}
}
break;
@ -242,6 +252,45 @@ void Geofence::_updateFence()
}
}
bool Geofence::checkHomeRequirementsForGeofence(const PolygonInfo &polygon)
{
bool checks_pass = true;
if (_navigator->home_global_position_valid()) {
checks_pass = checkPointAgainstPolygonCircle(polygon, _navigator->get_home_position()->lat,
_navigator->get_home_position()->lon,
_navigator->get_home_position()->alt);
}
if (!checks_pass) {
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Geofence invalid, doesn't contain Home position\t");
events::send(events::ID("navigator_geofence_invalid_against_home"), {events::Log::Critical, events::LogInternal::Warning},
"Geofence invalid, doesn't contain Home position");
}
return checks_pass;
}
bool Geofence::checkCurrentPositionRequirementsForGeofence(const PolygonInfo &polygon)
{
bool checks_pass = true;
// do not allow upload of geofence if vehicle is flying and current geofence would be immediately violated
if (getGeofenceAction() != geofence_result_s::GF_ACTION_NONE && !_navigator->get_land_detected()->landed) {
checks_pass = checkPointAgainstPolygonCircle(polygon, _navigator->get_global_position()->lat,
_navigator->get_global_position()->lon, _navigator->get_global_position()->alt);
}
if (!checks_pass) {
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Geofence invalid, doesn't contain current vehicle position\t");
events::send(events::ID("navigator_geofence_invalid_against_cur_pos"), {events::Log::Critical, events::LogInternal::Warning},
"Geofence invalid, doesn't contain current vehicle position");
}
return checks_pass;
}
bool Geofence::checkPointAgainstAllGeofences(double lat, double lon, float altitude)
{
@ -296,46 +345,33 @@ bool Geofence::isInsidePolygonOrCircle(double lat, double lon, float altitude)
}
/* Horizontal check: iterate all polygons & circles */
bool outside_exclusion = true;
bool inside_inclusion = false;
bool had_inclusion_areas = false;
bool checksPass = true;
for (int polygon_index = 0; polygon_index < _num_polygons; ++polygon_index) {
if (_polygons[polygon_index].fence_type == NAV_CMD_FENCE_CIRCLE_INCLUSION) {
bool inside = insideCircle(_polygons[polygon_index], lat, lon, altitude);
if (inside) {
inside_inclusion = true;
}
had_inclusion_areas = true;
} else if (_polygons[polygon_index].fence_type == NAV_CMD_FENCE_CIRCLE_EXCLUSION) {
bool inside = insideCircle(_polygons[polygon_index], lat, lon, altitude);
if (inside) {
outside_exclusion = false;
}
} else { // it's a polygon
bool inside = insidePolygon(_polygons[polygon_index], lat, lon, altitude);
if (_polygons[polygon_index].fence_type == NAV_CMD_FENCE_POLYGON_VERTEX_INCLUSION) {
if (inside) {
inside_inclusion = true;
}
had_inclusion_areas = true;
} else { // exclusion
if (inside) {
outside_exclusion = false;
}
}
}
checksPass &= checkPointAgainstPolygonCircle(_polygons[polygon_index], lat, lon, altitude);
}
return (!had_inclusion_areas || inside_inclusion) && outside_exclusion;
return checksPass;
}
bool Geofence::checkPointAgainstPolygonCircle(const PolygonInfo &polygon, double lat, double lon, float altitude)
{
bool checksPass = true;
if (polygon.fence_type == NAV_CMD_FENCE_CIRCLE_INCLUSION) {
checksPass &= insideCircle(polygon, lat, lon, altitude);
} else if (polygon.fence_type == NAV_CMD_FENCE_CIRCLE_EXCLUSION) {
checksPass &= !insideCircle(polygon, lat, lon, altitude);
} else if (polygon.fence_type == NAV_CMD_FENCE_POLYGON_VERTEX_INCLUSION) {
checksPass &= insidePolygon(polygon, lat, lon, altitude);
} else if (polygon.fence_type == NAV_CMD_FENCE_POLYGON_VERTEX_EXCLUSION) {
checksPass &= !insidePolygon(polygon, lat, lon, altitude);
}
return checksPass;
}
bool Geofence::insidePolygon(const PolygonInfo &polygon, double lat, double lon, float altitude)

30
src/modules/navigator/geofence.h
View File

@ -193,17 +193,6 @@ private:
*/
void _updateFence();
/**
* Check if a point passes the Geofence test.
* This takes all polygons and minimum & maximum altitude into account
*
* The check passes if: (inside(polygon_inclusion_1) || inside(polygon_inclusion_2) || ... ) &&
* !inside(polygon_exclusion_1) && !inside(polygon_exclusion_2) && ...
* && (altitude within [min, max])
* or: no polygon configured
* @return result of the check above (false for a geofence violation)
*/
bool checkPolygons(double lat, double lon, float altitude);
/**
* Check if a single point is within a polygon
@ -218,6 +207,25 @@ private:
*/
bool insideCircle(const PolygonInfo &polygon, double lat, double lon, float altitude);
/**
* Check if a single point is within a polygon or circle
* @return true if within polygon or circle
*/
bool checkPointAgainstPolygonCircle(const PolygonInfo &polygon, double lat, double lon, float altitude);
/**
* Check polygon or circle geofence fullfills the requirements relative to Home.
* @return true if checks pass
*/
bool checkHomeRequirementsForGeofence(const PolygonInfo &polygon);
/**
* Check polygon or circle geofence fullfills the requirements relative to the current vehicle position.
* @return true if checks pass
*/
bool checkCurrentPositionRequirementsForGeofence(const PolygonInfo &polygon);
DEFINE_PARAMETERS(
(ParamInt<px4::params::GF_ACTION>) _param_gf_action,
(ParamInt<px4::params::GF_SOURCE>) _param_gf_source,