SO3 estimator code has been cleaned

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

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

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

src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp

@@ -1,16 +1,49 @@
-/*
- * Author: Hyon Lim <limhyon@gmail.com, hyonlim@snu.ac.kr>
+/****************************************************************************
+ *
+ *   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.
  *
- * @file attitude_estimator_so3_comp_main.c
+ * 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) {

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 <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;
+}

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 <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);

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

src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_params.c

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

src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_params.h

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

src/modules/attitude_estimator_so3_comp/module.mk

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