Vector3f
AP_IMU::get_accel(void)
{
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)
_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
}
}
_accel_vector.x = accel_scale(_adc_in[3]);
_accel_vector.y = accel_scale(_adc_in[4]);
_accel_vector.z = accel_scale(_adc_in[5]);
return _accel_vector;
}
void
AP_DCM::_accel_adjust(void)
{
_accel_vector(1) += accel_scale((ground_speed / 100) * _omega(2)); // Centrifugal force on Acc_y = GPS_speed * GyroZ
_accel_vector(2) -= accel_scale((ground_speed / 100) * _omega(1)); // Centrifugal force on Acc_z = GPS_speed * GyroY
}