fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-01 09:04:07 +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-16 14:24:33 +01:00 committed by Daniel Agar
parent a41f24a2e2
commit 3c194d552a
4 changed files with 8 additions and 59 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
src/modules/navigator/geofence.cpp
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)
{

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

@ -81,28 +81,6 @@ public:
*/
void updateFence();
/**
* Return whether the system obeys the geofence.
*
* @return true: system is obeying fence, false: system is violating fence
*/
bool check(const vehicle_global_position_s &global_position, const sensor_gps_s &gps_position);
/**
* Return whether a mission item obeys the geofence.
*
* @return true: system is obeying fence, false: system is violating fence
*/
bool check(const struct mission_item_s &mission_item);
/**
* Check if a point passes the Geofence test.
* In addition to checkPolygons(), this takes all additional parameters into account.
*
* @return false for a geofence violation
*/
bool checkAll(double lat, double lon, float altitude);
bool isCloserThanMaxDistToHome(double lat, double lon, float altitude);
bool isBelowMaxAltitude(float altitude);
@ -210,10 +188,6 @@ private:
*/
bool checkPolygons(double lat, double lon, float altitude);
bool checkAll(const vehicle_global_position_s &global_position);
/**
* Check if a single point is within a polygon
* @return true if within polygon

3
src/modules/navigator/mission_feasibility_checker.cpp
View File

@ -137,7 +137,8 @@ MissionFeasibilityChecker::checkMissionAgainstGeofence(const mission_s &mission,
// Geofence function checks against home altitude amsl
missionitem.altitude = missionitem.altitude_is_relative ? missionitem.altitude + home_alt : missionitem.altitude;
if (MissionBlock::item_contains_position(missionitem) && !_navigator->get_geofence().check(missionitem)) {
if (MissionBlock::item_contains_position(missionitem) && !_navigator->get_geofence().checkPointAgainstAllGeofences(
missionitem.lat, missionitem.lon, missionitem.altitude)) {
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Geofence violation for waypoint %zu\t", i + 1);
events::send<int16_t>(events::ID("navigator_mis_geofence_violation"), {events::Log::Error, events::LogInternal::Info},

4
src/modules/navigator/navigator_main.cpp
View File

@ -1469,8 +1469,8 @@ bool Navigator::geofence_allows_position(const vehicle_global_position_s &pos)
if ((_geofence.getGeofenceAction() != geofence_result_s::GF_ACTION_NONE) &&
(_geofence.getGeofenceAction() != geofence_result_s::GF_ACTION_WARN)) {
if (PX4_ISFINITE(pos.lat) && PX4_ISFINITE(pos.lon)) {
return _geofence.check(pos, _gps_pos);
if (PX4_ISFINITE(pos.lat) && PX4_ISFINITE(pos.lon) && PX4_ISFINITE(pos.alt)) {
return _geofence.checkPointAgainstAllGeofences(pos.lat, pos.lon, pos.alt);
}
}