diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index de122c46fb..7f19fced95 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -691,7 +691,6 @@ void VtolAttitudeControl::task_main() /* wait for up to 100ms for data */ int pret = px4_poll(&fds[0], sizeof(fds) / sizeof(fds[0]), 100); - /* timed out - periodic check for _task_should_exit */ if (pret == 0) { continue; @@ -705,6 +704,14 @@ void VtolAttitudeControl::task_main() continue; } + if (fds[0].revents & POLLIN) { + orb_copy(ORB_ID(actuator_controls_virtual_mc), _actuator_inputs_mc, &_actuators_mc_in); + } + + if (fds[1].revents & POLLIN) { + orb_copy(ORB_ID(actuator_controls_virtual_fw), _actuator_inputs_fw, &_actuators_fw_in); + } + if (fds[2].revents & POLLIN) { //parameters were updated, read them now /* read from param to clear updated flag */ struct parameter_update_s update; @@ -762,8 +769,6 @@ void VtolAttitudeControl::task_main() // got data from mc attitude controller if (fds[0].revents & POLLIN) { - orb_copy(ORB_ID(actuator_controls_virtual_mc), _actuator_inputs_mc, &_actuators_mc_in); - _vtol_type->update_mc_state(); fill_mc_att_rates_sp(); @@ -776,7 +781,6 @@ void VtolAttitudeControl::task_main() // got data from fw attitude controller if (fds[1].revents & POLLIN) { - orb_copy(ORB_ID(actuator_controls_virtual_fw), _actuator_inputs_fw, &_actuators_fw_in); vehicle_manual_poll(); _vtol_type->update_fw_state();