dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Fixed few minor bug

This commit is contained in:
Hyon Lim (Retina) 2013-05-21 16:17:20 +10:00
parent cd7b0f7aab
commit 0c3412223b
2 changed files with 8 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp
View File

@ -7,6 +7,7 @@
#include <nuttx/config.h> #include <nuttx/config.h>
#include <unistd.h> #include <unistd.h>
#include <stdlib.h> #include <stdlib.h>
#include <string.h>
#include <stdio.h> #include <stdio.h>
#include <stdbool.h> #include <stdbool.h>
#include <poll.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 bool thread_running = false; /**< Deamon status flag */
static int attitude_estimator_so3_comp_task; /**< Handle of deamon task / thread */ 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 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. * 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 bSq = q1*q1;
float cSq = q2*q2; float cSq = q2*q2;
float dSq = q3*q3; 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[0] = aSq + bSq - cSq - dSq;
Rot_matrix[1] = 2.0 * (b * c - a * d); Rot_matrix[1] = 2.0 * (b * c - a * d);

4
src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_params.c
View File

@ -15,7 +15,7 @@ PARAM_DEFINE_FLOAT(ATT_ROLL_OFFS, 0.0f);
PARAM_DEFINE_FLOAT(ATT_PITCH_OFFS, 0.0f); PARAM_DEFINE_FLOAT(ATT_PITCH_OFFS, 0.0f);
PARAM_DEFINE_FLOAT(ATT_YAW_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 */ /* Filter gain parameters */
h->Kp = param_find("SO3_COMP_KP"); h->Kp = param_find("SO3_COMP_KP");
@ -29,7 +29,7 @@ int parameters_init(struct attitude_estimator_ekf_param_handles *h)
return OK; 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 */ /* Update filter gain */
param_get(h->Kp, &(p->Kp)); param_get(h->Kp, &(p->Kp));