diff --git a/libraries/AP_DCM/AP_DCM.cpp b/libraries/AP_DCM/AP_DCM.cpp index f038b3237b..acf62f130a 100644 --- a/libraries/AP_DCM/AP_DCM.cpp +++ b/libraries/AP_DCM/AP_DCM.cpp @@ -52,33 +52,6 @@ AP_DCM::update_DCM(float _G_Dt) } /**************************************************/ -Vector3f -AP_DCM::get_gyro(void) -{ - return _omega_integ_corr; -} // We return the raw gyro vector corrected for bias - -/**************************************************/ -Vector3f -AP_DCM::get_accel(void) -{ - return _accel_vector; -} - -/**************************************************/ -Matrix3f -AP_DCM::get_dcm_matrix(void) -{ - return _dcm_matrix; -} - -/**************************************************/ -Matrix3f -AP_DCM::get_dcm_transposed(void) -{ - return _dcm_matrix.transpose(); -} - //For Debugging /* diff --git a/libraries/AP_DCM/AP_DCM.h b/libraries/AP_DCM/AP_DCM.h index e760bc8392..e14f4375fc 100644 --- a/libraries/AP_DCM/AP_DCM.h +++ b/libraries/AP_DCM/AP_DCM.h @@ -44,10 +44,10 @@ public: {} // Accessors - Vector3f get_gyro(void); - Vector3f get_accel(void); - Matrix3f get_dcm_matrix(void); - Matrix3f get_dcm_transposed(void); + Vector3f get_gyro(void) {return _omega_integ_corr; } // We return the raw gyro vector corrected for bias + Vector3f get_accel(void) { return _accel_vector; } + Matrix3f get_dcm_matrix(void) {return _dcm_matrix; } + Matrix3f get_dcm_transposed(void) {Matrix3f temp = _dcm_matrix; return temp.transpose();} void set_centripetal(bool b); void set_compass(Compass *compass); diff --git a/libraries/AP_DCM/AP_DCM_HIL.h b/libraries/AP_DCM/AP_DCM_HIL.h index 1795eae2bf..1fce1c1f4f 100644 --- a/libraries/AP_DCM/AP_DCM_HIL.h +++ b/libraries/AP_DCM/AP_DCM_HIL.h @@ -25,6 +25,7 @@ public: Vector3f get_gyro(void) {return _omega_integ_corr; } Vector3f get_accel(void) { return _accel_vector; } Matrix3f get_dcm_matrix(void) {return _dcm_matrix; } + Matrix3f get_dcm_transposed(void) {Matrix3f temp = _dcm_matrix; return temp.transpose();} void set_centripetal(bool b) {} void set_compass(Compass *compass) {}