diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-01-31 18:14:27 +0100 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-01-31 18:14:27 +0100 |
commit | dba229ffa5fbdd6e7863f528412be429a84837ac (patch) | |
tree | c81b0dc89a326f026a2bd23b47bb541e8c95174b /src/modules/fw_att_pos_estimator | |
parent | 92cf8a4cd78d171c5ad969790258e93473c2c5fb (diff) | |
parent | 9cb59e33a6da2bcd2bb49bc3d49ce527597582f9 (diff) | |
download | px4-firmware-dba229ffa5fbdd6e7863f528412be429a84837ac.tar.gz px4-firmware-dba229ffa5fbdd6e7863f528412be429a84837ac.tar.bz2 px4-firmware-dba229ffa5fbdd6e7863f528412be429a84837ac.zip |
Merge branch 'paul_estimator' of github.com:PX4/Firmware into paul_estimator
Diffstat (limited to 'src/modules/fw_att_pos_estimator')
-rw-r--r-- | src/modules/fw_att_pos_estimator/estimator.cpp | 1 | ||||
-rw-r--r-- | src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp | 7 |
2 files changed, 6 insertions, 2 deletions
diff --git a/src/modules/fw_att_pos_estimator/estimator.cpp b/src/modules/fw_att_pos_estimator/estimator.cpp index c7c9b6476..9b57dfd55 100644 --- a/src/modules/fw_att_pos_estimator/estimator.cpp +++ b/src/modules/fw_att_pos_estimator/estimator.cpp @@ -1730,6 +1730,7 @@ void calcEarthRateNED(Vector3f &omega, float latitude) void CovarianceInit() { // 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[2][2] = 0.25f*sq(1.0f*deg2rad); P[3][3] = 0.25f*sq(10.0f*deg2rad); 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 5c977f608..444597e81 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 @@ -392,8 +392,6 @@ FixedwingEstimator::task_main() parameters_update(); - Vector3f lastAngRate; - Vector3f lastAccel; /* set initial filter state */ fuseVelData = false; fusePosData = false; @@ -402,6 +400,11 @@ FixedwingEstimator::task_main() fuseVtasData = 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) */ struct pollfd fds[2]; |