aleks/flix
1
0
Fork 0
mirror of https://github.com/okalachev/flix.git synced 2025-07-27 01:29:33 +00:00
Code Issues Packages Projects Releases Wiki Activity

Fix rates, acc and gyro coordinate frame in mavlink

Browse Source 
All of them should be in frd.
Get rid of fluToFrd function - there is no big need for it.
This commit is contained in:
Oleg Kalachev 2025-07-19 05:32:49 +03:00
parent cfb0f17a9a
commit a5504cc550
1 changed files with 3 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
flix/mavlink.ino
View File

@ -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