mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-06-01 02:55:07 +08:00
Further build cleanup
This commit is contained in:
@@ -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;
|
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -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;
|
||||||
|
|||||||
Reference in New Issue
Block a user