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

Make dt=0 on first step, simplify code

Browse Source
This commit is contained in:
Oleg Kalachev
2023-05-31 20:07:38 +03:00
parent 4160b8da07
commit 9a93367629
6 changed files with 29 additions and 51 deletions
Download Patch File Download Diff File Expand all files Collapse all files

40
flix/estimate.ino
View File

@@ -13,50 +13,36 @@
void estimate()
{
if (dt == 0) {
return;
}
applyGyro();
applyAcc();
signalizeHorizontality();
}
void applyGyro()
{
// applying gyro
attitude *= Quaternion::fromAngularRates(rates * dt);
attitude.normalize();
}
// test should we apply acc
void applyAcc()
{
// test should we apply accelerometer gravity correction
float accNorm = acc.norm();
if (accNorm < ACC_MIN * ONE_G || accNorm > ACC_MAX * ONE_G) {
// use acc only when we're not accelerating
// use accelerometer only when we're not accelerating
return;
}
// calculate accelerometer correction
Vector up = attitude.rotate(Vector(0, 0, -1));
Vector accCorrDirection = Vector::angularRatesBetweenVectors(acc, up);
accCorrDirection.normalize();
if (!accCorrDirection.finite()) {
return;
}
Vector accCorr = accCorrDirection * Vector::angleBetweenVectors(up, acc) * dt * WEIGHT_ACC;
if (!accCorr.finite()) {
return; // TODO
}
// apply correction
attitude *= Quaternion::fromAngularRates(accCorr);
attitude.normalize();
if (!attitude.finite()) {
Serial.print("dt "); Serial.println(dt, 15);
Serial.print("up "); Serial.println(up);
Serial.print("acc "); Serial.println(acc);
Serial.print("acc norm "); Serial.println(acc.norm());
Serial.print("upp norm "); Serial.println(up.norm());
Serial.print("acc dot up "); Serial.println(Vector::dot(up, acc), 15);
Serial.print("acc cor ang "); Serial.println(Vector::angleBetweenVectors(up, acc), 15);
Serial.print("acc corr dir "); Serial.println(accCorrDirection);
Serial.print("acc cor "); Serial.println(accCorr);
Serial.print("att "); Serial.println(attitude);
}
}
void signalizeHorizontality()