Fixed few minor bug

2 files changed, 8 insertions, 2 deletions

diff --git a/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp b/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp
index 381b0df75..81a5e5b07 100755
--- a/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp
+++ b/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp
@@ -7,6 +7,7 @@
 #include <nuttx/config.h>
 #include <unistd.h>
 #include <stdlib.h>
+#include <string.h>
 #include <stdio.h>
 #include <stdbool.h>
 #include <poll.h>
@@ -44,6 +45,7 @@ static bool thread_should_exit = false;		/**< Deamon exit flag */
 static bool thread_running = false;		/**< Deamon status flag */
 static int attitude_estimator_so3_comp_task;				/**< Handle of deamon task / thread */
 volatile float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;					// quaternion of sensor frame relative to auxiliary frame
+volatile float integralFBx = 0.0f,  integralFBy = 0.0f, integralFBz = 0.0f;	// integral error terms scaled by Ki
 
 /**
  * Mainloop of attitude_estimator_so3_comp.
@@ -525,6 +527,10 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 					float bSq = q1*q1;
 					float cSq = q2*q2;
 					float dSq = q3*q3;
+					float a = q0;
+					float b = q1;
+					float c = q2;
+					float d = q3;
 
 					Rot_matrix[0] = aSq + bSq - cSq - dSq;
         				Rot_matrix[1] = 2.0 * (b * c - a * d);
diff --git a/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_params.c b/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_params.c
index bf0f49db8..158eb1972 100755
--- a/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_params.c
+++ b/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_params.c
@@ -15,7 +15,7 @@ PARAM_DEFINE_FLOAT(ATT_ROLL_OFFS, 0.0f);
 PARAM_DEFINE_FLOAT(ATT_PITCH_OFFS, 0.0f);
 PARAM_DEFINE_FLOAT(ATT_YAW_OFFS, 0.0f);
 
-int parameters_init(struct attitude_estimator_ekf_param_handles *h)
+int parameters_init(struct attitude_estimator_so3_comp_param_handles *h)
 {
 	/* Filter gain parameters */
 	h->Kp = 	param_find("SO3_COMP_KP");
@@ -29,7 +29,7 @@ int parameters_init(struct attitude_estimator_ekf_param_handles *h)
 	return OK;
 }
 
-int parameters_update(const struct attitude_estimator_ekf_param_handles *h, struct attitude_estimator_ekf_params *p)
+int parameters_update(const struct attitude_estimator_so3_comp_param_handles *h, struct attitude_estimator_so3_comp_params *p)
 {
 	/* Update filter gain */
 	param_get(h->Kp, &(p->Kp));