fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

sensors: fix bug in thermal compensation temperature limit (+2 squashed commits)

Browse Source 
Squashed commits:
[2df1d9e] sensors: change definition of sensor offset and scale factor to match legacy code
[089e103] sensors: publish thermal corrections for all sensors
This commit is contained in:
Paul Riseborough 2017-01-20 21:41:32 +11:00 committed by Lorenz Meier
parent 1dd9a10260
commit 170bc91587
3 changed files with 35 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
src/modules/sensors/temperature_compensation.cpp
View File

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

4
src/modules/sensors/temperature_compensation.h
View File

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

36
src/modules/sensors/voted_sensors_update.cpp
View File

@ -67,8 +67,12 @@ VotedSensorsUpdate::VotedSensorsUpdate(const Parameters &parameters)
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 &parameters)
}
}
_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 */