Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: fixed invensense driver temp reading

Browse Source 
different parts have quite different zero offsets in temperature
This commit is contained in:
Andrew Tridgell 2017-03-29 22:09:16 +11:00
parent 41e54351d4
commit c952e58edb
2 changed files with 29 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp
View File

@ -406,6 +406,30 @@ void AP_InertialSensor_Invensense::start()
adev = DEVTYPE_ACC_MPU6000; adev = DEVTYPE_ACC_MPU6000;
break; break;
} }
/*
setup temperature sensitivity and offset. This varies
considerably between parts
*/
switch (_mpu_type) {
case Invensense_MPU9250:
temp_zero = 21;
temp_sensitivity = 1.0/340;
break;
case Invensense_MPU6000:
case Invensense_MPU6500:
temp_zero = 36.53;
temp_sensitivity = 1.0/340;
break;
case Invensense_ICM20608:
case Invensense_ICM20602:
temp_zero = 25;
temp_sensitivity = 1.0/326.8;
break;
}
_gyro_instance = _imu.register_gyro(1000, _dev->get_bus_id_devtype(gdev)); _gyro_instance = _imu.register_gyro(1000, _dev->get_bus_id_devtype(gdev));
_accel_instance = _imu.register_accel(1000, _dev->get_bus_id_devtype(adev)); _accel_instance = _imu.register_accel(1000, _dev->get_bus_id_devtype(adev));
@ -553,7 +577,7 @@ bool AP_InertialSensor_Invensense::_accumulate(uint8_t *samples, uint8_t n_sampl
_fifo_reset(); _fifo_reset();
return false; return false;
} }
float temp = t2/340.0f + 36.53f; float temp = t2 * temp_sensitivity + temp_zero;
gyro = Vector3f(int16_val(data, 5), gyro = Vector3f(int16_val(data, 5),
int16_val(data, 4), int16_val(data, 4),
@ -643,7 +667,7 @@ bool AP_InertialSensor_Invensense::_accumulate_fast_sampling(uint8_t *samples, u
} }
if (ret) { if (ret) {
float temp = (static_cast<float>(tsum)/n_samples)/340.0f + 36.53f; float temp = (static_cast<float>(tsum)/n_samples)*temp_sensitivity + temp_zero;
_temp_filtered = _temp_filter.apply(temp); _temp_filtered = _temp_filter.apply(temp);
} }

3
libraries/AP_InertialSensor/AP_InertialSensor_Invensense.h
View File

@ -106,6 +106,9 @@ private:
uint16_t _error_count; uint16_t _error_count;
float temp_sensitivity = 1.0/340; // degC/LSB
float temp_zero = 36.53; // degC
float _temp_filtered; float _temp_filtered;
float _accel_scale; float _accel_scale;
LowPassFilter2pFloat _temp_filter; LowPassFilter2pFloat _temp_filter;