Vector3f AP_IMU::get_gyro(void) { 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_comp(i,tc_temp); // Subtract temp compensated typical gyro bias if (_sensor_signs[i] < 0) _adc_in[i] = (_adc_offset[i] - _adc_in[i]); else _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]; return _gyro_vector; }
// Read the 6 ADC channels needed for the IMU // ------------------------------------------ void AP_DCM::read_adc_raw(void) { int tc_temp = adc.Ch(_gyro_temp_ch); for (int i = 0; i < 6; i++) { _adc_in[i] = adc.Ch(_sensors[i]); if (i < 3) { // XXX magic numbers! _adc_in[i] -= _gyro_temp_comp(i, tc_temp); // Subtract temp compensated typical gyro bias } else { _adc_in[i] -= 2025; // Subtract typical accel bias } } }
void AP_IMU::init(void) { float temp; int flashcount = 0; int tc_temp = APM_ADC.Ch(_gyro_temp_ch); delay(500); for(int c = 0; c < 200; c++) { digitalWrite(A_LED_PIN, LOW); digitalWrite(C_LED_PIN, HIGH); delay(20); for (int i = 0; i < 6; i++) _adc_in[i] = APM_ADC.Ch(_sensors[i]); digitalWrite(C_LED_PIN, LOW); digitalWrite(A_LED_PIN, HIGH); delay(20); } 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]); 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); if(flashcount == 5) { digitalWrite(A_LED_PIN, LOW); digitalWrite(C_LED_PIN, HIGH); } if(flashcount >= 10) { flashcount = 0; digitalWrite(C_LED_PIN, LOW); digitalWrite(A_LED_PIN, HIGH); } flashcount++; } _adc_offset[5] += GRAVITY * _sensor_signs[5]; // NOTE *** Need to addd code to save values to EEPROM }