From 236021cc01de850e7475c1b9a3749295f2a09edc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Wed, 16 Aug 2017 19:52:30 +0200 Subject: [PATCH] vmount mavlink input: fix polling for position_setpoint_triplet The code was violating the rule 'every successfull poll must be followed by an orb_copy'. The result was a busy loop. --- src/drivers/vmount/input_mavlink.cpp | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/src/drivers/vmount/input_mavlink.cpp b/src/drivers/vmount/input_mavlink.cpp index 725abc3bae..f5211a0e3a 100644 --- a/src/drivers/vmount/input_mavlink.cpp +++ b/src/drivers/vmount/input_mavlink.cpp @@ -150,9 +150,15 @@ int InputMavlinkROI::update_impl(unsigned int timeout_ms, ControlData **control_ } // check whether the position setpoint got updated - if ((polls[1].revents & POLLIN) && _cur_roi_mode == vehicle_roi_s::VEHICLE_ROI_WPNEXT) { - _read_control_data_from_position_setpoint_sub(); - *control_data = &_control_data; + if (polls[1].revents & POLLIN) { + if (_cur_roi_mode == vehicle_roi_s::VEHICLE_ROI_WPNEXT) { + _read_control_data_from_position_setpoint_sub(); + *control_data = &_control_data; + + } else { // must do an orb_copy() in *every* case + position_setpoint_triplet_s position_setpoint_triplet; + orb_copy(ORB_ID(position_setpoint_triplet), _position_setpoint_triplet_sub, &position_setpoint_triplet); + } } }