fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-31 22:11:37 +08:00
Code Issues Packages Projects Releases Wiki Activity

make geofence update rate independent from positon update rate

Browse Source
This commit is contained in:
Thomas Gubler
2014-09-28 16:08:25 +02:00
parent 3efffb68e7
commit 964fddb387
1 changed files with 7 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/navigator_main.cpp
+7 -5
View File
@@ -87,6 +87,7 @@
*/ */
extern "C" __EXPORT int navigator_main(int argc, char *argv[]); extern "C" __EXPORT int navigator_main(int argc, char *argv[]);
#define GEOFENCE_CHECK_INTERVAL 200000
namespace navigator namespace navigator
{ {
@@ -343,7 +344,7 @@ Navigator::task_main()
_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
} }
bool have_geofence_position_data = false; static bool have_geofence_position_data = false;
/* gps updated */ /* gps updated */
if (fds[7].revents & POLLIN) { if (fds[7].revents & POLLIN) {
@@ -388,17 +389,18 @@ Navigator::task_main()
if (fds[0].revents & POLLIN) { if (fds[0].revents & POLLIN) {
global_position_update(); global_position_update();
static int gposcounter = 0; static int gposcounter = 0;
if (_geofence.getSource() == Geofence::GF_SOURCE_GLOBALPOS && if (_geofence.getSource() == Geofence::GF_SOURCE_GLOBALPOS) {
gposcounter % 10 == 0) {
/* Geofence is checked only every 10th gpos update */
have_geofence_position_data = true; have_geofence_position_data = true;
} }
gposcounter++; gposcounter++;
} }
/* Check geofence violation */ /* 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); 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) { if (!inside) {
/* inform other apps via the mission result */ /* inform other apps via the mission result */
_mission_result.geofence_violated = true; _mission_result.geofence_violated = true;