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

geofence: add and report reason for violation

This commit is contained in:
Beat Küng 2022-09-07 09:23:11 +02:00 committed by Daniel Agar
parent ebc88afe46
commit acaa50a448
4 changed files with 37 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
msg/geofence_result.msg
View File

@ -6,6 +6,7 @@ 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 primary_geofence_breached # true if the primary geofence is breached
uint8 primary_geofence_action # action to take when the primary geofence is breached

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

@ -623,6 +623,24 @@
"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": {

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

@ -50,11 +50,13 @@ void GeofenceChecks::checkAndReport(const Context &context, Report &reporter)
* 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"),
events::Log::Error, "Vehicle outside geofence");
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);
if (reporter.mavlink_log_pub()) {
mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Vehicle outside geofence");
mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Geofence violation");
}
}

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

@ -870,6 +870,19 @@ void Navigator::geofence_breach_check(bool &have_geofence_position_data)
/* 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;
}
/* 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) {