aleks/flix
1
0
Fork 0
mirror of https://github.com/okalachev/flix.git synced 2025-07-27 17:49:33 +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
Show Stats 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 upActual = attitude.rotate(up);
Vector upTarget = attitudeTarget.rotate(up); Vector upTarget = attitudeTarget.rotate(up);
float angle = Vector::angleBetweenVectors(upTarget, upActual); Vector error = Vector::angularRatesBetweenVectors(upTarget, upActual);
Vector ratesTargetDir = Vector::angularRatesBetweenVectors(upTarget, upActual);
ratesTargetDir.normalize();
ratesTarget.x = rollPID.update(ratesTargetDir.x * angle, dt); ratesTarget.x = rollPID.update(error.x, dt);
ratesTarget.y = pitchPID.update(ratesTargetDir.y * angle, dt); ratesTarget.y = pitchPID.update(error.y, dt);
if (yawMode == YAW) { if (yawMode == YAW) {
ratesTarget.z = yawPID.update(wrapAngle(attitudeTarget.getYaw() - attitude.getYaw()), dt); ratesTarget.z = yawPID.update(wrapAngle(attitudeTarget.getYaw() - attitude.getYaw()), dt);

6
flix/estimate.ino
View File

@ -36,12 +36,10 @@ void applyAcc()
// calculate accelerometer correction // calculate accelerometer correction
Vector up = attitude.rotate(Vector(0, 0, -1)); Vector up = attitude.rotate(Vector(0, 0, -1));
Vector accCorrDirection = Vector::angularRatesBetweenVectors(acc, up); Vector correction = Vector::angularRatesBetweenVectors(acc, up) * dt * WEIGHT_ACC;
accCorrDirection.normalize();
Vector accCorr = accCorrDirection * Vector::angleBetweenVectors(up, acc) * dt * WEIGHT_ACC;
// apply correction // apply correction
attitude *= Quaternion::fromAngularRates(accCorr); attitude *= Quaternion::fromAngularRates(correction);
attitude.normalize(); attitude.normalize();
} }

5
flix/vector.h
View File

@ -79,7 +79,10 @@ public:
static Vector angularRatesBetweenVectors(const Vector& u, const Vector& v) static Vector angularRatesBetweenVectors(const Vector& u, const Vector& v)
{ {
return cross(u, v); Vector direction = cross(u, v);
direction.normalize();
float angle = angleBetweenVectors(u, v);
return direction * angle;
} }
size_t printTo(Print& p) const { size_t printTo(Print& p) const {