From 452cddfa3d3b5045c6a889f7c7049f30321651ff Mon Sep 17 00:00:00 2001
From: Oleg Kalachev <okalachev@gmail.com>
Date: Mon, 17 Aug 2026 04:33:09 +0300
Subject: [PATCH] Support rates feedforward in set_attitude_target in mavlink

Add rates_extra in flix.set_attitude in pyflix.
Simplify the code.
---
 flix/mavlink.ino     | 28 +++++++++++++++++-----------
 tools/pyflix/flix.py |  5 +++--
 2 files changed, 20 insertions(+), 13 deletions(-)

diff --git a/flix/mavlink.ino b/flix/mavlink.ino
index 5b4f023..51dd0a4 100644
--- a/flix/mavlink.ino
+++ b/flix/mavlink.ino
@@ -196,18 +196,24 @@ void handleMavlink(const void *_msg) {
 		mavlink_msg_set_attitude_target_decode(&msg, &m);
 		if (m.target_system && m.target_system != mavlinkSysId) return;
 
-		// copy attitude, rates and thrust targets
-		ratesTarget.x = m.body_roll_rate;
-		ratesTarget.y = -m.body_pitch_rate; // convert to flu
-		ratesTarget.z = -m.body_yaw_rate;
-		attitudeTarget.w = m.q[0];
-		attitudeTarget.x = m.q[1];
-		attitudeTarget.y = -m.q[2];
-		attitudeTarget.z = -m.q[3];
-		thrustTarget = m.thrust;
-		ratesExtra = Vector(0, 0, 0);
+		if (!(m.type_mask & ATTITUDE_TARGET_TYPEMASK_ATTITUDE_IGNORE)) {
+			// Attitude control
+			attitudeTarget.w = m.q[0];
+			attitudeTarget.x = m.q[1];
+			attitudeTarget.y = -m.q[2]; // convert to flu
+			attitudeTarget.z = -m.q[3];
+			ratesExtra.x = m.body_roll_rate;
+			ratesExtra.y = -m.body_pitch_rate;
+			ratesExtra.z = -m.body_yaw_rate;
+		} else {
+			// Rates control
+			attitudeTarget.invalidate();
+			ratesTarget.x = m.body_roll_rate;
+			ratesTarget.y = -m.body_pitch_rate;
+			ratesTarget.z = -m.body_yaw_rate;
+		}
 
-		if (m.type_mask & ATTITUDE_TARGET_TYPEMASK_ATTITUDE_IGNORE) attitudeTarget.invalidate();
+		thrustTarget = valid(m.thrust) ? m.thrust : thrustTarget;
 		armed = m.thrust > 0;
 	}
 
diff --git a/tools/pyflix/flix.py b/tools/pyflix/flix.py
index 00de7d6..b03bbcf 100644
--- a/tools/pyflix/flix.py
+++ b/tools/pyflix/flix.py
@@ -326,7 +326,7 @@ class Flix:
     def set_velocity(self, velocity: Sequence[float], yaw: Optional[float] = None):
         raise NotImplementedError('Velocity control is not implemented yet')
 
-    def set_attitude(self, attitude: Sequence[float], thrust: float):
+    def set_attitude(self, attitude: Sequence[float], thrust: float, rates_extra: Sequence[float] = (0, 0, 0)):
         if len(attitude) == 3:
             attitude = Quaternion([attitude[0], attitude[1], attitude[2]]).q  # type: ignore
         elif len(attitude) != 4:
@@ -334,10 +334,11 @@ class Flix:
         if not (0 <= thrust <= 1):
             raise ValueError('Thrust must be in range [0, 1]')
         attitude = self._flu_to_mavlink(attitude)
+        rates_extra = self._flu_to_mavlink(rates_extra)
         for _ in range(2): # duplicate to ensure delivery
             self.mavlink.set_attitude_target_send(0, self.system_id, 0, 0,
                 [attitude[0], attitude[1], attitude[2], attitude[3]],
-                0, 0, 0, thrust)
+                rates_extra[0], rates_extra[1], rates_extra[2], thrust)
 
     def set_rates(self, rates: Sequence[float], thrust: float):
         if len(rates) != 3: