From a5504cc550ad8bcc0c7309201d850d327fc864a0 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Sat, 19 Jul 2025 05:32:49 +0300 Subject: [PATCH] Fix rates, acc and gyro coordinate frame in mavlink All of them should be in frd. Get rid of fluToFrd function - there is no big need for it. --- flix/mavlink.ino | 12 +++--------- 1 file changed, 3 insertions(+), 9 deletions(-) diff --git a/flix/mavlink.ino b/flix/mavlink.ino index 8ff6bf2..054c905 100644 --- a/flix/mavlink.ino +++ b/flix/mavlink.ino @@ -40,9 +40,8 @@ void sendMavlink() { lastFast = t; const float zeroQuat[] = {0, 0, 0, 0}; - Quaternion att = fluToFrd(attitude); // MAVLink uses FRD coordinate system mavlink_msg_attitude_quaternion_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, - time, att.w, att.x, att.y, att.z, rates.x, rates.y, rates.z, zeroQuat); + time, attitude.w, attitude.x, -attitude.y, -attitude.z, rates.x, -rates.y, -rates.z, zeroQuat); // convert to frd sendMessage(&msg); mavlink_msg_rc_channels_raw_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, controlTime * 1000, 0, @@ -55,8 +54,8 @@ void sendMavlink() { sendMessage(&msg); mavlink_msg_scaled_imu_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, time, - acc.x * 1000, acc.y * 1000, acc.z * 1000, - gyro.x * 1000, gyro.y * 1000, gyro.z * 1000, + acc.x * 1000, -acc.y * 1000, -acc.z * 1000, // convert to frd + gyro.x * 1000, -gyro.y * 1000, -gyro.z * 1000, 0, 0, 0, 0); sendMessage(&msg); } @@ -100,9 +99,4 @@ void handleMavlink(const void *_msg) { } } -// Convert Forward-Left-Up to Forward-Right-Down quaternion -inline Quaternion fluToFrd(const Quaternion &q) { - return Quaternion(q.w, q.x, -q.y, -q.z); -} - #endif