diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2014-07-03 14:42:59 +0200 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2014-07-03 14:46:25 +0200 |
commit | 839710daf841a9528a58e915f8b04484bf54e7dc (patch) | |
tree | 997f95813e3a5cdf8af393323398e841f29c0fae /src/modules/mavlink | |
parent | 57f707af56be0d9281a95aebf64baf63ef022267 (diff) | |
download | px4-firmware-839710daf841a9528a58e915f8b04484bf54e7dc.tar.gz px4-firmware-839710daf841a9528a58e915f8b04484bf54e7dc.tar.bz2 px4-firmware-839710daf841a9528a58e915f8b04484bf54e7dc.zip |
Update offboard control uorb topic
Work towards supporting the new external setpoint mavlink messages
Diffstat (limited to 'src/modules/mavlink')
-rw-r--r-- | src/modules/mavlink/mavlink_receiver.cpp | 28 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_receiver.h | 1 |
2 files changed, 29 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; diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 65ef884a2..658f51562 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -115,6 +115,7 @@ private: void handle_message_set_mode(mavlink_message_t *msg); void handle_message_vicon_position_estimate(mavlink_message_t *msg); void handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message_t *msg); + void handle_message_local_ned_position_setpoint_external(mavlink_message_t *msg); void handle_message_radio_status(mavlink_message_t *msg); void handle_message_manual_control(mavlink_message_t *msg); void handle_message_heartbeat(mavlink_message_t *msg); |