diff options
Diffstat (limited to 'src/modules/mavlink/mavlink_receiver.cpp')
-rw-r--r-- | src/modules/mavlink/mavlink_receiver.cpp | 28 |
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; |