From e3ab1b03534adc008fd944d8c8c0e336143e49f5 Mon Sep 17 00:00:00 2001 From: Doug Weibel Date: Sat, 5 Nov 2011 09:02:23 -0600 Subject: [PATCH] Add an accessor for the DCM drift correction integrator to monitor performance Also made the get_health accessor an inline function. --- libraries/AP_DCM/AP_DCM.cpp | 8 -------- libraries/AP_DCM/AP_DCM.h | 6 +++--- 2 files changed, 3 insertions(+), 11 deletions(-) diff --git a/libraries/AP_DCM/AP_DCM.cpp b/libraries/AP_DCM/AP_DCM.cpp index 2d881e5866..751caa00b2 100644 --- a/libraries/AP_DCM/AP_DCM.cpp +++ b/libraries/AP_DCM/AP_DCM.cpp @@ -393,12 +393,4 @@ AP_DCM::euler_yaw(void) yaw_sensor += 36000; } -/**************************************************/ - -float -AP_DCM::get_health(void) -{ - return _health; -} - diff --git a/libraries/AP_DCM/AP_DCM.h b/libraries/AP_DCM/AP_DCM.h index 503f2ab1fa..afec0fcd45 100644 --- a/libraries/AP_DCM/AP_DCM.h +++ b/libraries/AP_DCM/AP_DCM.h @@ -43,7 +43,9 @@ public: 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();} - + Vector3f get_integrator(void) {return _omega_I; } // We return the current drift correction integrator values + + float get_health(void) {return _health;} void set_centripetal(bool b) {_centripetal = b;} bool get_centripetal(void) {return _centripetal;} void set_compass(Compass *compass); @@ -52,8 +54,6 @@ public: void update_DCM(void); void update_DCM_fast(void); - float get_health(void); - long roll_sensor; // Degrees * 100 long pitch_sensor; // Degrees * 100 long yaw_sensor; // Degrees * 100