summaryrefslogtreecommitdiff
path: root/src/AltEst/altitude.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'src/AltEst/altitude.cpp')
-rw-r--r--src/AltEst/altitude.cpp58
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;
-}