From a5cc4bcd08e8f3fec404528db9280b239a7289cc Mon Sep 17 00:00:00 2001 From: Florian Achermann Date: Thu, 23 Nov 2017 04:03:26 +0100 Subject: [PATCH] Update SDP3x Airspeed Correction (#8242) Update the model for the standard configuration based on the model from Sensirion and add also an option to do the tube pressure loss correction from Sensirion for other configurations. --- Tools/models/sdp3x_pitot_model.py | 49 +++++--- cmake/configs/nuttx_px4fmu-v2_default.cmake | 2 +- src/modules/sensors/parameters.cpp | 6 +- src/modules/sensors/parameters.h | 7 +- src/modules/sensors/sensor_params.c | 35 +++++- src/modules/sensors/sensors.cpp | 4 +- src/modules/systemlib/airspeed.c | 117 ++++++++++++++------ src/modules/systemlib/airspeed.h | 11 +- 8 files changed, 163 insertions(+), 68 deletions(-) diff --git a/Tools/models/sdp3x_pitot_model.py b/Tools/models/sdp3x_pitot_model.py index 3af8fe5813..44c013fdc8 100644 --- a/Tools/models/sdp3x_pitot_model.py +++ b/Tools/models/sdp3x_pitot_model.py @@ -30,18 +30,29 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +# formula for metal pitot tube with round tip as here: https://drotek.com/shop/2986-large_default/sdp3x-airspeed-sensor-kit-sdp31.jpg +# and tubing as provided by px4/drotek (1.5 mm diameter) + + + import numpy as np import matplotlib.pyplot as plt +P_cal=96600. #Pa +P_amb=96600. #dummy-value, use absolute pressure sensor!! ## differential pressure, sensor values in Pascal -dp_SDP33=np.linspace(0,800,100) +dp_SDP33_raw=np.linspace(0,80,100) + +dp_SDP33=dp_SDP33_raw*P_cal/P_amb + + ## total length tube in mm = length dynamic port+ length static port; compensation only valid for inner diameter of 1.5mm -l_tube=400 +l_tube=450 ## densitiy air in kg/m3 rho_air=1.29 @@ -50,19 +61,21 @@ rho_air=1.29 ## flow through sensor flow_SDP33=(300.805 - 300.878/(0.00344205*dp_SDP33**0.68698 + 1))*1.29/rho_air -## additional dp through pitot tube -dp_Pitot=28557670. - 28557670./(1 + (flow_SDP33/5027611)**1.227924) +## additional dp through pitot tube +dp_Pitot=(0.0032*flow_SDP33**2 + 0.0123*flow_SDP33+1.)*1.29/rho_air + ## pressure drop through tube -dp_Tube=flow_SDP33*0.000746124*l_tube*rho_air +dp_Tube=(flow_SDP33*0.674)/450*l_tube*rho_air/1.29 ## speed at pitot-tube tip due to flow through sensor -dv=0.0331582*flow_SDP33 +dv=0.125*flow_SDP33 ## sum of all pressure drops dp_tot=dp_SDP33+dp_Tube+dp_Pitot + ## computed airspeed without correction for inflow-speed at tip of pitot-tube airspeed_uncorrected=np.sqrt(2*dp_tot/rho_air) @@ -74,18 +87,24 @@ airspeed_corrected=airspeed_uncorrected+dv airspeed_raw=np.sqrt(2*dp_SDP33/rho_air) - - plt.figure() -plt.plot(dp_SDP33,airspeed_corrected/airspeed_raw) +plt.plot(dp_SDP33,airspeed_corrected) plt.xlabel('differential pressure raw value [Pa]') -plt.ylabel('correction factor [-]') +plt.ylabel('airspeed_corrected [m/s]') plt.show() -plt.figure() -plt.plot(airspeed_corrected,(airspeed_corrected-airspeed_raw)/airspeed_corrected) -plt.xlabel('airspeed [m/s]') -plt.ylabel('relative error [-]') -plt.show() +##plt.figure() +##plt.plot(dp_SDP33,airspeed_corrected/airspeed_raw) +##plt.xlabel('differential pressure raw value [Pa]') +##plt.ylabel('correction factor [-]') +##plt.show() +## +## +## +##plt.figure() +##plt.plot(airspeed_corrected,(airspeed_corrected-airspeed_raw)/airspeed_corrected) +##plt.xlabel('airspeed [m/s]') +##plt.ylabel('relative error [-]') +##plt.show() \ No newline at end of file diff --git a/cmake/configs/nuttx_px4fmu-v2_default.cmake b/cmake/configs/nuttx_px4fmu-v2_default.cmake index d7bafe40cd..3bb8b1cc77 100644 --- a/cmake/configs/nuttx_px4fmu-v2_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v2_default.cmake @@ -33,7 +33,7 @@ set(config_module_list #drivers/hott/hott_sensors #drivers/blinkm drivers/airspeed - drivers/ets_airspeed + #drivers/ets_airspeed drivers/ms4525_airspeed drivers/ms5525_airspeed drivers/sdp3x_airspeed diff --git a/src/modules/sensors/parameters.cpp b/src/modules/sensors/parameters.cpp index dc6483e8a7..c5bc9e8356 100644 --- a/src/modules/sensors/parameters.cpp +++ b/src/modules/sensors/parameters.cpp @@ -157,8 +157,9 @@ int initialize_parameter_handles(ParameterHandles ¶meter_handles) parameter_handles.vibe_thresh = param_find("ATT_VIBE_THRESH"); - parameter_handles.air_pmodel = param_find("CAL_AIR_PMODEL"); + parameter_handles.air_cmodel = param_find("CAL_AIR_CMODEL"); parameter_handles.air_tube_length = param_find("CAL_AIR_TUBELEN"); + parameter_handles.air_tube_diameter_mm = param_find("CAL_AIR_TUBED_MM"); // These are parameters for which QGroundControl always expects to be returned in a list request. // We do a param_find here to force them into the list. @@ -491,8 +492,9 @@ int update_parameters(const ParameterHandles ¶meter_handles, Parameters &par param_get(parameter_handles.vibe_thresh, ¶meters.vibration_warning_threshold); - param_get(parameter_handles.air_pmodel, ¶meters.air_pmodel); + param_get(parameter_handles.air_cmodel, ¶meters.air_cmodel); param_get(parameter_handles.air_tube_length, ¶meters.air_tube_length); + param_get(parameter_handles.air_tube_diameter_mm, ¶meters.air_tube_diameter_mm); return ret; } diff --git a/src/modules/sensors/parameters.h b/src/modules/sensors/parameters.h index 78b7c9302d..f3af261796 100644 --- a/src/modules/sensors/parameters.h +++ b/src/modules/sensors/parameters.h @@ -146,9 +146,9 @@ struct Parameters { float vibration_warning_threshold; - int32_t air_pmodel; + int32_t air_cmodel; float air_tube_length; - + float air_tube_diameter_mm; }; struct ParameterHandles { @@ -230,8 +230,9 @@ struct ParameterHandles { param_t vibe_thresh; /**< vibration threshold */ - param_t air_pmodel; + param_t air_cmodel; param_t air_tube_length; + param_t air_tube_diameter_mm; }; diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 03967fa9f7..093520c698 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -39,24 +39,49 @@ PARAM_DEFINE_INT32(CAL_BARO_PRIME, 0); /** - * Airspeed sensor pitot model + * Airspeed sensor compensation model for the SDP3x * - * @value 0 HB Pitot + * Model with Pitot + * CAL_AIR_TUBED_MM: Not used, 1.5 mm tubes assumed. + * CAL_AIR_TUBELEN: Length of the tubes connecting the pitot to the sensor. + * Model without Pitot (1.5 mm tubes) + * CAL_AIR_TUBED_MM: Not used, 1.5 mm tubes assumed. + * CAL_AIR_TUBELEN: Length of the tubes connecting the pitot to the sensor. + * Tube Pressure Drop + * CAL_AIR_TUBED_MM: Diameter in mm of the pitot and tubes, must have the same diameter. + * CAL_AIR_TUBELEN: Length of the tubes connecting the pitot to the sensor and the static + dynamic port length of the pitot. * + * @value 0 Model with Pitot + * @value 1 Model without Pitot (1.5 mm tubes) + * @value 2 Tube Pressure Drop * @group Sensor Calibration */ -PARAM_DEFINE_INT32(CAL_AIR_PMODEL, 0); +PARAM_DEFINE_INT32(CAL_AIR_CMODEL, 0); /** - * Airspeed sensor tube length + * Airspeed sensor tube length. + * + * See the CAL_AIR_CMODEL explanation on how this parameter should be set. + * * @min 0.01 - * @max 0.5 + * @max 2.00 * @unit meter * * @group Sensor Calibration */ PARAM_DEFINE_FLOAT(CAL_AIR_TUBELEN, 0.2f); +/** + * Airspeed sensor tube diameter. Only used for the Tube Pressure Drop Compensation. + * + * @min 0.1 + * @max 100 + * @unit millimeter + * + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(CAL_AIR_TUBED_MM, 1.5f); + /** * Differential pressure sensor offset * diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 6216c27039..b1d1b48c72 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -345,8 +345,8 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw) /* don't risk to feed negative airspeed into the system */ _airspeed.indicated_airspeed_m_s = math::max(0.0f, - calc_indicated_airspeed_corrected((enum AIRSPEED_PITOT_MODEL)_parameters.air_pmodel, - smodel, _parameters.air_tube_length, + calc_indicated_airspeed_corrected((enum AIRSPEED_COMPENSATION_MODEL)_parameters.air_cmodel, + smodel, _parameters.air_tube_length, _parameters.air_tube_diameter_mm, _diff_pres.differential_pressure_filtered_pa, _voted_sensors_update.baro_pressure(), air_temperature_celsius)); diff --git a/src/modules/systemlib/airspeed.c b/src/modules/systemlib/airspeed.c index 26a8aed81e..6bca3a96a0 100644 --- a/src/modules/systemlib/airspeed.c +++ b/src/modules/systemlib/airspeed.c @@ -55,59 +55,107 @@ * @param differential_pressure total_ pressure - static pressure * @return indicated airspeed in m/s */ -float calc_indicated_airspeed_corrected(enum AIRSPEED_PITOT_MODEL pmodel, enum AIRSPEED_SENSOR_MODEL smodel, - float tube_len, float differential_pressure, float pressure_ambient, float temperature_celsius) +float calc_indicated_airspeed_corrected(enum AIRSPEED_COMPENSATION_MODEL pmodel, enum AIRSPEED_SENSOR_MODEL smodel, + float tube_len, float tube_dia_mm, float differential_pressure, float pressure_ambient, float temperature_celsius) { // air density in kg/m3 - double rho_air = get_air_density(pressure_ambient, temperature_celsius); + const float rho_air = get_air_density(pressure_ambient, temperature_celsius); - double dp = fabsf(differential_pressure); - // additional dp through pitot tube - float dp_pitot; - float dp_tube; - float dv; + const float dp = fabsf(differential_pressure); + float dp_tot = dp; + + float dv = 0.0f; switch (smodel) { case AIRSPEED_SENSOR_MODEL_MEMBRANE: { - dp_pitot = 0.0f; - dp_tube = 0.0f; - dv = 0.0f; + // do nothing } break; case AIRSPEED_SENSOR_MODEL_SDP3X: { - // flow through sensor - double flow_SDP33 = (300.805 - 300.878 / (0.00344205 * pow(dp, 0.68698) + 1)) * 1.29 / rho_air; - - // for too small readings the compensation might result in a negative flow which causes numerical issues - if (flow_SDP33 < 0.0) { - flow_SDP33 = 0.0; - } - + // assumes a metal pitot tube with round tip as here: https://drotek.com/shop/2986-large_default/sdp3x-airspeed-sensor-kit-sdp31.jpg + // and tubing as provided by px4/drotek (1.5 mm diameter) + // The tube_len represents the length of the tubes connecting the pitot to the sensor. switch (pmodel) { - case AIRSPEED_PITOT_MODEL_HB: - dp_pitot = 28557670.0 - 28557670.0 / (1 + pow((flow_SDP33 / 5027611.0), 1.227924)); + case AIRSPEED_COMPENSATION_MODEL_PITOT: + case AIRSPEED_COMPENSATION_MODEL_NO_PITOT: { + const float dp_corr = dp * 96600.0f / pressure_ambient; + // flow through sensor + float flow_SDP33 = (300.805f - 300.878f / (0.00344205f * powf(dp_corr, 0.68698f) + 1.0f)) * 1.29f / rho_air; + + // for too small readings the compensation might result in a negative flow which causes numerical issues + if (flow_SDP33 < 0.0f) { + flow_SDP33 = 0.0f; + } + + float dp_pitot = 0.0f; + + switch (pmodel) { + case AIRSPEED_COMPENSATION_MODEL_PITOT: + dp_pitot = (0.0032f * flow_SDP33 * flow_SDP33 + 0.0123f * flow_SDP33 + 1.0f) * 1.29f / rho_air; + break; + + default: + // do nothing + break; + } + + // pressure drop through tube + const float dp_tube = (flow_SDP33 * 0.674f) / 450.0f * tube_len * rho_air / 1.29f; + + // speed at pitot-tube tip due to flow through sensor + dv = 0.125f * flow_SDP33; + + // sum of all pressure drops + dp_tot = dp_corr + dp_tube + dp_pitot; + } break; - default: - dp_pitot = 0.0f; + case AIRSPEED_COMPENSATION_TUBE_PRESSURE_LOSS: { + // Pressure loss compensation as defined in https://goo.gl/UHV1Vv. + // tube_dia_mm: Diameter in mm of the pitot and tubes, must have the same diameter. + // tube_len: Length of the tubes connecting the pitot to the sensor and the static + dynamic port length of the pitot. + + // check if the tube diameter and dp is nonzero to avoid division by 0 + if ((tube_dia_mm > 0.0f) && (dp > 0.0f)) { + const float d_tubePow4 = powf(tube_dia_mm * 1e-3f, 4); + const float denominator = (float)M_PI * d_tubePow4 * rho_air * dp; + + // avoid division by 0 + float eps = 0.0f; + + if (fabsf(denominator) > 1e-32f) { + const float viscosity = (18.205f + 0.0484f * (temperature_celsius - 20.0f)) * 1e-6f; + + // 4.79 * 1e-7 -> mass flow through sensor + // 59.5 -> dp sensor constant where linear and quadratic contribution to dp vs flow is equal + eps = -64.0f * tube_len * viscosity * 4.79f * 1e-7f * (sqrtf(1.0f + 8.0f * dp / 59.3319f) - 1.0f) / denominator; + } + + // range check on eps + if (fabsf(eps) >= 1.0f) { + eps = 0.0f; + } + + // pressure correction + dp_tot = dp / (1.0f + eps); + } + } + break; + + default: { + // do nothing + } break; } - // pressure drop through tube - dp_tube = flow_SDP33 * 0.000746124 * (double)tube_len * rho_air; - - // speed at pitot-tube tip due to flow through sensor - dv = 0.0331582 * flow_SDP33; } break; default: { - dp_pitot = 0.0f; - dp_tube = 0.0f; - dv = 0.0f; + // do nothing } break; } @@ -124,14 +172,11 @@ float calc_indicated_airspeed_corrected(enum AIRSPEED_PITOT_MODEL pmodel, enum A // dv = 0.0f; // } - // sum of all pressure drops - float dp_tot = (float)dp + dp_tube + dp_pitot; - // computed airspeed without correction for inflow-speed at tip of pitot-tube - float airspeed_uncorrected = sqrtf(2 * dp_tot / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C); + const float airspeed_uncorrected = sqrtf(2.0f * dp_tot / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C); // corrected airspeed - float airspeed_corrected = airspeed_uncorrected + dv; + const float airspeed_corrected = airspeed_uncorrected + dv; // return result with correct sign return (differential_pressure > 0.0f) ? airspeed_corrected : -airspeed_corrected; diff --git a/src/modules/systemlib/airspeed.h b/src/modules/systemlib/airspeed.h index 379de86b91..59331faf07 100644 --- a/src/modules/systemlib/airspeed.h +++ b/src/modules/systemlib/airspeed.h @@ -52,8 +52,10 @@ enum AIRSPEED_SENSOR_MODEL { AIRSPEED_SENSOR_MODEL_SDP3X, }; -enum AIRSPEED_PITOT_MODEL { - AIRSPEED_PITOT_MODEL_HB = 0 +enum AIRSPEED_COMPENSATION_MODEL { + AIRSPEED_COMPENSATION_MODEL_PITOT = 0, + AIRSPEED_COMPENSATION_MODEL_NO_PITOT = 1, + AIRSPEED_COMPENSATION_TUBE_PRESSURE_LOSS = 2 }; /** @@ -67,8 +69,9 @@ enum AIRSPEED_PITOT_MODEL { * @param static_pressure pressure at the side of the tube/airplane * @return indicated airspeed in m/s */ -__EXPORT float calc_indicated_airspeed_corrected(enum AIRSPEED_PITOT_MODEL pmodel, enum AIRSPEED_SENSOR_MODEL smodel, - float tube_len, float differential_pressure, float pressure_ambient, float temperature_celsius); +__EXPORT float calc_indicated_airspeed_corrected(enum AIRSPEED_COMPENSATION_MODEL pmodel, + enum AIRSPEED_SENSOR_MODEL smodel, + float tube_len, float tube_dia_mm, float differential_pressure, float pressure_ambient, float temperature_celsius); /** * Calculate indicated airspeed.