From b3f1adc54bac36b8da16651a545835bb1877f883 Mon Sep 17 00:00:00 2001 From: Hyon Lim Date: Fri, 29 Nov 2013 02:35:49 +0900 Subject: [PATCH] SO3 estimator code has been cleaned --- makefiles/config_px4fmu-v1_backside.mk | 3 +- makefiles/config_px4fmu-v1_default.mk | 4 +- makefiles/config_px4fmu-v2_default.mk | 3 +- .../README | 0 .../attitude_estimator_so3_main.cpp} | 71 +++++++++++---- .../attitude_estimator_so3_params.c | 86 +++++++++++++++++++ .../attitude_estimator_so3_params.h | 67 +++++++++++++++ src/modules/attitude_estimator_so3/module.mk | 8 ++ .../attitude_estimator_so3_comp_params.c | 63 -------------- .../attitude_estimator_so3_comp_params.h | 44 ---------- .../attitude_estimator_so3_comp/module.mk | 8 -- 11 files changed, 219 insertions(+), 138 deletions(-) rename src/modules/{attitude_estimator_so3_comp => attitude_estimator_so3}/README (100%) rename src/modules/{attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp => attitude_estimator_so3/attitude_estimator_so3_main.cpp} (87%) create mode 100755 src/modules/attitude_estimator_so3/attitude_estimator_so3_params.c create mode 100755 src/modules/attitude_estimator_so3/attitude_estimator_so3_params.h create mode 100644 src/modules/attitude_estimator_so3/module.mk delete mode 100755 src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_params.c delete mode 100755 src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_params.h delete mode 100644 src/modules/attitude_estimator_so3_comp/module.mk diff --git a/makefiles/config_px4fmu-v1_backside.mk b/makefiles/config_px4fmu-v1_backside.mk index e1a42530b1..5ecf93a3a1 100644 --- a/makefiles/config_px4fmu-v1_backside.mk +++ b/makefiles/config_px4fmu-v1_backside.mk @@ -69,12 +69,13 @@ MODULES += modules/mavlink_onboard MODULES += modules/gpio_led # -# Estimation modules (EKF / other filters) +# Estimation modules (EKF/ SO3 / other filters) # #MODULES += modules/attitude_estimator_ekf MODULES += modules/att_pos_estimator_ekf #MODULES += modules/position_estimator_inav MODULES += examples/flow_position_estimator +MODULES += modules/attitude_estimator_so3 # # Vehicle Control diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index a8aed6bb43..25c22f1fdf 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -69,10 +69,10 @@ MODULES += modules/mavlink_onboard MODULES += modules/gpio_led # -# Estimation modules (EKF / other filters) +# Estimation modules (EKF/ SO3 / other filters) # MODULES += modules/attitude_estimator_ekf -MODULES += modules/attitude_estimator_so3_comp +MODULES += modules/attitude_estimator_so3 MODULES += modules/att_pos_estimator_ekf MODULES += modules/position_estimator_inav MODULES += examples/flow_position_estimator diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 761fb8d9d6..015a7387a6 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -68,9 +68,10 @@ MODULES += modules/mavlink MODULES += modules/mavlink_onboard # -# Estimation modules (EKF / other filters) +# Estimation modules (EKF/ SO3 / other filters) # MODULES += modules/attitude_estimator_ekf +MODULES += modules/attitude_estimator_so3 MODULES += modules/att_pos_estimator_ekf MODULES += modules/position_estimator_inav MODULES += examples/flow_position_estimator diff --git a/src/modules/attitude_estimator_so3_comp/README b/src/modules/attitude_estimator_so3/README similarity index 100% rename from src/modules/attitude_estimator_so3_comp/README rename to src/modules/attitude_estimator_so3/README diff --git a/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp b/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp similarity index 87% rename from src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp rename to src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp index e12c0e16ae..e79726613d 100755 --- a/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp +++ b/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp @@ -1,16 +1,49 @@ -/* - * Author: Hyon Lim +/**************************************************************************** * - * @file attitude_estimator_so3_comp_main.c + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Hyon Lim + * Anton Babushkin + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + + /* + * @file attitude_estimator_so3_main.cpp * * Implementation of nonlinear complementary filters on the SO(3). * This code performs attitude estimation by using accelerometer, gyroscopes and magnetometer. * Result is provided as quaternion, 1-2-3 Euler angle and rotation matrix. - * + * * Theory of nonlinear complementary filters on the SO(3) is based on [1]. * Quaternion realization of [1] is based on [2]. * Optmized quaternion update code is based on Sebastian Madgwick's implementation. - * + * * References * [1] Mahony, R.; Hamel, T.; Pflimlin, Jean-Michel, "Nonlinear Complementary Filters on the Special Orthogonal Group," Automatic Control, IEEE Transactions on , vol.53, no.5, pp.1203,1218, June 2008 * [2] Euston, M.; Coote, P.; Mahony, R.; Jonghyuk Kim; Hamel, T., "A complementary filter for attitude estimation of a fixed-wing UAV," Intelligent Robots and Systems, 2008. IROS 2008. IEEE/RSJ International Conference on , vol., no., pp.340,345, 22-26 Sept. 2008 @@ -46,16 +79,16 @@ #ifdef __cplusplus extern "C" { #endif -#include "attitude_estimator_so3_comp_params.h" +#include "attitude_estimator_so3_params.h" #ifdef __cplusplus } #endif -extern "C" __EXPORT int attitude_estimator_so3_comp_main(int argc, char *argv[]); +extern "C" __EXPORT int attitude_estimator_so3_main(int argc, char *argv[]); static bool thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ -static int attitude_estimator_so3_comp_task; /**< Handle of deamon task / thread */ +static int attitude_estimator_so3_task; /**< Handle of deamon task / thread */ //! Auxiliary variables to reduce number of repeated operations static float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; /** quaternion of sensor frame relative to auxiliary frame */ @@ -68,9 +101,9 @@ static float q3q3; static bool bFilterInit = false; /** - * Mainloop of attitude_estimator_so3_comp. + * Mainloop of attitude_estimator_so3. */ -int attitude_estimator_so3_comp_thread_main(int argc, char *argv[]); +int attitude_estimator_so3_thread_main(int argc, char *argv[]); /** * Print the correct usage. @@ -88,19 +121,19 @@ usage(const char *reason) if (reason) fprintf(stderr, "%s\n", reason); - fprintf(stderr, "usage: attitude_estimator_so3_comp {start|stop|status}\n"); + fprintf(stderr, "usage: attitude_estimator_so3 {start|stop|status}\n"); exit(1); } /** - * The attitude_estimator_so3_comp app only briefly exists to start + * The attitude_estimator_so3 app only briefly exists to start * the background job. The stack size assigned in the * Makefile does only apply to this management task. * * The actual stack size should be set in the call * to task_create(). */ -int attitude_estimator_so3_comp_main(int argc, char *argv[]) +int attitude_estimator_so3_main(int argc, char *argv[]) { if (argc < 1) usage("missing command"); @@ -114,11 +147,11 @@ int attitude_estimator_so3_comp_main(int argc, char *argv[]) } thread_should_exit = false; - attitude_estimator_so3_comp_task = task_spawn_cmd("attitude_estimator_so3_comp", + attitude_estimator_so3_task = task_spawn_cmd("attitude_estimator_so3", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 14000, - attitude_estimator_so3_comp_thread_main, + attitude_estimator_so3_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); exit(0); } @@ -356,7 +389,7 @@ void NonlinearSO3AHRSupdate(float gx, float gy, float gz, float ax, float ay, fl * @param argc number of commandline arguments (plus command name) * @param argv strings containing the arguments */ -int attitude_estimator_so3_comp_thread_main(int argc, char *argv[]) +int attitude_estimator_so3_thread_main(int argc, char *argv[]) { const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds @@ -415,8 +448,8 @@ int attitude_estimator_so3_comp_thread_main(int argc, char *argv[]) uint32_t sensor_last_count[3] = {0, 0, 0}; uint64_t sensor_last_timestamp[3] = {0, 0, 0}; - struct attitude_estimator_so3_comp_params so3_comp_params; - struct attitude_estimator_so3_comp_param_handles so3_comp_param_handles; + struct attitude_estimator_so3_params so3_comp_params; + struct attitude_estimator_so3_param_handles so3_comp_param_handles; /* initialize parameter handles */ parameters_init(&so3_comp_param_handles); @@ -430,7 +463,7 @@ int attitude_estimator_so3_comp_thread_main(int argc, char *argv[]) unsigned offset_count = 0; /* register the perf counter */ - perf_counter_t so3_comp_loop_perf = perf_alloc(PC_ELAPSED, "attitude_estimator_so3_comp"); + perf_counter_t so3_comp_loop_perf = perf_alloc(PC_ELAPSED, "attitude_estimator_so3"); /* Main loop*/ while (!thread_should_exit) { diff --git a/src/modules/attitude_estimator_so3/attitude_estimator_so3_params.c b/src/modules/attitude_estimator_so3/attitude_estimator_so3_params.c new file mode 100755 index 0000000000..0c8d522b4d --- /dev/null +++ b/src/modules/attitude_estimator_so3/attitude_estimator_so3_params.c @@ -0,0 +1,86 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Hyon Lim + * Anton Babushkin + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* + * @file attitude_estimator_so3_params.c + * + * Parameters for nonlinear complementary filters on the SO(3). + */ + +#include "attitude_estimator_so3_params.h" + +/* This is filter gain for nonlinear SO3 complementary filter */ +/* NOTE : How to tune the gain? First of all, stick with this default gain. And let the quad in stable place. + Log the steady state reponse of filter. If it is too slow, increase SO3_COMP_KP. + If you are flying from ground to high altitude in short amount of time, please increase SO3_COMP_KI which + will compensate gyro bias which depends on temperature and vibration of your vehicle */ +PARAM_DEFINE_FLOAT(SO3_COMP_KP, 1.0f); //! This parameter will give you about 15 seconds convergence time. + //! You can set this gain higher if you want more fast response. + //! But note that higher gain will give you also higher overshoot. +PARAM_DEFINE_FLOAT(SO3_COMP_KI, 0.05f); //! This gain will incorporate slow time-varying bias (e.g., temperature change) + //! This gain is depend on your vehicle status. + +/* offsets in roll, pitch and yaw of sensor plane and body */ +PARAM_DEFINE_FLOAT(SO3_ROLL_OFFS, 0.0f); +PARAM_DEFINE_FLOAT(SO3_PITCH_OFFS, 0.0f); +PARAM_DEFINE_FLOAT(SO3_YAW_OFFS, 0.0f); + +int parameters_init(struct attitude_estimator_so3_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("SO3_ROLL_OFFS"); + h->pitch_off = param_find("SO3_PITCH_OFFS"); + h->yaw_off = param_find("SO3_YAW_OFFS"); + + return OK; +} + +int parameters_update(const struct attitude_estimator_so3_param_handles *h, struct attitude_estimator_so3_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; +} diff --git a/src/modules/attitude_estimator_so3/attitude_estimator_so3_params.h b/src/modules/attitude_estimator_so3/attitude_estimator_so3_params.h new file mode 100755 index 0000000000..dfb4cad05a --- /dev/null +++ b/src/modules/attitude_estimator_so3/attitude_estimator_so3_params.h @@ -0,0 +1,67 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Hyon Lim + * Anton Babushkin + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* + * @file attitude_estimator_so3_params.h + * + * Parameters for nonlinear complementary filters on the SO(3). + */ + +#include + +struct attitude_estimator_so3_params { + float Kp; + float Ki; + float roll_off; + float pitch_off; + float yaw_off; +}; + +struct attitude_estimator_so3_param_handles { + param_t Kp, Ki; + param_t roll_off, pitch_off, yaw_off; +}; + +/** + * Initialize all parameter handles and values + * + */ +int parameters_init(struct attitude_estimator_so3_param_handles *h); + +/** + * Update all parameters + * + */ +int parameters_update(const struct attitude_estimator_so3_param_handles *h, struct attitude_estimator_so3_params *p); diff --git a/src/modules/attitude_estimator_so3/module.mk b/src/modules/attitude_estimator_so3/module.mk new file mode 100644 index 0000000000..e29bb16a62 --- /dev/null +++ b/src/modules/attitude_estimator_so3/module.mk @@ -0,0 +1,8 @@ +# +# Attitude estimator (Nonlinear SO(3) complementary Filter) +# + +MODULE_COMMAND = attitude_estimator_so3 + +SRCS = attitude_estimator_so3_main.cpp \ + attitude_estimator_so3_params.c 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 deleted file mode 100755 index f962515dfb..0000000000 --- a/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_params.c +++ /dev/null @@ -1,63 +0,0 @@ -/* - * Author: Hyon Lim - * - * @file attitude_estimator_so3_comp_params.c - * - * Implementation of nonlinear complementary filters on the SO(3). - * This code performs attitude estimation by using accelerometer, gyroscopes and magnetometer. - * Result is provided as quaternion, 1-2-3 Euler angle and rotation matrix. - * - * Theory of nonlinear complementary filters on the SO(3) is based on [1]. - * Quaternion realization of [1] is based on [2]. - * Optmized quaternion update code is based on Sebastian Madgwick's implementation. - * - * References - * [1] Mahony, R.; Hamel, T.; Pflimlin, Jean-Michel, "Nonlinear Complementary Filters on the Special Orthogonal Group," Automatic Control, IEEE Transactions on , vol.53, no.5, pp.1203,1218, June 2008 - * [2] Euston, M.; Coote, P.; Mahony, R.; Jonghyuk Kim; Hamel, T., "A complementary filter for attitude estimation of a fixed-wing UAV," Intelligent Robots and Systems, 2008. IROS 2008. IEEE/RSJ International Conference on , vol., no., pp.340,345, 22-26 Sept. 2008 - */ - -#include "attitude_estimator_so3_comp_params.h" - -/* This is filter gain for nonlinear SO3 complementary filter */ -/* NOTE : How to tune the gain? First of all, stick with this default gain. And let the quad in stable place. - Log the steady state reponse of filter. If it is too slow, increase SO3_COMP_KP. - If you are flying from ground to high altitude in short amount of time, please increase SO3_COMP_KI which - will compensate gyro bias which depends on temperature and vibration of your vehicle */ -PARAM_DEFINE_FLOAT(SO3_COMP_KP, 1.0f); //! This parameter will give you about 15 seconds convergence time. - //! You can set this gain higher if you want more fast response. - //! But note that higher gain will give you also higher overshoot. -PARAM_DEFINE_FLOAT(SO3_COMP_KI, 0.05f); //! This gain will incorporate slow time-varying bias (e.g., temperature change) - //! This gain is depend on your vehicle status. - -/* 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_so3_comp_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_so3_comp_param_handles *h, struct attitude_estimator_so3_comp_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; -} diff --git a/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_params.h b/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_params.h deleted file mode 100755 index f006956308..0000000000 --- a/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_params.h +++ /dev/null @@ -1,44 +0,0 @@ -/* - * Author: Hyon Lim - * - * @file attitude_estimator_so3_comp_params.h - * - * Implementation of nonlinear complementary filters on the SO(3). - * This code performs attitude estimation by using accelerometer, gyroscopes and magnetometer. - * Result is provided as quaternion, 1-2-3 Euler angle and rotation matrix. - * - * Theory of nonlinear complementary filters on the SO(3) is based on [1]. - * Quaternion realization of [1] is based on [2]. - * Optmized quaternion update code is based on Sebastian Madgwick's implementation. - * - * References - * [1] Mahony, R.; Hamel, T.; Pflimlin, Jean-Michel, "Nonlinear Complementary Filters on the Special Orthogonal Group," Automatic Control, IEEE Transactions on , vol.53, no.5, pp.1203,1218, June 2008 - * [2] Euston, M.; Coote, P.; Mahony, R.; Jonghyuk Kim; Hamel, T., "A complementary filter for attitude estimation of a fixed-wing UAV," Intelligent Robots and Systems, 2008. IROS 2008. IEEE/RSJ International Conference on , vol., no., pp.340,345, 22-26 Sept. 2008 - */ - -#include - -struct attitude_estimator_so3_comp_params { - float Kp; - float Ki; - float roll_off; - float pitch_off; - float yaw_off; -}; - -struct attitude_estimator_so3_comp_param_handles { - param_t Kp, Ki; - param_t roll_off, pitch_off, yaw_off; -}; - -/** - * Initialize all parameter handles and values - * - */ -int parameters_init(struct attitude_estimator_so3_comp_param_handles *h); - -/** - * Update all parameters - * - */ -int parameters_update(const struct attitude_estimator_so3_comp_param_handles *h, struct attitude_estimator_so3_comp_params *p); diff --git a/src/modules/attitude_estimator_so3_comp/module.mk b/src/modules/attitude_estimator_so3_comp/module.mk deleted file mode 100644 index 92f43d9202..0000000000 --- a/src/modules/attitude_estimator_so3_comp/module.mk +++ /dev/null @@ -1,8 +0,0 @@ -# -# Attitude estimator (Nonlinear SO3 complementary Filter) -# - -MODULE_COMMAND = attitude_estimator_so3_comp - -SRCS = attitude_estimator_so3_comp_main.cpp \ - attitude_estimator_so3_comp_params.c