dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

SO3 estimator code has been cleaned

This commit is contained in:
Hyon Lim 2013-11-29 02:35:49 +09:00
parent bcd745fb0d
commit b3f1adc54b
11 changed files with 219 additions and 138 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
makefiles/config_px4fmu-v1_backside.mk
View File

@ -69,12 +69,13 @@ MODULES += modules/mavlink_onboard
MODULES += modules/gpio_led MODULES += modules/gpio_led
# #
# Estimation modules (EKF / other filters) # Estimation modules (EKF/ SO3 / other filters)
# #
#MODULES += modules/attitude_estimator_ekf #MODULES += modules/attitude_estimator_ekf
MODULES += modules/att_pos_estimator_ekf MODULES += modules/att_pos_estimator_ekf
#MODULES += modules/position_estimator_inav #MODULES += modules/position_estimator_inav
MODULES += examples/flow_position_estimator MODULES += examples/flow_position_estimator
MODULES += modules/attitude_estimator_so3
# #
# Vehicle Control # Vehicle Control

4
makefiles/config_px4fmu-v1_default.mk
View File

@ -69,10 +69,10 @@ MODULES += modules/mavlink_onboard
MODULES += modules/gpio_led MODULES += modules/gpio_led
# #
# Estimation modules (EKF / other filters) # Estimation modules (EKF/ SO3 / other filters)
# #
MODULES += modules/attitude_estimator_ekf MODULES += modules/attitude_estimator_ekf
MODULES += modules/attitude_estimator_so3_comp MODULES += modules/attitude_estimator_so3
MODULES += modules/att_pos_estimator_ekf MODULES += modules/att_pos_estimator_ekf
MODULES += modules/position_estimator_inav MODULES += modules/position_estimator_inav
MODULES += examples/flow_position_estimator MODULES += examples/flow_position_estimator

3
makefiles/config_px4fmu-v2_default.mk
View File

@ -68,9 +68,10 @@ MODULES += modules/mavlink
MODULES += modules/mavlink_onboard MODULES += modules/mavlink_onboard
# #
# Estimation modules (EKF / other filters) # Estimation modules (EKF/ SO3 / other filters)
# #
MODULES += modules/attitude_estimator_ekf MODULES += modules/attitude_estimator_ekf
MODULES += modules/attitude_estimator_so3
MODULES += modules/att_pos_estimator_ekf MODULES += modules/att_pos_estimator_ekf
MODULES += modules/position_estimator_inav MODULES += modules/position_estimator_inav
MODULES += examples/flow_position_estimator MODULES += examples/flow_position_estimator

0
src/modules/attitude_estimator_so3_comp/README → src/modules/attitude_estimator_so3/README
View File

67
src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp → src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp
View File

@ -1,7 +1,40 @@
/* /****************************************************************************
* Author: Hyon Lim <limhyon@gmail.com, hyonlim@snu.ac.kr>
* *
* @file attitude_estimator_so3_comp_main.c * Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Author: Hyon Lim <limhyon@gmail.com>
* Anton Babushkin <anton.babushkin@me.com>
*
* 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). * Implementation of nonlinear complementary filters on the SO(3).
* This code performs attitude estimation by using accelerometer, gyroscopes and magnetometer. * This code performs attitude estimation by using accelerometer, gyroscopes and magnetometer.
@ -46,16 +79,16 @@
#ifdef __cplusplus #ifdef __cplusplus
extern "C" { extern "C" {
#endif #endif
#include "attitude_estimator_so3_comp_params.h" #include "attitude_estimator_so3_params.h"
#ifdef __cplusplus #ifdef __cplusplus
} }
#endif #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_should_exit = false; /**< Deamon exit flag */
static bool thread_running = false; /**< Deamon status 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 //! 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 */ 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; 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. * Print the correct usage.
@ -88,19 +121,19 @@ usage(const char *reason)
if (reason) if (reason)
fprintf(stderr, "%s\n", 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); 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 * the background job. The stack size assigned in the
* Makefile does only apply to this management task. * Makefile does only apply to this management task.
* *
* The actual stack size should be set in the call * The actual stack size should be set in the call
* to task_create(). * 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) if (argc < 1)
usage("missing command"); usage("missing command");
@ -114,11 +147,11 @@ int attitude_estimator_so3_comp_main(int argc, char *argv[])
} }
thread_should_exit = false; 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_DEFAULT,
SCHED_PRIORITY_MAX - 5, SCHED_PRIORITY_MAX - 5,
14000, 14000,
attitude_estimator_so3_comp_thread_main, attitude_estimator_so3_thread_main,
(argv) ? (const char **)&argv[2] : (const char **)NULL); (argv) ? (const char **)&argv[2] : (const char **)NULL);
exit(0); 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 argc number of commandline arguments (plus command name)
* @param argv strings containing the arguments * @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 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}; uint32_t sensor_last_count[3] = {0, 0, 0};
uint64_t sensor_last_timestamp[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_params so3_comp_params;
struct attitude_estimator_so3_comp_param_handles so3_comp_param_handles; struct attitude_estimator_so3_param_handles so3_comp_param_handles;
/* initialize parameter handles */ /* initialize parameter handles */
parameters_init(&so3_comp_param_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; unsigned offset_count = 0;
/* register the perf counter */ /* 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*/ /* Main loop*/
while (!thread_should_exit) { while (!thread_should_exit) {

86
src/modules/attitude_estimator_so3/attitude_estimator_so3_params.c Executable file
View File

@ -0,0 +1,86 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Author: Hyon Lim <limhyon@gmail.com>
* Anton Babushkin <anton.babushkin@me.com>
*
* 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;
}

67
src/modules/attitude_estimator_so3/attitude_estimator_so3_params.h Executable file
View File

@ -0,0 +1,67 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Author: Hyon Lim <limhyon@gmail.com>
* Anton Babushkin <anton.babushkin@me.com>
*
* 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 <systemlib/param/param.h>
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);

8
src/modules/attitude_estimator_so3/module.mk Normal file
View File

@ -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

63
src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_params.c
View File

@ -1,63 +0,0 @@
/*
* Author: Hyon Lim <limhyon@gmail.com, hyonlim@snu.ac.kr>
*
* @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;
}

44
src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_params.h
View File

@ -1,44 +0,0 @@
/*
* Author: Hyon Lim <limhyon@gmail.com, hyonlim@snu.ac.kr>
*
* @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 <systemlib/param/param.h>
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);

8
src/modules/attitude_estimator_so3_comp/module.mk
View File

@ -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