diff options
| author | Benjamin O'Connell-Armand <benjamin.oconnell@polymtl.ca> | 2014-06-02 23:10:03 -0400 |
|---|---|---|
| committer | Benjamin O'Connell-Armand <benjamin.oconnell@polymtl.ca> | 2014-06-02 23:10:03 -0400 |
| commit | 5c536ae46ec44b75fea745dd79f20b0aef2c8f16 (patch) | |
| tree | 0955a800071e4557bc2c036e24f95ad68722e7fb /src/modules | |
| parent | 78637ff74b4c12edca0fac9ea8eb65f38993b49f (diff) | |
| download | px4-firmware-5c536ae46ec44b75fea745dd79f20b0aef2c8f16.tar.gz px4-firmware-5c536ae46ec44b75fea745dd79f20b0aef2c8f16.tar.bz2 px4-firmware-5c536ae46ec44b75fea745dd79f20b0aef2c8f16.zip | |
Added processing of postion_control offboard messages (quad_swarm)
Diffstat (limited to 'src/modules')
| -rw-r--r-- | src/modules/mavlink/mavlink_receiver.cpp | 17 | ||||
| -rw-r--r-- | src/modules/mavlink/mavlink_receiver.h | 1 |
2 files changed, 17 insertions, 1 deletions
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 561744755..78c50c422 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -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 diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index dc31b4c5a..a2736969f 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -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; |