diff options
Diffstat (limited to 'src/modules/fw_att_pos_estimator')
-rw-r--r-- | src/modules/fw_att_pos_estimator/estimator.cpp | 4 | ||||
-rw-r--r-- | src/modules/fw_att_pos_estimator/estimator.h | 3 | ||||
-rw-r--r-- | src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp | 4 |
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; |