diff --git a/libraries/AP_AHRS/AP_AHRS_Quaternion.cpp b/libraries/AP_AHRS/AP_AHRS_Quaternion.cpp index 25f82d303a..7986763c6e 100644 --- a/libraries/AP_AHRS/AP_AHRS_Quaternion.cpp +++ b/libraries/AP_AHRS/AP_AHRS_Quaternion.cpp @@ -1,5 +1,5 @@ /* - AP_Quaternion code, based on quaternion code from Jeb Madgwick + AP_AHRS_Quaternion code, based on quaternion code from Jeb Madgwick See http://www.x-io.co.uk/res/doc/madgwick_internal_report.pdf @@ -13,24 +13,7 @@ version. */ #include -#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; -} +#include // to keep the code as close to the original as possible, we use these // macros for quaternion access @@ -40,7 +23,7 @@ AP_Quaternion::set_compass(Compass *compass) #define SEq_4 q.q4 // Function to compute one quaternion iteration without magnetometer -void AP_Quaternion::update_IMU(float deltat, Vector3f &gyro, Vector3f &accel) +void AP_AHRS_Quaternion::update_IMU(float deltat, Vector3f &gyro, Vector3f &accel) { // Local system variables float norm; // vector norm @@ -134,7 +117,7 @@ void AP_Quaternion::update_IMU(float deltat, Vector3f &gyro, Vector3f &accel) // Function to compute one quaternion iteration including magnetometer -void AP_Quaternion::update_MARG(float deltat, Vector3f &gyro, Vector3f &accel, Vector3f &mag) +void AP_AHRS_Quaternion::update_MARG(float deltat, Vector3f &gyro, Vector3f &accel, Vector3f &mag) { // local system variables float norm; // vector norm @@ -253,7 +236,7 @@ void AP_Quaternion::update_MARG(float deltat, Vector3f &gyro, Vector3f &accel, V // don't allow the drift rate to be exceeded. This prevents a // sudden drift change coming from a outage in the compass - float max_change = gyroMeasDrift * deltat; + float max_change = _gyro_drift_limit * deltat; drift_delta.x = constrain(drift_delta.x, -max_change, max_change); drift_delta.y = constrain(drift_delta.y, -max_change, max_change); drift_delta.z = constrain(drift_delta.z, -max_change, max_change); @@ -308,161 +291,8 @@ void AP_Quaternion::update_MARG(float deltat, Vector3f &gyro, Vector3f &accel, V } -// Function to update our gyro_bias drift estimate using the accelerometer -// and magnetometer. This is a cut-down version of update_MARG(), but -// without the quaternion update. -void AP_Quaternion::update_drift(float deltat, Vector3f &mag) -{ - // local system variables - float norm; // vector norm - float 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 the magnetometer measurement - mag.normalize(); - if (mag.is_inf()) { - // discard this data point - renorm_range_count++; - return; - } - - // auxiliary variables to avoid repeated calculations - 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_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_41 * f_4 - J_51 * f_5 + J_61 * f_6; - SEqHatDot_2 = + J_42 * f_4 + J_52 * f_5 + J_62 * f_6; - SEqHatDot_3 = - J_43 * f_4 + J_53 * f_5 + J_63 * f_6; - SEqHatDot_4 = - 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 / safe_sqrt(SEqHatDot_1 * SEqHatDot_1 + SEqHatDot_2 * SEqHatDot_2 + SEqHatDot_3 * SEqHatDot_3 + SEqHatDot_4 * SEqHatDot_4); - if (isinf(norm)) { - // discard this data point - renorm_range_count++; - return; - } - 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; - - // keep track of the error rates - _error_rp_sum += 0.5*(fabs(w_err.x) + fabs(w_err.y)); - _error_yaw_sum += fabs(w_err.z); - _error_rp_count++; - _error_yaw_count++; - - // compute the gyroscope bias delta - Vector3f drift_delta = w_err * (deltat * zeta); - - // don't allow the drift rate to be exceeded. This prevents a - // sudden drift change coming from a outage in the compass - float max_change = gyroMeasDrift * deltat; - drift_delta.x = constrain(drift_delta.x, -max_change, max_change); - drift_delta.y = constrain(drift_delta.y, -max_change, max_change); - drift_delta.z = constrain(drift_delta.z, -max_change, max_change); - gyro_bias += drift_delta; - - // compute then integrate the estimated quaternion rate - SEq_1 -= (beta * SEqHatDot_1) * deltat; - SEq_2 -= (beta * SEqHatDot_2) * deltat; - SEq_3 -= (beta * SEqHatDot_3) * deltat; - SEq_4 -= (beta * SEqHatDot_4) * deltat; - - // normalise quaternion - norm = 1.0/safe_sqrt(SEq_1 * SEq_1 + SEq_2 * SEq_2 + SEq_3 * SEq_3 + SEq_4 * SEq_4); - if (isinf(norm)) { - // our quaternion is bad! Reset based on roll/pitch/yaw - // and hope for the best ... - renorm_blowup_count++; - _compass->null_offsets_disable(); - q.from_euler(roll, pitch, yaw); - _compass->null_offsets_disable(); - return; - } - 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(void) +void AP_AHRS_Quaternion::update(void) { Vector3f gyro, accel; float deltat; @@ -518,42 +348,15 @@ void AP_Quaternion::update(void) accel.z += (gyro.y - gyro_bias.y) * veloc; } -#define SEPARATE_DRIFT 0 -#if SEPARATE_DRIFT - /* - The full Madgwick quaternion 'MARG' system assumes you get - gyro, accel and magnetometer updates at the same rate. On - APM we get them at very different rates, and re-calculating - our drift due to the magnetometer in the fast loop is very - wasteful of CPU. - - Instead, we only update the gyro_bias vector when we get a - new magnetometer point, and use the much simpler - update_IMU() as the main quaternion update function. - */ - _gyro_sum += gyro; - _accel_sum += accel; - _sum_count++; - - if (_compass != NULL && _compass->use_for_yaw() && - _compass->last_update != _compass_last_update && - _sum_count != 0) { - // use new compass sample for drift correction - float mag_deltat = 1.0e-6*(_compass->last_update - _compass_last_update); + if (_compass != NULL && _compass->use_for_yaw()) { Vector3f mag = Vector3f(_compass->mag_x, _compass->mag_y, - _compass->mag_z); - _sum_count = 0; - update_drift(mag_deltat, mag); - _compass_last_update = _compass->last_update; + update_MARG(deltat, gyro, accel, mag); + } else { + // step the quaternion solution using just gyros and accels + gyro -= gyro_bias; + update_IMU(deltat, gyro, accel); } - // step the quaternion solution using just gyros and accels - gyro -= gyro_bias; - update_IMU(deltat, gyro, accel); -#else - Vector3f mag = Vector3f(_compass->mag_x, _compass->mag_y, - _compass->mag_z); - update_MARG(deltat, gyro, accel, mag); -#endif - #ifdef DESKTOP_BUILD if (q.is_nan()) { SITL_debug("QUAT NAN: deltat=%f roll=%f pitch=%f yaw=%f q=[%f %f %f %f] a=[%f %f %f] g=(%f %f %f)\n", @@ -587,7 +390,7 @@ void AP_Quaternion::update(void) } // average error in roll/pitch since last call -float AP_Quaternion::get_error_rp(void) +float AP_AHRS_Quaternion::get_error_rp(void) { float ret; if (_error_rp_count == 0) { @@ -600,7 +403,7 @@ float AP_Quaternion::get_error_rp(void) } // average error in yaw since last call -float AP_Quaternion::get_error_yaw(void) +float AP_AHRS_Quaternion::get_error_yaw(void) { float ret; if (_error_yaw_count == 0) { @@ -611,3 +414,18 @@ float AP_Quaternion::get_error_yaw(void) _error_yaw_count = 0; return ret; } + +// reset attitude system +void AP_AHRS_Quaternion::reset(bool recover_eulers) +{ + if (recover_eulers) { + q.from_euler(roll, pitch, yaw); + } else { + q(1, 0, 0, 0); + } + gyro_bias.zero(); + + // reference direction of flux in earth frame + b_x = 0; + b_z = -1; +} diff --git a/libraries/AP_AHRS/AP_AHRS_Quaternion.h b/libraries/AP_AHRS/AP_AHRS_Quaternion.h index 4d8b061765..921d925155 100644 --- a/libraries/AP_AHRS/AP_AHRS_Quaternion.h +++ b/libraries/AP_AHRS/AP_AHRS_Quaternion.h @@ -13,66 +13,41 @@ #include "WProgram.h" #endif -class AP_Quaternion +class AP_AHRS_Quaternion : public AP_AHRS { public: // Constructor - AP_Quaternion(IMU *imu, GPS *&gps) : - _imu(imu), - _gps(gps) + AP_AHRS_Quaternion(IMU *imu, GPS *&gps) : AP_AHRS(imu, gps) { - // reference direction of flux in earth frame - b_x = 0; - b_z = -1; - - // limit the drift to the drift rate reported by the - // sensor driver - gyroMeasDrift = imu->get_gyro_drift_rate(); - // scaled gyro drift limits beta = sqrt(3.0f / 4.0f) * gyroMeasError; - zeta = sqrt(3.0f / 4.0f) * gyroMeasDrift; - } + zeta = sqrt(3.0f / 4.0f) * _gyro_drift_limit; - // Accessors - void set_centripetal(bool b) {_centripetal = b;} - bool get_centripetal(void) {return _centripetal;} - void set_compass(Compass *compass); + // reset attitude + reset(); + } // Methods void update(void); + void reset(bool recover_eulers=false); - // 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; - - - // compatibility methods with DCM - void update_DCM(void) { update(); } - void update_DCM_fast(void) { update(); } + // get corrected gyro vector Vector3f get_gyro(void) { // notice the sign reversals here return Vector3f(-_gyro_corrected.x, -_gyro_corrected.y, _gyro_corrected.z); } + Vector3f get_gyro_drift(void) { - // notice the sign reversals here - return Vector3f(-gyro_bias.x, -gyro_bias.y, gyro_bias.z); + // notice the sign reversals here. The quaternion + // system uses a -ve gyro bias, DCM uses a +ve + return Vector3f(gyro_bias.x, gyro_bias.y, -gyro_bias.z); } - float get_accel_weight(void) { return 0; } - float get_renorm_val(void) { return 0; } - float get_health(void) { return 0; } - void matrix_reset(void) { } - uint8_t gyro_sat_count; - uint8_t renorm_range_count; - uint8_t renorm_blowup_count; + float get_error_rp(void); float get_error_yaw(void); + + // convert quaternion to a DCM matrix, used by compass + // null offsets code Matrix3f get_dcm_matrix(void) { Matrix3f ret; q.rotation_matrix(ret); @@ -82,31 +57,16 @@ public: private: void update_IMU(float deltat, Vector3f &gyro, Vector3f &accel); void update_MARG(float deltat, Vector3f &gyro, Vector3f &accel, Vector3f &mag); - void update_drift(float deltat, Vector3f &mag); bool _have_initial_yaw; // Methods void accel_adjust(void); - // members - Compass * _compass; - // time in microseconds of last compass update - uint32_t _compass_last_update; - - // 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 7 degrees/second) static const float gyroMeasError = 20.0 * (M_PI/180.0); - float gyroMeasDrift; - + // scaled tuning constants float beta; float zeta; @@ -124,11 +84,6 @@ private: // the current corrected gyro vector Vector3f _gyro_corrected; - // accel and gyro accumulators for drift correction - Vector3f _gyro_sum; - Vector3f _accel_sum; - uint32_t _sum_count; - // estimate of error float _error_rp_sum; uint16_t _error_rp_count;