/**************************************************************************** * * Copyright (c) 2013 PX4 Development Team. All rights reserved. * * 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 * * @author Hyon Lim * @author Anton Babushkin * * 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 #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #ifdef __cplusplus extern "C" { #endif #include "attitude_estimator_so3_params.h" #ifdef __cplusplus } #endif 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_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. */ int attitude_estimator_so3_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 {start|stop|status}\n"); exit(1); } /** * 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_spawn_cmd(). */ int attitude_estimator_so3_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_task = task_spawn_cmd("attitude_estimator_so3", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 14000, attitude_estimator_so3_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_thread_main(int argc, char *argv[]) { //! 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; thread_running = true; // XXX write this out to perf regs /* keep track of sensor updates */ uint64_t sensor_last_timestamp[3] = {0, 0, 0}; 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); 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"); /* 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", (double)gyro_offsets[0], (double)gyro_offsets[1], (double)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; /* Fill in gyro measurements */ if (sensor_last_timestamp[0] != raw.timestamp) { 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_timestamp[1] != raw.accelerometer_timestamp) { sensor_last_timestamp[1] = raw.accelerometer_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_timestamp[2] != raw.magnetometer_timestamp) { sensor_last_timestamp[2] = raw.magnetometer_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; } // 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", (double)Rot_matrix[0], (double)Rot_matrix[1], (double)Rot_matrix[2]); warnx("%.3f %.3f %.3f", (double)Rot_matrix[3], (double)Rot_matrix[4], (double)Rot_matrix[5]); warnx("%.3f %.3f %.3f", (double)Rot_matrix[6], (double)Rot_matrix[7], (double)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; }