Airspeed measurement: Add accurate models for dynamic pressure

This allows to get very accurate readings from the SDP3x sensor series from Sensirion using a complete sensor model.

src/modules/sensors/parameters.cpp

@@ -157,6 +157,10 @@ 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_smodel = param_find("CAL_AIR_SMODEL");
+	parameter_handles.air_tube_length = param_find("CAL_AIR_TUBELEN");
+
 	// 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.
 	(void)param_find("RC_CHAN_CNT");
@@ -488,6 +492,10 @@ 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_smodel, &parameters.air_smodel);
+	param_get(parameter_handles.air_tube_length, &parameters.air_tube_length);
+
 	return ret;
 }
 

src/modules/sensors/parameters.h

@@ -146,6 +146,10 @@ struct Parameters {
 
 	float vibration_warning_threshold;
 
+	int32_t air_pmodel;
+	int32_t air_smodel;
+	float air_tube_length;
+
 };
 
 struct ParameterHandles {
@@ -227,6 +231,10 @@ struct ParameterHandles {
 
 	param_t vibe_thresh; /**< vibration threshold */
 
+	param_t air_pmodel;
+	param_t air_smodel;
+	param_t air_tube_length;
+
 };
 
 /**

src/modules/sensors/sensor_params.c

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012-2016 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2017 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
@@ -814,6 +814,35 @@ PARAM_DEFINE_INT32(CAL_MAG_SIDES, 63);
  */
 PARAM_DEFINE_INT32(CAL_BARO_PRIME, 0);
 
+/**
+ * Airspeed sensor pitot model
+ *
+ * @value 0 HB Pitot
+ *
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_INT32(CAL_AIR_PMODEL, 0);
+
+/**
+ * Airspeed sensor model
+ *
+ * @value 0 Membrane sensor
+ * @value 1 Sensirion SDP3x
+ *
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_INT32(CAL_AIR_SMODEL, 0);
+
+/**
+ * Airspeed sensor tube length
+ * @min 0.01
+ * @max 0.5
+ * @unit meter
+ *
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_FLOAT(CAL_AIR_TUBELEN, 0.15);
+
 /**
  * Differential pressure sensor offset
  *

src/modules/sensors/sensors.cpp

@@ -326,10 +326,13 @@ 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(_diff_pres.differential_pressure_filtered_pa));
+						   calc_indicated_airspeed_corrected((enum AIRSPEED_PITOT_MODEL)_parameters.air_pmodel,
+								   (enum AIRSPEED_SENSOR_MODEL)_parameters.air_smodel, _parameters.air_tube_length,
+								   _diff_pres.differential_pressure_filtered_pa, _voted_sensors_update.baro_pressure(),
+								   air_temperature_celsius));
 
 		_airspeed.true_airspeed_m_s = math::max(0.0f,
-							calc_true_airspeed(_diff_pres.differential_pressure_filtered_pa + _voted_sensors_update.baro_pressure(),
+							calc_true_airspeed_from_indicated(_airspeed.indicated_airspeed_m_s,
 									_voted_sensors_update.baro_pressure(), air_temperature_celsius));
 
 		_airspeed.true_airspeed_unfiltered_m_s = math::max(0.0f,

src/modules/systemlib/airspeed.c

@@ -45,6 +45,93 @@
 #include <geo/geo.h>
 #include "airspeed.h"
 
+/**
+ * Calculate indicated airspeed.
+ *
+ * Note that the indicated airspeed is not the true airspeed because it
+ * lacks the air density compensation. Use the calc_true_airspeed functions to get
+ * the true airspeed.
+ *
+ * @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)
+{
+
+	// air density in kg/m3
+	double 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;
+
+	switch (smodel) {
+
+	case AIRSPEED_SENSOR_MODEL_MEMBRANE: {
+			dp_pitot = 0.0f;
+			dp_tube = 0.0f;
+			dv = 0.0f;
+		}
+		break;
+
+	case AIRSPEED_SENSOR_MODEL_SDP3X: {
+			//
+			double flow_SDP33 = (300.805 - 300.878 / (0.00344205 * dp * 0.68698 * 0.68698 + 1)) * 1.29 / rho_air;
+
+			switch (pmodel) {
+			case AIRSPEED_PITOT_MODEL_HB:
+				dp_pitot = 28557670.0 - 28557670.0 / (1 + (flow_SDP33 / 5027611.0) * 1.227924 * 1.227924);
+				break;
+
+			default:
+				dp_pitot = 0.0f;
+				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;
+		}
+		break;
+	}
+
+	// if (!PX4_ISFINITE(dp_tube)) {
+	// 	dp_tube = 0.0f;
+	// }
+
+	// if (!PX4_ISFINITE(dp_pitot)) {
+	// 	dp_pitot = 0.0f;
+	// }
+
+	// if (!PX4_ISFINITE(dv)) {
+	// 	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);
+
+	// corrected airspeed
+	float airspeed_corrected = airspeed_uncorrected + dv;
+
+	// return result with correct sign
+	return (differential_pressure > 0.0f) ? airspeed_corrected : -airspeed_corrected;
+}
+
 
 /**
  * Calculate indicated airspeed.
@@ -59,6 +146,7 @@
 float calc_indicated_airspeed(float differential_pressure)
 {
 
+
 	if (differential_pressure > 0.0f) {
 		return sqrtf((2.0f * differential_pressure) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C);
 

src/modules/systemlib/airspeed.h

@@ -1,7 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012-2013 PX4 Development Team. All rights reserved.
- *   Author: Lorenz Meier <lm@inf.ethz.ch>
+ *   Copyright (c) 2012-2013, 2017 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
@@ -36,7 +35,7 @@
  * @file airspeed.h
  * Airspeed estimation declarations
  *
- * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Lorenz Meier <lorenz@px4.io>
  *
  */
 
@@ -48,6 +47,29 @@
 
 __BEGIN_DECLS
 
+enum AIRSPEED_SENSOR_MODEL {
+	AIRSPEED_SENSOR_MODEL_MEMBRANE = 0,
+	AIRSPEED_SENSOR_MODEL_SDP3X,
+};
+
+enum AIRSPEED_PITOT_MODEL {
+	AIRSPEED_PITOT_MODEL_HB = 0
+};
+
+/**
+ * Calculate indicated airspeed.
+ *
+ * Note that the indicated airspeed is not the true airspeed because it
+ * lacks the air density compensation. Use the calc_true_airspeed functions to get
+ * the true airspeed.
+ *
+ * @param total_pressure pressure inside the pitot/prandtl tube
+ * @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);
+
 /**
  * Calculate indicated airspeed.
  *