From 126065e95ceaf76f01bfec04142212840997f4c7 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 5 Nov 2018 12:52:20 +1100 Subject: [PATCH] AP_Mount: trim out dead getDebug method --- libraries/AP_Mount/SoloGimbalEKF.cpp | 13 ------------- libraries/AP_Mount/SoloGimbalEKF.h | 3 --- 2 files changed, 16 deletions(-) diff --git a/libraries/AP_Mount/SoloGimbalEKF.cpp b/libraries/AP_Mount/SoloGimbalEKF.cpp index c12a5bf6ae..ebdb37ff36 100644 --- a/libraries/AP_Mount/SoloGimbalEKF.cpp +++ b/libraries/AP_Mount/SoloGimbalEKF.cpp @@ -930,19 +930,6 @@ void SoloGimbalEKF::fixCovariance() } } -// return data for debugging EKF -void SoloGimbalEKF::getDebug(float &tilt, Vector3f &velocity, Vector3f &euler, Vector3f &gyroBias) const -{ - tilt = TiltCorrection; - velocity = state.velocity; - state.quat.to_euler(euler.x, euler.y, euler.z); - if (dtIMU < 1.0e-6f) { - gyroBias.zero(); - } else { - gyroBias = state.delAngBias / dtIMU; - } -} - // get gyro bias data void SoloGimbalEKF::getGyroBias(Vector3f &gyroBias) const { diff --git a/libraries/AP_Mount/SoloGimbalEKF.h b/libraries/AP_Mount/SoloGimbalEKF.h index 1f50f00692..b7033ef6b4 100644 --- a/libraries/AP_Mount/SoloGimbalEKF.h +++ b/libraries/AP_Mount/SoloGimbalEKF.h @@ -82,9 +82,6 @@ public: void reset(); - // get some debug information - void getDebug(float &tilt, Vector3f &velocity, Vector3f &euler, Vector3f &gyroBias) const; - // get gyro bias data void getGyroBias(Vector3f &gyroBias) const;