From 8da42b7a8ba3c189fe32645bd462b08820bd1385 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Wed, 9 Nov 2016 21:54:27 +1100
Subject: [PATCH] AP_InertialSensor: fixed temperature for fast sampling case
---
libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp | 4 ++++
libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp | 6 +++++-
2 files changed, 9 insertions(+), 1 deletion(-)
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
index d49460fad8..507521d175 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
@@ -513,6 +513,7 @@ void AP_InertialSensor_MPU6000::_accumulate(uint8_t *samples, uint8_t n_samples)
void AP_InertialSensor_MPU6000::_accumulate_fast_sampling(uint8_t *samples, uint8_t n_samples)
{
Vector3l asum, gsum;
+ float tsum = 0;
for (uint8_t i = 0; i < n_samples; i++) {
uint8_t *data = samples + MPU6000_SAMPLE_SIZE * i;
@@ -525,6 +526,7 @@ void AP_InertialSensor_MPU6000::_accumulate_fast_sampling(uint8_t *samples, uint
float temp = int16_val(data, 3);
temp = temp/340 + 36.53;
+ tsum += temp;
_last_temp = temp;
}
@@ -539,6 +541,8 @@ void AP_InertialSensor_MPU6000::_accumulate_fast_sampling(uint8_t *samples, uint
_notify_new_accel_raw_sample(_accel_instance, accel, AP_HAL::micros64(), false);
_notify_new_gyro_raw_sample(_gyro_instance, gyro);
+
+ _temp_filtered = _temp_filter.apply(tsum / n_samples);
}
/*
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp
index 031ec62b91..19b56ddcb3 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp
@@ -442,7 +442,8 @@ void AP_InertialSensor_MPU9250::_accumulate(uint8_t *samples, uint8_t n_samples)
void AP_InertialSensor_MPU9250::_accumulate_fast_sampling(uint8_t *samples, uint8_t n_samples)
{
Vector3l asum, gsum;
-
+ float tsum = 0;
+
for (uint8_t i = 0; i < n_samples; i++) {
uint8_t *data = samples + MPU9250_SAMPLE_SIZE * i;
asum += Vector3l(int16_val(data, 1),
@@ -454,6 +455,7 @@ void AP_InertialSensor_MPU9250::_accumulate_fast_sampling(uint8_t *samples, uint
float temp = int16_val(data, 3);
temp = temp/340 + 36.53;
+ tsum += temp;
_last_temp = temp;
}
@@ -468,6 +470,8 @@ void AP_InertialSensor_MPU9250::_accumulate_fast_sampling(uint8_t *samples, uint
_notify_new_accel_raw_sample(_accel_instance, accel, AP_HAL::micros64(), false);
_notify_new_gyro_raw_sample(_gyro_instance, gyro);
+
+ _temp_filtered = _temp_filter.apply(tsum / n_samples);
}
--
GitLab