diff options
author | James Goppert <james.goppert@gmail.com> | 2013-01-15 13:12:00 -0500 |
---|---|---|
committer | James Goppert <james.goppert@gmail.com> | 2013-01-15 13:12:00 -0500 |
commit | 9cf3d51aec4e73f626cfdea29b19bdfe80eea384 (patch) | |
tree | 63f7ee5dbd3b79dbd2c96bdc7f7f7b037e10d924 | |
parent | edf0a6bae7fa5ae8c6d4df63489dcf75ec008517 (diff) | |
download | px4-firmware-9cf3d51aec4e73f626cfdea29b19bdfe80eea384.tar.gz px4-firmware-9cf3d51aec4e73f626cfdea29b19bdfe80eea384.tar.bz2 px4-firmware-9cf3d51aec4e73f626cfdea29b19bdfe80eea384.zip |
Made fault tolerances adjustable.
-rw-r--r-- | apps/examples/kalman_demo/KalmanNav.cpp | 6 | ||||
-rw-r--r-- | apps/examples/kalman_demo/KalmanNav.hpp | 2 | ||||
-rw-r--r-- | apps/examples/kalman_demo/params.c | 2 |
3 files changed, 8 insertions, 2 deletions
diff --git a/apps/examples/kalman_demo/KalmanNav.cpp b/apps/examples/kalman_demo/KalmanNav.cpp index 68deae185..918dfd9df 100644 --- a/apps/examples/kalman_demo/KalmanNav.cpp +++ b/apps/examples/kalman_demo/KalmanNav.cpp @@ -98,6 +98,8 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : _rAccel(this, "R_ACCEL"), _magDip(this, "MAG_DIP"), _magDec(this, "MAG_DEC"), + _faultPos(this, "FAULT_POS"), + _faultAtt(this, "FAULT_ATT"), _positionInitialized(false) { using namespace math; @@ -601,7 +603,7 @@ void KalmanNav::correctAtt() // fault detection float beta = y.dot(S.inverse() * y); - if (beta > 1000.0f) { + if (beta > _faultAtt.get()) { printf("fault in attitude: beta = %8.4f\n", (double)beta); printf("y:\n"); y.print(); } @@ -679,7 +681,7 @@ void KalmanNav::correctPos() // fault detetcion float beta = y.dot(S.inverse() * y); - if (beta > 1000.0f) { + if (beta > _faultPos.get()) { 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) / noiseVel), diff --git a/apps/examples/kalman_demo/KalmanNav.hpp b/apps/examples/kalman_demo/KalmanNav.hpp index ba3756f46..4af8bbf5c 100644 --- a/apps/examples/kalman_demo/KalmanNav.hpp +++ b/apps/examples/kalman_demo/KalmanNav.hpp @@ -159,6 +159,8 @@ protected: control::BlockParam<float> _rAccel; /**< accelerometer measurement noise */ control::BlockParam<float> _magDip; /**< magnetic inclination with level */ control::BlockParam<float> _magDec; /**< magnetic declination, clockwise rotation */ + control::BlockParam<float> _faultPos; /**< fault detection threshold for position */ + control::BlockParam<float> _faultAtt; /**< fault detection threshold for attitude */ // status bool _positionInitialized; /**< status, if position has been init. */ // accessors diff --git a/apps/examples/kalman_demo/params.c b/apps/examples/kalman_demo/params.c index 55be116fa..b98dd8fd7 100644 --- a/apps/examples/kalman_demo/params.c +++ b/apps/examples/kalman_demo/params.c @@ -10,3 +10,5 @@ PARAM_DEFINE_FLOAT(KF_R_GPS_ALT, 1.0f); PARAM_DEFINE_FLOAT(KF_R_ACCEL, 1.0f); PARAM_DEFINE_FLOAT(KF_MAG_DIP, 60.0f); PARAM_DEFINE_FLOAT(KF_MAG_DEC, 0.0f); +PARAM_DEFINE_FLOAT(KF_FAULT_POS, 1000.0f); +PARAM_DEFINE_FLOAT(KF_FAULT_ATT, 10.0f); |