From 5eccb3f0c4022b028f89848e3b85122133312f11 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 c3cded8..98315ce 100644 --- a/flix/mavlink.ino +++ b/flix/mavlink.ino @@ -49,9 +49,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, @@ -64,8 +63,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); } @@ -215,9 +214,4 @@ void sendMavlinkPrint() { mavlinkPrintBuffer.clear(); } -// 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