From 0b8719c12462eb41889151e94cc91ce516736ec3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Roman=20Dvo=C5=99=C3=A1k?= Date: Sun, 9 May 2021 13:03:21 +0200 Subject: [PATCH 1/3] add calculation of airspeed profile for TFSLOT, selectable by parameter --- src/lib/airspeed/airspeed.cpp | 33 ++++++++++++++++++++++++++++- src/lib/airspeed/airspeed.h | 3 ++- src/modules/sensors/sensor_params.c | 1 + 3 files changed, 35 insertions(+), 2 deletions(-) diff --git a/src/lib/airspeed/airspeed.cpp b/src/lib/airspeed/airspeed.cpp index 40677569a23c..8cf28ec508d2 100644 --- a/src/lib/airspeed/airspeed.cpp +++ b/src/lib/airspeed/airspeed.cpp @@ -141,6 +141,31 @@ float calc_IAS_corrected(enum AIRSPEED_COMPENSATION_MODEL pmodel, enum AIRSPEED_ } break; + case AIRSPEED_COMPENSATION_TFSLOT: { + 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; + + dp_pitot = (0.0032f * flow_SDP33 * flow_SDP33 + 0.0123f * flow_SDP33 + 1.0f) * 1.29f / rho_air; + + // 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: { // do nothing } @@ -163,7 +188,13 @@ float calc_IAS_corrected(enum AIRSPEED_COMPENSATION_MODEL pmodel, enum AIRSPEED_ const float airspeed_corrected = airspeed_uncorrected + dv; // return result with correct sign - return (differential_pressure > 0.0f) ? airspeed_corrected : -airspeed_corrected; + if (pmodel == AIRSPEED_COMPENSATION_TFSLOT) { + // TFSLOT has reversed polarity + return (differential_pressure > 0.0f) ? - airspeed_corrected : airspeed_corrected; + + } else { + return (differential_pressure > 0.0f) ? airspeed_corrected : -airspeed_corrected; + } } float calc_IAS(float differential_pressure) diff --git a/src/lib/airspeed/airspeed.h b/src/lib/airspeed/airspeed.h index a5c0c14fdea6..adb2c69fec5f 100644 --- a/src/lib/airspeed/airspeed.h +++ b/src/lib/airspeed/airspeed.h @@ -54,7 +54,8 @@ enum AIRSPEED_SENSOR_MODEL { enum AIRSPEED_COMPENSATION_MODEL { AIRSPEED_COMPENSATION_MODEL_PITOT = 0, AIRSPEED_COMPENSATION_MODEL_NO_PITOT = 1, - AIRSPEED_COMPENSATION_TUBE_PRESSURE_LOSS = 2 + AIRSPEED_COMPENSATION_TUBE_PRESSURE_LOSS = 2, + AIRSPEED_COMPENSATION_TFSLOT = 3 }; /** diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index a69f2a79de1f..321cb8991c10 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -47,6 +47,7 @@ * @value 0 Model with Pitot * @value 1 Model without Pitot (1.5 mm tubes) * @value 2 Tube Pressure Drop + * @value 3 TFSLOT - NACAxxxx (Venturi based airspeed sensor) * * @group Sensors */ From fa62e222ff9f406674caf0ca2c6bff09b27ba513 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Roman=20Dvo=C5=99=C3=A1k?= Date: Mon, 12 Sep 2022 16:45:12 +0200 Subject: [PATCH 2/3] Update sensor_params.c Update sensor_params.c Update airspeed.cpp Update airspeed.h Update sensor_params.c, sync submodule --- src/lib/airspeed/airspeed.cpp | 2 +- src/lib/airspeed/airspeed.h | 2 +- src/modules/sensors/sensor_params.c | 6 ++++-- 3 files changed, 6 insertions(+), 4 deletions(-) diff --git a/src/lib/airspeed/airspeed.cpp b/src/lib/airspeed/airspeed.cpp index 8cf28ec508d2..166672d75f76 100644 --- a/src/lib/airspeed/airspeed.cpp +++ b/src/lib/airspeed/airspeed.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012-2013 PX4 Development Team. All rights reserved. + * Copyright (C) 2012-2021 PX4 Development Team. All rights reserved. * Author: Lorenz Meier * * Redistribution and use in source and binary forms, with or without diff --git a/src/lib/airspeed/airspeed.h b/src/lib/airspeed/airspeed.h index adb2c69fec5f..243d99cbd798 100644 --- a/src/lib/airspeed/airspeed.h +++ b/src/lib/airspeed/airspeed.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2013, 2017 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2021, 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 diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 321cb8991c10..059bcd16181e 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2021 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 @@ -43,11 +43,13 @@ * 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. + * Venturi effect sensor + * Airspeed sensor based on Venturi effect with NACA profile. Supports TFSLOT airspeed sensor. * * @value 0 Model with Pitot * @value 1 Model without Pitot (1.5 mm tubes) * @value 2 Tube Pressure Drop - * @value 3 TFSLOT - NACAxxxx (Venturi based airspeed sensor) + * @value 3 Venturi effect airspeed sensor * * @group Sensors */ From 222f787f01bda1f9dca19850dabab3f7941c8d3d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Roman=20Dvo=C5=99=C3=A1k?= Date: Thu, 4 May 2023 19:45:48 +0200 Subject: [PATCH 3/3] Add new airspeed equation for TFSLOT (or Venturi based aspd sensors) --- src/lib/airspeed/airspeed.cpp | 23 +++++++++++++----- src/lib/airspeed/airspeed.h | 7 +++--- src/modules/sensors/sensor_params.c | 36 +++++++++++++++++++++++++++++ src/modules/sensors/sensors.cpp | 13 +++++++++-- src/modules/sensors/sensors.hpp | 8 ++++++- 5 files changed, 75 insertions(+), 12 deletions(-) diff --git a/src/lib/airspeed/airspeed.cpp b/src/lib/airspeed/airspeed.cpp index 166672d75f76..12dcf529c110 100644 --- a/src/lib/airspeed/airspeed.cpp +++ b/src/lib/airspeed/airspeed.cpp @@ -49,7 +49,8 @@ using atmosphere::getDensityFromPressureAndTemp; using atmosphere::kAirDensitySeaLevelStandardAtmos; float calc_IAS_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) + float tube_len, float tube_dia_mm, float venturi_input_crs, float venturi_1st_crs, + float venturi_2nd_crs, float differential_pressure, float pressure_ambient, float temperature_celsius) { if (!PX4_ISFINITE(temperature_celsius)) { temperature_celsius = 15.f; // ICAO Standard Atmosphere 15 degrees Celsius @@ -181,18 +182,28 @@ float calc_IAS_corrected(enum AIRSPEED_COMPENSATION_MODEL pmodel, enum AIRSPEED_ break; } - // computed airspeed without correction for inflow-speed at tip of pitot-tube - const float airspeed_uncorrected = sqrtf(2.0f * dp_tot / kAirDensitySeaLevelStandardAtmos); - - // corrected airspeed - const float airspeed_corrected = airspeed_uncorrected + dv; // return result with correct sign if (pmodel == AIRSPEED_COMPENSATION_TFSLOT) { + // computed airspeed without correction for inflow-speed at tip of pitot-tube + const float airspeed_uncorrected = sqrtf(((2.0f * dp_tot) / (rho_air * powf((venturi_1st_crs / venturi_2nd_crs), + 4) - 1))); + + // corrected airspeed + const float airspeed_corrected = airspeed_uncorrected + dv; + // TFSLOT has reversed polarity return (differential_pressure > 0.0f) ? - airspeed_corrected : airspeed_corrected; } else { + + + // computed airspeed without correction for inflow-speed at tip of pitot-tube + const float airspeed_uncorrected = sqrtf(2.0f * dp_tot / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C); + + // corrected airspeed + const float airspeed_corrected = airspeed_uncorrected + dv; + return (differential_pressure > 0.0f) ? airspeed_corrected : -airspeed_corrected; } } diff --git a/src/lib/airspeed/airspeed.h b/src/lib/airspeed/airspeed.h index 243d99cbd798..128156657eb2 100644 --- a/src/lib/airspeed/airspeed.h +++ b/src/lib/airspeed/airspeed.h @@ -69,9 +69,10 @@ enum AIRSPEED_COMPENSATION_MODEL { * @param static_pressure pressure at the side of the tube/airplane * @return indicated airspeed in m/s */ -__EXPORT float calc_IAS_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); +__EXPORT float calc_IAS_corrected(enum AIRSPEED_COMPENSATION_MODEL pmodel, enum AIRSPEED_SENSOR_MODEL smodel, + float tube_len, float tube_dia_mm, float venturi_input_crs, float venturi_1st_crs, + float venturi_2nd_crs, float differential_pressure, float pressure_ambient, float temperature_celsius); + /** * Calculate indicated airspeed (IAS). diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 059bcd16181e..c54f2088ded0 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -79,6 +79,42 @@ PARAM_DEFINE_FLOAT(CAL_AIR_TUBELEN, 0.2f); */ PARAM_DEFINE_FLOAT(CAL_AIR_TUBED_MM, 1.5f); +/** + * Venturi tube sensor + * + * @min 0 + * @max 10000 + * @unit mm + * +*/ + +PARAM_DEFINE_FLOAT(CAL_AIR_ENT_CRS, 1600); + + +/** + * Venturi tube sensor + * + * @min 0 + * @max 10000 + * @unit mm + * +*/ + +PARAM_DEFINE_FLOAT(CAL_AIR_SMP1_CRS, 600); + + +/** + * Venturi tube sensor + * + * @min 0 + * @max 10000 + * @unit mm + * +*/ + +PARAM_DEFINE_FLOAT(CAL_AIR_SMP2_CRS, 400); + + /** * Differential pressure sensor offset * diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 97ceb6db216c..16b7afd49f30 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2022 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2023 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 @@ -66,6 +66,9 @@ Sensors::Sensors(bool hil_enabled) : _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"); + _parameter_handles.air_venturi_crossection_diameter_out = param_find("CAL_AIR_ENT_CRS"); + _parameter_handles.air_venturi_crossection_diameter_1st = param_find("CAL_AIR_SMP1_CRS"); + _parameter_handles.air_venturi_crossection_diameter_2nd = param_find("CAL_AIR_SMP2_CRS"); _airspeed_validator.set_timeout(300000); _airspeed_validator.set_equal_value_threshold(100); @@ -173,6 +176,10 @@ int Sensors::parameters_update() 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); + param_get(_parameter_handles.air_venturi_crossection_diameter_out, &_parameters.air_venturi_crossection_diameter_out); + param_get(_parameter_handles.air_venturi_crossection_diameter_1st, &_parameters.air_venturi_crossection_diameter_1st); + param_get(_parameter_handles.air_venturi_crossection_diameter_2nd, &_parameters.air_venturi_crossection_diameter_2nd); + #endif // CONFIG_SENSORS_VEHICLE_AIRSPEED _voted_sensors_update.parametersUpdate(); @@ -324,7 +331,9 @@ void Sensors::diff_pres_poll() float indicated_airspeed_m_s = calc_IAS_corrected((enum AIRSPEED_COMPENSATION_MODEL)_parameters.air_cmodel, smodel, _parameters.air_tube_length, _parameters.air_tube_diameter_mm, - differential_pressure_pa, baro_pressure_pa, temperature); + _parameters.air_venturi_crossection_diameter_out, _parameters.air_venturi_crossection_diameter_1st, + _parameters.air_venturi_crossection_diameter_2nd, differential_pressure_pa, + baro_pressure_pa, temperature); // assume that CAS = IAS as we don't have an CAS-scale here float true_airspeed_m_s = calc_TAS_from_CAS(indicated_airspeed_m_s, baro_pressure_pa, temperature); diff --git a/src/modules/sensors/sensors.hpp b/src/modules/sensors/sensors.hpp index 3a8bcd50bc7a..04ae086280e6 100644 --- a/src/modules/sensors/sensors.hpp +++ b/src/modules/sensors/sensors.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2022 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2023 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 @@ -205,6 +205,9 @@ class Sensors : public ModuleBase, public ModuleParams, public px4::Sch int32_t air_cmodel; float air_tube_length; float air_tube_diameter_mm; + float air_venturi_crossection_diameter_out; + float air_venturi_crossection_diameter_1st; + float air_venturi_crossection_diameter_2nd; } _parameters{}; /**< local copies of interesting parameters */ struct ParameterHandles { @@ -216,6 +219,9 @@ class Sensors : public ModuleBase, public ModuleParams, public px4::Sch param_t air_cmodel; param_t air_tube_length; param_t air_tube_diameter_mm; + param_t air_venturi_crossection_diameter_out; + param_t air_venturi_crossection_diameter_1st; + param_t air_venturi_crossection_diameter_2nd; } _parameter_handles{}; /**< handles for interesting parameters */ #endif // CONFIG_SENSORS_VEHICLE_AIRSPEED