Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-23 09:08:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

Added faster conversion from and to degrees*100 / rads

Browse Source
This commit is contained in:
Jason Short 2011-12-03 17:56:41 -08:00
parent 50afa16f31
commit d54d9b618c
1 changed files with 8 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
libraries/AP_DCM/AP_DCM.cpp
View File

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