diff --git a/libraries/AP_IMU/AP_IMU_Oilpan.cpp b/libraries/AP_IMU/AP_IMU_Oilpan.cpp index e8b81ceedc..f4de20ac96 100644 --- a/libraries/AP_IMU/AP_IMU_Oilpan.cpp +++ b/libraries/AP_IMU/AP_IMU_Oilpan.cpp @@ -222,12 +222,12 @@ AP_IMU_Oilpan::_sensor_compensation(uint8_t channel, int temperature) const // do gyro temperature compensation if (channel < 3) { - //return 1658; - //* + return 1658; + /* return _gyro_temp_curve[channel][0] + _gyro_temp_curve[channel][1] * temperature + _gyro_temp_curve[channel][2] * temperature * temperature; - // */ + //*/ } // do fixed-offset accelerometer compensation diff --git a/libraries/AP_IMU/AP_IMU_Oilpan.h b/libraries/AP_IMU/AP_IMU_Oilpan.h index a849b1e349..dc1c8635b9 100644 --- a/libraries/AP_IMU/AP_IMU_Oilpan.h +++ b/libraries/AP_IMU/AP_IMU_Oilpan.h @@ -95,14 +95,14 @@ private: // Maximum possible value returned by an offset-corrected sensor channel // static const float _adc_constraint = 900; - + // Gyro and Accelerometer calibration criterial // static const float _gyro_total_cal_change = 4.0; // Experimentally derived - allows for some minor motion - static const float _gyro_max_cal_offset = 320.0; + static const float _gyro_max_cal_offset = 320.0; static const float _accel_total_cal_change = 4.0; static const float _accel_max_cal_offset = 250.0; - + }; #endif