From 2b6fbe5c0192f6722ac015bf05deca7f6a51b884 Mon Sep 17 00:00:00 2001 From: rmackay9 Date: Wed, 12 Dec 2012 16:22:56 +0900 Subject: [PATCH] AP_AHRS: add get_accel_ef method to return earth frame accelerometer values for use in ArduCopter's inertial nav and accel based throttle --- libraries/AP_AHRS/AP_AHRS.h | 6 ++++++ libraries/AP_AHRS/AP_AHRS_DCM.cpp | 5 ++++- libraries/AP_AHRS/AP_AHRS_MPU6000.cpp | 3 +++ 3 files changed, 13 insertions(+), 1 deletion(-) diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index c971dc8f40..d8a3376a7e 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 a39780a99e..1d2e95201e 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp +++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp @@ -396,8 +396,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 bfbd954df5..4bbead7c22 100644 --- a/libraries/AP_AHRS/AP_AHRS_MPU6000.cpp +++ b/libraries/AP_AHRS/AP_AHRS_MPU6000.cpp @@ -70,6 +70,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