diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index 1145de92b6..8375e51d1f 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/libraries/AP_AHRS/AP_AHRS.h @@ -62,6 +62,9 @@ public: return _ins; } + // accelerometer values in the earth frame in m/s/s + Vector3f get_accel_ef(void) { return _accel_ef; } + // Methods virtual void update(void) = 0; @@ -192,6 +195,9 @@ protected: // radians/s/s float _gyro_drift_limit; + // accelerometer values in the earth frame in m/s/s + Vector3f _accel_ef; + // acceleration due to gravity in m/s/s static const float _gravity = 9.80665; diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp index 56eaf796a3..f7d33049a1 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp +++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp @@ -393,8 +393,11 @@ AP_AHRS_DCM::drift_correction(float deltat) // apply trim temp_dcm.rotate(_trim); + // rotate accelerometer values into the earth frame + _accel_ef = temp_dcm * _accel_vector; + // integrate the accel vector in the earth frame between GPS readings - _ra_sum += temp_dcm * (_accel_vector * deltat); + _ra_sum += _accel_ef * deltat; // keep a sum of the deltat values, so we know how much time // we have integrated over diff --git a/libraries/AP_AHRS/AP_AHRS_MPU6000.cpp b/libraries/AP_AHRS/AP_AHRS_MPU6000.cpp index 3b7202d759..5f1c55543f 100644 --- a/libraries/AP_AHRS/AP_AHRS_MPU6000.cpp +++ b/libraries/AP_AHRS/AP_AHRS_MPU6000.cpp @@ -66,6 +66,9 @@ AP_AHRS_MPU6000::update(void) // Calculate pitch, roll, yaw for stabilization and navigation euler_angles(); + + // prepare earth frame accelerometer values for ArduCopter Inertial Navigation and accel-based throttle + _accel_ef = _dcm_matrix * _ins->get_accel(); } // wrap_PI - ensure an angle (expressed in radians) is between -PI and PI