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.
2026-06-26 02:40:34 +08:00 · 2017-11-23 04:03:26 +01:00
parent 85a7a0a86a
commit a5cc4bcd08
8 changed files with 163 additions and 68 deletions
@@ -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;
@@ -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.