diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2014-01-05 11:29:06 +0100 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2014-01-05 11:29:06 +0100 |
commit | 95c20ba9f9cc57fffb64f13c7108f4cbb149bf7b (patch) | |
tree | c66bcbce6a5e21f25da647aafedcd8c156570673 /src/modules | |
parent | 26af21619b3f2a67c2480872e0f7c14d0572626e (diff) | |
download | px4-firmware-95c20ba9f9cc57fffb64f13c7108f4cbb149bf7b.tar.gz px4-firmware-95c20ba9f9cc57fffb64f13c7108f4cbb149bf7b.tar.bz2 px4-firmware-95c20ba9f9cc57fffb64f13c7108f4cbb149bf7b.zip |
add inflight geofence check, issues warning on gcs for now
Diffstat (limited to 'src/modules')
-rw-r--r-- | src/modules/navigator/navigator_main.cpp | 20 |
1 files changed, 19 insertions, 1 deletions
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index b6059a01e..bb7520a03 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -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 */ |