dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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.
This commit is contained in:
Florian Achermann 2017-11-23 04:03:26 +01:00 committed by Daniel Agar
parent 85a7a0a86a
commit a5cc4bcd08
8 changed files with 163 additions and 68 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

49
Tools/models/sdp3x_pitot_model.py
View File

@ -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()

2
cmake/configs/nuttx_px4fmu-v2_default.cmake
View File

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

6
src/modules/sensors/parameters.cpp
View File

@ -157,8 +157,9 @@ int initialize_parameter_handles(ParameterHandles &parameter_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 &parameter_handles, Parameters &par
param_get(parameter_handles.vibe_thresh, &parameters.vibration_warning_threshold);
param_get(parameter_handles.air_pmodel, &parameters.air_pmodel);
param_get(parameter_handles.air_cmodel, &parameters.air_cmodel);
param_get(parameter_handles.air_tube_length, &parameters.air_tube_length);
param_get(parameter_handles.air_tube_diameter_mm, &parameters.air_tube_diameter_mm);
return ret;
}

7
src/modules/sensors/parameters.h
View File

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

35
src/modules/sensors/sensor_params.c
View File

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

4
src/modules/sensors/sensors.cpp
View File

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

117
src/modules/systemlib/airspeed.c
View File

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

11
src/modules/systemlib/airspeed.h
View File

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