fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-28 06:23:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
Matt Beall
2015-02-28 15:19:16 +01:00
committed by Thomas Gubler
parent 9be3cd4a55
commit 40ae0ebdac
2 changed files with 31 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_receiver.cpp
+29
View File
@@ -109,6 +109,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
_flow_pub(-1), _flow_pub(-1),
_range_pub(-1), _range_pub(-1),
_offboard_control_mode_pub(-1), _offboard_control_mode_pub(-1),
_actuator_controls_pub(-1),
_global_vel_sp_pub(-1), _global_vel_sp_pub(-1),
_att_sp_pub(-1), _att_sp_pub(-1),
_rates_sp_pub(-1), _rates_sp_pub(-1),
@@ -170,6 +171,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
handle_message_set_attitude_target(msg); handle_message_set_attitude_target(msg);
break; break;
case MAVLINK_MSG_ID_SET_ACTUATOR_CONTROL_TARGET:
handle_message_set_actuator_control_target(msg);
break;
case MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE: case MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE:
handle_message_vision_position_estimate(msg); handle_message_vision_position_estimate(msg);
break; break;
@@ -648,6 +653,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 void
MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg) MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg)
{ {
src/modules/mavlink/mavlink_receiver.h
+2
View File
@@ -122,6 +122,7 @@ private:
void handle_message_vision_position_estimate(mavlink_message_t *msg); 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_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_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_set_attitude_target(mavlink_message_t *msg);
void handle_message_radio_status(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_manual_control(mavlink_message_t *msg);
@@ -163,6 +164,7 @@ private:
orb_advert_t _flow_pub; orb_advert_t _flow_pub;
orb_advert_t _range_pub; orb_advert_t _range_pub;
orb_advert_t _offboard_control_mode_pub; orb_advert_t _offboard_control_mode_pub;
orb_advert_t _actuator_controls_pub;
orb_advert_t _global_vel_sp_pub; orb_advert_t _global_vel_sp_pub;
orb_advert_t _att_sp_pub; orb_advert_t _att_sp_pub;
orb_advert_t _rates_sp_pub; orb_advert_t _rates_sp_pub;