diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-08-13 09:08:16 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-08-13 09:08:16 +0200 |
commit | 3f63b67eb66b44524b74847696a65d7a531d73d6 (patch) | |
tree | bf4954fb553edb1d9e200899ed531efa87505a90 /src/modules/mavlink/mavlink_receiver.cpp | |
parent | ee42d5f53d81580bee56b1cdcf69ebfdefb6ba09 (diff) | |
parent | fd435532b50428d7aab452d19db1d9c93f47dd5c (diff) | |
download | px4-firmware-3f63b67eb66b44524b74847696a65d7a531d73d6.tar.gz px4-firmware-3f63b67eb66b44524b74847696a65d7a531d73d6.tar.bz2 px4-firmware-3f63b67eb66b44524b74847696a65d7a531d73d6.zip |
Merged master into vision_estimate
Diffstat (limited to 'src/modules/mavlink/mavlink_receiver.cpp')
-rw-r--r-- | src/modules/mavlink/mavlink_receiver.cpp | 36 |
1 files changed, 1 insertions, 35 deletions
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 54a7fb065..c0fae0a2f 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -153,10 +153,6 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_vision_position_estimate(msg); break; - case MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST: - handle_message_quad_swarm_roll_pitch_yaw_thrust(msg); - break; - case MAVLINK_MSG_ID_RADIO_STATUS: handle_message_radio_status(msg); break; @@ -385,7 +381,7 @@ MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg) vision_position.y = pos.y; vision_position.z = pos.z; - // XXX fis this + // XXX fix this vision_position.vx = 0.0f; vision_position.vy = 0.0f; vision_position.vz = 0.0f; @@ -407,36 +403,6 @@ MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg) } void -MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message_t *msg) -{ - mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t swarm_offboard_control; - mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_decode(msg, &swarm_offboard_control); - - /* Only accept system IDs from 1 to 4 */ - if (mavlink_system.sysid >= 1 && mavlink_system.sysid <= 4) { - struct offboard_control_setpoint_s offboard_control_sp; - memset(&offboard_control_sp, 0, sizeof(offboard_control_sp)); - - /* Convert values * 1000 back */ - offboard_control_sp.p1 = (float)swarm_offboard_control.roll[mavlink_system.sysid - 1] / 1000.0f; - offboard_control_sp.p2 = (float)swarm_offboard_control.pitch[mavlink_system.sysid - 1] / 1000.0f; - offboard_control_sp.p3 = (float)swarm_offboard_control.yaw[mavlink_system.sysid - 1] / 1000.0f; - offboard_control_sp.p4 = (float)swarm_offboard_control.thrust[mavlink_system.sysid - 1] / 1000.0f; - - offboard_control_sp.mode = (enum OFFBOARD_CONTROL_MODE)swarm_offboard_control.mode; - - 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) { /* telemetry status supported only on first TELEMETRY_STATUS_ORB_ID_NUM mavlink channels */ |