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
}
