Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-23 16:23:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: fixed temperature for fast sampling case

Browse Source
This commit is contained in:
Andrew Tridgell 2016-11-09 21:54:27 +11:00
parent cb70bae167
commit 8da42b7a8b
2 changed files with 9 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
View File

@ -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) void AP_InertialSensor_MPU6000::_accumulate_fast_sampling(uint8_t *samples, uint8_t n_samples)
{ {
Vector3l asum, gsum; Vector3l asum, gsum;
float tsum = 0;
for (uint8_t i = 0; i < n_samples; i++) { for (uint8_t i = 0; i < n_samples; i++) {
uint8_t *data = samples + MPU6000_SAMPLE_SIZE * 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); float temp = int16_val(data, 3);
temp = temp/340 + 36.53; temp = temp/340 + 36.53;
tsum += temp;
_last_temp = 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_accel_raw_sample(_accel_instance, accel, AP_HAL::micros64(), false);
_notify_new_gyro_raw_sample(_gyro_instance, gyro); _notify_new_gyro_raw_sample(_gyro_instance, gyro);
_temp_filtered = _temp_filter.apply(tsum / n_samples);
} }
/* /*

4
libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp
View File

@ -442,6 +442,7 @@ 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) void AP_InertialSensor_MPU9250::_accumulate_fast_sampling(uint8_t *samples, uint8_t n_samples)
{ {
Vector3l asum, gsum; Vector3l asum, gsum;
float tsum = 0;
for (uint8_t i = 0; i < n_samples; i++) { for (uint8_t i = 0; i < n_samples; i++) {
uint8_t *data = samples + MPU9250_SAMPLE_SIZE * i; uint8_t *data = samples + MPU9250_SAMPLE_SIZE * i;
@ -454,6 +455,7 @@ void AP_InertialSensor_MPU9250::_accumulate_fast_sampling(uint8_t *samples, uint
float temp = int16_val(data, 3); float temp = int16_val(data, 3);
temp = temp/340 + 36.53; temp = temp/340 + 36.53;
tsum += temp;
_last_temp = 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_accel_raw_sample(_accel_instance, accel, AP_HAL::micros64(), false);
_notify_new_gyro_raw_sample(_gyro_instance, gyro); _notify_new_gyro_raw_sample(_gyro_instance, gyro);
_temp_filtered = _temp_filter.apply(tsum / n_samples);
} }