diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2013-01-09 16:52:15 +0100 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2013-01-09 16:52:15 +0100 |
commit | 4435befefdbaedc85bd94a4240b0ebe9590fd391 (patch) | |
tree | fb18844f7ca29d5521139aa4090640b4b61aa7d8 /apps | |
parent | 8cc9fb9e2c47af213962ece3be3f883dd5cd71ea (diff) | |
download | px4-firmware-4435befefdbaedc85bd94a4240b0ebe9590fd391.tar.gz px4-firmware-4435befefdbaedc85bd94a4240b0ebe9590fd391.tar.bz2 px4-firmware-4435befefdbaedc85bd94a4240b0ebe9590fd391.zip |
Added offset parameters for roll, pitch and yaw
Diffstat (limited to 'apps')
3 files changed, 19 insertions, 3 deletions
diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c index 6533390bc..e373c531b 100644 --- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c +++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c @@ -413,9 +413,9 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) /* send out */ att.timestamp = raw.timestamp; - att.roll = euler[0]; - att.pitch = euler[1]; - att.yaw = euler[2]; + 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.rollspeed = x_aposteriori[0]; att.pitchspeed = x_aposteriori[1]; diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c index 6879f9dc8..75a2479ed 100644 --- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c +++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c @@ -72,6 +72,10 @@ PARAM_DEFINE_FLOAT(EKF_ATT_R5, 1e2f); PARAM_DEFINE_FLOAT(EKF_ATT_R6, 1e-1f); PARAM_DEFINE_FLOAT(EKF_ATT_R7, 1e-1f); PARAM_DEFINE_FLOAT(EKF_ATT_R8, 1e-1f); +/* offsets in roll, pitch and yaw of sensor plane and body */ +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) { @@ -99,6 +103,10 @@ int parameters_init(struct attitude_estimator_ekf_param_handles *h) h->r7 = param_find("EKF_ATT_R7"); h->r8 = param_find("EKF_ATT_R8"); + h->roll_off = param_find("ATT_ROLL_OFFS"); + h->pitch_off = param_find("ATT_PITCH_OFFS"); + h->yaw_off = param_find("ATT_YAW_OFFS"); + return OK; } @@ -127,5 +135,9 @@ int parameters_update(const struct attitude_estimator_ekf_param_handles *h, stru param_get(h->r7, &(p->r[7])); param_get(h->r8, &(p->r[8])); + param_get(h->roll_off, &(p->roll_off)); + param_get(h->pitch_off, &(p->pitch_off)); + param_get(h->yaw_off, &(p->yaw_off)); + return OK; } diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.h b/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.h index 6a63f9767..ad775002b 100644 --- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.h +++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.h @@ -44,11 +44,15 @@ struct attitude_estimator_ekf_params { float r[9]; float q[12]; + float roll_off; + float pitch_off; + float yaw_off; }; struct attitude_estimator_ekf_param_handles { param_t r0, r1, r2, r3, r4, r5, r6, r7, r8; param_t q0, q1, q2, q3, q4, q5, q6, q7, q8, q9, q10, q11; + param_t roll_off, pitch_off, yaw_off; }; /** |