diff options
Diffstat (limited to 'src/modules/attitude_estimator_so3_comp')
5 files changed, 0 insertions, 763 deletions
diff --git a/src/modules/attitude_estimator_so3_comp/README b/src/modules/attitude_estimator_so3_comp/README deleted file mode 100644 index 02ab66ee4..000000000 --- a/src/modules/attitude_estimator_so3_comp/README +++ /dev/null @@ -1,3 +0,0 @@ -Synopsis - - nsh> attitude_estimator_so3_comp start
\ No newline at end of file diff --git a/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp b/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp deleted file mode 100755 index e12c0e16a..000000000 --- a/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp +++ /dev/null @@ -1,645 +0,0 @@ -/* - * Author: Hyon Lim <limhyon@gmail.com, hyonlim@snu.ac.kr> - * - * @file attitude_estimator_so3_comp_main.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 <nuttx/config.h> -#include <unistd.h> -#include <stdlib.h> -#include <string.h> -#include <stdio.h> -#include <stdbool.h> -#include <poll.h> -#include <fcntl.h> -#include <float.h> -#include <nuttx/sched.h> -#include <sys/prctl.h> -#include <termios.h> -#include <errno.h> -#include <limits.h> -#include <math.h> -#include <uORB/uORB.h> -#include <uORB/topics/debug_key_value.h> -#include <uORB/topics/sensor_combined.h> -#include <uORB/topics/vehicle_attitude.h> -#include <uORB/topics/vehicle_control_mode.h> -#include <uORB/topics/parameter_update.h> -#include <drivers/drv_hrt.h> - -#include <systemlib/systemlib.h> -#include <systemlib/perf_counter.h> -#include <systemlib/err.h> - -#ifdef __cplusplus -extern "C" { -#endif -#include "attitude_estimator_so3_comp_params.h" -#ifdef __cplusplus -} -#endif - -extern "C" __EXPORT int attitude_estimator_so3_comp_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 */ - -//! 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 dq0 = 0.0f, dq1 = 0.0f, dq2 = 0.0f, dq3 = 0.0f; /** quaternion of sensor frame relative to auxiliary frame */ -static float gyro_bias[3] = {0.0f, 0.0f, 0.0f}; /** bias estimation */ -static float q0q0, q0q1, q0q2, q0q3; -static float q1q1, q1q2, q1q3; -static float q2q2, q2q3; -static float q3q3; -static bool bFilterInit = false; - -/** - * Mainloop of attitude_estimator_so3_comp. - */ -int attitude_estimator_so3_comp_thread_main(int argc, char *argv[]); - -/** - * Print the correct usage. - */ -static void usage(const char *reason); - -/* Function prototypes */ -float invSqrt(float number); -void NonlinearSO3AHRSinit(float ax, float ay, float az, float mx, float my, float mz); -void NonlinearSO3AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float twoKp, float twoKi, float dt); - -static void -usage(const char *reason) -{ - if (reason) - fprintf(stderr, "%s\n", reason); - - fprintf(stderr, "usage: attitude_estimator_so3_comp {start|stop|status}\n"); - exit(1); -} - -/** - * The attitude_estimator_so3_comp 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[]) -{ - if (argc < 1) - usage("missing command"); - - if (!strcmp(argv[1], "start")) { - - if (thread_running) { - warnx("already running\n"); - /* this is not an error */ - exit(0); - } - - thread_should_exit = false; - attitude_estimator_so3_comp_task = task_spawn_cmd("attitude_estimator_so3_comp", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 14000, - attitude_estimator_so3_comp_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); - exit(0); - } - - if (!strcmp(argv[1], "stop")) { - thread_should_exit = true; - - while (thread_running){ - usleep(200000); - } - - warnx("stopped"); - exit(0); - } - - if (!strcmp(argv[1], "status")) { - if (thread_running) { - warnx("running"); - exit(0); - - } else { - warnx("not started"); - exit(1); - } - - exit(0); - } - - usage("unrecognized command"); - exit(1); -} - -//--------------------------------------------------------------------------------------------------- -// Fast inverse square-root -// See: http://en.wikipedia.org/wiki/Fast_inverse_square_root -float invSqrt(float number) -{ - volatile long i; - volatile float x, y; - volatile const float f = 1.5F; - - x = number * 0.5F; - y = number; - i = * (( long * ) &y); - i = 0x5f375a86 - ( i >> 1 ); - y = * (( float * ) &i); - y = y * ( f - ( x * y * y ) ); - return y; -} - -//! Using accelerometer, sense the gravity vector. -//! Using magnetometer, sense yaw. -void NonlinearSO3AHRSinit(float ax, float ay, float az, float mx, float my, float mz) -{ - float initialRoll, initialPitch; - float cosRoll, sinRoll, cosPitch, sinPitch; - float magX, magY; - float initialHdg, cosHeading, sinHeading; - - initialRoll = atan2(-ay, -az); - initialPitch = atan2(ax, -az); - - cosRoll = cosf(initialRoll); - sinRoll = sinf(initialRoll); - cosPitch = cosf(initialPitch); - sinPitch = sinf(initialPitch); - - magX = mx * cosPitch + my * sinRoll * sinPitch + mz * cosRoll * sinPitch; - - magY = my * cosRoll - mz * sinRoll; - - initialHdg = atan2f(-magY, magX); - - cosRoll = cosf(initialRoll * 0.5f); - sinRoll = sinf(initialRoll * 0.5f); - - cosPitch = cosf(initialPitch * 0.5f); - sinPitch = sinf(initialPitch * 0.5f); - - cosHeading = cosf(initialHdg * 0.5f); - sinHeading = sinf(initialHdg * 0.5f); - - q0 = cosRoll * cosPitch * cosHeading + sinRoll * sinPitch * sinHeading; - q1 = sinRoll * cosPitch * cosHeading - cosRoll * sinPitch * sinHeading; - q2 = cosRoll * sinPitch * cosHeading + sinRoll * cosPitch * sinHeading; - q3 = cosRoll * cosPitch * sinHeading - sinRoll * sinPitch * cosHeading; - - // auxillary variables to reduce number of repeated operations, for 1st pass - q0q0 = q0 * q0; - q0q1 = q0 * q1; - q0q2 = q0 * q2; - q0q3 = q0 * q3; - q1q1 = q1 * q1; - q1q2 = q1 * q2; - q1q3 = q1 * q3; - q2q2 = q2 * q2; - q2q3 = q2 * q3; - q3q3 = q3 * q3; -} - -void NonlinearSO3AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float twoKp, float twoKi, float dt) -{ - float recipNorm; - float halfex = 0.0f, halfey = 0.0f, halfez = 0.0f; - - // Make filter converge to initial solution faster - // This function assumes you are in static position. - // WARNING : in case air reboot, this can cause problem. But this is very unlikely happen. - if(bFilterInit == false) { - NonlinearSO3AHRSinit(ax,ay,az,mx,my,mz); - bFilterInit = true; - } - - //! If magnetometer measurement is available, use it. - if(!((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f))) { - float hx, hy, hz, bx, bz; - float halfwx, halfwy, halfwz; - - // Normalise magnetometer measurement - // Will sqrt work better? PX4 system is powerful enough? - recipNorm = invSqrt(mx * mx + my * my + mz * mz); - mx *= recipNorm; - my *= recipNorm; - mz *= recipNorm; - - // Reference direction of Earth's magnetic field - hx = 2.0f * (mx * (0.5f - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + q0q2)); - hy = 2.0f * (mx * (q1q2 + q0q3) + my * (0.5f - q1q1 - q3q3) + mz * (q2q3 - q0q1)); - hz = 2.0f * mx * (q1q3 - q0q2) + 2.0f * my * (q2q3 + q0q1) + 2.0f * mz * (0.5f - q1q1 - q2q2); - bx = sqrt(hx * hx + hy * hy); - bz = hz; - - // Estimated direction of magnetic field - halfwx = bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2); - halfwy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3); - halfwz = bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2); - - // Error is sum of cross product between estimated direction and measured direction of field vectors - halfex += (my * halfwz - mz * halfwy); - halfey += (mz * halfwx - mx * halfwz); - halfez += (mx * halfwy - my * halfwx); - } - - // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) - if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { - float halfvx, halfvy, halfvz; - - // Normalise accelerometer measurement - recipNorm = invSqrt(ax * ax + ay * ay + az * az); - ax *= recipNorm; - ay *= recipNorm; - az *= recipNorm; - - // Estimated direction of gravity and magnetic field - halfvx = q1q3 - q0q2; - halfvy = q0q1 + q2q3; - halfvz = q0q0 - 0.5f + q3q3; - - // Error is sum of cross product between estimated direction and measured direction of field vectors - halfex += ay * halfvz - az * halfvy; - halfey += az * halfvx - ax * halfvz; - halfez += ax * halfvy - ay * halfvx; - } - - // Apply feedback only when valid data has been gathered from the accelerometer or magnetometer - if(halfex != 0.0f && halfey != 0.0f && halfez != 0.0f) { - // Compute and apply integral feedback if enabled - if(twoKi > 0.0f) { - gyro_bias[0] += twoKi * halfex * dt; // integral error scaled by Ki - gyro_bias[1] += twoKi * halfey * dt; - gyro_bias[2] += twoKi * halfez * dt; - - // apply integral feedback - gx += gyro_bias[0]; - gy += gyro_bias[1]; - gz += gyro_bias[2]; - } - else { - gyro_bias[0] = 0.0f; // prevent integral windup - gyro_bias[1] = 0.0f; - gyro_bias[2] = 0.0f; - } - - // Apply proportional feedback - gx += twoKp * halfex; - gy += twoKp * halfey; - gz += twoKp * halfez; - } - - //! Integrate rate of change of quaternion -#if 0 - gx *= (0.5f * dt); // pre-multiply common factors - gy *= (0.5f * dt); - gz *= (0.5f * dt); -#endif - - // Time derivative of quaternion. q_dot = 0.5*q\otimes omega. - //! q_k = q_{k-1} + dt*\dot{q} - //! \dot{q} = 0.5*q \otimes P(\omega) - dq0 = 0.5f*(-q1 * gx - q2 * gy - q3 * gz); - dq1 = 0.5f*(q0 * gx + q2 * gz - q3 * gy); - dq2 = 0.5f*(q0 * gy - q1 * gz + q3 * gx); - dq3 = 0.5f*(q0 * gz + q1 * gy - q2 * gx); - - q0 += dt*dq0; - q1 += dt*dq1; - q2 += dt*dq2; - q3 += dt*dq3; - - // Normalise quaternion - recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); - q0 *= recipNorm; - q1 *= recipNorm; - q2 *= recipNorm; - q3 *= recipNorm; - - // Auxiliary variables to avoid repeated arithmetic - q0q0 = q0 * q0; - q0q1 = q0 * q1; - q0q2 = q0 * q2; - q0q3 = q0 * q3; - q1q1 = q1 * q1; - q1q2 = q1 * q2; - q1q3 = q1 * q3; - q2q2 = q2 * q2; - q2q3 = q2 * q3; - q3q3 = q3 * q3; -} - -/* - * Nonliner complementary filter on SO(3), attitude estimator main function. - * - * Estimates the attitude once started. - * - * @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[]) -{ - const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds - - //! Time constant - float dt = 0.005f; - - /* output euler angles */ - float euler[3] = {0.0f, 0.0f, 0.0f}; - - /* Initialization */ - float Rot_matrix[9] = {1.f, 0.0f, 0.0f, 0.0f, 1.f, 0.0f, 0.0f, 0.0f, 1.f }; /**< init: identity matrix */ - float acc[3] = {0.0f, 0.0f, 0.0f}; - float gyro[3] = {0.0f, 0.0f, 0.0f}; - float mag[3] = {0.0f, 0.0f, 0.0f}; - - warnx("main thread started"); - - struct sensor_combined_s raw; - memset(&raw, 0, sizeof(raw)); - - //! Initialize attitude vehicle uORB message. - struct vehicle_attitude_s att; - memset(&att, 0, sizeof(att)); - - struct vehicle_control_mode_s control_mode; - memset(&control_mode, 0, sizeof(control_mode)); - - uint64_t last_data = 0; - uint64_t last_measurement = 0; - - /* subscribe to raw data */ - int sub_raw = orb_subscribe(ORB_ID(sensor_combined)); - /* rate-limit raw data updates to 333 Hz (sensors app publishes at 200, so this is just paranoid) */ - orb_set_interval(sub_raw, 3); - - /* subscribe to param changes */ - int sub_params = orb_subscribe(ORB_ID(parameter_update)); - - /* subscribe to control mode */ - int sub_control_mode = orb_subscribe(ORB_ID(vehicle_control_mode)); - - /* advertise attitude */ - //orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att); - //orb_advert_t att_pub = -1; - orb_advert_t att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att); - - int loopcounter = 0; - int printcounter = 0; - - thread_running = true; - - float sensor_update_hz[3] = {0.0f, 0.0f, 0.0f}; - // XXX write this out to perf regs - - /* keep track of sensor updates */ - 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; - - /* initialize parameter handles */ - parameters_init(&so3_comp_param_handles); - parameters_update(&so3_comp_param_handles, &so3_comp_params); - - uint64_t start_time = hrt_absolute_time(); - bool initialized = false; - bool state_initialized = false; - - float gyro_offsets[3] = { 0.0f, 0.0f, 0.0f }; - unsigned offset_count = 0; - - /* register the perf counter */ - perf_counter_t so3_comp_loop_perf = perf_alloc(PC_ELAPSED, "attitude_estimator_so3_comp"); - - /* Main loop*/ - while (!thread_should_exit) { - - struct pollfd fds[2]; - fds[0].fd = sub_raw; - fds[0].events = POLLIN; - fds[1].fd = sub_params; - fds[1].events = POLLIN; - int ret = poll(fds, 2, 1000); - - if (ret < 0) { - /* XXX this is seriously bad - should be an emergency */ - } else if (ret == 0) { - /* check if we're in HIL - not getting sensor data is fine then */ - orb_copy(ORB_ID(vehicle_control_mode), sub_control_mode, &control_mode); - - if (!control_mode.flag_system_hil_enabled) { - warnx("WARNING: Not getting sensors - sensor app running?"); - } - } else { - /* only update parameters if they changed */ - if (fds[1].revents & POLLIN) { - /* read from param to clear updated flag */ - struct parameter_update_s update; - orb_copy(ORB_ID(parameter_update), sub_params, &update); - - /* update parameters */ - parameters_update(&so3_comp_param_handles, &so3_comp_params); - } - - /* only run filter if sensor values changed */ - if (fds[0].revents & POLLIN) { - - /* get latest measurements */ - orb_copy(ORB_ID(sensor_combined), sub_raw, &raw); - - if (!initialized) { - - gyro_offsets[0] += raw.gyro_rad_s[0]; - gyro_offsets[1] += raw.gyro_rad_s[1]; - gyro_offsets[2] += raw.gyro_rad_s[2]; - offset_count++; - - if (hrt_absolute_time() > start_time + 3000000l) { - initialized = true; - gyro_offsets[0] /= offset_count; - gyro_offsets[1] /= offset_count; - gyro_offsets[2] /= offset_count; - warnx("gyro initialized, offsets: %.5f %.5f %.5f", gyro_offsets[0], gyro_offsets[1], gyro_offsets[2]); - } - - } else { - - perf_begin(so3_comp_loop_perf); - - /* Calculate data time difference in seconds */ - dt = (raw.timestamp - last_measurement) / 1000000.0f; - last_measurement = raw.timestamp; - uint8_t update_vect[3] = {0, 0, 0}; - - /* Fill in gyro measurements */ - if (sensor_last_count[0] != raw.gyro_counter) { - update_vect[0] = 1; - sensor_last_count[0] = raw.gyro_counter; - sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]); - sensor_last_timestamp[0] = raw.timestamp; - } - - gyro[0] = raw.gyro_rad_s[0] - gyro_offsets[0]; - gyro[1] = raw.gyro_rad_s[1] - gyro_offsets[1]; - gyro[2] = raw.gyro_rad_s[2] - gyro_offsets[2]; - - /* update accelerometer measurements */ - if (sensor_last_count[1] != raw.accelerometer_counter) { - update_vect[1] = 1; - sensor_last_count[1] = raw.accelerometer_counter; - sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]); - sensor_last_timestamp[1] = raw.timestamp; - } - - acc[0] = raw.accelerometer_m_s2[0]; - acc[1] = raw.accelerometer_m_s2[1]; - acc[2] = raw.accelerometer_m_s2[2]; - - /* update magnetometer measurements */ - if (sensor_last_count[2] != raw.magnetometer_counter) { - update_vect[2] = 1; - sensor_last_count[2] = raw.magnetometer_counter; - sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]); - sensor_last_timestamp[2] = raw.timestamp; - } - - mag[0] = raw.magnetometer_ga[0]; - mag[1] = raw.magnetometer_ga[1]; - mag[2] = raw.magnetometer_ga[2]; - - /* initialize with good values once we have a reasonable dt estimate */ - if (!state_initialized && dt < 0.05f && dt > 0.001f) { - state_initialized = true; - warnx("state initialized"); - } - - /* do not execute the filter if not initialized */ - if (!state_initialized) { - continue; - } - - uint64_t timing_start = hrt_absolute_time(); - - // NOTE : Accelerometer is reversed. - // Because proper mount of PX4 will give you a reversed accelerometer readings. - NonlinearSO3AHRSupdate(gyro[0], gyro[1], gyro[2], - -acc[0], -acc[1], -acc[2], - mag[0], mag[1], mag[2], - so3_comp_params.Kp, - so3_comp_params.Ki, - dt); - - // Convert q->R, This R converts inertial frame to body frame. - Rot_matrix[0] = q0q0 + q1q1 - q2q2 - q3q3;// 11 - Rot_matrix[1] = 2.f * (q1*q2 + q0*q3); // 12 - Rot_matrix[2] = 2.f * (q1*q3 - q0*q2); // 13 - Rot_matrix[3] = 2.f * (q1*q2 - q0*q3); // 21 - Rot_matrix[4] = q0q0 - q1q1 + q2q2 - q3q3;// 22 - Rot_matrix[5] = 2.f * (q2*q3 + q0*q1); // 23 - Rot_matrix[6] = 2.f * (q1*q3 + q0*q2); // 31 - Rot_matrix[7] = 2.f * (q2*q3 - q0*q1); // 32 - Rot_matrix[8] = q0q0 - q1q1 - q2q2 + q3q3;// 33 - - //1-2-3 Representation. - //Equation (290) - //Representing Attitude: Euler Angles, Unit Quaternions, and Rotation Vectors, James Diebel. - // Existing PX4 EKF code was generated by MATLAB which uses coloum major order matrix. - euler[0] = atan2f(Rot_matrix[5], Rot_matrix[8]); //! Roll - euler[1] = -asinf(Rot_matrix[2]); //! Pitch - euler[2] = atan2f(Rot_matrix[1], Rot_matrix[0]); //! Yaw - - /* swap values for next iteration, check for fatal inputs */ - if (isfinite(euler[0]) && isfinite(euler[1]) && isfinite(euler[2])) { - // Publish only finite euler angles - att.roll = euler[0] - so3_comp_params.roll_off; - att.pitch = euler[1] - so3_comp_params.pitch_off; - att.yaw = euler[2] - so3_comp_params.yaw_off; - } else { - /* due to inputs or numerical failure the output is invalid, skip it */ - // Due to inputs or numerical failure the output is invalid - warnx("infinite euler angles, rotation matrix:"); - warnx("%.3f %.3f %.3f", Rot_matrix[0], Rot_matrix[1], Rot_matrix[2]); - warnx("%.3f %.3f %.3f", Rot_matrix[3], Rot_matrix[4], Rot_matrix[5]); - warnx("%.3f %.3f %.3f", Rot_matrix[6], Rot_matrix[7], Rot_matrix[8]); - // Don't publish anything - continue; - } - - if (last_data > 0 && raw.timestamp > last_data + 12000) { - warnx("sensor data missed"); - } - - last_data = raw.timestamp; - - /* send out */ - att.timestamp = raw.timestamp; - - // Quaternion - att.q[0] = q0; - att.q[1] = q1; - att.q[2] = q2; - att.q[3] = q3; - att.q_valid = true; - - // Euler angle rate. But it needs to be investigated again. - /* - att.rollspeed = 2.0f*(-q1*dq0 + q0*dq1 - q3*dq2 + q2*dq3); - att.pitchspeed = 2.0f*(-q2*dq0 + q3*dq1 + q0*dq2 - q1*dq3); - att.yawspeed = 2.0f*(-q3*dq0 -q2*dq1 + q1*dq2 + q0*dq3); - */ - att.rollspeed = gyro[0]; - att.pitchspeed = gyro[1]; - att.yawspeed = gyro[2]; - - att.rollacc = 0; - att.pitchacc = 0; - att.yawacc = 0; - - /* TODO: Bias estimation required */ - memcpy(&att.rate_offsets, &(gyro_bias), sizeof(att.rate_offsets)); - - /* copy rotation matrix */ - memcpy(&att.R, Rot_matrix, sizeof(float)*9); - att.R_valid = true; - - // Publish - if (att_pub > 0) { - orb_publish(ORB_ID(vehicle_attitude), att_pub, &att); - } else { - warnx("NaN in roll/pitch/yaw estimate!"); - orb_advertise(ORB_ID(vehicle_attitude), &att); - } - - perf_end(so3_comp_loop_perf); - } - } - } - - loopcounter++; - } - - thread_running = false; - - return 0; -} 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 f962515df..000000000 --- a/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_params.c +++ /dev/null @@ -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; -} 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 f00695630..000000000 --- a/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_params.h +++ /dev/null @@ -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); 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 92f43d920..000000000 --- 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 |