From af546def913c7f4d6f1257267ecb8556aba6a277 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 29 Apr 2015 15:54:21 +0200 Subject: [PATCH] mavlink: use 'buttons' field of MANUAL_CONTROL message as mode switches --- src/modules/mavlink/mavlink_receiver.cpp | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 29b4fbcabb..8f31b2c858 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -899,12 +899,21 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg) } } +static switch_pos_t decode_switch_pos(uint16_t buttons, int sw) { + return (buttons >> (sw * 2)) & 3; +} + void MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) { mavlink_manual_control_t man; mavlink_msg_manual_control_decode(msg, &man); + // Check target + if (man.target != 0 && man.target != _mavlink->get_system_id()) { + return; + } + struct manual_control_setpoint_s manual; memset(&manual, 0, sizeof(manual)); @@ -914,6 +923,13 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) manual.r = man.r / 1000.0f; manual.z = man.z / 1000.0f; + manual.mode_switch = decode_switch_pos(man.buttons, 0); + manual.return_switch = decode_switch_pos(man.buttons, 1); + manual.posctl_switch = decode_switch_pos(man.buttons, 2); + manual.loiter_switch = decode_switch_pos(man.buttons, 3); + manual.acro_switch = decode_switch_pos(man.buttons, 4); + manual.offboard_switch = decode_switch_pos(man.buttons, 5); + // warnx("pitch: %.2f, roll: %.2f, yaw: %.2f, throttle: %.2f", (double)manual.x, (double)manual.y, (double)manual.r, (double)manual.z); if (_manual_pub < 0) {