From d54d9b618cfe44298d2fbde97644438532a95f94 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sat, 3 Dec 2011 17:56:41 -0800 Subject: [PATCH] Added faster conversion from and to degrees*100 / rads --- libraries/AP_DCM/AP_DCM.cpp | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/libraries/AP_DCM/AP_DCM.cpp b/libraries/AP_DCM/AP_DCM.cpp index 751caa00b2..95a11c5bb4 100644 --- a/libraries/AP_DCM/AP_DCM.cpp +++ b/libraries/AP_DCM/AP_DCM.cpp @@ -1,3 +1,6 @@ + +#define RADX100 0.000174532925 +#define DEGX100 5729.57795 /* APM_DCM_FW.cpp - DCM AHRS Library, fixed wing version, for Ardupilot Mega Code by Doug Weibel, Jordi Muņoz and Jose Julio. DIYDrones.com @@ -274,7 +277,7 @@ AP_DCM::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 - 3 * fabs(1 - accel_magnitude), 0, 1); // upped to (<0.66G = 0.0, 1G = 1.0 , >1.33G = 0.0) + accel_weight = constrain(1 - _clamp * fabs(1 - accel_magnitude), 0, 1); // upped to (<0.66G = 0.0, 1G = 1.0 , >1.33G = 0.0) // We monitor the amount that the accelerometer based drift correction is deweighted for performance reporting _health = constrain(_health+(0.02 * (accel_weight - .5)), 0, 1); @@ -346,7 +349,7 @@ AP_DCM::drift_correction(void) // Here we will place a limit on the integrator so that the integrator cannot ever exceed ~30 degrees/second integrator_magnitude = _omega_I.length(); if (integrator_magnitude > radians(30)) { - _omega_I *= (radians(30) / integrator_magnitude); + _omega_I *= (radians(30) / integrator_magnitude); } //Serial.print("*"); } @@ -379,18 +382,16 @@ AP_DCM::euler_rp(void) { pitch = -asin(_dcm_matrix.c.x); roll = atan2(_dcm_matrix.c.y, _dcm_matrix.c.z); - roll_sensor = degrees(roll) * 100; - pitch_sensor = degrees(pitch) * 100; + roll_sensor = roll * DEGX100; //degrees(roll) * 100; + pitch_sensor = pitch * DEGX100; //degrees(pitch) * 100; } void AP_DCM::euler_yaw(void) { yaw = atan2(_dcm_matrix.b.x, _dcm_matrix.a.x); - yaw_sensor = degrees(yaw) * 100; + yaw_sensor = yaw * DEGX100; //degrees(yaw) * 100; if (yaw_sensor < 0) yaw_sensor += 36000; } - -