fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-18 06:27:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Check if offboard mode was activated before publishing controls

Browse Source
This commit is contained in:
Matt Beall
2015-02-25 17:31:56 -07:00
committed by Thomas Gubler
parent 220fb19eb7
commit 0f51907dd6
1 changed files with 19 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_receiver.cpp
+19 -9
View File
@@ -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);
}
}
}
}