fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Mavlink SET_ACTUATOR_CONTROL_TARGET support group_mlx (#7477)

Browse Source 
- Allow to use the 4 groups from Mavlink
 - Allow an Offboard control of the Gimball, Gripper or servoes.
 - Fixes issue #7440

Signed-off-by: Alexis Paques <alexis.paques@gmail.com>
This commit is contained in:
Alexis Paques 2018-08-12 16:08:50 +02:00 committed by Daniel Agar
parent 56ea4fa6fa
commit 6b8ea8e543
2 changed files with 45 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

50
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -125,7 +125,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
_flow_distance_sensor_pub(nullptr),
_distance_sensor_pub(nullptr),
_offboard_control_mode_pub(nullptr),
_actuator_controls_pub(nullptr),
_actuator_controls_pubs{nullptr, nullptr, nullptr, nullptr},
_att_sp_pub(nullptr),
_rates_sp_pub(nullptr),
_pos_sp_triplet_pub(nullptr),
@ -1099,16 +1099,54 @@ MavlinkReceiver::handle_message_set_actuator_control_target(mavlink_message_t *m
actuator_controls.timestamp = hrt_absolute_time();
/* Set duty cycles for the servos in actuator_controls_0 */
/* Set duty cycles for the servos in the actuator_controls message */
for (size_t i = 0; i < 8; i++) {
actuator_controls.control[i] = set_actuator_control_target.controls[i];
}
if (_actuator_controls_pub == nullptr) {
_actuator_controls_pub = orb_advertise(ORB_ID(actuator_controls_0), &actuator_controls);
switch (set_actuator_control_target.group_mlx) {
case 0:
if (_actuator_controls_pubs[0] == nullptr) {
_actuator_controls_pubs[0] = orb_advertise(ORB_ID(actuator_controls_0), &actuator_controls);
} else {
orb_publish(ORB_ID(actuator_controls_0), _actuator_controls_pub, &actuator_controls);
} else {
orb_publish(ORB_ID(actuator_controls_0), _actuator_controls_pubs[0], &actuator_controls);
}
break;
case 1:
if (_actuator_controls_pubs[1] == nullptr) {
_actuator_controls_pubs[1] = orb_advertise(ORB_ID(actuator_controls_1), &actuator_controls);
} else {
orb_publish(ORB_ID(actuator_controls_1), _actuator_controls_pubs[1], &actuator_controls);
}
break;
case 2:
if (_actuator_controls_pubs[2] == nullptr) {
_actuator_controls_pubs[2] = orb_advertise(ORB_ID(actuator_controls_2), &actuator_controls);
} else {
orb_publish(ORB_ID(actuator_controls_2), _actuator_controls_pubs[2], &actuator_controls);
}
break;
case 3:
if (_actuator_controls_pubs[3] == nullptr) {
_actuator_controls_pubs[3] = orb_advertise(ORB_ID(actuator_controls_3), &actuator_controls);
} else {
orb_publish(ORB_ID(actuator_controls_3), _actuator_controls_pubs[3], &actuator_controls);
}
break;
default:
break;
}
}
}

2
src/modules/mavlink/mavlink_receiver.h
View File

@ -222,7 +222,7 @@ private:
orb_advert_t _flow_distance_sensor_pub;
orb_advert_t _distance_sensor_pub;
orb_advert_t _offboard_control_mode_pub;
orb_advert_t _actuator_controls_pub;
orb_advert_t _actuator_controls_pubs[4];
orb_advert_t _att_sp_pub;
orb_advert_t _rates_sp_pub;
orb_advert_t _pos_sp_triplet_pub;