diff options
Diffstat (limited to 'src/modules/navigator/navigator_main.cpp')
-rw-r--r-- | src/modules/navigator/navigator_main.cpp | 38 |
1 files changed, 30 insertions, 8 deletions
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index c569ee7b5..81ceaaca2 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -100,6 +100,7 @@ Navigator::Navigator() : _navigator_task(-1), _mavlink_fd(-1), _global_pos_sub(-1), + _gps_pos_sub(-1), _home_pos_sub(-1), _vstatus_sub(-1), _capabilities_sub(-1), @@ -114,6 +115,7 @@ Navigator::Navigator() : _vstatus{}, _control_mode{}, _global_pos{}, + _gps_pos{}, _sensor_combined{}, _home_pos{}, _mission_item{}, @@ -188,6 +190,12 @@ Navigator::global_position_update() } void +Navigator::gps_position_update() +{ + orb_copy(ORB_ID(vehicle_gps_position), _gps_pos_sub, &_gps_pos); +} + +void Navigator::sensor_combined_update() { orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined); @@ -263,6 +271,7 @@ Navigator::task_main() /* do subscriptions */ _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); + _gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); _sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); _capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities)); _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); @@ -277,6 +286,7 @@ Navigator::task_main() vehicle_status_update(); vehicle_control_mode_update(); global_position_update(); + gps_position_update(); sensor_combined_update(); home_position_update(); navigation_capabilities_update(); @@ -289,7 +299,7 @@ Navigator::task_main() const hrt_abstime mavlink_open_interval = 500000; /* wakeup source(s) */ - struct pollfd fds[7]; + struct pollfd fds[8]; /* Setup of loop */ fds[0].fd = _global_pos_sub; @@ -306,6 +316,8 @@ Navigator::task_main() fds[5].events = POLLIN; fds[6].fd = _sensor_combined_sub; fds[6].events = POLLIN; + fds[7].fd = _gps_pos_sub; + fds[7].events = POLLIN; while (!_task_should_exit) { @@ -330,6 +342,16 @@ Navigator::task_main() _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); } + bool have_geofence_position_data = false; + + /* gps updated */ + if (fds[7].revents & POLLIN) { + gps_position_update(); + if (_geofence.getSource() == Geofence::GF_SOURCE_GPS) { + have_geofence_position_data = true; + } + } + /* sensors combined updated */ if (fds[6].revents & POLLIN) { sensor_combined_update(); @@ -364,14 +386,14 @@ Navigator::task_main() /* global position updated */ if (fds[0].revents & POLLIN) { global_position_update(); - - /* Check geofence violation */ - bool inside = false; - if (_geofence.getAltitudeMode() == Geofence::GF_ALT_MODE_GPS) { - inside = _geofence.inside(_global_pos); - } else { - inside = _geofence.inside(_global_pos, _sensor_combined.baro_alt_meter); + if (_geofence.getSource() == Geofence::GF_SOURCE_GLOBALPOS) { + have_geofence_position_data = true; } + } + + /* Check geofence violation */ + if (have_geofence_position_data) { + bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter); if (!inside) { /* inform other apps via the sp triplet */ _pos_sp_triplet.geofence_violated = true; |