From 0c3412223b0961798e0fa9c27042132ebdfc0bdb Mon Sep 17 00:00:00 2001 From: "Hyon Lim (Retina)" Date: Tue, 21 May 2013 16:17:20 +1000 Subject: Fixed few minor bug --- .../attitude_estimator_so3_comp_main.cpp | 6 ++++++ .../attitude_estimator_so3_comp_params.c | 4 ++-- 2 files changed, 8 insertions(+), 2 deletions(-) (limited to 'src/modules/attitude_estimator_so3_comp') 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 #include #include +#include #include #include #include @@ -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)); -- cgit v1.2.3