Minor code simplifications

This commit is contained in:
Oleg Kalachev
2025-04-13 01:42:47 +03:00
parent 253f2fe3dd
commit fe98a5bf97
2 changed files with 9 additions and 33 deletions

View File

@@ -47,9 +47,9 @@ void sendMavlink() {
lastFast = t;
const float zeroQuat[] = {0, 0, 0, 0};
Quaternion attitudeFRD = fluToFrd(attitude); // MAVLink uses FRD coordinate system
Quaternion att = fluToFrd(attitude); // MAVLink uses FRD coordinate system
mavlink_msg_attitude_quaternion_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg,
time, attitudeFRD.w, attitudeFRD.x, attitudeFRD.y, attitudeFRD.z, rates.x, rates.y, rates.z, zeroQuat);
time, att.w, att.x, att.y, att.z, rates.x, rates.y, rates.z, zeroQuat);
sendMessage(&msg);
mavlink_msg_rc_channels_scaled_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, controlsTime * 1000, 0,