Update offboard control uorb topic

Work towards supporting the new external setpoint mavlink messages

1 files changed, 28 insertions, 0 deletions

diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 3747ad3ba..73222637f 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -154,6 +154,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 		handle_message_quad_swarm_roll_pitch_yaw_thrust(msg);
 		break;
 
+	case MAVLINK_MSG_ID_LOCAL_NED_POSITION_SETPOINT_EXTERNAL:
+		handle_message_local_ned_position_setpoint_external(msg);
+		break;
+
 	case MAVLINK_MSG_ID_RADIO_STATUS:
 		handle_message_radio_status(msg);
 		break;
@@ -394,6 +398,30 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message
 }
 
 void
+MavlinkReceiver::handle_message_local_ned_position_setpoint_external(mavlink_message_t *msg)
+{
+	mavlink_local_ned_position_setpoint_external_t local_ned_position_setpoint_external;
+	mavlink_msg_local_ned_position_setpoint_external_decode(msg, &local_ned_position_setpoint_external);
+
+	struct offboard_control_setpoint_s offboard_control_sp;
+	memset(&offboard_control_sp, 0, sizeof(offboard_control_sp));
+
+	//XXX check if target system/component is correct
+
+	/* convert mavlink type (local, NED) to uORB offboard control struct */
+	//XXX do the conversion
+	//
+	offboard_control_sp.timestamp = hrt_absolute_time();
+
+	if (_offboard_control_sp_pub < 0) {
+		_offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp);
+
+	} else {
+		orb_publish(ORB_ID(offboard_control_setpoint), _offboard_control_sp_pub, &offboard_control_sp);
+	}
+}
+
+void
 MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg)
 {
 	mavlink_radio_status_t rstatus;