aleks/flix
1
0
Fork 0
mirror of https://github.com/okalachev/flix.git synced 2026-02-17 15:41:32 +00:00
Code Issues Packages Projects Releases Wiki Activity

Simplify code using angularRatesBetweenVectors

Browse Source
This commit is contained in:
Oleg Kalachev
2023-12-13 08:25:11 +03:00
parent 24b62e5145
commit 4fcf2109ce
3 changed files with 9 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
flix/control.ino
View File

@@ -115,12 +115,10 @@ void controlAttitude()
Vector upActual = attitude.rotate(up);
Vector upTarget = attitudeTarget.rotate(up);
float angle = Vector::angleBetweenVectors(upTarget, upActual);
Vector ratesTargetDir = Vector::angularRatesBetweenVectors(upTarget, upActual);
ratesTargetDir.normalize();
Vector error = Vector::angularRatesBetweenVectors(upTarget, upActual);
ratesTarget.x = rollPID.update(ratesTargetDir.x * angle, dt);
ratesTarget.y = pitchPID.update(ratesTargetDir.y * angle, dt);
ratesTarget.x = rollPID.update(error.x, dt);
ratesTarget.y = pitchPID.update(error.y, dt);
if (yawMode == YAW) {
ratesTarget.z = yawPID.update(wrapAngle(attitudeTarget.getYaw() - attitude.getYaw()), dt);