diff options
| author | Lorenz Meier <lm@inf.ethz.ch> | 2013-06-06 07:57:31 +0200 |
|---|---|---|
| committer | Lorenz Meier <lm@inf.ethz.ch> | 2013-06-06 07:57:31 +0200 |
| commit | 8ad3aa315f09c32cc3e1a85e89f8f6bb9017676c (patch) | |
| tree | a8f0c7e23f5d64c715cfeade003fd878e3ef027a /src/modules/att_pos_estimator_ekf | |
| parent | 971e8103014412d8453b010063318cf46e996288 (diff) | |
| parent | 68931f38d56c82c67d7d01e4db3157fac5815258 (diff) | |
| download | px4-firmware-8ad3aa315f09c32cc3e1a85e89f8f6bb9017676c.tar.gz px4-firmware-8ad3aa315f09c32cc3e1a85e89f8f6bb9017676c.tar.bz2 px4-firmware-8ad3aa315f09c32cc3e1a85e89f8f6bb9017676c.zip | |
Merged master
Diffstat (limited to 'src/modules/att_pos_estimator_ekf')
| -rw-r--r-- | src/modules/att_pos_estimator_ekf/KalmanNav.cpp | 4 |
1 files changed, 3 insertions, 1 deletions
diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp index 4ef150da1..c3836bdfa 100644 --- a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp +++ b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp @@ -683,7 +683,8 @@ int KalmanNav::correctPos() // fault detetcion float beta = y.dot(S.inverse() * y); - if (beta > _faultPos.get()) { + static int counter = 0; + if (beta > _faultPos.get() && (counter % 10 == 0)) { printf("fault in gps: beta = %8.4f\n", (double)beta); printf("Y/N: vN: %8.4f, vE: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f\n", double(y(0) / sqrtf(RPos(0, 0))), @@ -693,6 +694,7 @@ int KalmanNav::correctPos() double(y(4) / sqrtf(RPos(4, 4))), double(y(5) / sqrtf(RPos(5, 5)))); } + counter++; return ret_ok; } |