Merge branch 'pullrequest-imu-temperature' of git://github.com/tridge/Firmware

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