diff options
author | Matt Beall <matt.beall@greypointcorp.com> | 2015-02-19 15:17:52 -0700 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2015-02-28 15:19:16 +0100 |
commit | 40ae0ebdaca797ef9265c00ebb7f38eb82df7336 (patch) | |
tree | cb2af265494f32f78ae33880a33c844894492593 /src/modules | |
parent | 9be3cd4a55767ba7748a565c508a9a7592d25c60 (diff) | |
download | px4-firmware-40ae0ebdaca797ef9265c00ebb7f38eb82df7336.tar.gz px4-firmware-40ae0ebdaca797ef9265c00ebb7f38eb82df7336.tar.bz2 px4-firmware-40ae0ebdaca797ef9265c00ebb7f38eb82df7336.zip |
Set up to receive mavlink actuator control messages and publish to uorb
Diffstat (limited to 'src/modules')
-rw-r--r-- | src/modules/mavlink/mavlink_receiver.cpp | 29 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_receiver.h | 2 |
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; |