diff options
| author | Thomas Gubler <thomasgubler@gmail.com> | 2014-09-30 15:38:27 +0200 |
|---|---|---|
| committer | Thomas Gubler <thomasgubler@gmail.com> | 2014-09-30 15:38:27 +0200 |
| commit | cc05f0f18587b35452fe5e27a55769cefe18c4e5 (patch) | |
| tree | ef787183da27b6a4fdf6de16ae843aed6985ff9b /src/modules/navigator/navigator_main.cpp | |
| parent | 15eee418a035a0109bc0a33fa5889352aa3a1799 (diff) | |
| parent | ab400089bc2a42f1f0ace569d8f0ee58f4338e1d (diff) | |
| download | px4-firmware-cc05f0f18587b35452fe5e27a55769cefe18c4e5.tar.gz px4-firmware-cc05f0f18587b35452fe5e27a55769cefe18c4e5.tar.bz2 px4-firmware-cc05f0f18587b35452fe5e27a55769cefe18c4e5.zip | |
Merge remote-tracking branch 'upstream/obcfailsafe' into swissfang
Conflicts:
src/lib/external_lgpl/tecs/tecs.cpp
src/modules/commander/commander_params.c
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
src/modules/navigator/navigator_main.cpp
Diffstat (limited to 'src/modules/navigator/navigator_main.cpp')
| -rw-r--r-- | src/modules/navigator/navigator_main.cpp | 12 |
1 files changed, 7 insertions, 5 deletions
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index b63394544..fad46998e 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -87,6 +87,7 @@ */ extern "C" __EXPORT int navigator_main(int argc, char *argv[]); +#define GEOFENCE_CHECK_INTERVAL 200000 namespace navigator { @@ -343,7 +344,7 @@ Navigator::task_main() _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); } - bool have_geofence_position_data = false; + static bool have_geofence_position_data = false; /* gps updated */ if (fds[7].revents & POLLIN) { @@ -388,17 +389,18 @@ Navigator::task_main() if (fds[0].revents & POLLIN) { global_position_update(); static int gposcounter = 0; - if (_geofence.getSource() == Geofence::GF_SOURCE_GLOBALPOS && - gposcounter % 10 == 0) { - /* Geofence is checked only every 10th gpos update */ + if (_geofence.getSource() == Geofence::GF_SOURCE_GLOBALPOS) { have_geofence_position_data = true; } gposcounter++; } /* Check geofence violation */ - if (have_geofence_position_data) { + static hrt_abstime last_geofence_check = 0; + if (have_geofence_position_data && hrt_elapsed_time(&last_geofence_check) > GEOFENCE_CHECK_INTERVAL) { bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter); + last_geofence_check = hrt_absolute_time(); + have_geofence_position_data = false; if (!inside) { /* inform other apps via the mission result */ _mission_result.geofence_violated = true; |