From b59aefc989d34cad32317b0235192310ce4d47e7 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lorenz@px4.io>
Date: Sun, 30 Jul 2017 21:45:43 +0200
Subject: [PATCH] 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 | 8 +++
src/modules/sensors/parameters.h | 8 +++
src/modules/sensors/sensor_params.c | 31 +++++++++-
src/modules/sensors/sensors.cpp | 7 ++-
src/modules/systemlib/airspeed.c | 88 +++++++++++++++++++++++++++++
src/modules/systemlib/airspeed.h | 28 ++++++++-
6 files changed, 164 insertions(+), 6 deletions(-)
diff --git a/src/modules/sensors/parameters.cpp b/src/modules/sensors/parameters.cpp
index 65ff620a41..6f8a8b2ed4 100644
--- a/src/modules/sensors/parameters.cpp
+++ b/src/modules/sensors/parameters.cpp
@@ -157,6 +157,10 @@ 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_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 ¶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_smodel, ¶meters.air_smodel);
+ param_get(parameter_handles.air_tube_length, ¶meters.air_tube_length);
+
return ret;
}
diff --git a/src/modules/sensors/parameters.h b/src/modules/sensors/parameters.h
index 072dd3d689..73dd03b746 100644
--- a/src/modules/sensors/parameters.h
+++ b/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;
+
};
/**
diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c
index 7baffd06ac..b7f2ea8ba9 100644
--- a/src/modules/sensors/sensor_params.c
+++ b/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
*
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 41fa63e5b4..9d3087d557 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/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,
diff --git a/src/modules/systemlib/airspeed.c b/src/modules/systemlib/airspeed.c
index fa5db7c89b..aad1540ea0 100644
--- a/src/modules/systemlib/airspeed.c
+++ b/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);
diff --git a/src/modules/systemlib/airspeed.h b/src/modules/systemlib/airspeed.h
index 2a2b0873da..379de86b91 100644
--- a/src/modules/systemlib/airspeed.h
+++ b/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.
*
--
GitLab