forked from Archive/PX4-Autopilot
Merge branch 'paul_estimator' of github.com:PX4/Firmware into paul_estimator
This commit is contained in:
commit
dba229ffa5
|
@ -1730,6 +1730,7 @@ void calcEarthRateNED(Vector3f &omega, float latitude)
|
||||||
void CovarianceInit()
|
void CovarianceInit()
|
||||||
{
|
{
|
||||||
// Calculate the initial covariance matrix P
|
// Calculate the initial covariance matrix P
|
||||||
|
P[0][0] = 0.25f*sq(1.0f*deg2rad);
|
||||||
P[1][1] = 0.25f*sq(1.0f*deg2rad);
|
P[1][1] = 0.25f*sq(1.0f*deg2rad);
|
||||||
P[2][2] = 0.25f*sq(1.0f*deg2rad);
|
P[2][2] = 0.25f*sq(1.0f*deg2rad);
|
||||||
P[3][3] = 0.25f*sq(10.0f*deg2rad);
|
P[3][3] = 0.25f*sq(10.0f*deg2rad);
|
||||||
|
|
|
@ -392,8 +392,6 @@ FixedwingEstimator::task_main()
|
||||||
|
|
||||||
parameters_update();
|
parameters_update();
|
||||||
|
|
||||||
Vector3f lastAngRate;
|
|
||||||
Vector3f lastAccel;
|
|
||||||
/* set initial filter state */
|
/* set initial filter state */
|
||||||
fuseVelData = false;
|
fuseVelData = false;
|
||||||
fusePosData = false;
|
fusePosData = false;
|
||||||
|
@ -402,6 +400,11 @@ FixedwingEstimator::task_main()
|
||||||
fuseVtasData = false;
|
fuseVtasData = false;
|
||||||
statesInitialised = false;
|
statesInitialised = false;
|
||||||
|
|
||||||
|
/* initialize measurement data */
|
||||||
|
VtasMeas = 0.0f;
|
||||||
|
Vector3f lastAngRate = {0.0f, 0.0f, 0.0f};
|
||||||
|
Vector3f lastAccel = {0.0f, 0.0f, 0.0f};
|
||||||
|
|
||||||
/* wakeup source(s) */
|
/* wakeup source(s) */
|
||||||
struct pollfd fds[2];
|
struct pollfd fds[2];
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue