att ekf: add param to enable/disable J

3 files changed, 18 insertions, 5 deletions

diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
index e1bbf5bc7..667b74d1d 100755
--- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
+++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
@@ -512,7 +512,7 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 
 					/* Call the estimator */
 					AttitudeEKF(false, // approx_prediction
-							false, // use_inertia_matrix
+							(unsigned char)ekf_params.use_moment_inertia,
 							update_vect,
 							dt,
 							z_k,
diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c
index 5b57bfb4d..5c33bc2ac 100755
--- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c
+++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c
@@ -79,7 +79,6 @@ PARAM_DEFINE_FLOAT(ATT_J11, 0.0018);
  */
 PARAM_DEFINE_FLOAT(ATT_J22, 0.0018);
 
-
 /*
  * Moment of inertia matrix diagonal entry (3, 3)
  *
@@ -88,6 +87,16 @@ PARAM_DEFINE_FLOAT(ATT_J22, 0.0018);
  */
 PARAM_DEFINE_FLOAT(ATT_J33, 0.0037);
 
+/*
+ * Moment of inertia enabled in estimator
+ *
+ * If set to != 0 the moment of inertia will be used in the estimator
+ *
+ * @group attitude_ekf
+ * @min 0
+ * @max 1
+ */
+PARAM_DEFINE_INT32(ATT_J_EN, 0);
 
 int parameters_init(struct attitude_estimator_ekf_param_handles *h)
 {
@@ -105,9 +114,10 @@ int parameters_init(struct attitude_estimator_ekf_param_handles *h)
 
 	h->acc_comp   =	param_find("ATT_ACC_COMP");
 
-	h->moment_inertia_J[0]   =   param_find("ATT_J11");
-	h->moment_inertia_J[1]   =   param_find("ATT_J22");
-	h->moment_inertia_J[2]   =   param_find("ATT_J33");
+	h->moment_inertia_J[0]  =   param_find("ATT_J11");
+	h->moment_inertia_J[1]  =   param_find("ATT_J22");
+	h->moment_inertia_J[2]  =   param_find("ATT_J33");
+	h->use_moment_inertia	=   param_find("ATT_J_EN");
 
 	return OK;
 }
@@ -131,6 +141,7 @@ int parameters_update(const struct attitude_estimator_ekf_param_handles *h, stru
 	for (int i = 0; i < 3; i++) {
 		param_get(h->moment_inertia_J[i], &(p->moment_inertia_J[3 * i + i]));
 	}
+	param_get(h->use_moment_inertia, &(p->use_moment_inertia));
 
 	return OK;
 }
diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h
index fbb6a18ff..5d3b6b244 100755
--- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h
+++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h
@@ -45,6 +45,7 @@ struct attitude_estimator_ekf_params {
 	float r[3];
 	float q[4];
 	float moment_inertia_J[9];
+	int32_t use_moment_inertia;
 	float roll_off;
 	float pitch_off;
 	float yaw_off;
@@ -56,6 +57,7 @@ struct attitude_estimator_ekf_param_handles {
 	param_t r0, r1, r2;
 	param_t q0, q1, q2, q3;
 	param_t moment_inertia_J[3]; /**< diagonal entries of the matrix */
+	param_t use_moment_inertia;
 	param_t mag_decl;
 	param_t acc_comp;
 };