From 1aadcdf538adf0bf330d3a2702e54babfb995ee8 Mon Sep 17 00:00:00 2001 From: Julien BERAUD Date: Fri, 25 Sep 2015 16:04:49 +0200 Subject: [PATCH] AP_InertialSensor_MPU6000: read temperature Read temperature as part of the normal burst. This is not very costly since it is part of the burst read in i2c and already read in spi. It is meant to be used for imu heating. The filter is set to 1Hz on temperature because of the inherent inertia of heating systems. --- .../AP_InertialSensor_MPU6000.cpp | 29 ++++++++++++++----- .../AP_InertialSensor_MPU6000.h | 7 ++++- 2 files changed, 28 insertions(+), 8 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp index a913cbad6a..9cdb253ad8 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp @@ -295,9 +295,8 @@ void AP_MPU6000_BusDriver_SPI::read_data_transaction(uint8_t *samples, } n_samples = 1; - /* remove temperature and cmd from data sample */ - memcpy(&samples[0], &rx.d[0], 6); - memcpy(&samples[6], &rx.d[8], 6); + /* remove cmd from data sample */ + memcpy(&samples[0], &rx.d[0], 14); return; } @@ -328,7 +327,7 @@ void AP_MPU6000_BusDriver_I2C::start(bool &fifo_mode, uint8_t &max_samples) // enable fifo mode fifo_mode = true; write8(MPUREG_FIFO_EN, BIT_XG_FIFO_EN | BIT_YG_FIFO_EN | - BIT_ZG_FIFO_EN | BIT_ACCEL_FIFO_EN); + BIT_ZG_FIFO_EN | BIT_ACCEL_FIFO_EN | BIT_TEMP_FIFO_EN); write8(MPUREG_USER_CTRL, BIT_USER_CTRL_FIFO_RESET | BIT_USER_CTRL_SIG_COND_RESET); write8(MPUREG_USER_CTRL, BIT_USER_CTRL_FIFO_EN); /* maximum number of samples read by a burst @@ -336,6 +335,7 @@ void AP_MPU6000_BusDriver_I2C::start(bool &fifo_mode, uint8_t &max_samples) * gyro_x * gyro_y * gyro_z + * temperature * accel_x * accel_y * accel_z @@ -444,6 +444,7 @@ AP_InertialSensor_MPU6000::AP_InertialSensor_MPU6000(AP_InertialSensor &imu, AP_ #if MPU6000_FAST_SAMPLING _accel_filter(1000, 15), _gyro_filter(1000, 15), + _temp_filter(1000, 1), #else _sample_count(0), _accel_sum(), @@ -651,27 +652,33 @@ bool AP_InertialSensor_MPU6000::update( void ) // we have a full set of samples uint16_t num_samples; Vector3f accel, gyro; + float temp; hal.scheduler->suspend_timer_procs(); #if MPU6000_FAST_SAMPLING gyro = _gyro_filtered; accel = _accel_filtered; + temp = _temp_filtered; num_samples = 1; #else gyro(_gyro_sum.x, _gyro_sum.y, _gyro_sum.z); accel(_accel_sum.x, _accel_sum.y, _accel_sum.z); + temp = _temp_sum; num_samples = _sum_count; _accel_sum.zero(); _gyro_sum.zero(); + _temp_sum = 0; #endif _sum_count = 0; hal.scheduler->resume_timer_procs(); gyro /= num_samples; accel /= num_samples; + temp /= num_samples; _publish_accel(_accel_instance, accel); _publish_gyro(_gyro_instance, gyro); + _publish_temperature(_accel_instance, temp); #if MPU6000_FAST_SAMPLING if (_last_accel_filter_hz != _accel_filter_cutoff()) { @@ -745,17 +752,22 @@ void AP_InertialSensor_MPU6000::_accumulate(uint8_t *samples, uint8_t n_samples) for(uint8_t i=0; i < n_samples; i++) { uint8_t *data = samples + MPU6000_SAMPLE_SIZE * i; Vector3f accel, gyro; + float temp; accel = Vector3f(int16_val(data, 1), int16_val(data, 0), -int16_val(data, 2)); accel *= MPU6000_ACCEL_SCALE_1G; - gyro = Vector3f(int16_val(data, 4), - int16_val(data, 3), - -int16_val(data, 5)); + gyro = Vector3f(int16_val(data, 5), + int16_val(data, 4), + -int16_val(data, 6)); gyro *= _gyro_scale; + temp = int16_val(data, 3); + /* scaling/offset values from the datasheet */ + temp = temp/340 + 36.53; + #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXF accel.rotate(ROTATION_PITCH_180_YAW_90); gyro.rotate(ROTATION_PITCH_180_YAW_90); @@ -772,9 +784,11 @@ void AP_InertialSensor_MPU6000::_accumulate(uint8_t *samples, uint8_t n_samples) #if MPU6000_FAST_SAMPLING _accel_filtered = _accel_filter.apply(accel); _gyro_filtered = _gyro_filter.apply(gyro); + _temp_filtered = _temp_filter.apply(temp); #else _accel_sum += accel; _gyro_sum += gyro; + _temp_sum += temp; #endif _sum_count++; @@ -783,6 +797,7 @@ void AP_InertialSensor_MPU6000::_accumulate(uint8_t *samples, uint8_t n_samples) // rollover - v unlikely _accel_sum.zero(); _gyro_sum.zero(); + _temp_sum = 0; } #endif } diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h index 9e2e0e3722..2383f02e85 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h @@ -24,9 +24,10 @@ #if MPU6000_FAST_SAMPLING #include #include +#include #endif -#define MPU6000_SAMPLE_SIZE 12 +#define MPU6000_SAMPLE_SIZE 14 #define MPU6000_MAX_FIFO_SAMPLES 3 #define MAX_DATA_READ (MPU6000_MAX_FIFO_SAMPLES * MPU6000_SAMPLE_SIZE) @@ -127,15 +128,19 @@ private: #if MPU6000_FAST_SAMPLING Vector3f _accel_filtered; Vector3f _gyro_filtered; + float _temp_filtered; // Low Pass filters for gyro and accel LowPassFilter2pVector3f _accel_filter; LowPassFilter2pVector3f _gyro_filter; + LowPassFilter2pFloat _temp_filter; #else // accumulation in timer - must be read with timer disabled // the sum of the values since last read Vector3l _accel_sum; Vector3l _gyro_sum; + int32_t _temp_sum; + #endif volatile uint16_t _sum_count; bool _fifo_mode;