aboutsummaryrefslogtreecommitdiff
path: root/src/modules/fw_att_pos_estimator
diff options
context:
space:
mode:
authorLorenz Meier <lm@inf.ethz.ch>2014-02-09 23:54:04 +0100
committerLorenz Meier <lm@inf.ethz.ch>2014-02-09 23:54:04 +0100
commitefecd85658eecd0b0651a21c37d3936589074e91 (patch)
treec61937fd22ffc9e2d3440b26eb3fe947df23776f /src/modules/fw_att_pos_estimator
parentc13f7d1f4b6db3d6be99b744eb454a6fcb706311 (diff)
downloadpx4-firmware-efecd85658eecd0b0651a21c37d3936589074e91.tar.gz
px4-firmware-efecd85658eecd0b0651a21c37d3936589074e91.tar.bz2
px4-firmware-efecd85658eecd0b0651a21c37d3936589074e91.zip
Further build cleanup
Diffstat (limited to 'src/modules/fw_att_pos_estimator')
-rw-r--r--src/modules/fw_att_pos_estimator/estimator.cpp4
-rw-r--r--src/modules/fw_att_pos_estimator/estimator.h3
-rw-r--r--src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp4
3 files changed, 5 insertions, 6 deletions
diff --git a/src/modules/fw_att_pos_estimator/estimator.cpp b/src/modules/fw_att_pos_estimator/estimator.cpp
index 24e3f4f16..9be12e700 100644
--- a/src/modules/fw_att_pos_estimator/estimator.cpp
+++ b/src/modules/fw_att_pos_estimator/estimator.cpp
@@ -22,7 +22,6 @@ Vector3f accel; // acceleration vector in XYZ body axes measured by the IMU (m/s
Vector3f dVelIMU;
Vector3f dAngIMU;
float dtIMU; // time lapsed since the last IMU measurement or covariance update (sec)
-float dt; // time lapsed since last covariance prediction
uint8_t fusionModeGPS = 0; // 0 = GPS outputs 3D velocity, 1 = GPS outputs 2D velocity, 2 = GPS outputs no velocity
float innovVelPos[6]; // innovation output
float varInnovVelPos[6]; // innovation variance output
@@ -292,7 +291,7 @@ void UpdateStrapdownEquationsNED()
}
-void CovariancePrediction()
+void CovariancePrediction(float dt)
{
// scalars
float windVelSigma;
@@ -1868,5 +1867,4 @@ void InitialiseFilter(float (&initvelNED)[3])
summedDelVel.x = 0.0f;
summedDelVel.y = 0.0f;
summedDelVel.z = 0.0f;
- dt = 0.0f;
}
diff --git a/src/modules/fw_att_pos_estimator/estimator.h b/src/modules/fw_att_pos_estimator/estimator.h
index 01a1ace99..088304cb3 100644
--- a/src/modules/fw_att_pos_estimator/estimator.h
+++ b/src/modules/fw_att_pos_estimator/estimator.h
@@ -68,7 +68,6 @@ extern Vector3f correctedDelVel; // delta velocities along the XYZ body axes cor
extern Vector3f summedDelAng; // summed delta angles about the xyz body axes corrected for errors (rad)
extern Vector3f summedDelVel; // summed delta velocities along the XYZ body axes corrected for errors (m/s)
-extern float dt; // time lapsed since last covariance prediction
extern float dtIMU; // time lapsed since the last IMU measurement or covariance update (sec)
extern bool onGround; // boolean true when the flight vehicle is on the ground (not flying)
@@ -126,7 +125,7 @@ const float covDelAngMax = 0.05f; // maximum delta angle between covariance pred
void UpdateStrapdownEquationsNED();
-void CovariancePrediction();
+void CovariancePrediction(float dt);
void FuseVelposNED();
diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp
index 71df13d27..23cd98530 100644
--- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp
+++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp
@@ -405,6 +405,8 @@ FixedwingEstimator::task_main()
Vector3f lastAngRate = {0.0f, 0.0f, 0.0f};
Vector3f lastAccel = {0.0f, 0.0f, 0.0f};
+ float dt = 0.0f; // time lapsed since last covariance prediction
+
/* wakeup source(s) */
struct pollfd fds[2];
@@ -675,7 +677,7 @@ FixedwingEstimator::task_main()
* or the time limit will be exceeded on the next measurement update
*/
if ((dt >= (covTimeStepMax - dtIMU)) || (summedDelAng.length() > covDelAngMax)) {
- CovariancePrediction();
+ CovariancePrediction(dt);
summedDelAng = summedDelAng.zero();
summedDelVel = summedDelVel.zero();
dt = 0.0f;