aleks/flix
1
0
Fork 0
mirror of https://github.com/okalachev/flix.git synced 2026-09-05 16:00:56 +00:00
Code Issues Packages Projects Releases Wiki Activity

Support rates feedforward in set_attitude_target in mavlink

Browse Source 
Add rates_extra in flix.set_attitude in pyflix.
Simplify the code.
This commit is contained in:
Oleg Kalachev
2026-08-17 04:33:09 +03:00
parent d825652a71
commit 452cddfa3d
2 changed files with 20 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files
flix/mavlink.ino
+17 -11
View File
@@ -196,18 +196,24 @@ void handleMavlink(const void *_msg) {
mavlink_msg_set_attitude_target_decode(&msg, &m); mavlink_msg_set_attitude_target_decode(&msg, &m);
if (m.target_system && m.target_system != mavlinkSysId) return; if (m.target_system && m.target_system != mavlinkSysId) return;
// copy attitude, rates and thrust targets if (!(m.type_mask & ATTITUDE_TARGET_TYPEMASK_ATTITUDE_IGNORE)) {
ratesTarget.x = m.body_roll_rate; // Attitude control
ratesTarget.y = -m.body_pitch_rate; // convert to flu attitudeTarget.w = m.q[0];
ratesTarget.z = -m.body_yaw_rate; attitudeTarget.x = m.q[1];
attitudeTarget.w = m.q[0]; attitudeTarget.y = -m.q[2]; // convert to flu
attitudeTarget.x = m.q[1]; attitudeTarget.z = -m.q[3];
attitudeTarget.y = -m.q[2]; ratesExtra.x = m.body_roll_rate;
attitudeTarget.z = -m.q[3]; ratesExtra.y = -m.body_pitch_rate;
thrustTarget = m.thrust; ratesExtra.z = -m.body_yaw_rate;
ratesExtra = Vector(0, 0, 0); } 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; armed = m.thrust > 0;
} }
tools/pyflix/flix.py
+3 -2
View File
@@ -326,7 +326,7 @@ class Flix:
def set_velocity(self, velocity: Sequence[float], yaw: Optional[float] = None): def set_velocity(self, velocity: Sequence[float], yaw: Optional[float] = None):
raise NotImplementedError('Velocity control is not implemented yet') 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: if len(attitude) == 3:
attitude = Quaternion([attitude[0], attitude[1], attitude[2]]).q # type: ignore attitude = Quaternion([attitude[0], attitude[1], attitude[2]]).q # type: ignore
elif len(attitude) != 4: elif len(attitude) != 4:
@@ -334,10 +334,11 @@ class Flix:
if not (0 <= thrust <= 1): if not (0 <= thrust <= 1):
raise ValueError('Thrust must be in range [0, 1]') raise ValueError('Thrust must be in range [0, 1]')
attitude = self._flu_to_mavlink(attitude) attitude = self._flu_to_mavlink(attitude)
rates_extra = self._flu_to_mavlink(rates_extra)
for _ in range(2): # duplicate to ensure delivery for _ in range(2): # duplicate to ensure delivery
self.mavlink.set_attitude_target_send(0, self.system_id, 0, 0, self.mavlink.set_attitude_target_send(0, self.system_id, 0, 0,
[attitude[0], attitude[1], attitude[2], attitude[3]], [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): def set_rates(self, rates: Sequence[float], thrust: float):
if len(rates) != 3: if len(rates) != 3: