diff options
author | Hyon Lim (Retina) <limhyon@gmail.com> | 2013-05-21 16:11:03 +1000 |
---|---|---|
committer | Hyon Lim (Retina) <limhyon@gmail.com> | 2013-05-23 16:20:38 +1000 |
commit | 1caddb7bbb53f3017e2ee67742531b2159999658 (patch) | |
tree | da8cc4b202cb6bd8e8ad7295fbf408a3ab0edd5a /src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_params.c | |
parent | 2135628254fa9035c3cbb7db8ed9c05bb3dd172a (diff) | |
download | px4-firmware-1caddb7bbb53f3017e2ee67742531b2159999658.tar.gz px4-firmware-1caddb7bbb53f3017e2ee67742531b2159999658.tar.bz2 px4-firmware-1caddb7bbb53f3017e2ee67742531b2159999658.zip |
Initial work of so3 nonlinear complementary filter
Diffstat (limited to 'src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_params.c')
-rwxr-xr-x | src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_params.c | 44 |
1 files changed, 44 insertions, 0 deletions
diff --git a/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_params.c b/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_params.c new file mode 100755 index 000000000..bf0f49db8 --- /dev/null +++ b/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_params.c @@ -0,0 +1,44 @@ +/* + * @file attitude_estimator_so3_comp_params.c + * + * Parameters for SO3 complementary filter + */ + +#include "attitude_estimator_so3_comp_params.h" + +/* This is filter gain for nonlinear SO3 complementary filter */ +PARAM_DEFINE_FLOAT(SO3_COMP_KP, 1.0f); +PARAM_DEFINE_FLOAT(SO3_COMP_KI, 0.0f); + +/* 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) +{ + /* Filter gain parameters */ + h->Kp = param_find("SO3_COMP_KP"); + h->Ki = param_find("SO3_COMP_KI"); + + /* Attitude offset (WARNING: Do not change if you do not know what exactly this variable wil lchange) */ + 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; +} + +int parameters_update(const struct attitude_estimator_ekf_param_handles *h, struct attitude_estimator_ekf_params *p) +{ + /* Update filter gain */ + param_get(h->Kp, &(p->Kp)); + param_get(h->Ki, &(p->Ki)); + + /* Update attitude offset */ + 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; +} |