diff options
| -rw-r--r-- | src/modules/mavlink/mavlink_receiver.cpp | 28 |
1 files changed, 19 insertions, 9 deletions
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 27a6d4bca..bce93cc6a 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -684,19 +684,29 @@ MavlinkReceiver::handle_message_set_actuator_control_target(mavlink_message_t *m orb_publish(ORB_ID(offboard_control_mode), _offboard_control_mode_pub, &offboard_control_mode); } - actuator_controls.timestamp = hrt_absolute_time(); - /* Set duty cycles for the servos in actuator_controls_0 */ - for(size_t i = 0; i < 8; i++) { - actuator_controls.control[i] = set_actuator_control_target.controls[i]; + /* If we are in offboard control mode, publish the actuator controls */ + bool updated; + orb_check(_control_mode_sub, &updated); + if (updated) { + orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); } - if (_actuator_controls_pub < 0) { - _actuator_controls_pub = orb_advertise(ORB_ID(actuator_controls_0), &actuator_controls); - } else { - orb_publish(ORB_ID(actuator_controls_0), _actuator_controls_pub, &actuator_controls); + if (_control_mode.flag_control_offboard_enabled) { + + actuator_controls.timestamp = hrt_absolute_time(); + + /* Set duty cycles for the servos in actuator_controls_0 */ + for(size_t i = 0; i < 8; i++) { + actuator_controls.control[i] = set_actuator_control_target.controls[i]; + } + + if (_actuator_controls_pub < 0) { + _actuator_controls_pub = orb_advertise(ORB_ID(actuator_controls_0), &actuator_controls); + } else { + orb_publish(ORB_ID(actuator_controls_0), _actuator_controls_pub, &actuator_controls); + } } - } } |