dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Geofence: rework messaging and notification 

- do reporting of breaching in-air only in geofenceCheck
- remove geofence_violation_reason_t
- replace geofence_breached field in GeofenceResult.msg with 3 fields
(one for each GF type: max dist, max alt, custom geofence)
- the warning message after breaching a GF is only done by Commander,
and it's specific to the GF type failure

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer 2023-11-16 14:33:44 +01:00 committed by Daniel Agar
parent 3c194d552a
commit 2865bbb8ab
10 changed files with 127 additions and 123 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
msg/FailsafeFlags.msg
View File

@ -43,7 +43,7 @@ bool battery_low_remaining_time # Low battery based on remaining flight ti
bool battery_unhealthy # Battery unhealthy
# Other
bool primary_geofence_breached # Primary Geofence breached
bool geofence_breached # Geofence breached (one or multiple)
bool mission_failure # Mission failure
bool vtol_fixed_wing_system_failure # vehicle in fixed-wing system failure failsafe mode (after quad-chute)
bool wind_limit_exceeded # Wind limit exceeded

7
msg/GeofenceResult.msg
View File

@ -6,7 +6,8 @@ uint8 GF_ACTION_RTL = 3 # switch to AUTO|RTL
uint8 GF_ACTION_TERMINATE = 4 # flight termination
uint8 GF_ACTION_LAND = 5 # switch to AUTO|LAND
uint8 geofence_violation_reason # one of geofence_violation_reason_t::*
bool geofence_max_dist_triggered # true the check for max distance from Home is triggered
bool geofence_max_alt_triggered # true the check for max altitude above Home is triggered
bool geofence_custom_fence_triggered # true the check for custom inclusion/exclusion geofence(s) is triggered
bool primary_geofence_breached # true if the primary geofence is breached
uint8 primary_geofence_action # action to take when the primary geofence is breached
uint8 geofence_action # action to take when the geofence is breached

18
src/lib/events/enums.json
View File

