diff --git a/src/modules/sensors/temp_comp_params_baro.c b/src/modules/sensors/temp_comp_params_baro.c new file mode 100644 index 0000000000..1e0403d116 --- /dev/null +++ b/src/modules/sensors/temp_comp_params_baro.c @@ -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 + */ + +/** + * 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); diff --git a/src/modules/sensors/temperature_compensation.cpp b/src/modules/sensors/temperature_compensation.cpp index be8f07ff88..619dcbee9b 100644 --- a/src/modules/sensors/temperature_compensation.cpp +++ b/src/modules/sensors/temperature_compensation.cpp @@ -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; diff --git a/src/modules/sensors/temperature_compensation.h b/src/modules/sensors/temperature_compensation.h index b77c8959d9..48ab652f3a 100644 --- a/src/modules/sensors/temperature_compensation.h +++ b/src/modules/sensors/temperature_compensation.h @@ -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]; }; /** diff --git a/src/modules/sensors/voted_sensors_update.cpp b/src/modules/sensors/voted_sensors_update.cpp index a4e25d26ba..4e61406786 100644 --- a/src/modules/sensors/voted_sensors_update.cpp +++ b/src/modules/sensors/voted_sensors_update.cpp @@ -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; + } } } diff --git a/src/modules/sensors/voted_sensors_update.h b/src/modules/sensors/voted_sensors_update.h index 47823b2e17..1891cec99e 100644 --- a/src/modules/sensors/voted_sensors_update.h +++ b/src/modules/sensors/voted_sensors_update.h @@ -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 */ };