From dd1ea4f60482e3be7c075368394e0b8d159e5d5f Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Fri, 28 Feb 2025 23:12:57 +0300 Subject: [PATCH] Cleanup mavlink subsystem code --- flix/mavlink.ino | 17 +++++++++-------- 1 file changed, 9 insertions(+), 8 deletions(-) diff --git a/flix/mavlink.ino b/flix/mavlink.ino index 16b0a5a..f26cd61 100644 --- a/flix/mavlink.ino +++ b/flix/mavlink.ino @@ -85,14 +85,15 @@ void receiveMavlink() { } void handleMavlink(const void *_msg) { - mavlink_message_t *msg = (mavlink_message_t *)_msg; - if (msg->msgid == MAVLINK_MSG_ID_MANUAL_CONTROL) { - mavlink_manual_control_t manualControl; - mavlink_msg_manual_control_decode(msg, &manualControl); - controls[RC_CHANNEL_THROTTLE] = manualControl.z / 1000.0f; - controls[RC_CHANNEL_PITCH] = manualControl.x / 1000.0f * MAVLINK_CONTROL_SCALE; - controls[RC_CHANNEL_ROLL] = manualControl.y / 1000.0f * MAVLINK_CONTROL_SCALE; - controls[RC_CHANNEL_YAW] = manualControl.r / 1000.0f * MAVLINK_CONTROL_SCALE; + const mavlink_message_t &msg = *(mavlink_message_t *)_msg; + + if (msg.msgid == MAVLINK_MSG_ID_MANUAL_CONTROL) { + mavlink_manual_control_t m; + mavlink_msg_manual_control_decode(&msg, &m); + controls[RC_CHANNEL_THROTTLE] = m.z / 1000.0f; + controls[RC_CHANNEL_PITCH] = m.x / 1000.0f * MAVLINK_CONTROL_SCALE; + controls[RC_CHANNEL_ROLL] = m.y / 1000.0f * MAVLINK_CONTROL_SCALE; + controls[RC_CHANNEL_YAW] = m.r / 1000.0f * MAVLINK_CONTROL_SCALE; controls[RC_CHANNEL_MODE] = 1; // STAB mode controls[RC_CHANNEL_ARMED] = 1; // armed controlsTime = t;