mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-24 07:09:48 +08:00
sensors: add baro pressure temperature compensation and reporting
This commit is contained in:
Paul Riseborough
committed by
Lorenz Meier
Lorenz Meier
parent
c21b4aaf2e
commit
0485c2b94a
@@ -0,0 +1,286 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2012-2016 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file temp_comp_params_baro.c
|
||||
*
|
||||
* Parameters required for temperature compensation of barometers.
|
||||
*
|
||||
* @author Paul Riseborough <gncsolns@gmail.com>
|
||||
*/
|
||||
|
||||
/**
|
||||
* Set to 1 to enable thermal compensation for barometric pressure sensors. Set to 0 to disable.
|
||||
*
|
||||
* @group Sensor Thermal Compensation
|
||||
* @min 0
|
||||
* @max 1
|
||||
*/
|
||||
PARAM_DEFINE_INT32(TC_B_ENABLE, 0);
|
||||
|
||||
/* Barometer 0 */
|
||||
|
||||
/**
|
||||
* ID of Barometer that the calibration is for.
|
||||
*
|
||||
* @group Sensor Thermal Compensation
|
||||
*/
|
||||
PARAM_DEFINE_INT32(TC_B0_ID, 0);
|
||||
|
||||
/**
|
||||
* Barometer offset temperature ^5 polynomial coefficient.
|
||||
*
|
||||
* @group Sensor Thermal Compensation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(TC_B0_X5, 0.0f);
|
||||
|
||||
/**
|
||||
* Barometer offset temperature ^4 polynomial coefficient.
|
||||
*
|
||||
* @group Sensor Thermal Compensation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(TC_B0_X4, 0.0f);
|
||||
|
||||
/**
|
||||
* Barometer offset temperature ^3 polynomial coefficient.
|
||||
*
|
||||
* @group Sensor Thermal Compensation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(TC_B0_X3, 0.0f);
|
||||
|
||||
/**
|
||||
* Barometer offset temperature ^2 polynomial coefficient.
|
||||
*
|
||||
* @group Sensor Thermal Compensation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(TC_B0_X2, 0.0f);
|
||||
|
||||
/**
|
||||
* Barometer offset temperature ^1 polynomial coefficients.
|
||||
*
|
||||
* @group Sensor Thermal Compensation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(TC_B0_X1, 0.0f);
|
||||
|
||||
/**
|
||||
* Barometer offset temperature ^0 polynomial coefficient.
|
||||
*
|
||||
* @group Sensor Thermal Compensation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(TC_B0_X0, 0.0f);
|
||||
|
||||
/**
|
||||
* Barometer scale factor - X axis.
|
||||
*
|
||||
* @group Sensor Thermal Compensation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(TC_B0_SCL, 1.0f);
|
||||
|
||||
/**
|
||||
* Barometer calibration reference temperature.
|
||||
*
|
||||
* @group Sensor Thermal Compensation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(TC_B0_TREF, 40.0f);
|
||||
|
||||
/**
|
||||
* Barometer calibration minimum temperature.
|
||||
*
|
||||
* @group Sensor Thermal Compensation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(TC_B0_TMIN, 5.0f);
|
||||
|
||||
/**
|
||||
* Barometer calibration maximum temperature.
|
||||
*
|
||||
* @group Sensor Thermal Compensation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(TC_B0_TMAX, 75.0f);
|
||||
|
||||
/* Barometer 1 */
|
||||
|
||||
/**
|
||||
* ID of Barometer that the calibration is for.
|
||||
*
|
||||
* @group Sensor Thermal Compensation
|
||||
*/
|
||||
PARAM_DEFINE_INT32(TC_B1_ID, 0);
|
||||
|
||||
/**
|
||||
* Barometer offset temperature ^5 polynomial coefficient.
|
||||
*
|
||||
* @group Sensor Thermal Compensation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(TC_B1_X5, 0.0f);
|
||||
|
||||
/**
|
||||
* Barometer offset temperature ^4 polynomial coefficient.
|
||||
*
|
||||
* @group Sensor Thermal Compensation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(TC_B1_X4, 0.0f);
|
||||
|
||||
/**
|
||||
* Barometer offset temperature ^3 polynomial coefficient.
|
||||
*
|
||||
* @group Sensor Thermal Compensation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(TC_B1_X3, 0.0f);
|
||||
|
||||
/**
|
||||
* Barometer offset temperature ^2 polynomial coefficient.
|
||||
*
|
||||
* @group Sensor Thermal Compensation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(TC_B1_X2, 0.0f);
|
||||
|
||||
/**
|
||||
* Barometer offset temperature ^1 polynomial coefficients.
|
||||
*
|
||||
* @group Sensor Thermal Compensation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(TC_B1_X1, 0.0f);
|
||||
|
||||
/**
|
||||
* Barometer offset temperature ^0 polynomial coefficient.
|
||||
*
|
||||
* @group Sensor Thermal Compensation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(TC_B1_X0, 0.0f);
|
||||
|
||||
/**
|
||||
* Barometer scale factor - X axis.
|
||||
*
|
||||
* @group Sensor Thermal Compensation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(TC_B1_SCL, 1.0f);
|
||||
|
||||
/**
|
||||
* Barometer calibration reference temperature.
|
||||
*
|
||||
* @group Sensor Thermal Compensation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(TC_B1_TREF, 40.0f);
|
||||
|
||||
/**
|
||||
* Barometer calibration minimum temperature.
|
||||
*
|
||||
* @group Sensor Thermal Compensation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(TC_B1_TMIN, 5.0f);
|
||||
|
||||
/**
|
||||
* Barometer calibration maximum temperature.
|
||||
*
|
||||
* @group Sensor Thermal Compensation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(TC_B1_TMAX, 75.0f);
|
||||
|
||||
/* Barometer 2 */
|
||||
|
||||
/**
|
||||
* ID of Barometer that the calibration is for.
|
||||
*
|
||||
* @group Sensor Thermal Compensation
|
||||
*/
|
||||
PARAM_DEFINE_INT32(TC_B2_ID, 0);
|
||||
|
||||
/**
|
||||
* Barometer offset temperature ^5 polynomial coefficient.
|
||||
*
|
||||
* @group Sensor Thermal Compensation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(TC_B2_X5, 0.0f);
|
||||
|
||||
/**
|
||||
* Barometer offset temperature ^4 polynomial coefficient.
|
||||
*
|
||||
* @group Sensor Thermal Compensation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(TC_B2_X4, 0.0f);
|
||||
|
||||
/**
|
||||
* Barometer offset temperature ^3 polynomial coefficient.
|
||||
*
|
||||
* @group Sensor Thermal Compensation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(TC_B2_X3, 0.0f);
|
||||
|
||||
/**
|
||||
* Barometer offset temperature ^2 polynomial coefficient.
|
||||
*
|
||||
* @group Sensor Thermal Compensation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(TC_B2_X2, 0.0f);
|
||||
|
||||
/**
|
||||
* Barometer offset temperature ^1 polynomial coefficients.
|
||||
*
|
||||
* @group Sensor Thermal Compensation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(TC_B2_X1, 0.0f);
|
||||
|
||||
/**
|
||||
* Barometer offset temperature ^0 polynomial coefficient.
|
||||
*
|
||||
* @group Sensor Thermal Compensation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(TC_B2_X0, 0.0f);
|
||||
|
||||
/**
|
||||
* Barometer scale factor - X axis.
|
||||
*
|
||||
* @group Sensor Thermal Compensation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(TC_B2_SCL, 1.0f);
|
||||
|
||||
/**
|
||||
* Barometer calibration reference temperature.
|
||||
*
|
||||
* @group Sensor Thermal Compensation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(TC_B2_TREF, 40.0f);
|
||||
|
||||
/**
|
||||
* Barometer calibration minimum temperature.
|
||||
*
|
||||
* @group Sensor Thermal Compensation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(TC_B2_TMIN, 5.0f);
|
||||
|
||||
/**
|
||||
* Barometer calibration maximum temperature.
|
||||
*
|
||||
* @group Sensor Thermal Compensation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(TC_B2_TMAX, 75.0f);
|
||||
@@ -109,6 +109,35 @@ int initialize_parameter_handles(ParameterHandles ¶meter_handles)
|
||||
parameter_handles.accel_cal_handles[j].max_temp = param_find(nbuf);
|
||||
}
|
||||
|
||||
/* barometer calibration parameters */
|
||||
sprintf(nbuf, "TC_B_ENABLE");
|
||||
parameter_handles.baro_tc_enable = param_find(nbuf);
|
||||
|
||||
for (unsigned j = 0; j < 3; j++) {
|
||||
sprintf(nbuf, "TC_B%d_ID", j);
|
||||
parameter_handles.baro_cal_handles[j].ID = param_find(nbuf);
|
||||
sprintf(nbuf, "TC_B%d_X5", j);
|
||||
parameter_handles.baro_cal_handles[j].x5 = param_find(nbuf);
|
||||
sprintf(nbuf, "TC_B%d_X4", j);
|
||||
parameter_handles.baro_cal_handles[j].x4 = param_find(nbuf);
|
||||
sprintf(nbuf, "TC_B%d_X3", j);
|
||||
parameter_handles.baro_cal_handles[j].x3 = param_find(nbuf);
|
||||
sprintf(nbuf, "TC_B%d_X2", j);
|
||||
parameter_handles.baro_cal_handles[j].x2 = param_find(nbuf);
|
||||
sprintf(nbuf, "TC_B%d_X1", j);
|
||||
parameter_handles.baro_cal_handles[j].x1 = param_find(nbuf);
|
||||
sprintf(nbuf, "TC_B%d_X0", j);
|
||||
parameter_handles.baro_cal_handles[j].x0 = param_find(nbuf);
|
||||
sprintf(nbuf, "TC_B%d_SCL", j);
|
||||
parameter_handles.baro_cal_handles[j].scale = param_find(nbuf);
|
||||
sprintf(nbuf, "TC_B%d_TREF", j);
|
||||
parameter_handles.baro_cal_handles[j].ref_temp = param_find(nbuf);
|
||||
sprintf(nbuf, "TC_B%d_TMIN", j);
|
||||
parameter_handles.baro_cal_handles[j].min_temp = param_find(nbuf);
|
||||
sprintf(nbuf, "TC_B%d_TMAX", j);
|
||||
parameter_handles.baro_cal_handles[j].max_temp = param_find(nbuf);
|
||||
}
|
||||
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
@@ -178,7 +207,33 @@ int update_parameters(const ParameterHandles ¶meter_handles, Parameters &par
|
||||
}
|
||||
}
|
||||
|
||||
return ret;
|
||||
/* barometer calibration parameters */
|
||||
param_get(parameter_handles.baro_tc_enable, &(parameters.baro_tc_enable));
|
||||
|
||||
for (unsigned j = 0; j < 3; j++) {
|
||||
if (param_get(parameter_handles.baro_cal_handles[j].ID, &(parameters.baro_cal_data[j].ID)) == PX4_OK) {
|
||||
param_get(parameter_handles.baro_cal_handles[j].ref_temp, &(parameters.baro_cal_data[j].ref_temp));
|
||||
param_get(parameter_handles.baro_cal_handles[j].min_temp, &(parameters.baro_cal_data[j].min_temp));
|
||||
param_get(parameter_handles.baro_cal_handles[j].min_temp, &(parameters.baro_cal_data[j].min_temp));
|
||||
param_get(parameter_handles.baro_cal_handles[j].x5, &(parameters.baro_cal_data[j].x5));
|
||||
param_get(parameter_handles.baro_cal_handles[j].x4, &(parameters.baro_cal_data[j].x4));
|
||||
param_get(parameter_handles.baro_cal_handles[j].x3, &(parameters.baro_cal_data[j].x3));
|
||||
param_get(parameter_handles.baro_cal_handles[j].x2, &(parameters.baro_cal_data[j].x2));
|
||||
param_get(parameter_handles.baro_cal_handles[j].x1, &(parameters.baro_cal_data[j].x1));
|
||||
param_get(parameter_handles.baro_cal_handles[j].x0, &(parameters.baro_cal_data[j].x0));
|
||||
param_get(parameter_handles.baro_cal_handles[j].scale, &(parameters.baro_cal_data[j].scale));
|
||||
|
||||
} else {
|
||||
// Set all cal values to zero and scale factor to unity
|
||||
memset(¶meters.baro_cal_data[j], 0, sizeof(parameters.baro_cal_data[j]));
|
||||
|
||||
// Set the scale factor to unity
|
||||
parameters.baro_cal_data[j].scale = 1.0f;
|
||||
|
||||
PX4_WARN("FAIL BARO %d CAL PARAM LOAD - USING DEFAULTS", j);
|
||||
ret = PX4_ERROR;
|
||||
}
|
||||
} return ret;
|
||||
}
|
||||
|
||||
bool calc_thermal_offsets_1D(struct SENSOR_CAL_DATA_1D &coef, const float &measured_temp, float &offset)
|
||||
@@ -189,19 +244,19 @@ bool calc_thermal_offsets_1D(struct SENSOR_CAL_DATA_1D &coef, const float &measu
|
||||
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;
|
||||
delta_temp = measured_temp - coef.ref_temp;
|
||||
|
||||
// calulate the offset
|
||||
offset = coef.x0 + coef.x1 * delta_temp;
|
||||
|
||||
@@ -134,7 +134,7 @@ struct Parameters {
|
||||
int accel_tc_enable;
|
||||
SENSOR_CAL_DATA_3D accel_cal_data[3];
|
||||
int baro_tc_enable;
|
||||
SENSOR_CAL_DATA_1D baro_cal_data;
|
||||
SENSOR_CAL_DATA_1D baro_cal_data[3];
|
||||
};
|
||||
|
||||
// create a struct containing the handles required to access all calibration parameters
|
||||
@@ -144,7 +144,7 @@ struct ParameterHandles {
|
||||
param_t accel_tc_enable;
|
||||
SENSOR_CAL_HANDLES_3D accel_cal_handles[3];
|
||||
param_t baro_tc_enable;
|
||||
SENSOR_CAL_HANDLES_1D baro_cal_handles;
|
||||
SENSOR_CAL_HANDLES_1D baro_cal_handles[3];
|
||||
};
|
||||
|
||||
/**
|
||||
|
||||
@@ -84,16 +84,22 @@ int VotedSensorsUpdate::init(sensor_combined_s &raw)
|
||||
memset(&_corrections, 0, sizeof(_corrections));
|
||||
memset(&_accel_offset, 0, sizeof(_accel_offset));
|
||||
memset(&_gyro_offset, 0, sizeof(_gyro_offset));
|
||||
memset(&_baro_offset, 0, sizeof(_baro_offset));
|
||||
|
||||
for (unsigned i = 0; i < 3; i++) {
|
||||
_corrections.gyro_scale[i] = 1.0f;
|
||||
_corrections.accel_scale[i] = 1.0f;
|
||||
_corrections.baro_scale = 1.0f;
|
||||
|
||||
for (unsigned j = 0; j < SENSOR_COUNT_MAX; j++) {
|
||||
_accel_scale[j][i] = 1.0f;
|
||||
_gyro_scale[j][i] = 1.0f;
|
||||
_baro_scale[j] = 1.0f;
|
||||
}
|
||||
}
|
||||
|
||||
_msl_pressure = 101325.0f;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
@@ -450,12 +456,14 @@ void VotedSensorsUpdate::accel_poll(struct sensor_combined_s &raw)
|
||||
for (unsigned param_index = 0; param_index < 3; param_index++) {
|
||||
if (accel_report.device_id == _thermal_correction_param.accel_cal_data[param_index].ID) {
|
||||
// get the offsets
|
||||
sensors_temp_comp::calc_thermal_offsets_3D(_thermal_correction_param.accel_cal_data[param_index], accel_report.temperature, _accel_offset[uorb_index]);
|
||||
sensors_temp_comp::calc_thermal_offsets_3D(_thermal_correction_param.accel_cal_data[param_index],
|
||||
accel_report.temperature, _accel_offset[uorb_index]);
|
||||
|
||||
// get the scale factors and correct the data
|
||||
for (unsigned axis_index = 0; axis_index < 3; axis_index++) {
|
||||
_accel_scale[uorb_index][axis_index] = _thermal_correction_param.accel_cal_data[param_index].scale[axis_index];
|
||||
accel_data(axis_index) = accel_data(axis_index) * _accel_scale[uorb_index][axis_index] + _accel_offset[uorb_index][axis_index];
|
||||
accel_data(axis_index) = accel_data(axis_index) * _accel_scale[uorb_index][axis_index] +
|
||||
_accel_offset[uorb_index][axis_index];
|
||||
}
|
||||
|
||||
break;
|
||||
@@ -485,12 +493,14 @@ void VotedSensorsUpdate::accel_poll(struct sensor_combined_s &raw)
|
||||
for (unsigned param_index = 0; param_index < 3; param_index++) {
|
||||
if (accel_report.device_id == _thermal_correction_param.accel_cal_data[param_index].ID) {
|
||||
// get the offsets
|
||||
sensors_temp_comp::calc_thermal_offsets_3D(_thermal_correction_param.accel_cal_data[param_index], accel_report.temperature, _accel_offset[uorb_index]);
|
||||
sensors_temp_comp::calc_thermal_offsets_3D(_thermal_correction_param.accel_cal_data[param_index],
|
||||
accel_report.temperature, _accel_offset[uorb_index]);
|
||||
|
||||
// get the scale factors and correct the data
|
||||
for (unsigned axis_index = 0; axis_index < 3; axis_index++) {
|
||||
_accel_scale[uorb_index][axis_index] = _thermal_correction_param.gyro_cal_data[param_index].scale[axis_index];
|
||||
accel_data(axis_index) = accel_data(axis_index) * _accel_scale[uorb_index][axis_index] + _accel_offset[uorb_index][axis_index];
|
||||
accel_data(axis_index) = accel_data(axis_index) * _accel_scale[uorb_index][axis_index] +
|
||||
_accel_offset[uorb_index][axis_index];
|
||||
}
|
||||
|
||||
break;
|
||||
@@ -519,7 +529,8 @@ void VotedSensorsUpdate::accel_poll(struct sensor_combined_s &raw)
|
||||
}
|
||||
|
||||
_last_accel_timestamp[uorb_index] = accel_report.timestamp;
|
||||
_accel.voter.put(uorb_index, accel_report.timestamp, _last_sensor_data[uorb_index].accelerometer_m_s2, accel_report.error_count, _accel.priority[uorb_index]);
|
||||
_accel.voter.put(uorb_index, accel_report.timestamp, _last_sensor_data[uorb_index].accelerometer_m_s2,
|
||||
accel_report.error_count, _accel.priority[uorb_index]);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -532,6 +543,7 @@ void VotedSensorsUpdate::accel_poll(struct sensor_combined_s &raw)
|
||||
raw.accelerometer_integral_dt = _last_sensor_data[best_index].accelerometer_integral_dt;
|
||||
_accel.last_best_vote = (uint8_t)best_index;
|
||||
_corrections.accel_select = (uint8_t)best_index;
|
||||
|
||||
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];
|
||||
@@ -580,12 +592,14 @@ void VotedSensorsUpdate::gyro_poll(struct sensor_combined_s &raw)
|
||||
for (unsigned param_index = 0; param_index < 3; param_index++) {
|
||||
if (gyro_report.device_id == _thermal_correction_param.gyro_cal_data[param_index].ID) {
|
||||
// get the offsets
|
||||
sensors_temp_comp::calc_thermal_offsets_3D(_thermal_correction_param.gyro_cal_data[param_index], gyro_report.temperature, _gyro_offset[uorb_index]);
|
||||
sensors_temp_comp::calc_thermal_offsets_3D(_thermal_correction_param.gyro_cal_data[param_index],
|
||||
gyro_report.temperature, _gyro_offset[uorb_index]);
|
||||
|
||||
// get the sensor scale factors and correct the data
|
||||
for (unsigned axis_index = 0; axis_index < 3; axis_index++) {
|
||||
_gyro_scale[uorb_index][axis_index] = _thermal_correction_param.gyro_cal_data[param_index].scale[axis_index];
|
||||
gyro_rate(axis_index) = gyro_rate(axis_index) * _gyro_scale[uorb_index][axis_index] + _gyro_offset[uorb_index][axis_index];
|
||||
gyro_rate(axis_index) = gyro_rate(axis_index) * _gyro_scale[uorb_index][axis_index] +
|
||||
_gyro_offset[uorb_index][axis_index];
|
||||
}
|
||||
|
||||
break;
|
||||
@@ -615,12 +629,14 @@ void VotedSensorsUpdate::gyro_poll(struct sensor_combined_s &raw)
|
||||
for (unsigned param_index = 0; param_index < 3; param_index++) {
|
||||
if (gyro_report.device_id == _thermal_correction_param.gyro_cal_data[param_index].ID) {
|
||||
// get the offsets
|
||||
sensors_temp_comp::calc_thermal_offsets_3D(_thermal_correction_param.gyro_cal_data[param_index], gyro_report.temperature, _gyro_offset[uorb_index]);
|
||||
sensors_temp_comp::calc_thermal_offsets_3D(_thermal_correction_param.gyro_cal_data[param_index],
|
||||
gyro_report.temperature, _gyro_offset[uorb_index]);
|
||||
|
||||
// get the sensor scale factors and correct the data
|
||||
for (unsigned axis_index = 0; axis_index < 3; axis_index++) {
|
||||
_gyro_scale[uorb_index][axis_index] = _thermal_correction_param.gyro_cal_data[param_index].scale[axis_index];
|
||||
gyro_rate(axis_index) = gyro_rate(axis_index) * _gyro_scale[uorb_index][axis_index] + _gyro_offset[uorb_index][axis_index];
|
||||
gyro_rate(axis_index) = gyro_rate(axis_index) * _gyro_scale[uorb_index][axis_index] +
|
||||
_gyro_offset[uorb_index][axis_index];
|
||||
}
|
||||
|
||||
break;
|
||||
@@ -664,6 +680,7 @@ void VotedSensorsUpdate::gyro_poll(struct sensor_combined_s &raw)
|
||||
raw.timestamp = _last_sensor_data[best_index].timestamp;
|
||||
_gyro.last_best_vote = (uint8_t)best_index;
|
||||
_corrections.gyro_select = (uint8_t)best_index;
|
||||
|
||||
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];
|
||||
@@ -722,36 +739,59 @@ void VotedSensorsUpdate::baro_poll(struct sensor_combined_s &raw)
|
||||
{
|
||||
bool got_update = false;
|
||||
|
||||
for (unsigned i = 0; i < _baro.subscription_count; i++) {
|
||||
for (unsigned uorb_index = 0; uorb_index < _baro.subscription_count; uorb_index++) {
|
||||
bool baro_updated;
|
||||
orb_check(_baro.subscription[i], &baro_updated);
|
||||
orb_check(_baro.subscription[uorb_index], &baro_updated);
|
||||
|
||||
if (baro_updated) {
|
||||
struct baro_report baro_report;
|
||||
|
||||
orb_copy(ORB_ID(sensor_baro), _baro.subscription[i], &baro_report);
|
||||
orb_copy(ORB_ID(sensor_baro), _baro.subscription[uorb_index], &baro_report);
|
||||
|
||||
if (baro_report.timestamp == 0) {
|
||||
continue; //ignore invalid data
|
||||
}
|
||||
|
||||
// Convert from millibar to Pa
|
||||
float corrected_pressure = 100.0f * baro_report.pressure;
|
||||
|
||||
// Apply thermal compensation if available
|
||||
if (_thermal_correction_param.baro_tc_enable == 1) {
|
||||
// search through the available compensation parameter sets looking for one with a matching sensor ID and correct data if found
|
||||
for (unsigned param_index = 0; param_index < 3; param_index++) {
|
||||
if (baro_report.device_id == _thermal_correction_param.baro_cal_data[param_index].ID) {
|
||||
// get the offsets
|
||||
sensors_temp_comp::calc_thermal_offsets_1D(_thermal_correction_param.baro_cal_data[param_index],
|
||||
baro_report.temperature, _baro_offset[uorb_index]);
|
||||
|
||||
// get the sensor scale factors and correct the data
|
||||
// convert pressure reading from millibar to Pa
|
||||
_baro_scale[uorb_index] = _thermal_correction_param.baro_cal_data[param_index].scale;
|
||||
corrected_pressure = corrected_pressure * _baro_scale[uorb_index] + _baro_offset[uorb_index];
|
||||
|
||||
break;
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// First publication with data
|
||||
if (_baro.priority[i] == 0) {
|
||||
if (_baro.priority[uorb_index] == 0) {
|
||||
int32_t priority = 0;
|
||||
orb_priority(_baro.subscription[i], &priority);
|
||||
_baro.priority[i] = (uint8_t)priority;
|
||||
orb_priority(_baro.subscription[uorb_index], &priority);
|
||||
_baro.priority[uorb_index] = (uint8_t)priority;
|
||||
}
|
||||
|
||||
got_update = true;
|
||||
math::Vector<3> vect(baro_report.altitude, 0.f, 0.f);
|
||||
|
||||
_last_sensor_data[i].baro_alt_meter = baro_report.altitude;
|
||||
_last_sensor_data[i].baro_temp_celcius = baro_report.temperature;
|
||||
_last_baro_pressure[i] = baro_report.pressure;
|
||||
_last_sensor_data[uorb_index].baro_alt_meter = baro_report.altitude;
|
||||
_last_sensor_data[uorb_index].baro_temp_celcius = baro_report.temperature;
|
||||
_last_baro_pressure[uorb_index] = corrected_pressure;
|
||||
|
||||
_last_baro_timestamp[i] = baro_report.timestamp;
|
||||
_baro.voter.put(i, baro_report.timestamp, vect.data,
|
||||
baro_report.error_count, _baro.priority[i]);
|
||||
_last_baro_timestamp[uorb_index] = baro_report.timestamp;
|
||||
_baro.voter.put(uorb_index, baro_report.timestamp, vect.data,
|
||||
baro_report.error_count, _baro.priority[uorb_index]);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -760,10 +800,50 @@ void VotedSensorsUpdate::baro_poll(struct sensor_combined_s &raw)
|
||||
_baro.voter.get_best(hrt_absolute_time(), &best_index);
|
||||
|
||||
if (best_index >= 0) {
|
||||
raw.baro_alt_meter = _last_sensor_data[best_index].baro_alt_meter;
|
||||
raw.baro_temp_celcius = _last_sensor_data[best_index].baro_temp_celcius;
|
||||
_last_best_baro_pressure = _last_baro_pressure[best_index];
|
||||
_baro.last_best_vote = (uint8_t)best_index;
|
||||
_corrections.baro_select = (uint8_t)best_index;
|
||||
_corrections.baro_offset = _baro_offset[best_index];
|
||||
_corrections.baro_scale = _baro_scale[best_index];
|
||||
|
||||
/* altitude calculations based on http://www.kansasflyer.org/index.asp?nav=Avi&sec=Alti&tab=Theory&pg=1 */
|
||||
|
||||
/*
|
||||
* PERFORMANCE HINT:
|
||||
*
|
||||
* The single precision calculation is 50 microseconds faster than the double
|
||||
* precision variant. It is however not obvious if double precision is required.
|
||||
* Pending more inspection and tests, we'll leave the double precision variant active.
|
||||
*
|
||||
* Measurements:
|
||||
* double precision: ms5611_read: 992 events, 258641us elapsed, min 202us max 305us
|
||||
* single precision: ms5611_read: 963 events, 208066us elapsed, min 202us max 241us
|
||||
*/
|
||||
|
||||
/* tropospheric properties (0-11km) for standard atmosphere */
|
||||
const double T1 = 15.0 + 273.15; /* temperature at base height in Kelvin */
|
||||
const double a = -6.5 / 1000; /* temperature gradient in degrees per metre */
|
||||
const double g = 9.80665; /* gravity constant in m/s/s */
|
||||
const double R = 287.05; /* ideal gas constant in J/kg/K */
|
||||
|
||||
/* current pressure at MSL in kPa */
|
||||
double p1 = _msl_pressure / 1000.0;
|
||||
|
||||
/* measured pressure in kPa */
|
||||
double p = 0.001f * _last_best_baro_pressure;
|
||||
|
||||
/*
|
||||
* Solve:
|
||||
*
|
||||
* / -(aR / g) \
|
||||
* | (p / p1) . T1 | - T1
|
||||
* \ /
|
||||
* h = ------------------------------- + h1
|
||||
* a
|
||||
*/
|
||||
raw.baro_alt_meter = (((pow((p / p1), (-(a * R) / g))) * T1) - T1) / a;
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -265,13 +265,19 @@ private:
|
||||
|
||||
/* sensor thermal compensation */
|
||||
sensors_temp_comp::Parameters _thermal_correction_param; /**< copy of the thermal correction parameters*/
|
||||
sensors_temp_comp::ParameterHandles _thermal_correction_param_handles; /**< handles for the thermal correction parameters */
|
||||
sensors_temp_comp::ParameterHandles
|
||||
_thermal_correction_param_handles; /**< handles for the thermal correction parameters */
|
||||
struct sensor_correction_s _corrections = {}; /**< struct containing the sensor corrections to be published to the uORB*/
|
||||
orb_advert_t _sensor_correction_pub; /**< handle to the sensor correction uORB topic */
|
||||
float _accel_offset[SENSOR_COUNT_MAX][3]; /**< offsets to be added to the raw accel data after scale factor correction */
|
||||
float _accel_scale[SENSOR_COUNT_MAX][3]; /**< scale factor corrections to be applied to the raw accel data before offsets are added */
|
||||
float _gyro_offset[SENSOR_COUNT_MAX][3]; /**< offsets to be added to the raw angular rate data after scale factor correction */
|
||||
float _gyro_scale[SENSOR_COUNT_MAX][3]; /**< scale factor corrections to be applied to the raw angular rate data before offsets are added */
|
||||
float _baro_offset[SENSOR_COUNT_MAX]; /**< offsets to be added to the raw baro pressure data after scale factor correction */
|
||||
float _baro_scale[SENSOR_COUNT_MAX]; /**< scale factor corrections to be applied to the raw barp pressure data before offsets are added */
|
||||
|
||||
/* altitude conversion calibration */
|
||||
unsigned _msl_pressure; /* in Pa */
|
||||
|
||||
};
|
||||
|
||||
|
||||
Reference in New Issue
Block a user