diff --git a/libraries/AP_Quaternion/AP_Quaternion.cpp b/libraries/AP_Quaternion/AP_Quaternion.cpp new file mode 100644 index 0000000000..646b44806a --- /dev/null +++ b/libraries/AP_Quaternion/AP_Quaternion.cpp @@ -0,0 +1,296 @@ +/* + AP_Quaternion code, based on quaternion code from Jeb Madgwick + + See http://www.x-io.co.uk/res/doc/madgwick_internal_report.pdf + + adapted to APM by Andrew Tridgell + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later + version. +*/ +#include + +#define ToRad(x) (x*0.01745329252) // *pi/180 +#define ToDeg(x) (x*57.2957795131) // *180/pi + +// this is the speed in cm/s above which we first get a yaw lock with +// the GPS +#define GPS_SPEED_MIN 300 + +// this is the speed in cm/s at which we stop using drift correction +// from the GPS and wait for the ground speed to get above GPS_SPEED_MIN +#define GPS_SPEED_RESET 100 + +void +AP_Quaternion::set_compass(Compass *compass) +{ + _compass = compass; +} + +// Function to compute one quaternion iteration without magnetometer +void AP_Quaternion::update_IMU(float deltat, Vector3f &gyro, Vector3f &accel) +{ + // Local system variables + float norm; // vector norm + float SEqDot_omega_1, SEqDot_omega_2, SEqDot_omega_3, SEqDot_omega_4; // quaternion derrivative from gyroscopes elements + float f_1, f_2, f_3; // objective function elements + float J_11or24, J_12or23, J_13or22, J_14or21, J_32, J_33; // objective function Jacobian elements + float SEqHatDot_1, SEqHatDot_2, SEqHatDot_3, SEqHatDot_4; // estimated direction of the gyroscope error + + // Axulirary variables to avoid reapeated calcualtions + float halfSEq_1 = 0.5f * SEq_1; + float halfSEq_2 = 0.5f * SEq_2; + float halfSEq_3 = 0.5f * SEq_3; + float halfSEq_4 = 0.5f * SEq_4; + float twoSEq_1 = 2.0f * SEq_1; + float twoSEq_2 = 2.0f * SEq_2; + float twoSEq_3 = 2.0f * SEq_3; + + // estimated direction of the gyroscope error (radians) + Vector3f w_err; + + // normalise accelerometer vector + accel.normalize(); + + // Compute the objective function and Jacobian + f_1 = twoSEq_2 * SEq_4 - twoSEq_1 * SEq_3 - accel.x; + f_2 = twoSEq_1 * SEq_2 + twoSEq_3 * SEq_4 - accel.y; + f_3 = 1.0f - twoSEq_2 * SEq_2 - twoSEq_3 * SEq_3 - accel.z; + J_11or24 = twoSEq_3; // J_11 negated in matrix multiplication + J_12or23 = 2.0f * SEq_4; + J_13or22 = twoSEq_1; // J_12 negated in matrix multiplication + J_14or21 = twoSEq_2; + J_32 = 2.0f * J_14or21; // negated in matrix multiplication + J_33 = 2.0f * J_11or24; // negated in matrix multiplication + + // Compute the gradient (matrix multiplication) + SEqHatDot_1 = J_14or21 * f_2 - J_11or24 * f_1; + SEqHatDot_2 = J_12or23 * f_1 + J_13or22 * f_2 - J_32 * f_3; + SEqHatDot_3 = J_12or23 * f_2 - J_33 * f_3 - J_13or22 * f_1; + SEqHatDot_4 = J_14or21 * f_1 + J_11or24 * f_2; + + // Normalise the gradient + norm = 1.0/sqrt(SEqHatDot_1 * SEqHatDot_1 + SEqHatDot_2 * SEqHatDot_2 + SEqHatDot_3 * SEqHatDot_3 + SEqHatDot_4 * SEqHatDot_4); + if (!isinf(norm)) { + SEqHatDot_1 *= norm; + SEqHatDot_2 *= norm; + SEqHatDot_3 *= norm; + SEqHatDot_4 *= norm; + } + + // Compute the quaternion derrivative measured by gyroscopes + SEqDot_omega_1 = -halfSEq_2 * gyro.x - halfSEq_3 * gyro.y - halfSEq_4 * gyro.z; + SEqDot_omega_2 = halfSEq_1 * gyro.x + halfSEq_3 * gyro.z - halfSEq_4 * gyro.y; + SEqDot_omega_3 = halfSEq_1 * gyro.y - halfSEq_2 * gyro.z + halfSEq_4 * gyro.x; + SEqDot_omega_4 = halfSEq_1 * gyro.z + halfSEq_2 * gyro.y - halfSEq_3 * gyro.x; + + // Compute then integrate the estimated quaternion derrivative + SEq_1 += (SEqDot_omega_1 - (beta * SEqHatDot_1)) * deltat; + SEq_2 += (SEqDot_omega_2 - (beta * SEqHatDot_2)) * deltat; + SEq_3 += (SEqDot_omega_3 - (beta * SEqHatDot_3)) * deltat; + SEq_4 += (SEqDot_omega_4 - (beta * SEqHatDot_4)) * deltat; + + // Normalise quaternion + norm = 1.0/sqrt(SEq_1 * SEq_1 + SEq_2 * SEq_2 + SEq_3 * SEq_3 + SEq_4 * SEq_4); + if (!isinf(norm)) { + SEq_1 *= norm; + SEq_2 *= norm; + SEq_3 *= norm; + SEq_4 *= norm; + } +} + + +// Function to compute one quaternion iteration including magnetometer +void AP_Quaternion::update_MARG(float deltat, Vector3f &gyro, Vector3f &accel, Vector3f &mag) +{ + // local system variables + float norm; // vector norm + float SEqDot_omega_1, SEqDot_omega_2, SEqDot_omega_3, SEqDot_omega_4; // quaternion rate from gyroscopes elements + float f_1, f_2, f_3, f_4, f_5, f_6; // objective function elements + float J_11or24, J_12or23, J_13or22, J_14or21, J_32, J_33, // objective function Jacobian elements + J_41, J_42, J_43, J_44, J_51, J_52, J_53, J_54, J_61, J_62, J_63, J_64; // + float SEqHatDot_1, SEqHatDot_2, SEqHatDot_3, SEqHatDot_4; // estimated direction of the gyroscope error + + // computed flux in the earth frame + Vector3f flux; + + // estimated direction of the gyroscope error (radians) + Vector3f w_err; + + // normalise accelerometer vector + accel.normalize(); + + // normalise the magnetometer measurement + mag.normalize(); + + // auxiliary variables to avoid repeated calculations + float halfSEq_1 = 0.5f * SEq_1; + float halfSEq_2 = 0.5f * SEq_2; + float halfSEq_3 = 0.5f * SEq_3; + float halfSEq_4 = 0.5f * SEq_4; + float twoSEq_1 = 2.0f * SEq_1; + float twoSEq_2 = 2.0f * SEq_2; + float twoSEq_3 = 2.0f * SEq_3; + float twoSEq_4 = 2.0f * SEq_4; + float twob_x = 2.0f * b_x; + float twob_z = 2.0f * b_z; + float twob_xSEq_1 = 2.0f * b_x * SEq_1; + float twob_xSEq_2 = 2.0f * b_x * SEq_2; + float twob_xSEq_3 = 2.0f * b_x * SEq_3; + float twob_xSEq_4 = 2.0f * b_x * SEq_4; + float twob_zSEq_1 = 2.0f * b_z * SEq_1; + float twob_zSEq_2 = 2.0f * b_z * SEq_2; + float twob_zSEq_3 = 2.0f * b_z * SEq_3; + float twob_zSEq_4 = 2.0f * b_z * SEq_4; + float SEq_1SEq_2; + float SEq_1SEq_3 = SEq_1 * SEq_3; + float SEq_1SEq_4; + float SEq_2SEq_3; + float SEq_2SEq_4 = SEq_2 * SEq_4; + float SEq_3SEq_4; + Vector3f twom = mag * 2.0; + + // compute the objective function and Jacobian + f_1 = twoSEq_2 * SEq_4 - twoSEq_1 * SEq_3 - accel.x; + f_2 = twoSEq_1 * SEq_2 + twoSEq_3 * SEq_4 - accel.y; + f_3 = 1.0f - twoSEq_2 * SEq_2 - twoSEq_3 * SEq_3 - accel.z; + f_4 = twob_x * (0.5f - SEq_3 * SEq_3 - SEq_4 * SEq_4) + twob_z * (SEq_2SEq_4 - SEq_1SEq_3) - mag.x; + f_5 = twob_x * (SEq_2 * SEq_3 - SEq_1 * SEq_4) + twob_z * (SEq_1 * SEq_2 + SEq_3 * SEq_4) - mag.y; + f_6 = twob_x * (SEq_1SEq_3 + SEq_2SEq_4) + twob_z * (0.5f - SEq_2 * SEq_2 - SEq_3 * SEq_3) - mag.z; + J_11or24 = twoSEq_3; // J_11 negated in matrix multiplication + J_12or23 = 2.0f * SEq_4; + J_13or22 = twoSEq_1; // J_12 negated in matrix multiplication + J_14or21 = twoSEq_2; + J_32 = 2.0f * J_14or21; // negated in matrix multiplication + J_33 = 2.0f * J_11or24; // negated in matrix multiplication + J_41 = twob_zSEq_3; // negated in matrix multiplication + J_42 = twob_zSEq_4; + J_43 = 2.0f * twob_xSEq_3 + twob_zSEq_1; // negated in matrix multiplication + J_44 = 2.0f * twob_xSEq_4 - twob_zSEq_2; // negated in matrix multiplication + J_51 = twob_xSEq_4 - twob_zSEq_2; // negated in matrix multiplication + J_52 = twob_xSEq_3 + twob_zSEq_1; + J_53 = twob_xSEq_2 + twob_zSEq_4; + J_54 = twob_xSEq_1 - twob_zSEq_3; // negated in matrix multiplication + J_61 = twob_xSEq_3; + J_62 = twob_xSEq_4 - 2.0f * twob_zSEq_2; + J_63 = twob_xSEq_1 - 2.0f * twob_zSEq_3; + J_64 = twob_xSEq_2; + + // compute the gradient (matrix multiplication) + SEqHatDot_1 = J_14or21 * f_2 - J_11or24 * f_1 - J_41 * f_4 - J_51 * f_5 + J_61 * f_6; + SEqHatDot_2 = J_12or23 * f_1 + J_13or22 * f_2 - J_32 * f_3 + J_42 * f_4 + J_52 * f_5 + J_62 * f_6; + SEqHatDot_3 = J_12or23 * f_2 - J_33 * f_3 - J_13or22 * f_1 - J_43 * f_4 + J_53 * f_5 + J_63 * f_6; + SEqHatDot_4 = J_14or21 * f_1 + J_11or24 * f_2 - J_44 * f_4 - J_54 * f_5 + J_64 * f_6; + + // normalise the gradient to estimate direction of the gyroscope error + norm = 1.0 / sqrt(SEqHatDot_1 * SEqHatDot_1 + SEqHatDot_2 * SEqHatDot_2 + SEqHatDot_3 * SEqHatDot_3 + SEqHatDot_4 * SEqHatDot_4); + SEqHatDot_1 *= norm; + SEqHatDot_2 *= norm; + SEqHatDot_3 *= norm; + SEqHatDot_4 *= norm; + + // compute angular estimated direction of the gyroscope error + w_err.x = twoSEq_1 * SEqHatDot_2 - twoSEq_2 * SEqHatDot_1 - twoSEq_3 * SEqHatDot_4 + twoSEq_4 * SEqHatDot_3; + w_err.y = twoSEq_1 * SEqHatDot_3 + twoSEq_2 * SEqHatDot_4 - twoSEq_3 * SEqHatDot_1 - twoSEq_4 * SEqHatDot_2; + w_err.z = twoSEq_1 * SEqHatDot_4 - twoSEq_2 * SEqHatDot_3 + twoSEq_3 * SEqHatDot_2 - twoSEq_4 * SEqHatDot_1; + + // compute and remove the gyroscope baises + gyro_bias += w_err * (deltat * zeta); + gyro -= gyro_bias; + + // compute the quaternion rate measured by gyroscopes + SEqDot_omega_1 = -halfSEq_2 * gyro.x - halfSEq_3 * gyro.y - halfSEq_4 * gyro.z; + SEqDot_omega_2 = halfSEq_1 * gyro.x + halfSEq_3 * gyro.z - halfSEq_4 * gyro.y; + SEqDot_omega_3 = halfSEq_1 * gyro.y - halfSEq_2 * gyro.z + halfSEq_4 * gyro.x; + SEqDot_omega_4 = halfSEq_1 * gyro.z + halfSEq_2 * gyro.y - halfSEq_3 * gyro.x; + + // compute then integrate the estimated quaternion rate + SEq_1 += (SEqDot_omega_1 - (beta * SEqHatDot_1)) * deltat; + SEq_2 += (SEqDot_omega_2 - (beta * SEqHatDot_2)) * deltat; + SEq_3 += (SEqDot_omega_3 - (beta * SEqHatDot_3)) * deltat; + SEq_4 += (SEqDot_omega_4 - (beta * SEqHatDot_4)) * deltat; + + // normalise quaternion + norm = 1.0/sqrt(SEq_1 * SEq_1 + SEq_2 * SEq_2 + SEq_3 * SEq_3 + SEq_4 * SEq_4); + if (!isinf(norm)) { + SEq_1 *= norm; + SEq_2 *= norm; + SEq_3 *= norm; + SEq_4 *= norm; + } + + // compute flux in the earth frame + // recompute axulirary variables + SEq_1SEq_2 = SEq_1 * SEq_2; + SEq_1SEq_3 = SEq_1 * SEq_3; + SEq_1SEq_4 = SEq_1 * SEq_4; + SEq_3SEq_4 = SEq_3 * SEq_4; + SEq_2SEq_3 = SEq_2 * SEq_3; + SEq_2SEq_4 = SEq_2 * SEq_4; + flux.x = twom.x * (0.5f - SEq_3 * SEq_3 - SEq_4 * SEq_4) + twom.y * (SEq_2SEq_3 - SEq_1SEq_4) + twom.z * (SEq_2SEq_4 + SEq_1SEq_3); + flux.y = twom.x * (SEq_2SEq_3 + SEq_1SEq_4) + twom.y * (0.5f - SEq_2 * SEq_2 - SEq_4 * SEq_4) + twom.z * (SEq_3SEq_4 - SEq_1SEq_2); + flux.z = twom.x * (SEq_2SEq_4 - SEq_1SEq_3) + twom.y * (SEq_3SEq_4 + SEq_1SEq_2) + twom.z * (0.5f - SEq_2 * SEq_2 - SEq_3 * SEq_3); + + // normalise the flux vector to have only components in the x and z + b_x = sqrt((flux.x * flux.x) + (flux.y * flux.y)); + b_z = flux.z; +} + + +// Function to compute one quaternion iteration +void AP_Quaternion::update(float deltat) +{ + Vector3f gyro, accel; + + _imu->update(); + + // get current IMU state + gyro = _imu->get_gyro(); + gyro.x = -gyro.x; + gyro.y = -gyro.y; + + accel = _imu->get_accel(); + accel.z = -accel.z; + + if (_compass == NULL) { + update_IMU(deltat, gyro, accel); + } else { + Vector3f mag = Vector3f(_compass->mag_x, _compass->mag_y, - _compass->mag_z); + update_MARG(deltat, gyro, accel, mag); + } + + // compute the Eulers + float test = (SEq_1*SEq_3 - SEq_4*SEq_2); + const float singularity = 0.499; // 86.3 degrees? + if (test > singularity) { + // singularity at south pole + // this one is ok.. + yaw = 2.0 * atan2(SEq_4, SEq_1); + pitch = ToRad(-90.0); + roll = 0.0; + } else if (test < -singularity) { + // singularity at north pole + // this one is invalid :( .. fix it. + yaw = -2.0 * atan2(SEq_4, SEq_1); + pitch = ToRad(90.0); + roll = 0.0; + } else { + roll = -(atan2(2.0*(SEq_1*SEq_2 + SEq_3*SEq_4), + 1 - 2.0*(SEq_2*SEq_2 + SEq_3*SEq_3))); + pitch = -safe_asin(2.0*test); + yaw = atan2(2.0*(SEq_1*SEq_4 + SEq_2*SEq_3), + 1 - 2.0*(SEq_3*SEq_3 + SEq_4*SEq_4)); + } + + // and integer Eulers + roll_sensor = 100 * ToDeg(roll); + pitch_sensor = 100 * ToDeg(pitch); + yaw_sensor = 100 * ToDeg(yaw); + if (yaw_sensor < 0) { + yaw_sensor += 36000; + } +} diff --git a/libraries/AP_Quaternion/AP_Quaternion.h b/libraries/AP_Quaternion/AP_Quaternion.h new file mode 100644 index 0000000000..4d3cb03356 --- /dev/null +++ b/libraries/AP_Quaternion/AP_Quaternion.h @@ -0,0 +1,98 @@ +#ifndef AP_Quaternion_h +#define AP_Quaternion_h + +#include +#include +#include +#include +#include + +#if defined(ARDUINO) && ARDUINO >= 100 + #include "Arduino.h" +#else + #include "WProgram.h" +#endif + +class AP_Quaternion +{ +public: + // Constructor + AP_Quaternion(IMU *imu, GPS *&gps, Compass *compass = NULL) : + _imu(imu), + _gps(gps), + _compass(compass) + { + // initial quaternion + SEq_1 = 1; + SEq_2 = 0; + SEq_3 = 0; + SEq_4 = 0; + + // reference direction of flux in earth frame + b_x = 0; + b_z = -1; + + // scaled gyro drift limits + beta = sqrt(3.0f / 4.0f) * gyroMeasError; + zeta = sqrt(3.0f / 4.0f) * gyroMeasDrift; + } + + // Accessors + void set_centripetal(bool b) {_centripetal = b;} + bool get_centripetal(void) {return _centripetal;} + void set_compass(Compass *compass); + + // Methods + void update(float deltat); + + // Euler angles (radians) + float roll; + float pitch; + float yaw; + + // integer Euler angles (Degrees * 100) + int32_t roll_sensor; + int32_t pitch_sensor; + int32_t yaw_sensor; + + +private: + void update_IMU(float deltat, Vector3f &gyro, Vector3f &accel); + void update_MARG(float deltat, Vector3f &gyro, Vector3f &accel, Vector3f &mag); + + bool _have_initial_yaw; + + // Methods + void accel_adjust(void); + + // members + Compass * _compass; + + // note: we use ref-to-pointer here so that our caller can change the GPS without our noticing + // IMU under us without our noticing. + GPS *&_gps; + IMU *_imu; + + // true if we are doing centripetal acceleration correction + bool _centripetal; + + // maximum gyroscope measurement error in rad/s (set to 5 degrees/second) + static const float gyroMeasError = 5.0 * (M_PI/180.0); + + // maximum gyroscope drift rate in radians/s/s (set to 0.2 degrees/s/s) + static const float gyroMeasDrift = 0.2 * (PI/180.0); + + float beta; + float zeta; + + // quaternion elements + float SEq_1, SEq_2, SEq_3, SEq_4; + + float b_x; + float b_z; + + // estimate gyroscope biases error + Vector3f gyro_bias; +}; + +#endif diff --git a/libraries/AP_Quaternion/examples/Quaternion/Makefile b/libraries/AP_Quaternion/examples/Quaternion/Makefile new file mode 100644 index 0000000000..fcdc8ff8fe --- /dev/null +++ b/libraries/AP_Quaternion/examples/Quaternion/Makefile @@ -0,0 +1,4 @@ +include ../../../AP_Common/Arduino.mk + +sitl: + make -f ../../../../libraries/Desktop/Desktop.mk diff --git a/libraries/AP_Quaternion/examples/Quaternion/Quaternion.pde b/libraries/AP_Quaternion/examples/Quaternion/Quaternion.pde new file mode 100644 index 0000000000..e5a5157db1 --- /dev/null +++ b/libraries/AP_Quaternion/examples/Quaternion/Quaternion.pde @@ -0,0 +1,149 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- + +// +// Simple test for the AP_Quaternion library +// + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// uncomment this for a APM2 board +//#define APM2_HARDWARE + +FastSerialPort(Serial, 0); + +Arduino_Mega_ISR_Registry isr_registry; +AP_TimerProcess scheduler; + +#ifdef DESKTOP_BUILD +AP_Compass_HIL compass; +#else +AP_Compass_HMC5843 compass; +#endif + +#ifdef APM2_HARDWARE + AP_InertialSensor_MPU6000 ins( 53 ); +# else + AP_ADC_ADS7844 adc; + AP_InertialSensor_Oilpan ins( &adc ); +#endif // CONFIG_IMU_TYPE + +static GPS *g_gps; + +AP_IMU_INS imu(&ins); +AP_Quaternion quaternion(&imu, g_gps); + +AP_Baro_BMP085_HIL barometer; + + +#ifdef APM2_HARDWARE +# define A_LED_PIN 27 +# define C_LED_PIN 25 +# define LED_ON LOW +# define LED_OFF HIGH +# define MAG_ORIENTATION AP_COMPASS_APM2_SHIELD +#else +# define A_LED_PIN 37 +# define C_LED_PIN 35 +# define LED_ON HIGH +# define LED_OFF LOW +# define MAG_ORIENTATION AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD +#endif + + +static void flash_leds(bool on) +{ + digitalWrite(A_LED_PIN, on?LED_OFF:LED_ON); + digitalWrite(C_LED_PIN, on?LED_ON:LED_OFF); +} + +void setup(void) +{ + Serial.begin(115200); + Serial.println("Starting up..."); + +#ifndef DESKTOP_BUILD + I2c.begin(); + I2c.timeOut(5); + I2c.setSpeed(true); +#endif + + SPI.begin(); + SPI.setClockDivider(SPI_CLOCK_DIV16); + +#ifdef APM2_HARDWARE + // we need to stop the barometer from holding the SPI bus + pinMode(40, OUTPUT); + digitalWrite(40, HIGH); +#endif + + isr_registry.init(); + scheduler.init(&isr_registry); + + imu.init(IMU::COLD_START, delay, flash_leds, &scheduler); + imu.init_accel(delay, flash_leds); + + compass.set_orientation(MAG_ORIENTATION); + if (compass.init()) { + Serial.printf("Enabling compass\n"); + compass.null_offsets_enable(); + quaternion.set_compass(&compass); + } +} + +void loop(void) +{ + static uint16_t counter; + static uint32_t last_t, last_print, last_compass; + uint32_t now = micros(); + float deltat; + static uint32_t compass_reads; + static uint32_t compass_time; + + if (last_t == 0) { + last_t = now; + return; + } + deltat = (now - last_t) * 1.0e-6; + last_t = now; + + if (now - last_compass > 100*1000UL) { + // read compass at 10Hz + compass.read(); + last_compass = now; + } + + quaternion.update(deltat); + counter++; + + if (now - last_print >= 0.5e6) { + Vector3f accel = imu.get_accel(); + Vector3f gyro = imu.get_gyro(); + Serial.printf_P(PSTR("r:%4.1f p:%4.1f y:%4.1f g=(%5.1f %5.1f %5.1f) a=(%5.1f %5.1f %5.1f) rate=%.1f\n"), + ToDeg(quaternion.roll), + ToDeg(quaternion.pitch), + ToDeg(quaternion.yaw), + gyro.x, gyro.y, gyro.z, + accel.x, accel.y, accel.z, + (1.0e6*counter)/(now-last_print)); + last_print = now; + counter = 0; + compass_reads=0; + compass_time = 0; + } +}