fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-19 20:49:07 +08:00
Code Issues Packages Projects Releases Wiki Activity

Added processing of postion_control offboard messages (quad_swarm)

Browse Source
This commit is contained in:
Benjamin O'Connell-Armand 2014-06-02 23:10:03 -04:00
parent 78637ff74b
commit 5c536ae46e
2 changed files with 17 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -102,6 +102,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
_cmd_pub(-1),
_flow_pub(-1),
_offboard_control_sp_pub(-1),
_local_pos_sp_pub(-1),
_att_sp_pub(-1),
_rates_sp_pub(-1),
_vicon_position_pub(-1),
@ -411,7 +412,21 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message
if (_control_mode.flag_control_offboard_enabled) {
if (_control_mode.flag_control_position_enabled) {
// TODO
// TODO Use something else then quad_swarm_roll_pitch_yaw_thrust
struct vehicle_local_position_setpoint_s loc_pos_sp;
memset(&loc_pos_sp, 0, sizeof(loc_pos_sp));
loc_pos_sp.x = offboard_control_sp.p1;
loc_pos_sp.y = offboard_control_sp.p2;
loc_pos_sp.yaw = offboard_control_sp.p3;
loc_pos_sp.z = offboard_control_sp.p4;
if (_local_pos_sp_pub < 0) {
_local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &_local_pos_sp_pub);
} else {
orb_publish(ORB_ID(vehicle_local_position_setpoint), _local_pos_sp_pub, &loc_pos_sp);
}
} else if (_control_mode.flag_control_velocity_enabled) {
// TODO

1
src/modules/mavlink/mavlink_receiver.h
View File

@ -137,6 +137,7 @@ private:
orb_advert_t _cmd_pub;
orb_advert_t _flow_pub;
orb_advert_t _offboard_control_sp_pub;
orb_advert_t _local_pos_sp_pub
orb_advert_t _att_sp_pub;
orb_advert_t _rates_sp_pub;
orb_advert_t _vicon_position_pub;