diff options
| author | Anton <rk3dov@gmail.com> | 2013-04-08 17:04:04 +0400 |
|---|---|---|
| committer | Anton <rk3dov@gmail.com> | 2013-04-08 17:04:04 +0400 |
| commit | 4a062086aefd4fcd25f40d0cfa65ab3a1c3c3984 (patch) | |
| tree | 175e40fcbbd9939a63f7595e277268b4215af45a /apps/position_estimator_inav/position_estimator_inav_params.c | |
| parent | d536e3d5f9b0fc3187cdcd8fc20bb8bb600b9e98 (diff) | |
| download | px4-firmware-4a062086aefd4fcd25f40d0cfa65ab3a1c3c3984.tar.gz px4-firmware-4a062086aefd4fcd25f40d0cfa65ab3a1c3c3984.tar.bz2 px4-firmware-4a062086aefd4fcd25f40d0cfa65ab3a1c3c3984.zip | |
position_estimator_inav: bugfixes, some refactoring
Diffstat (limited to 'apps/position_estimator_inav/position_estimator_inav_params.c')
| -rw-r--r-- | apps/position_estimator_inav/position_estimator_inav_params.c | 23 |
1 files changed, 16 insertions, 7 deletions
diff --git a/apps/position_estimator_inav/position_estimator_inav_params.c b/apps/position_estimator_inav/position_estimator_inav_params.c index fb082fbcb..d3a165947 100644 --- a/apps/position_estimator_inav/position_estimator_inav_params.c +++ b/apps/position_estimator_inav/position_estimator_inav_params.c @@ -42,6 +42,8 @@ /* Kalman Filter covariances */ /* gps measurement noise standard deviation */ +PARAM_DEFINE_INT32(INAV_USE_GPS, 0); + PARAM_DEFINE_FLOAT(INAV_K_ALT_00, 0.0f); PARAM_DEFINE_FLOAT(INAV_K_ALT_01, 0.0f); PARAM_DEFINE_FLOAT(INAV_K_ALT_10, 0.0f); @@ -63,8 +65,9 @@ PARAM_DEFINE_FLOAT(INAV_ACC_T_20, 0.0f); PARAM_DEFINE_FLOAT(INAV_ACC_T_21, 0.0f); PARAM_DEFINE_FLOAT(INAV_ACC_T_22, 0.0021f); -int parameters_init(struct position_estimator_inav_param_handles *h) -{ +int parameters_init(struct position_estimator_inav_param_handles *h) { + h->use_gps = param_find("INAV_USE_GPS"); + h->k_alt_00 = param_find("INAV_K_ALT_00"); h->k_alt_01 = param_find("INAV_K_ALT_01"); h->k_alt_10 = param_find("INAV_K_ALT_10"); @@ -88,8 +91,9 @@ int parameters_init(struct position_estimator_inav_param_handles *h) return OK; } -int parameters_update(const struct position_estimator_inav_param_handles *h, struct position_estimator_inav_params *p) -{ +int parameters_update(const struct position_estimator_inav_param_handles *h, struct position_estimator_inav_params *p) { + param_get(h->use_gps, &(p->use_gps)); + param_get(h->k_alt_00, &(p->k[0][0])); param_get(h->k_alt_01, &(p->k[0][1])); param_get(h->k_alt_10, &(p->k[1][0])); @@ -97,9 +101,14 @@ int parameters_update(const struct position_estimator_inav_param_handles *h, str param_get(h->k_alt_20, &(p->k[2][0])); param_get(h->k_alt_21, &(p->k[2][1])); - param_get(h->acc_offs_0, &(p->acc_offs[0])); - param_get(h->acc_offs_1, &(p->acc_offs[1])); - param_get(h->acc_offs_2, &(p->acc_offs[2])); + /* read int32 and cast to int16 */ + int32_t t; + param_get(h->acc_offs_0, &t); + p->acc_offs[0] = t; + param_get(h->acc_offs_1, &t); + p->acc_offs[1] = t; + param_get(h->acc_offs_2, &t); + p->acc_offs[2] = t; param_get(h->acc_t_00, &(p->acc_T[0][0])); param_get(h->acc_t_01, &(p->acc_T[0][1])); |