Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

fix bugs from factoring imu object. 

Tested & working - integration and roll/pitch drift correction
Untested - yaw drift correction and centrepetal comp

git-svn-id: https://arducopter.googlecode.com/svn/trunk@729 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
deweibel 2010-10-25 02:03:52 +00:00
parent 6272955567
commit a80eb66e77
2 changed files with 37 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/AP_DCM/AP_DCM_FW.cpp
View File

@ -7,8 +7,8 @@
#define ToRad(x) (x*0.01745329252) // *pi/180
#define ToDeg(x) (x*57.2957795131) // *180/pi
#define Kp_ROLLPITCH 0.0014 // .0014 Pitch&Roll Drift Correction Proportional Gain
#define Ki_ROLLPITCH 0.0000003 // 0.0000003 Pitch&Roll Drift Correction Integrator Gain
#define Kp_ROLLPITCH 0.5852 // .0014 * 418 Pitch&Roll Drift Correction Proportional Gain
#define Ki_ROLLPITCH 0.0001254 // 0.0000003 * 418 Pitch&Roll Drift Correction Integrator Gain
#define Kp_YAW 0.8 // Yaw Drift Correction Porportional Gain
#define Ki_YAW 0.00004 // Yaw Drift CorrectionIntegrator Gain
@ -154,7 +154,7 @@ AP_DCM_FW::_accel_adjust(void)
Vector3f _veloc, _temp;
float _vel;
_veloc.x = _gps->ground_speed / 100;
_veloc.x = (_gps->ground_speed / 100) / 9.81; // We are working with acceleration in g units
//_accel_vector += _omega_integ_corr % _veloc; // Equation 26 This line is giving the compiler a problem so we break it up below
_temp.y = _omega_integ_corr.z * _veloc.x; // only computing the non-zero terms
@ -253,9 +253,9 @@ AP_DCM_FW::drift_correction(void)
// error_roll_pitch are in Accel ADC units
// Limit max error_roll_pitch to limit max omega_P and omega_I
_error_roll_pitch.x = constrain(_error_roll_pitch.x, -50, 50);
_error_roll_pitch.y = constrain(_error_roll_pitch.y, -50, 50);
_error_roll_pitch.z = constrain(_error_roll_pitch.z, -50, 50);
_error_roll_pitch.x = constrain(_error_roll_pitch.x, -.1196f, .1196f);
_error_roll_pitch.y = constrain(_error_roll_pitch.y, -.1196f, .1196f);
_error_roll_pitch.z = constrain(_error_roll_pitch.z, -.1196f, .1196f);
_omega_P = _error_roll_pitch * (Kp_ROLLPITCH * accel_weight);
_omega_I += _error_roll_pitch * (Ki_ROLLPITCH * accel_weight);

53
libraries/AP_IMU/AP_IMU.cpp
View File

@ -8,7 +8,7 @@
// ADXL335 Sensitivity(from datasheet) => 330mV/g, 0.8mV/ADC step => 330/0.8 = 412
// Tested value : 418
#define GRAVITY 418 //this equivalent to 1G in the raw data coming from the accelerometer
#define accel_scale(x) x*(9.81/GRAVITY)//Scaling the raw data of the accel to actual acceleration in meters per second squared
#define accel_scale(x) (x/GRAVITY)//Scaling the raw data of the accel to actual acceleration in meters per second squared
#define ToRad(x) (x*0.01745329252) // *pi/180
#define ToDeg(x) (x*57.2957795131) // *180/pi
@ -75,19 +75,17 @@ AP_IMU::init(void)
delay(20);
}
for(int y = 0; y <= 5; y++){ // Read first initial ADC values for offset.
_adc_offset[y] = _adc_in[y];
}
for(int i = 0; i < 200; i++){ // We take some readings...
for (int j = 0; j < 6; j++) {
_adc_in[j] = APM_ADC.Ch(_sensors[j]);
_adc_in[j] -= _gyro_temp_curve[j][tc_temp]; // Subtract temp compensated typical gyro bias
if (_sensor_signs[j] < 0)
temp = (_adc_offset[j] - _adc_in[j]);
else
temp = (_adc_in[j] - _adc_offset[j]);
_adc_offset[j] = _adc_offset[j] * 0.9 + temp * 0.1;
if (j < 3) {
_adc_in[j] -= _gyro_temp_comp(j, tc_temp); // Subtract temp compensated typical gyro bias
} else {
_adc_in[j] -= 2025;
}
_adc_offset[j] = _adc_offset[j] * 0.9 + _adc_in[j] * 0.1;
}
delay(20);
@ -106,27 +104,36 @@ AP_IMU::init(void)
_adc_offset[5] += GRAVITY * _sensor_signs[5];
// NOTE *** Need to addd code to save values to EEPROM
}
/**************************************************/
// Returns the temperature compensated raw gyro value
//---------------------------------------------------
float
AP_IMU::_gyro_temp_comp(int i, int temp) const
{
// We use a 2nd order curve of the form Gtc = A + B * Graw + C * (Graw)**2
//------------------------------------------------------------------------
return _gyro_temp_curve[i][0] + _gyro_temp_curve[i][1] * temp + _gyro_temp_curve[i][2] * temp * temp;
}
/**************************************************/
Vector3f
AP_IMU::get_gyro(void)
{
float temp;
int tc_temp = APM_ADC.Ch(_gyro_temp_ch);
for (int i = 0; i < 3; i++) {
_adc_in[i] = APM_ADC.Ch(_sensors[i]);
_adc_in[i] -= _gyro_temp_curve[i][tc_temp]; // Subtract temp compensated typical gyro bias
_adc_in[i] -= _gyro_temp_comp(i,tc_temp); // Subtract temp compensated typical gyro bias
if (_sensor_signs[i] < 0)
temp = (_adc_offset[i] - _adc_in[i]);
_adc_in[i] = (_adc_offset[i] - _adc_in[i]);
else
temp = (_adc_in[i] - _adc_offset[i]);
if (abs(temp) > ADC_CONSTRAINT) {
_adc_in[i] = (_adc_in[i] - _adc_offset[i]);
if (abs(_adc_in[i]) > ADC_CONSTRAINT) {
adc_constraints++; // We keep track of the number of times
_adc_in[i] = constrain(temp, -ADC_CONSTRAINT, ADC_CONSTRAINT); // Throw out nonsensical values
_adc_in[i] = constrain(_adc_in[i], -ADC_CONSTRAINT, ADC_CONSTRAINT); // Throw out nonsensical values
}
}
@ -141,18 +148,16 @@ AP_IMU::get_gyro(void)
Vector3f
AP_IMU::get_accel(void)
{
float temp;
for (int i = 3; i < 6; i++) {
_adc_in[i] = APM_ADC.Ch(_sensors[i]);
_adc_in[i] -= 2025; // Subtract typical accel bias
if (_sensor_signs[i] < 0)
temp = (_adc_offset[i] - _adc_in[i]);
_adc_in[i] = (_adc_offset[i] - _adc_in[i]);
else
temp = (_adc_in[i] - _adc_offset[i]);
if (abs(temp) > ADC_CONSTRAINT) {
_adc_in[i] = (_adc_in[i] - _adc_offset[i]);
if (abs(_adc_in[i]) > ADC_CONSTRAINT) {
adc_constraints++; // We keep track of the number of times
_adc_in[i] = constrain(temp, -ADC_CONSTRAINT, ADC_CONSTRAINT); // Throw out nonsensical values
_adc_in[i] = constrain(_adc_in[i], -ADC_CONSTRAINT, ADC_CONSTRAINT); // Throw out nonsensical values
}
}