mavlink: use 'buttons' field of MANUAL_CONTROL message as mode switchesmavlink_manual_switches

1 files changed, 16 insertions, 0 deletions

diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index c4e332bf1..03443d545 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -897,12 +897,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));
 
@@ -912,6 +921,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) {