Added faster conversion from and to degrees*100 / rads

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;
 }
-
-