dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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

This commit is contained in:
Lorenz Meier 2015-03-25 21:53:03 -07:00
parent 4318a26b64 9442fc13e4
commit 1bbfe571fa
2 changed files with 25 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
src/drivers/lsm303d/lsm303d.cpp
View File

@ -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

13
src/drivers/mpu6000/mpu6000.cpp
View File

@ -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