diff --git a/libraries/AP_IMU/AP_IMU.cpp b/libraries/AP_IMU/AP_IMU.cpp index 223a18fe1d..abf078887b 100644 --- a/libraries/AP_IMU/AP_IMU.cpp +++ b/libraries/AP_IMU/AP_IMU.cpp @@ -224,10 +224,12 @@ AP_IMU::get_accel(void) for (int i = 3; i < 6; i++) { _adc_in[i] = _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]); + _adc_in[i] = _adc_offset[i] - _adc_in[i]; else - _adc_in[i] = (_adc_in[i] - _adc_offset[i]); + _adc_in[i] = _adc_in[i] - _adc_offset[i]; + if (fabs(_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 diff --git a/libraries/AP_IMU/AP_IMU.h b/libraries/AP_IMU/AP_IMU.h index c1151d3aa4..a17e05ed02 100644 --- a/libraries/AP_IMU/AP_IMU.h +++ b/libraries/AP_IMU/AP_IMU.h @@ -31,6 +31,11 @@ public: void save_accel_eeprom(void); void print_accel_offsets(void); void print_gyro_offsets(void); + + void ax(const int v) { _adc_offset[3] = v; } + void ay(const int v) { _adc_offset[4] = v; } + void az(const int v) { _adc_offset[5] = v; } + // raw ADC values - called by DCM Vector3f get_gyro(void); // Radians/second