Set up to receive mavlink actuator control messages and publish to uorb

2 files changed, 31 insertions, 0 deletions

diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 67942912f..86dff69c5 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -109,6 +109,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
 	_flow_pub(-1),
 	_range_pub(-1),
 	_offboard_control_mode_pub(-1),
+	_actuator_controls_pub(-1),
 	_global_vel_sp_pub(-1),
 	_att_sp_pub(-1),
 	_rates_sp_pub(-1),
@@ -170,6 +171,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 		handle_message_set_attitude_target(msg);
 		break;
 
+	case MAVLINK_MSG_ID_SET_ACTUATOR_CONTROL_TARGET:
+		handle_message_set_actuator_control_target(msg);
+		break;
+
 	case MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE:
 		handle_message_vision_position_estimate(msg);
 		break;
@@ -649,6 +654,30 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
 }
 
 void
+MavlinkReceiver::handle_message_set_actuator_control_target(mavlink_message_t *msg)
+{
+	mavlink_set_actuator_control_target_t set_actuator_control_target;
+	mavlink_msg_set_actuator_control_target_decode(msg, &set_actuator_control_target);
+
+	struct actuator_controls_s actuator_controls;
+	memset(&actuator_controls, 0, sizeof(actuator_controls));//XXX breaks compatibility with multiple setpoints
+
+	if ((mavlink_system.sysid                         == set_actuator_control_target.target_system ||
+		 set_actuator_control_target.target_system    == 0) &&
+		(mavlink_system.compid 						  == set_actuator_control_target.target_component ||
+		 set_actuator_control_target.target_component == 0)){
+
+		actuator_controls.timestamp = hrt_absolute_time();
+
+		for(size_t i = 0; i < 8 ; i++){
+			actuator_controls.control[i] = set_actuator_control_target.controls[i];
+		}
+		_actuator_controls_pub = orb_advertise(ORB_ID(actuator_controls_0), &actuator_controls);
+	}
+
+}
+
+void
 MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg)
 {
 	mavlink_vision_position_estimate_t pos;
diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h
index 15943753d..4886bbd0a 100644
--- a/src/modules/mavlink/mavlink_receiver.h
+++ b/src/modules/mavlink/mavlink_receiver.h
@@ -122,6 +122,7 @@ private:
 	void handle_message_vision_position_estimate(mavlink_message_t *msg);
 	void handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message_t *msg);
 	void handle_message_set_position_target_local_ned(mavlink_message_t *msg);
+	void handle_message_set_actuator_control_target(mavlink_message_t *msg);
 	void handle_message_set_attitude_target(mavlink_message_t *msg);
 	void handle_message_radio_status(mavlink_message_t *msg);
 	void handle_message_manual_control(mavlink_message_t *msg);
@@ -163,6 +164,7 @@ private:
 	orb_advert_t _flow_pub;
 	orb_advert_t _range_pub;
 	orb_advert_t _offboard_control_mode_pub;
+	orb_advert_t _actuator_controls_pub;
 	orb_advert_t _global_vel_sp_pub;
 	orb_advert_t _att_sp_pub;
 	orb_advert_t _rates_sp_pub;