Further build cleanup

This commit is contained in:
Lorenz Meier
2014-02-09 23:54:04 +01:00
parent c13f7d1f4b
commit efecd85658
3 changed files with 5 additions and 6 deletions
@@ -22,7 +22,6 @@ Vector3f accel; // acceleration vector in XYZ body axes measured by the IMU (m/s
Vector3f dVelIMU; Vector3f dVelIMU;
Vector3f dAngIMU; Vector3f dAngIMU;
float dtIMU; // time lapsed since the last IMU measurement or covariance update (sec) 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 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 innovVelPos[6]; // innovation output
float varInnovVelPos[6]; // innovation variance output float varInnovVelPos[6]; // innovation variance output
@@ -292,7 +291,7 @@ void UpdateStrapdownEquationsNED()
} }
void CovariancePrediction() void CovariancePrediction(float dt)
{ {
// scalars // scalars
float windVelSigma; float windVelSigma;
@@ -1868,5 +1867,4 @@ void InitialiseFilter(float (&initvelNED)[3])
summedDelVel.x = 0.0f; summedDelVel.x = 0.0f;
summedDelVel.y = 0.0f; summedDelVel.y = 0.0f;
summedDelVel.z = 0.0f; summedDelVel.z = 0.0f;
dt = 0.0f;
} }
+1 -2
View File
@@ -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 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 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 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) 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 UpdateStrapdownEquationsNED();
void CovariancePrediction(); void CovariancePrediction(float dt);
void FuseVelposNED(); void FuseVelposNED();
@@ -405,6 +405,8 @@ FixedwingEstimator::task_main()
Vector3f lastAngRate = {0.0f, 0.0f, 0.0f}; Vector3f lastAngRate = {0.0f, 0.0f, 0.0f};
Vector3f lastAccel = {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) */ /* wakeup source(s) */
struct pollfd fds[2]; struct pollfd fds[2];
@@ -675,7 +677,7 @@ FixedwingEstimator::task_main()
* or the time limit will be exceeded on the next measurement update * or the time limit will be exceeded on the next measurement update
*/ */
if ((dt >= (covTimeStepMax - dtIMU)) || (summedDelAng.length() > covDelAngMax)) { if ((dt >= (covTimeStepMax - dtIMU)) || (summedDelAng.length() > covDelAngMax)) {
CovariancePrediction(); CovariancePrediction(dt);
summedDelAng = summedDelAng.zero(); summedDelAng = summedDelAng.zero();
summedDelVel = summedDelVel.zero(); summedDelVel = summedDelVel.zero();
dt = 0.0f; dt = 0.0f;