fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-02 05:04:08 +08:00
Code Issues Packages Projects Releases Wiki Activity

attitude_estimator_ekf: mag declination parameter implemented

Browse Source
This commit is contained in:
Anton Babushkin 2013-11-22 21:44:37 +04:00
parent 4afb420bed
commit dada0b8599
3 changed files with 25 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
View File

@ -61,6 +61,8 @@
#include <uORB/topics/parameter_update.h>
#include <drivers/drv_hrt.h>
#include <lib/mathlib/mathlib.h>
#include <systemlib/systemlib.h>
#include <systemlib/perf_counter.h>
#include <systemlib/err.h>
@ -433,10 +435,9 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
/* send out */
att.timestamp = raw.timestamp;
// XXX Apply the same transformation to the rotation matrix
att.roll = euler[0] - ekf_params.roll_off;
att.pitch = euler[1] - ekf_params.pitch_off;
att.yaw = euler[2] - ekf_params.yaw_off;
att.roll = euler[0];
att.pitch = euler[1];
att.yaw = euler[2] + ekf_params.mag_decl;
att.rollspeed = x_aposteriori[0];
att.pitchspeed = x_aposteriori[1];
@ -445,12 +446,21 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
att.pitchacc = x_aposteriori[4];
att.yawacc = x_aposteriori[5];
//att.yawspeed =z_k[2] ;
/* copy offsets */
memcpy(&att.rate_offsets, &(x_aposteriori[3]), sizeof(att.rate_offsets));
/* magnetic declination */
math::EulerAngles eulerMagDecl(0.0f, 0.0f, ekf_params.mag_decl);
math::Dcm R(eulerMagDecl);
math::Dcm R_body(&Rot_matrix[0]);
R = R * R_body;
/* copy rotation matrix */
memcpy(&att.R, Rot_matrix, sizeof(Rot_matrix));
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
att.R[i][j] = R(i, j);
}
}
att.R_valid = true;
if (isfinite(att.roll) && isfinite(att.pitch) && isfinite(att.yaw)) {

7
src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c
View File

@ -63,6 +63,9 @@ PARAM_DEFINE_FLOAT(ATT_ROLL_OFF3, 0.0f);
PARAM_DEFINE_FLOAT(ATT_PITCH_OFF3, 0.0f);
PARAM_DEFINE_FLOAT(ATT_YAW_OFF3, 0.0f);
/* magnetic declination, in degrees */
PARAM_DEFINE_FLOAT(ATT_MAG_DECL, 0.0f);
int parameters_init(struct attitude_estimator_ekf_param_handles *h)
{
/* PID parameters */
@ -81,6 +84,8 @@ int parameters_init(struct attitude_estimator_ekf_param_handles *h)
h->pitch_off = param_find("ATT_PITCH_OFF3");
h->yaw_off = param_find("ATT_YAW_OFF3");
h->mag_decl = param_find("ATT_MAG_DECL");
return OK;
}
@ -101,5 +106,7 @@ int parameters_update(const struct attitude_estimator_ekf_param_handles *h, stru
param_get(h->pitch_off, &(p->pitch_off));
param_get(h->yaw_off, &(p->yaw_off));
param_get(h->mag_decl, &(p->mag_decl));
return OK;
}

2
src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h
View File

@ -47,12 +47,14 @@ struct attitude_estimator_ekf_params {
float roll_off;
float pitch_off;
float yaw_off;
float mag_decl;
};
struct attitude_estimator_ekf_param_handles {
param_t r0, r1, r2, r3;
param_t q0, q1, q2, q3, q4;
param_t roll_off, pitch_off, yaw_off;
param_t mag_decl;
};
/**