diff --git a/src/modules/attitude_estimator_ekf/CMakeLists.txt b/src/modules/attitude_estimator_ekf/CMakeLists.txt index 9a6ef24a4b..2c037d46bb 100644 --- a/src/modules/attitude_estimator_ekf/CMakeLists.txt +++ b/src/modules/attitude_estimator_ekf/CMakeLists.txt @@ -40,7 +40,6 @@ px4_add_module( SRCS attitude_estimator_ekf_main.cpp - attitude_estimator_ekf_params.c codegen/AttitudeEKF.c DEPENDS platforms__common diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index 14f5470425..e408fb3c99 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -108,6 +108,46 @@ usage(const char *reason) fprintf(stderr, "usage: attitude_estimator_ekf {start|stop|status} [-p ]\n\n"); } +int parameters_init(struct attitude_estimator_ekf_param_handles *h) +{ + /* PID parameters */ + h->q0 = param_find("EKF_ATT_V3_Q0"); + h->q1 = param_find("EKF_ATT_V3_Q1"); + h->q2 = param_find("EKF_ATT_V3_Q2"); + h->q3 = param_find("EKF_ATT_V3_Q3"); + + 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->moment_inertia_J[0] = param_find("ATT_J11"); + h->moment_inertia_J[1] = param_find("ATT_J22"); + h->moment_inertia_J[2] = param_find("ATT_J33"); + h->use_moment_inertia = param_find("ATT_J_EN"); + + return OK; +} + +int parameters_update(const struct attitude_estimator_ekf_param_handles *h, struct attitude_estimator_ekf_params *p) +{ + param_get(h->q0, &(p->q[0])); + param_get(h->q1, &(p->q[1])); + param_get(h->q2, &(p->q[2])); + param_get(h->q3, &(p->q[3])); + + param_get(h->r0, &(p->r[0])); + param_get(h->r1, &(p->r[1])); + param_get(h->r2, &(p->r[2])); + + for (int i = 0; i < 3; i++) { + param_get(h->moment_inertia_J[i], &(p->moment_inertia_J[3 * i + i])); + } + + param_get(h->use_moment_inertia, &(p->use_moment_inertia)); + + return OK; +} + /** * The attitude_estimator_ekf app only briefly exists to start * the background job. The stack size assigned in the 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 31496bd0c6..92156c93e8 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c @@ -129,43 +129,3 @@ PARAM_DEFINE_FLOAT(ATT_J33, 0.0037); * @max 1 */ PARAM_DEFINE_INT32(ATT_J_EN, 0); - -int parameters_init(struct attitude_estimator_ekf_param_handles *h) -{ - /* PID parameters */ - h->q0 = param_find("EKF_ATT_V3_Q0"); - h->q1 = param_find("EKF_ATT_V3_Q1"); - h->q2 = param_find("EKF_ATT_V3_Q2"); - h->q3 = param_find("EKF_ATT_V3_Q3"); - - 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->moment_inertia_J[0] = param_find("ATT_J11"); - h->moment_inertia_J[1] = param_find("ATT_J22"); - h->moment_inertia_J[2] = param_find("ATT_J33"); - h->use_moment_inertia = param_find("ATT_J_EN"); - - return OK; -} - -int parameters_update(const struct attitude_estimator_ekf_param_handles *h, struct attitude_estimator_ekf_params *p) -{ - param_get(h->q0, &(p->q[0])); - param_get(h->q1, &(p->q[1])); - param_get(h->q2, &(p->q[2])); - param_get(h->q3, &(p->q[3])); - - param_get(h->r0, &(p->r[0])); - param_get(h->r1, &(p->r[1])); - param_get(h->r2, &(p->r[2])); - - for (int i = 0; i < 3; i++) { - param_get(h->moment_inertia_J[i], &(p->moment_inertia_J[3 * i + i])); - } - - param_get(h->use_moment_inertia, &(p->use_moment_inertia)); - - return OK; -}