diff options
| author | Simon Wilks <sjwilks@gmail.com> | 2013-06-04 00:10:58 +0200 |
|---|---|---|
| committer | Simon Wilks <sjwilks@gmail.com> | 2013-06-04 00:10:58 +0200 |
| commit | f435025d26f49d1d9069282aa72c7e1cb9201773 (patch) | |
| tree | eb1e9907db514b5c97140de2392d99409b605e96 /src/modules/att_pos_estimator_ekf | |
| parent | 6571629dcac252165a210f8a96983fe96be64538 (diff) | |
| download | px4-firmware-f435025d26f49d1d9069282aa72c7e1cb9201773.tar.gz px4-firmware-f435025d26f49d1d9069282aa72c7e1cb9201773.tar.bz2 px4-firmware-f435025d26f49d1d9069282aa72c7e1cb9201773.zip | |
Completed main implementation and debugging
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; } |