diff options
author | Anton <rk3dov@gmail.com> | 2013-03-30 21:30:58 +0400 |
---|---|---|
committer | Anton <rk3dov@gmail.com> | 2013-03-30 21:30:58 +0400 |
commit | 114685a40bf38dc4281224ea18f898b6159df037 (patch) | |
tree | 07497fe63e3b2c1d44badc10521d9e7aa0c8347c /apps/position_estimator_inav/position_estimator_inav_params.c | |
parent | 72b8abca22d467dc2ab7ebcd5cdc286cf80c97ad (diff) | |
download | px4-firmware-114685a40bf38dc4281224ea18f898b6159df037.tar.gz px4-firmware-114685a40bf38dc4281224ea18f898b6159df037.tar.bz2 px4-firmware-114685a40bf38dc4281224ea18f898b6159df037.zip |
position_estimator_inav - first working version
Diffstat (limited to 'apps/position_estimator_inav/position_estimator_inav_params.c')
-rw-r--r-- | apps/position_estimator_inav/position_estimator_inav_params.c | 69 |
1 files changed, 62 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 5567fd2cf..fb082fbcb 100644 --- a/apps/position_estimator_inav/position_estimator_inav_params.c +++ b/apps/position_estimator_inav/position_estimator_inav_params.c @@ -1,9 +1,7 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Damian Aregger <daregger@student.ethz.ch> - * Tobias Naegeli <naegelit@student.ethz.ch> -* Lorenz Meier <lm@inf.ethz.ch> + * Copyright (C) 2013 Anton Babushkin. All rights reserved. + * Author: Anton Babushkin <rk3dov@gmail.com> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -44,16 +42,73 @@ /* Kalman Filter covariances */ /* gps measurement noise standard deviation */ -PARAM_DEFINE_FLOAT(POS_EST_R, 1.0f); +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); +PARAM_DEFINE_FLOAT(INAV_K_ALT_11, 0.0f); +PARAM_DEFINE_FLOAT(INAV_K_ALT_20, 0.0f); +PARAM_DEFINE_FLOAT(INAV_K_ALT_21, 0.0f); + +PARAM_DEFINE_INT32(INAV_ACC_OFFS_0, 0); +PARAM_DEFINE_INT32(INAV_ACC_OFFS_1, 0); +PARAM_DEFINE_INT32(INAV_ACC_OFFS_2, 0); + +PARAM_DEFINE_FLOAT(INAV_ACC_T_00, 0.0021f); +PARAM_DEFINE_FLOAT(INAV_ACC_T_01, 0.0f); +PARAM_DEFINE_FLOAT(INAV_ACC_T_02, 0.0f); +PARAM_DEFINE_FLOAT(INAV_ACC_T_10, 0.0f); +PARAM_DEFINE_FLOAT(INAV_ACC_T_11, 0.0021f); +PARAM_DEFINE_FLOAT(INAV_ACC_T_12, 0.0f); +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) { - h->r = param_find("POS_EST_R"); + 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"); + h->k_alt_11 = param_find("INAV_K_ALT_11"); + h->k_alt_20 = param_find("INAV_K_ALT_20"); + h->k_alt_21 = param_find("INAV_K_ALT_21"); + + h->acc_offs_0 = param_find("INAV_ACC_OFFS_0"); + h->acc_offs_1 = param_find("INAV_ACC_OFFS_1"); + h->acc_offs_2 = param_find("INAV_ACC_OFFS_2"); + + h->acc_t_00 = param_find("INAV_ACC_T_00"); + h->acc_t_01 = param_find("INAV_ACC_T_01"); + h->acc_t_02 = param_find("INAV_ACC_T_02"); + h->acc_t_10 = param_find("INAV_ACC_T_10"); + h->acc_t_11 = param_find("INAV_ACC_T_11"); + h->acc_t_12 = param_find("INAV_ACC_T_12"); + h->acc_t_20 = param_find("INAV_ACC_T_20"); + h->acc_t_21 = param_find("INAV_ACC_T_21"); + h->acc_t_22 = param_find("INAV_ACC_T_22"); return OK; } int parameters_update(const struct position_estimator_inav_param_handles *h, struct position_estimator_inav_params *p) { - param_get(h->r, &(p->r)); + 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])); + param_get(h->k_alt_11, &(p->k[1][1])); + 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])); + + param_get(h->acc_t_00, &(p->acc_T[0][0])); + param_get(h->acc_t_01, &(p->acc_T[0][1])); + param_get(h->acc_t_02, &(p->acc_T[0][2])); + param_get(h->acc_t_10, &(p->acc_T[1][0])); + param_get(h->acc_t_11, &(p->acc_T[1][1])); + param_get(h->acc_t_12, &(p->acc_T[1][2])); + param_get(h->acc_t_20, &(p->acc_T[2][0])); + param_get(h->acc_t_21, &(p->acc_T[2][1])); + param_get(h->acc_t_22, &(p->acc_T[2][2])); return OK; } |