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

add inflight geofence check, issues warning on gcs for now

This commit is contained in:
Thomas Gubler 2014-01-05 11:29:06 +01:00
parent 26af21619b
commit 95c20ba9f9
1 changed files with 19 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -160,6 +160,8 @@ private:
perf_counter_t _loop_perf; /**< loop performance counter */
Geofence _geofence;
bool _geofence_violation_warning_sent;
bool _fence_valid; /**< flag if fence is valid */
bool _inside_fence; /**< vehicle is inside fence */
@ -393,7 +395,8 @@ Navigator::Navigator() :
_time_first_inside_orbit(0),
_set_nav_state_timestamp(0),
_need_takeoff(true),
_do_takeoff(false)
_do_takeoff(false),
_geofence_violation_warning_sent(false)
{
_parameter_handles.min_altitude = param_find("NAV_MIN_ALT");
_parameter_handles.acceptance_radius = param_find("NAV_ACCEPT_RAD");
@ -766,6 +769,21 @@ Navigator::task_main()
on_mission_item_reached();
}
}
/* Check geofence violation */
if(!_geofence.inside(&_global_pos)) {
//xxx: publish geofence violation here (or change local flag depending on which app handles the flight termination)
/* Issue a warning about the geofence violation once */
if (!_geofence_violation_warning_sent)
{
mavlink_log_critical(_mavlink_fd, "#audio: Geofence violation");
_geofence_violation_warning_sent = true;
}
} else {
/* Reset the _geofence_violation_warning_sent field */
_geofence_violation_warning_sent = false;
}
}
/* notify user about state changes */