diff --git a/libraries/AP_DCM/AP_DCM_FW.cpp b/libraries/AP_DCM/AP_DCM_FW.cpp index 214a22c86f..bf8d678e7b 100644 --- a/libraries/AP_DCM/AP_DCM_FW.cpp +++ b/libraries/AP_DCM/AP_DCM_FW.cpp @@ -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); diff --git a/libraries/AP_IMU/AP_IMU.cpp b/libraries/AP_IMU/AP_IMU.cpp index 94d65a1735..b5e79591ba 100644 --- a/libraries/AP_IMU/AP_IMU.cpp +++ b/libraries/AP_IMU/AP_IMU.cpp @@ -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 @@ -74,20 +74,18 @@ AP_IMU::init(void) digitalWrite(A_LED_PIN, HIGH); 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,30 +104,39 @@ 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_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] = (_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(_adc_in[i], -ADC_CONSTRAINT, ADC_CONSTRAINT); // Throw out nonsensical values } } - + _gyro_vector.x = ToRad(_gyro_gain_x) * _adc_in[0]; _gyro_vector.y = ToRad(_gyro_gain_y) * _adc_in[1]; _gyro_vector.z = ToRad(_gyro_gain_z) * _adc_in[2]; @@ -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 } }