diff --git a/msg/geofence_result.msg b/msg/geofence_result.msg index 86639d2e7c..4121f6f502 100644 --- a/msg/geofence_result.msg +++ b/msg/geofence_result.msg @@ -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 diff --git a/src/lib/events/enums.json b/src/lib/events/enums.json index 9a1c060f8e..4c23d304bd 100644 --- a/src/lib/events/enums.json +++ b/src/lib/events/enums.json @@ -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": { diff --git a/src/modules/commander/HealthAndArmingChecks/checks/geofenceCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/geofenceCheck.cpp index 570df97233..ecb3324ab8 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/geofenceCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/geofenceCheck.cpp @@ -50,11 +50,13 @@ void GeofenceChecks::checkAndReport(const Context &context, Report &reporter) * This check can be configured via GF_ACTION parameter. * */ - reporter.armingCheckFailure(NavModes::All, health_component_t::system, events::ID("check_gf_violation"), - events::Log::Error, "Vehicle outside geofence"); + reporter.armingCheckFailure(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"); } } diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 070fd90dd9..37bdb83ac6 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -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) {