diff --git a/libraries/AP_Quaternion/AP_Quaternion.cpp b/libraries/AP_Quaternion/AP_Quaternion.cpp index 1b4346657f..6c9455714e 100644 --- a/libraries/AP_Quaternion/AP_Quaternion.cpp +++ b/libraries/AP_Quaternion/AP_Quaternion.cpp @@ -269,27 +269,30 @@ void AP_Quaternion::update(void) gyro = _imu->get_gyro(); accel = _imu->get_accel(); - // keep a smoothed gyro vector for centripetal and reporting - // to mainline code - _gyro_smoothed = (_gyro_smoothed * 0.95) + (gyro * 0.05); - - // Quaternion code uses opposite x and y gyro sense + // Quaternion code uses opposite x and y gyro sense from the + // rest of APM gyro.x = -gyro.x; gyro.y = -gyro.y; + // Quaternion code uses opposite z accel as well + accel.z = -accel.z; if (_centripetal && _gps && _gps->status() == GPS::GPS_OK) { + // compensate for linear acceleration, limited to 1g + float acceleration = _gps->acceleration(); + acceleration = constrain(acceleration, 0, 9.8); + accel.x -= acceleration; + // compensate for centripetal acceleration float veloc; veloc = _gps->ground_speed / 100; - accel.x -= _gps->acceleration(); - accel.y -= _gyro_smoothed.z * veloc; - accel.z += _gyro_smoothed.y * veloc; + // be careful of the signs in this calculation. the + // quaternion system uses different signs than the + // rest of APM + accel.y -= (gyro.z - gyro_bias.z) * veloc; + accel.z += (gyro.y - gyro_bias.y) * veloc; } - // Quaternion code uses opposite z accel - accel.z = -accel.z; - if (_compass == NULL) { update_IMU(deltat, gyro, accel); } else { @@ -298,6 +301,9 @@ void AP_Quaternion::update(void) update_MARG(deltat, gyro, accel, mag); } + // keep the corrected gyro for reporting + _gyro_corrected = gyro; + // compute the Eulers float test = (SEq_1*SEq_3 - SEq_4*SEq_2); const float singularity = 0.499; // 86.3 degrees? diff --git a/libraries/AP_Quaternion/AP_Quaternion.h b/libraries/AP_Quaternion/AP_Quaternion.h index 4550a62dd2..543c37f579 100644 --- a/libraries/AP_Quaternion/AP_Quaternion.h +++ b/libraries/AP_Quaternion/AP_Quaternion.h @@ -59,8 +59,14 @@ public: // compatibility methods with DCM void update_DCM(void) { update(); } void update_DCM_fast(void) { update(); } - Vector3f get_gyro(void) { return _gyro_smoothed; } - Vector3f get_integrator(void) { return gyro_bias; } + Vector3f get_gyro(void) { + // notice the sign reversals here + return Vector3f(-_gyro_corrected.x, -_gyro_corrected.y, _gyro_corrected.z); + } + Vector3f get_integrator(void) { + // notice the sign reversals here + 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; } @@ -91,10 +97,11 @@ private: // 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 measurement error in rad/s (set to 10 degrees/second) + static const float gyroMeasError = 10.0 * (M_PI/180.0); - // maximum gyroscope drift rate in radians/s/s (set to 0.02 degrees/s/s) + // maximum gyroscope drift rate in radians/s/s (set to 0.02 + // degrees/s/s, which is 1.2 degrees/s/minute) static const float gyroMeasDrift = 0.02 * (PI/180.0); float beta; @@ -109,8 +116,8 @@ private: // estimate gyroscope biases error Vector3f gyro_bias; - // smoothed gyro estimate - Vector3f _gyro_smoothed; + // the current corrected gyro vector + Vector3f _gyro_corrected; // estimate of error float _error_rp_sum;