Skip to content

armgit / flix

Public Imported from GitHub
0

Making an ESP32-based quadcopter from scratch

Support rates feedforward in set_attitude_target in mavlink

Oleg Kalachev452cddfa3d3b
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) attitudeTarget.invalidate();
+ 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;
+ }
+
+ 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: