Browse Source

Further build cleanup

sbg
Lorenz Meier 11 years ago
parent
commit
efecd85658
  1. 4
      src/modules/fw_att_pos_estimator/estimator.cpp
  2. 3
      src/modules/fw_att_pos_estimator/estimator.h
  3. 4
      src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp

4
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 @@ -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() @@ -292,7 +291,7 @@ void UpdateStrapdownEquationsNED()
}
void CovariancePrediction()
void CovariancePrediction(float dt)
{
// scalars
float windVelSigma;
@ -1868,5 +1867,4 @@ void InitialiseFilter(float (&initvelNED)[3]) @@ -1868,5 +1867,4 @@ void InitialiseFilter(float (&initvelNED)[3])
summedDelVel.x = 0.0f;
summedDelVel.y = 0.0f;
summedDelVel.z = 0.0f;
dt = 0.0f;
}

3
src/modules/fw_att_pos_estimator/estimator.h

@ -68,7 +68,6 @@ extern Vector3f correctedDelVel; // delta velocities along the XYZ body axes cor @@ -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 @@ -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();

4
src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp

@ -405,6 +405,8 @@ FixedwingEstimator::task_main() @@ -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() @@ -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;

Loading…
Cancel
Save