AP_InertialSensor: provide IMU temperature to HAL for all boards

This commit is contained in:
Andrew Tridgell 2016-06-15 18:02:12 +10:00
parent 7c2e4d0419
commit 834acaffee
2 changed files with 5 additions and 3 deletions

View File

@ -226,6 +226,11 @@ uint16_t AP_InertialSensor_Backend::get_sample_rate_hz(void) const
void AP_InertialSensor_Backend::_publish_temperature(uint8_t instance, float temperature)
{
_imu._temperature[instance] = temperature;
/* give the temperature to the control loop in order to keep it constant*/
if (instance == 0) {
hal.util->set_imu_temp(temperature);
}
}
/*

View File

@ -387,9 +387,6 @@ bool AP_InertialSensor_MPU6000::update()
_publish_temperature(_accel_instance, _temp_filtered);
/* give the temperature to the control loop in order to keep it constant*/
hal.util->set_imu_temp(_temp_filtered);
return true;
}