diff options
-rw-r--r-- | src/drivers/lsm303d/lsm303d.cpp | 17 | ||||
-rw-r--r-- | src/drivers/mpu6000/mpu6000.cpp | 13 |
2 files changed, 25 insertions, 5 deletions
diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 84f7fb5c8..11a046a8d 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -305,6 +305,9 @@ private: float _last_accel[3]; uint8_t _constant_accel_count; + // last temperature value + float _last_temperature; + // this is used to support runtime checking of key // configuration registers to detect SPI bus errors and sensor // reset @@ -567,6 +570,7 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device, enum Rotation rota _accel_filter_z(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _rotation(rotation), _constant_accel_count(0), + _last_temperature(0), _checked_next(0) { @@ -711,7 +715,7 @@ LSM303D::reset() /* enable mag */ write_checked_reg(ADDR_CTRL_REG7, REG7_CONT_MODE_M); - write_checked_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M); + write_checked_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M | REG5_ENABLE_T); write_checked_reg(ADDR_CTRL_REG3, 0x04); // DRDY on ACCEL on INT1 write_checked_reg(ADDR_CTRL_REG4, 0x04); // DRDY on MAG on INT2 @@ -1507,6 +1511,9 @@ LSM303D::measure() accel_report.timestamp = hrt_absolute_time(); + // use the temperature from the last mag reading + accel_report.temperature = _last_temperature; + // report the error count as the sum of the number of bad // register reads and bad values. This allows the higher level // code to decide if it should use this sensor based on @@ -1588,6 +1595,7 @@ LSM303D::mag_measure() #pragma pack(push, 1) struct { uint8_t cmd; + int16_t temperature; uint8_t status; int16_t x; int16_t y; @@ -1603,7 +1611,7 @@ LSM303D::mag_measure() /* fetch data from the sensor */ memset(&raw_mag_report, 0, sizeof(raw_mag_report)); - raw_mag_report.cmd = ADDR_STATUS_M | DIR_READ | ADDR_INCREMENT; + raw_mag_report.cmd = ADDR_OUT_TEMP_L | DIR_READ | ADDR_INCREMENT; transfer((uint8_t *)&raw_mag_report, (uint8_t *)&raw_mag_report, sizeof(raw_mag_report)); /* @@ -1642,6 +1650,10 @@ LSM303D::mag_measure() mag_report.range_ga = (float)_mag_range_ga; mag_report.error_count = perf_event_count(_bad_registers) + perf_event_count(_bad_values); + // remember the temperature. The datasheet isn't clear, but it + // seems to be a signed offset from 25 degrees C in units of 0.125C + _last_temperature = 25 + (raw_mag_report.temperature*0.125f); + _mag_reports->force(&mag_report); /* XXX please check this poll_notify, is it the right one? */ @@ -1681,6 +1693,7 @@ LSM303D::print_info() (unsigned)_checked_values[i]); } } + ::printf("temperature: %.2f\n", _last_temperature); } void diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index eaf25a9f3..727132a64 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -284,6 +284,9 @@ private: // self test volatile bool _in_factory_test; + // last temperature reading for print_info() + float _last_temperature; + /** * Start automatic measurement. */ @@ -518,7 +521,8 @@ MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev _gyro_filter_z(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ), _rotation(rotation), _checked_next(0), - _in_factory_test(false) + _in_factory_test(false), + _last_temperature(0) { // disable debug() calls _debug_enabled = false; @@ -1729,8 +1733,10 @@ MPU6000::measure() arb.scaling = _accel_range_scale; arb.range_m_s2 = _accel_range_m_s2; + _last_temperature = (report.temp) / 361.0f + 35.0f; + arb.temperature_raw = report.temp; - arb.temperature = (report.temp) / 361.0f + 35.0f; + arb.temperature = _last_temperature; grb.x_raw = report.gyro_x; grb.y_raw = report.gyro_y; @@ -1755,7 +1761,7 @@ MPU6000::measure() grb.range_rad_s = _gyro_range_rad_s; grb.temperature_raw = report.temp; - grb.temperature = (report.temp) / 361.0f + 35.0f; + grb.temperature = _last_temperature; _accel_reports->force(&arb); _gyro_reports->force(&grb); @@ -1803,6 +1809,7 @@ MPU6000::print_info() (unsigned)_checked_values[i]); } } + ::printf("temperature: %.1f\n", _last_temperature); } void |