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

Implement motors output desaturation

Browse Source 
So the drone continues stabilization on max thrust.
This commit is contained in:
Oleg Kalachev
2026-04-28 13:23:42 +03:00
parent 485a39e740
commit 2ccda03573
3 changed files with 16 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
flix/control.ino
+13
View File
@@ -149,12 +149,25 @@ void controlTorque() {
motors[MOTOR_REAR_LEFT] = thrustTarget + torqueTarget.x + torqueTarget.y - torqueTarget.z;
motors[MOTOR_REAR_RIGHT] = thrustTarget - torqueTarget.x + torqueTarget.y + torqueTarget.z;
desaturate(motors[MOTOR_FRONT_LEFT], motors[MOTOR_FRONT_RIGHT], motors[MOTOR_REAR_LEFT], motors[MOTOR_REAR_RIGHT]);
motors[0] = constrain(motors[0], 0, 1);
motors[1] = constrain(motors[1], 0, 1);
motors[2] = constrain(motors[2], 0, 1);
motors[3] = constrain(motors[3], 0, 1);
}
void desaturate(float& a, float& b, float& c, float& d) {
float maxThrust = max(max(a, b), max(c, d));
if (maxThrust > 1) {
float diff = maxThrust - 1;
a -= diff;
b -= diff;
c -= diff;
d -= diff;
}
}
const char* getModeName() {
switch (mode) {
case RAW: return "RAW";