From 170bc9158782de935ab4c2b02f17845afdeca931 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Fri, 20 Jan 2017 21:41:32 +1100 Subject: [PATCH] sensors: fix bug in thermal compensation temperature limit (+2 squashed commits) Squashed commits: [2df1d9e] sensors: change definition of sensor offset and scale factor to match legacy code [089e103] sensors: publish thermal corrections for all sensors --- .../sensors/temperature_compensation.cpp | 14 ++++---- .../sensors/temperature_compensation.h | 4 +-- src/modules/sensors/voted_sensors_update.cpp | 36 ++++++++++++++----- 3 files changed, 35 insertions(+), 19 deletions(-) diff --git a/src/modules/sensors/temperature_compensation.cpp b/src/modules/sensors/temperature_compensation.cpp index 54907a011e..84b96d3bb8 100644 --- a/src/modules/sensors/temperature_compensation.cpp +++ b/src/modules/sensors/temperature_compensation.cpp @@ -285,20 +285,18 @@ bool TemperatureCompensation::calc_thermal_offsets_3D(SensorCalData3D &coef, flo float delta_temp; if (measured_temp > coef.max_temp) { - delta_temp = coef.max_temp; + delta_temp = coef.max_temp - coef.ref_temp; ret = false; } else if (measured_temp < coef.min_temp) { - delta_temp = coef.min_temp; + delta_temp = coef.min_temp - coef.ref_temp; ret = false; } else { - delta_temp = measured_temp; + delta_temp = measured_temp - coef.ref_temp; } - delta_temp -= coef.ref_temp; - // calulate the offsets float delta_temp_2 = delta_temp * delta_temp; float delta_temp_3 = delta_temp_2 * delta_temp; @@ -371,7 +369,7 @@ int TemperatureCompensation::apply_corrections_gyro(int topic_instance, math::Ve // get the sensor scale factors and correct the data for (unsigned axis_index = 0; axis_index < 3; axis_index++) { scales[axis_index] = _parameters.gyro_cal_data[mapping].scale[axis_index]; - sensor_data(axis_index) = sensor_data(axis_index) * scales[axis_index] + offsets[axis_index]; + sensor_data(axis_index) = (sensor_data(axis_index) - offsets[axis_index]) * scales[axis_index]; } int8_t temperaturei = (int8_t)temperature; @@ -402,7 +400,7 @@ int TemperatureCompensation::apply_corrections_accel(int topic_instance, math::V // get the sensor scale factors and correct the data for (unsigned axis_index = 0; axis_index < 3; axis_index++) { scales[axis_index] = _parameters.accel_cal_data[mapping].scale[axis_index]; - sensor_data(axis_index) = sensor_data(axis_index) * scales[axis_index] + offsets[axis_index]; + sensor_data(axis_index) = (sensor_data(axis_index) - offsets[axis_index]) * scales[axis_index]; } int8_t temperaturei = (int8_t)temperature; @@ -432,7 +430,7 @@ int TemperatureCompensation::apply_corrections_baro(int topic_instance, float &s // get the sensor scale factors and correct the data *scales = _parameters.baro_cal_data[mapping].scale; - sensor_data = sensor_data * *scales + *offsets; + sensor_data = (sensor_data - *offsets) * *scales; int8_t temperaturei = (int8_t)temperature; diff --git a/src/modules/sensors/temperature_compensation.h b/src/modules/sensors/temperature_compensation.h index 9f2696a119..0317ee0c16 100644 --- a/src/modules/sensors/temperature_compensation.h +++ b/src/modules/sensors/temperature_compensation.h @@ -108,7 +108,7 @@ private: delta_temp = measured_temp - ref_temp offset = x5 * delta_temp^5 + x4 * delta_temp^4 + x3 * delta_temp^3 + x2 * delta_temp^2 + x1 * delta_temp + x0 - corrected_value = raw_value * scale + offset + corrected_value = (raw_value - offset) * scale */ struct SensorCalData1D { @@ -152,7 +152,7 @@ private: delta_temp = measured_temp - ref_temp offset = x3 * delta_temp^3 + x2 * delta_temp^2 + x1 * delta_temp + x0 - corrected_value = raw_value * scale + offset + corrected_value = (raw_value - offset) * scale */ struct SensorCalData3D { diff --git a/src/modules/sensors/voted_sensors_update.cpp b/src/modules/sensors/voted_sensors_update.cpp index 8f4f930041..8ea6158dff 100644 --- a/src/modules/sensors/voted_sensors_update.cpp +++ b/src/modules/sensors/voted_sensors_update.cpp @@ -67,8 +67,12 @@ VotedSensorsUpdate::VotedSensorsUpdate(const Parameters ¶meters) memset(&_corrections, 0, sizeof(_corrections)); for (unsigned i = 0; i < 3; i++) { - _corrections.gyro_scale[i] = 1.0f; - _corrections.accel_scale[i] = 1.0f; + _corrections.gyro_scale_0[i] = 1.0f; + _corrections.accel_scale_0[i] = 1.0f; + _corrections.gyro_scale_1[i] = 1.0f; + _corrections.accel_scale_1[i] = 1.0f; + _corrections.gyro_scale_2[i] = 1.0f; + _corrections.accel_scale_2[i] = 1.0f; for (unsigned j = 0; j < SENSOR_COUNT_MAX; j++) { _accel_scale[j][i] = 1.0f; @@ -77,7 +81,9 @@ VotedSensorsUpdate::VotedSensorsUpdate(const Parameters ¶meters) } } - _corrections.baro_scale = 1.0f; + _corrections.baro_scale_0 = 1.0f; + _corrections.baro_scale_1 = 1.0f; + _corrections.baro_scale_2 = 1.0f; memset(&_accel_offset, 0, sizeof(_accel_offset)); memset(&_gyro_offset, 0, sizeof(_gyro_offset)); @@ -593,8 +599,12 @@ void VotedSensorsUpdate::accel_poll(struct sensor_combined_s &raw) for (unsigned axis_index = 0; axis_index < 3; axis_index++) { raw.accelerometer_m_s2[axis_index] = _last_sensor_data[best_index].accelerometer_m_s2[axis_index]; - _corrections.accel_offset[axis_index] = _accel_offset[best_index][axis_index]; - _corrections.accel_scale[axis_index] = _accel_scale[best_index][axis_index]; + _corrections.accel_offset_0[axis_index] = _accel_offset[0][axis_index]; + _corrections.accel_scale_0[axis_index] = _accel_scale[0][axis_index]; + _corrections.accel_offset_1[axis_index] = _accel_offset[1][axis_index]; + _corrections.accel_scale_1[axis_index] = _accel_scale[1][axis_index]; + _corrections.accel_offset_2[axis_index] = _accel_offset[2][axis_index]; + _corrections.accel_scale_2[axis_index] = _accel_scale[2][axis_index]; } } } @@ -691,8 +701,12 @@ void VotedSensorsUpdate::gyro_poll(struct sensor_combined_s &raw) for (unsigned axis_index = 0; axis_index < 3; axis_index++) { raw.gyro_rad[axis_index] = _last_sensor_data[best_index].gyro_rad[axis_index]; - _corrections.gyro_offset[axis_index] = _gyro_offset[best_index][axis_index]; - _corrections.gyro_scale[axis_index] = _gyro_scale[best_index][axis_index]; + _corrections.gyro_offset_0[axis_index] = _gyro_offset[0][axis_index]; + _corrections.gyro_scale_0[axis_index] = _gyro_scale[0][axis_index]; + _corrections.gyro_offset_1[axis_index] = _gyro_offset[1][axis_index]; + _corrections.gyro_scale_1[axis_index] = _gyro_scale[1][axis_index]; + _corrections.gyro_offset_2[axis_index] = _gyro_offset[2][axis_index]; + _corrections.gyro_scale_2[axis_index] = _gyro_scale[2][axis_index]; } } } @@ -803,8 +817,12 @@ void VotedSensorsUpdate::baro_poll(struct sensor_combined_s &raw) _corrections_changed = true; } - _corrections.baro_offset = _baro_offset[best_index]; - _corrections.baro_scale = _baro_scale[best_index]; + _corrections.baro_offset_0 = _baro_offset[0]; + _corrections.baro_scale_0 = _baro_scale[0]; + _corrections.baro_offset_1 = _baro_offset[1]; + _corrections.baro_scale_1 = _baro_scale[1]; + _corrections.baro_offset_2 = _baro_offset[2]; + _corrections.baro_scale_2 = _baro_scale[2]; /* altitude calculations based on http://www.kansasflyer.org/index.asp?nav=Avi&sec=Alti&tab=Theory&pg=1 */