diff options
author | Daniel Agar <daniel@agar.ca> | 2015-02-22 23:04:23 -0500 |
---|---|---|
committer | Daniel Agar <daniel@agar.ca> | 2015-04-13 22:22:19 -0400 |
commit | 25dfd84b40177d40a5848f6c549f5d326f670bee (patch) | |
tree | 2728c49f18a6fa723179357b8ca3a1ce7e12637f /src/modules/navigator/navigator_main.cpp | |
parent | 3c36a615692d746996e4d32a97e8e24285330913 (diff) | |
download | px4-firmware-25dfd84b40177d40a5848f6c549f5d326f670bee.tar.gz px4-firmware-25dfd84b40177d40a5848f6c549f5d326f670bee.tar.bz2 px4-firmware-25dfd84b40177d40a5848f6c549f5d326f670bee.zip |
Geofence max horizontal and vertical distance
-changes GF_ON to GF_MODE
-adds GF_MAX_HOR_DIST and GF_MAX_VER_DIST params
Diffstat (limited to 'src/modules/navigator/navigator_main.cpp')
-rw-r--r-- | src/modules/navigator/navigator_main.cpp | 11 |
1 files changed, 9 insertions, 2 deletions
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index ad2349c94..05fac7b81 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -123,6 +123,7 @@ Navigator::Navigator() : _pos_sp_triplet{}, _mission_result{}, _att_sp{}, + _home_position_set(false), _mission_item_valid(false), _loop_perf(perf_alloc(PC_ELAPSED, "navigator")), _geofence{}, @@ -203,7 +204,13 @@ Navigator::sensor_combined_update() void Navigator::home_position_update() { - orb_copy(ORB_ID(home_position), _home_pos_sub, &_home_pos); + bool updated = false; + orb_check(_home_pos_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(home_position), _home_pos_sub, &_home_pos); + _home_position_set = true; + } } void @@ -392,7 +399,7 @@ Navigator::task_main() /* Check geofence violation */ 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, _home_pos, _home_position_set); last_geofence_check = hrt_absolute_time(); have_geofence_position_data = false; if (!inside) { |