diff options
Diffstat (limited to 'src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c')
| -rwxr-xr-x | src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c | 33 |
1 files changed, 24 insertions, 9 deletions
diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c index 52dac652b..4154e3db4 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c @@ -52,17 +52,24 @@ PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q3, 0.005f); PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q4, 0.0f); /* gyro measurement noise */ -PARAM_DEFINE_FLOAT(EKF_ATT_V3_R0, 0.0008f); -PARAM_DEFINE_FLOAT(EKF_ATT_V3_R1, 10000.0f); -PARAM_DEFINE_FLOAT(EKF_ATT_V3_R2, 1.0f); -/* accelerometer measurement noise */ -PARAM_DEFINE_FLOAT(EKF_ATT_V3_R3, 0.0f); +PARAM_DEFINE_FLOAT(EKF_ATT_V4_R0, 0.0008f); +/* accel measurement noise */ +PARAM_DEFINE_FLOAT(EKF_ATT_V4_R1, 10000.0f); +/* mag measurement noise */ +PARAM_DEFINE_FLOAT(EKF_ATT_V4_R2, 100.0f); +/* offset estimation - UNUSED */ +PARAM_DEFINE_FLOAT(EKF_ATT_V4_R3, 0.0f); /* offsets in roll, pitch and yaw of sensor plane and body */ 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); + +PARAM_DEFINE_INT32(ATT_ACC_COMP, 2); + int parameters_init(struct attitude_estimator_ekf_param_handles *h) { /* PID parameters */ @@ -72,15 +79,19 @@ int parameters_init(struct attitude_estimator_ekf_param_handles *h) h->q3 = param_find("EKF_ATT_V3_Q3"); h->q4 = param_find("EKF_ATT_V3_Q4"); - h->r0 = param_find("EKF_ATT_V3_R0"); - h->r1 = param_find("EKF_ATT_V3_R1"); - h->r2 = param_find("EKF_ATT_V3_R2"); - h->r3 = param_find("EKF_ATT_V3_R3"); + h->r0 = param_find("EKF_ATT_V4_R0"); + h->r1 = param_find("EKF_ATT_V4_R1"); + h->r2 = param_find("EKF_ATT_V4_R2"); + h->r3 = param_find("EKF_ATT_V4_R3"); h->roll_off = param_find("ATT_ROLL_OFF3"); h->pitch_off = param_find("ATT_PITCH_OFF3"); h->yaw_off = param_find("ATT_YAW_OFF3"); + h->mag_decl = param_find("ATT_MAG_DECL"); + + h->acc_comp = param_find("ATT_ACC_COMP"); + return OK; } @@ -101,5 +112,9 @@ 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)); + + param_get(h->acc_comp, &(p->acc_comp)); + return OK; } |