Browse Source

Use fabs() for floating point values, not abs(). The latter causes truncation (cast to int) and generates a lot more code.

git-svn-id: https://arducopter.googlecode.com/svn/trunk@956 f9c3cf11-9bcb-44bc-f272-b75c42450872
mission-4.1.18
DrZiplok@gmail.com 14 years ago
parent
commit
d159986be4
  1. 8
      libraries/AP_DCM/AP_DCM_FW.cpp

8
libraries/AP_DCM/AP_DCM_FW.cpp

@ -142,9 +142,9 @@ AP_DCM_FW::matrix_update(float _G_Dt) @@ -142,9 +142,9 @@ AP_DCM_FW::matrix_update(float _G_Dt)
Matrix3f temp_matrix;
//Record when you saturate any of the gyros.
if((abs(_gyro_vector.x) >= radians(300)) ||
(abs(_gyro_vector.y) >= radians(300)) ||
(abs(_gyro_vector.z) >= radians(300)))
if((fabs(_gyro_vector.x) >= radians(300)) ||
(fabs(_gyro_vector.y) >= radians(300)) ||
(fabs(_gyro_vector.z) >= radians(300)))
gyro_sat_count++;
_omega_integ_corr = _gyro_vector + _omega_I; // Used for centrep correction (theoretically better than _omega)
@ -280,7 +280,7 @@ AP_DCM_FW::drift_correction(void) @@ -280,7 +280,7 @@ AP_DCM_FW::drift_correction(void)
// Dynamic weighting of accelerometer info (reliability filter)
// Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0)
accel_weight = constrain(1 - 2 * abs(1 - accel_magnitude), 0, 1); //
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
imu_health = imu_health + 0.02 * (accel_weight-.5);

Loading…
Cancel
Save