From 0642e1144d08ee1cc4989fd8ad4a5c82595709a3 Mon Sep 17 00:00:00 2001 From: "deweibel@gmail.com" Date: Fri, 25 Feb 2011 20:09:00 +0000 Subject: [PATCH] Fix imu_health metric git-svn-id: https://arducopter.googlecode.com/svn/trunk@1728 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- libraries/AP_DCM/AP_DCM.cpp | 2 +- libraries/AP_DCM/AP_DCM.h | 3 ++- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/libraries/AP_DCM/AP_DCM.cpp b/libraries/AP_DCM/AP_DCM.cpp index 76cede5225..8c370d05d7 100644 --- a/libraries/AP_DCM/AP_DCM.cpp +++ b/libraries/AP_DCM/AP_DCM.cpp @@ -228,7 +228,7 @@ AP_DCM::drift_correction(void) accel_weight = constrain(1 - 2 * fabs(1 - accel_magnitude), 0, 1); // // We monitor the amount that the accelerometer based drift correction is deweighted for performance reporting - _health += constrain((0.02 * (accel_weight - .5)), 0, 1); + _health = constrain(_health+(0.02 * (accel_weight - .5)), 0, 1); // adjust the ground of reference _error_roll_pitch = _dcm_matrix.c % _accel_vector; // Equation 27 *** sign changed from prev implementation??? diff --git a/libraries/AP_DCM/AP_DCM.h b/libraries/AP_DCM/AP_DCM.h index 88749404c9..b267d6a783 100644 --- a/libraries/AP_DCM/AP_DCM.h +++ b/libraries/AP_DCM/AP_DCM.h @@ -29,7 +29,8 @@ public: 0, 1, 0, 0, 0, 1), _course_over_ground_x(0), - _course_over_ground_y(1) + _course_over_ground_y(1), + _health(1.) {} // Accessors