diff options
Diffstat (limited to 'src/AltEst/altitude.cpp')
| -rw-r--r-- | src/AltEst/altitude.cpp | 58 |
1 files changed, 0 insertions, 58 deletions
diff --git a/src/AltEst/altitude.cpp b/src/AltEst/altitude.cpp deleted file mode 100644 index 8838b36..0000000 --- a/src/AltEst/altitude.cpp +++ /dev/null @@ -1,58 +0,0 @@ -/* - altitude.cpp: Altitude estimation via barometer/accelerometer fusion -*/ - -#include "filters.h" -#include "algebra.h" -#include "altitude.h" - -AltitudeEstimator::AltitudeEstimator(float sigmaAccel, float sigmaGyro, float sigmaBaro, - float ca, float accelThreshold) -:kalman(ca, sigmaGyro, sigmaAccel), complementary(sigmaAccel, sigmaBaro, accelThreshold) -{ - this->sigmaAccel = sigmaAccel; - this->sigmaGyro = sigmaGyro; - this->sigmaBaro = sigmaBaro; - this->ca = ca; - this->accelThreshold = accelThreshold; -} - -void AltitudeEstimator::estimate(float accel[3], float gyro[3], float baroHeight, uint32_t timestamp) -{ - float deltat = (float)(timestamp-previousTime)/1000000.0f; - float verticalAccel = kalman.estimate(pastGyro, - pastAccel, - deltat); - complementary.estimate(& estimatedVelocity, - & estimatedAltitude, - baroHeight, - pastAltitude, - pastVerticalVelocity, - pastVerticalAccel, - deltat); - // update values for next iteration - copyVector(pastGyro, gyro); - copyVector(pastAccel, accel); - pastAltitude = estimatedAltitude; - pastVerticalVelocity = estimatedVelocity; - pastVerticalAccel = verticalAccel; - previousTime = timestamp; -} - -float AltitudeEstimator::getAltitude() -{ - // return the last estimated altitude - return estimatedAltitude; -} - -float AltitudeEstimator::getVerticalVelocity() -{ - // return the last estimated vertical velocity - return estimatedVelocity; -} - -float AltitudeEstimator::getVerticalAcceleration() -{ - // return the last estimated vertical acceleration - return pastVerticalAccel; -} |
