Making an ESP32-based quadcopter from scratch
Python arduinoautopilotcontrol-theorydroneesp32flight-controlflight-controllermavlinkquadcopterquadcopter-firmwareuav
Support rates feedforward in set_attitude_target in mavlink
Oleg Kalachev452cddfa3d3b
diff --git a/flix/mavlink.ino b/flix/mavlink.inoindex 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.pyindex 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: ignoreelif 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 deliveryself.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: