diff options
author | Hyon Lim (Retina) <limhyon@gmail.com> | 2013-05-21 16:17:20 +1000 |
---|---|---|
committer | Hyon Lim (Retina) <limhyon@gmail.com> | 2013-05-23 16:20:38 +1000 |
commit | 0c3412223b0961798e0fa9c27042132ebdfc0bdb (patch) | |
tree | 8be06a030b5672d0a30f361a6acc4f9c123f6f46 /src | |
parent | cd7b0f7aab39099353bda46ba9a498c242d75791 (diff) | |
download | px4-firmware-0c3412223b0961798e0fa9c27042132ebdfc0bdb.tar.gz px4-firmware-0c3412223b0961798e0fa9c27042132ebdfc0bdb.tar.bz2 px4-firmware-0c3412223b0961798e0fa9c27042132ebdfc0bdb.zip |
Fixed few minor bug
Diffstat (limited to 'src')
-rwxr-xr-x | src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp | 6 | ||||
-rwxr-xr-x | src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_params.c | 4 |
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)); |