@ -733,24 +733,6 @@
"description": "Reduce throttle!"
}
}
},
"geofence_violation_reason_t": {
"type": "uint8_t",
"description": "Reason for geofence violation",
"entries": {
"0": {
"name": "dist_to_home_exceeded",
"description": "maximum distance to home exceeded"
},
"1": {
"name": "max_altitude_exceeded",
"description": "maximum altitude exceeded"
},
"2": {
"name": "fence_violation",
"description": "approaching or outside geofence"
}
}
}
},
"navigation_mode_groups": {

68
src/modules/commander/HealthAndArmingChecks/checks/geofenceCheck.cpp
View File

@ -41,26 +41,64 @@ void GeofenceChecks::checkAndReport(const Context &context, Report &reporter)
geofence_result = {};
}
reporter.failsafeFlags().primary_geofence_breached = geofence_result.primary_geofence_breached;
const bool any_geofence_triggered = geofence_result.geofence_max_dist_triggered ||
geofence_result.geofence_max_alt_triggered ||
geofence_result.geofence_custom_fence_triggered;
if (geofence_result.primary_geofence_action != 0 && reporter.failsafeFlags().primary_geofence_breached) {
/* EVENT
* @description
* <profile name="dev">
* This check can be configured via <param>GF_ACTION</param> parameter.
* </profile>
*/
reporter.armingCheckFailure<events::px4::enums::geofence_violation_reason_t>(NavModes::All, health_component_t::system,
events::ID("check_gf_violation"),
events::Log::Error, "Geofence violation: {1}",
(events::px4::enums::geofence_violation_reason_t)geofence_result.geofence_violation_reason);
reporter.failsafeFlags().geofence_breached = any_geofence_triggered;
if (reporter.mavlink_log_pub()) {
mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Geofence violation");
if (geofence_result.geofence_action != geofence_result_s::GF_ACTION_NONE && any_geofence_triggered) {
if (geofence_result.geofence_max_dist_triggered) {
/* EVENT
* @description
* <profile name="dev">
* This check can be configured via <param>GF_ACTION</param> and <param>GF_MAX_HOR_DIST</param> parameters.
* </profile>
*/
reporter.armingCheckFailure(NavModes::All, health_component_t::system,
events::ID("check_gf_violation_max_hor_dist"),
events::Log::Error, "Geofence violation: exceeding maximum distance to Home");
if (reporter.mavlink_log_pub()) {
mavlink_log_critical(reporter.mavlink_log_pub(), "Geofence violation: exceeding maximum distance to Home");
}
}
if (geofence_result.geofence_max_alt_triggered) {
/* EVENT
* @description
* <profile name="dev">
* This check can be configured via <param>GF_ACTION</param> and <param>GF_MAX_VER_DIST</param> parameters.
* </profile>
*/
reporter.armingCheckFailure(NavModes::All, health_component_t::system,
events::ID("check_gf_violation_max_alt"),
events::Log::Error, "Geofence violation: exceeding maximum altitude above Home");
if (reporter.mavlink_log_pub()) {
mavlink_log_critical(reporter.mavlink_log_pub(), "Geofence violation: exceeding maximum altitude above Home");
}
}
if (geofence_result.geofence_custom_fence_triggered) {
/* EVENT
* @description
* <profile name="dev">
* This check can be configured via <param>GF_ACTION</param> parameter.
* </profile>
*/
reporter.armingCheckFailure(NavModes::All, health_component_t::system,
events::ID("check_gf_violation_custom_gf"),
events::Log::Error, "Geofence violation: approaching or outside geofence");
if (reporter.mavlink_log_pub()) {
mavlink_log_critical(reporter.mavlink_log_pub(), "Geofence violation: approaching or outside geofence");
}
}
}
if (geofence_result.primary_geofence_action == geofence_result_s::GF_ACTION_RTL
if (geofence_result.geofence_action == geofence_result_s::GF_ACTION_RTL
&& reporter.failsafeFlags().home_position_invalid) {
/* EVENT
* @description

2
src/modules/commander/failsafe/failsafe.cpp
View File

@ -442,7 +442,7 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state,
CHECK_FAILSAFE(status_flags, local_position_accuracy_low, ActionOptions(Action::RTL));
}
CHECK_FAILSAFE(status_flags, primary_geofence_breached, fromGfActParam(_param_gf_action.get()).cannotBeDeferred());
CHECK_FAILSAFE(status_flags, geofence_breached, fromGfActParam(_param_gf_action.get()).cannotBeDeferred());
// Battery
CHECK_FAILSAFE(status_flags, battery_low_remaining_time,

3
src/modules/mavlink/streams/HIGH_LATENCY2.hpp
View File

@ -339,7 +339,8 @@ private:
geofence_result_s geofence;
if (_geofence_sub.update(&geofence)) {
if (geofence.primary_geofence_breached) {
if (geofence.geofence_max_dist_triggered || geofence.geofence_max_alt_triggered
|| geofence.geofence_custom_fence_triggered) {
msg->failure_flags |= HL_FAILURE_FLAG_GEOFENCE;
}

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

@ -51,8 +51,6 @@
#include "navigator.h"
#define GEOFENCE_RANGE_WARNING_LIMIT 5000000
Geofence::Geofence(Navigator *navigator) :
ModuleParams(navigator),
_navigator(navigator)
@ -256,31 +254,15 @@ bool Geofence::isCloserThanMaxDistToHome(double lat, double lon, float altitude)
{
bool inside_fence = true;
if (isHomeRequired() && _navigator->home_global_position_valid()) {
const float max_horizontal_distance = _param_gf_max_hor_dist.get();
const double home_lat = _navigator->get_home_position()->lat;
const double home_lon = _navigator->get_home_position()->lon;
const float home_alt = _navigator->get_home_position()->alt;
if (_param_gf_max_hor_dist.get() > FLT_EPSILON && _navigator->home_global_position_valid()) {
float dist_xy = -1.0f;
float dist_z = -1.0f;
get_distance_to_point_global_wgs84(lat, lon, altitude, home_lat, home_lon, home_alt, &dist_xy, &dist_z);
get_distance_to_point_global_wgs84(lat, lon, altitude, _navigator->get_home_position()->lat,
_navigator->get_home_position()->lon, _navigator->get_home_position()->alt, &dist_xy, &dist_z);
if (max_horizontal_distance > FLT_EPSILON && (dist_xy > max_horizontal_distance)) {
if (hrt_elapsed_time(&_last_horizontal_range_warning) > GEOFENCE_RANGE_WARNING_LIMIT) {
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Maximum distance from home reached (%.5f)\t",
(double)max_horizontal_distance);
events::send<float>(events::ID("navigator_geofence_max_dist_from_home"), {events::Log::Critical, events::LogInternal::Warning},
"Geofence: maximum distance from home reached ({1:.0m})",
max_horizontal_distance);
_last_horizontal_range_warning = hrt_absolute_time();
}
inside_fence = false;
}
inside_fence = dist_xy < _param_gf_max_hor_dist.get();
}
return inside_fence;
@ -290,25 +272,10 @@ bool Geofence::isBelowMaxAltitude(float altitude)
{
bool inside_fence = true;
if (isHomeRequired() && _navigator->home_alt_valid()) {
if (_param_gf_max_ver_dist.get() > FLT_EPSILON && _navigator->home_alt_valid()) {
const float max_vertical_distance = _param_gf_max_ver_dist.get();
const float home_alt = _navigator->get_home_position()->alt;
float dist_z = altitude - home_alt;
if (max_vertical_distance > FLT_EPSILON && (dist_z > max_vertical_distance)) {
if (hrt_elapsed_time(&_last_vertical_range_warning) > GEOFENCE_RANGE_WARNING_LIMIT) {
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Maximum altitude above home reached (%.5f)\t",
(double)max_vertical_distance);
events::send<float>(events::ID("navigator_geofence_max_alt_from_home"), {events::Log::Critical, events::LogInternal::Warning},
"Geofence: maximum altitude above home reached ({1:.0m_v})",
max_vertical_distance);
_last_vertical_range_warning = hrt_absolute_time();
}
inside_fence = false;
}
const float dist_z = altitude - _navigator->get_home_position()->alt;
inside_fence = dist_z < _param_gf_max_ver_dist.get();
}
return inside_fence;

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

@ -81,8 +81,29 @@ public:
*/
void updateFence();
/**
* Check if a 3D point passes the Geofence test.
* Checks max distance, max altitude, inside polygon or circle.
* In addition to checkPolygons(), this takes all additional parameters into account.
*
* @return false for a geofence violation
*/
bool checkPointAgainstAllGeofences(double lat, double lon, float altitude);
/**
* @brief check if the horizontal distance to Home is greater than the maximum allowed distance
*
* @return true if the horizontal distance to Home is smaller than the maximum allowed distance
*/
bool isCloserThanMaxDistToHome(double lat, double lon, float altitude);
/**
* @brief check if the altitude above Home is greater than the maximum allowed altitude
*
* @return true if the altitude above Home is smaller than the maximum allowed altitude
*/
bool isBelowMaxAltitude(float altitude);
virtual bool isInsidePolygonOrCircle(double lat, double lon, float altitude);
@ -118,7 +139,6 @@ public:
int getGeofenceAction() { return _param_gf_action.get(); }
float getMaxHorDistanceHome() { return _param_gf_max_hor_dist.get(); }
float getMaxVerDistanceHome() { return _param_gf_max_ver_dist.get(); }
bool getPredict() { return _param_gf_predict.get(); }
bool isHomeRequired();
@ -156,9 +176,6 @@ private:
DatamanCache _dataman_cache{"geofence_dm_cache_miss", 4};
DatamanClient &_dataman_client = _dataman_cache.client();
hrt_abstime _last_horizontal_range_warning{0};
hrt_abstime _last_vertical_range_warning{0};
float _altitude_min{0.0f};
float _altitude_max{0.0f};

16
src/modules/navigator/geofence_params.c
View File

@ -76,12 +76,11 @@ PARAM_DEFINE_INT32(GF_ACTION, 2);
*/
PARAM_DEFINE_INT32(GF_SOURCE, 0);
/**
* Max horizontal distance in meters.
* Max horizontal distance from Home
*
* Maximum horizontal distance in meters the vehicle can be from home before triggering a geofence action. Disabled if 0.
* Maximum horizontal distance in meters the vehicle can be from Home before triggering a geofence action.
* Disabled if 0.
*
* @unit m
* @min 0
@ -89,12 +88,13 @@ PARAM_DEFINE_INT32(GF_SOURCE, 0);
* @increment 1
* @group Geofence
*/
PARAM_DEFINE_FLOAT(GF_MAX_HOR_DIST, 0);
PARAM_DEFINE_FLOAT(GF_MAX_HOR_DIST, 0.0f);
/**
* Max vertical distance in meters.
* Max vertical distance from Home
*
* Maximum vertical distance in meters the vehicle can be from home before triggering a geofence action. Disabled if 0.
* Maximum vertical distance in meters the vehicle can be from Home before triggering a geofence action.
* Disabled if 0.
*
* @unit m
* @min 0
@ -102,7 +102,7 @@ PARAM_DEFINE_FLOAT(GF_MAX_HOR_DIST, 0);
* @increment 1
* @group Geofence
*/
PARAM_DEFINE_FLOAT(GF_MAX_VER_DIST, 0);
PARAM_DEFINE_FLOAT(GF_MAX_VER_DIST, 0.0f);
/**
* [EXPERIMENTAL] Use Pre-emptive geofence triggering

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

@ -961,58 +961,58 @@ void Navigator::geofence_breach_check(bool &have_geofence_position_data)
}
}
double current_latitude = _global_pos.lat;
double current_longitude = _global_pos.lon;
float current_altitude = _global_pos.alt;
bool position_valid = _global_pos.timestamp > 0;
if (_geofence.getSource() == Geofence::GF_SOURCE_GPS) {
current_latitude = _gps_pos.latitude_deg;
current_longitude = _gps_pos.longitude_deg;
current_altitude = _gps_pos.altitude_msl_m;
position_valid = _global_pos.timestamp > 0;
}
_gf_breach_avoidance.setHorizontalTestPointDistance(test_point_distance);
_gf_breach_avoidance.setVerticalTestPointDistance(vertical_test_point_distance);
_gf_breach_avoidance.setTestPointBearing(test_point_bearing);
_gf_breach_avoidance.setCurrentPosition(_global_pos.lat, _global_pos.lon, _global_pos.alt);
_gf_breach_avoidance.setCurrentPosition(current_latitude, current_longitude, current_altitude);
_gf_breach_avoidance.setMaxHorDistHome(_geofence.getMaxHorDistanceHome());
if (home_global_position_valid()) {
_gf_breach_avoidance.setHomePosition(_home_pos.lat, _home_pos.lon, _home_pos.alt);
}
double test_point_latitude = current_latitude;
double test_point_longitude = current_longitude;
float test_point_altitude = current_altitude;
if (_geofence.getPredict()) {
fence_violation_test_point = _gf_breach_avoidance.getFenceViolationTestPoint();
matrix::Vector2<double>fence_violation_test_point = _gf_breach_avoidance.getFenceViolationTestPoint();
test_point_latitude = fence_violation_test_point(0);
test_point_longitude = fence_violation_test_point(1);
test_point_altitude = current_altitude + vertical_test_point_distance;
}
} else {
fence_violation_test_point = matrix::Vector2d(_global_pos.lat, _global_pos.lon);
vertical_test_point_distance = 0;
}
gf_violation_type.flags.dist_to_home_exceeded = !_geofence.isCloserThanMaxDistToHome(fence_violation_test_point(0),
fence_violation_test_point(1),
_global_pos.alt);
gf_violation_type.flags.max_altitude_exceeded = !_geofence.isBelowMaxAltitude(_global_pos.alt +
vertical_test_point_distance);
gf_violation_type.flags.fence_violation = !_geofence.isInsidePolygonOrCircle(fence_violation_test_point(0),
fence_violation_test_point(1),
_global_pos.alt);
_geofence_result.geofence_max_dist_triggered = !_geofence.isCloserThanMaxDistToHome(test_point_latitude,
test_point_longitude, test_point_altitude);
_geofence_result.geofence_max_alt_triggered = !_geofence.isBelowMaxAltitude(test_point_altitude);
_geofence_result.geofence_custom_fence_triggered = !_geofence.isInsidePolygonOrCircle(test_point_latitude,
test_point_longitude, test_point_altitude);
_last_geofence_check = hrt_absolute_time();
have_geofence_position_data = false;
_geofence_result.timestamp = hrt_absolute_time();
_geofence_result.primary_geofence_action = _geofence.getGeofenceAction();
_geofence_result.geofence_action = _geofence.getGeofenceAction();
if (gf_violation_type.value) {
/* inform other apps via the mission result */
_geofence_result.primary_geofence_breached = true;
using geofence_violation_reason_t = events::px4::enums::geofence_violation_reason_t;
if (gf_violation_type.flags.fence_violation) {
_geofence_result.geofence_violation_reason = (uint8_t)geofence_violation_reason_t::fence_violation;
} else if (gf_violation_type.flags.max_altitude_exceeded) {
_geofence_result.geofence_violation_reason = (uint8_t)geofence_violation_reason_t::max_altitude_exceeded;
} else if (gf_violation_type.flags.dist_to_home_exceeded) {
_geofence_result.geofence_violation_reason = (uint8_t)geofence_violation_reason_t::dist_to_home_exceeded;
}
if (_geofence_result.geofence_max_dist_triggered || _geofence_result.geofence_max_alt_triggered ||
_geofence_result.geofence_custom_fence_triggered) {
/* Issue a warning about the geofence violation once and only if we are armed */
if (!_geofence_violation_warning_sent && _vstatus.arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
@ -1060,8 +1060,6 @@ void Navigator::geofence_breach_check(bool &have_geofence_position_data)
}
} else {
/* inform other apps via the mission result */
_geofence_result.primary_geofence_breached = false;
/* Reset the _geofence_violation_warning_sent field */
_geofence_violation_warning_sent = false;