diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp index 017d1ce713..f58cdd6158 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp @@ -387,6 +387,7 @@ AP_InertialSensor_LSM9DS0::AP_InertialSensor_LSM9DS0(AP_InertialSensor &imu, , _rotation_a(rotation_a) , _rotation_g(rotation_g) , _rotation_gH(rotation_gH) + , _temp_filter(80, 1) { } @@ -639,6 +640,9 @@ void AP_InertialSensor_LSM9DS0::_accel_init() /* Accel data ready on INT1 */ _register_write_xm(CTRL_REG3_XM, CTRL_REG3_XM_P1_DRDYA, true); hal.scheduler->delay(1); + + // enable temperature sensor + _register_write_xm(CTRL_REG5_XM, _register_read_xm(CTRL_REG5_XM) | 0x80, false); } void AP_InertialSensor_LSM9DS0::_set_gyro_scale(gyro_scale scale) @@ -725,7 +729,6 @@ void AP_InertialSensor_LSM9DS0::_read_data_transaction_a() const uint8_t reg = OUT_X_L_A | 0xC0; if (!_dev_accel->transfer(®, 1, (uint8_t *) &raw_data, sizeof(raw_data))) { - hal.console->printf("LSM9DS0: error reading accelerometer\n"); return; } @@ -734,6 +737,15 @@ void AP_InertialSensor_LSM9DS0::_read_data_transaction_a() _rotate_and_correct_accel(_accel_instance, accel_data); _notify_new_accel_raw_sample(_accel_instance, accel_data, AP_HAL::micros64()); + + // read temperature every 10th sample + if (_temp_counter++ % 10 == 0) { + int16_t traw; + const uint8_t regtemp = OUT_TEMP_L_XM | 0xC0; + if (_dev_accel->transfer(®temp, 1, (uint8_t *)&traw, sizeof(traw))) { + _temperature = _temp_filter.apply(traw * 0.125 + 25); + } + } } /* @@ -745,7 +757,6 @@ void AP_InertialSensor_LSM9DS0::_read_data_transaction_g() const uint8_t reg = OUT_X_L_G | 0xC0; if (!_dev_gyro->transfer(®, 1, (uint8_t *) &raw_data, sizeof(raw_data))) { - hal.console->printf("LSM9DS0: error reading gyroscope\n"); return; } @@ -761,6 +772,7 @@ bool AP_InertialSensor_LSM9DS0::update() { update_gyro(_gyro_instance); update_accel(_accel_instance); + _publish_temperature(_accel_instance, _temperature); return true; } diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.h b/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.h index 1a8094b982..29fff90270 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.h @@ -7,6 +7,7 @@ #include "AP_InertialSensor.h" #include "AP_InertialSensor_Backend.h" +#include class AP_InertialSensor_LSM9DS0 : public AP_InertialSensor_Backend { @@ -98,6 +99,9 @@ private: int _drdy_pin_num_g; uint8_t _gyro_instance; uint8_t _accel_instance; + float _temperature; + uint8_t _temp_counter; + LowPassFilter2pFloat _temp_filter; // gyro whoami uint8_t whoami_g;