Made fault tolerances adjustable.

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);