zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

sensors: add baro pressure temperature compensation and reporting

sbg
Paul Riseborough 8 years ago committed by Lorenz Meier
parent
c21b4aaf2e
commit
0485c2b94a
5 changed files with 457 additions and 30 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 286
      src/modules/sensors/temp_comp_params_baro.c
  2. 65
      src/modules/sensors/temperature_compensation.cpp
  3. 4
      src/modules/sensors/temperature_compensation.h
  4. 124
      src/modules/sensors/voted_sensors_update.cpp
  5. 8
      src/modules/sensors/voted_sensors_update.h

286
src/modules/sensors/temp_comp_params_baro.c
Unescape View File

@ -0,0 +1,286 @@ @@ -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);

65
src/modules/sensors/temperature_compensation.cpp
Unescape View File

@ -109,6 +109,35 @@ int initialize_parameter_handles(ParameterHandles &parameter_handles) @@ -109,6 +109,35 @@ int initialize_parameter_handles(ParameterHandles &parameter_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 &parameter_handles, Parameters &par @@ -178,7 +207,33 @@ int update_parameters(const ParameterHandles &parameter_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(&parameters.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 @@ -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;

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

@ -134,7 +134,7 @@ struct Parameters { @@ -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 { @@ -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];
};
/**

124
src/modules/sensors/voted_sensors_update.cpp
Unescape View File

@ -84,16 +84,22 @@ int VotedSensorsUpdate::init(sensor_combined_s &raw) @@ -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) @@ -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) @@ -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) @@ -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) @@ -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) @@ -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) @@ -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) @@ -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) @@ -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) @@ -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;
}
}
}

8
src/modules/sensors/voted_sensors_update.h
Unescape View File

@ -265,13 +265,19 @@ private: @@ -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 */
};

Write Preview
Loading…
Cancel
Save