mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-07-06 07:50:34 +08:00
move systemlib/airspeed to standalone lib
This commit is contained in:
Daniel Agar
committed by
Lorenz Meier
Lorenz Meier
parent
b23e40ca42
commit
20aabd3566
@@ -32,7 +32,6 @@
|
||||
############################################################################
|
||||
|
||||
set(SRCS
|
||||
airspeed.cpp
|
||||
conversions.c
|
||||
cpuload.c
|
||||
crc.c
|
||||
|
||||
@@ -1,256 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012-2013 PX4 Development Team. All rights reserved.
|
||||
* Author: Lorenz Meier <lm@inf.ethz.ch>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file airspeed.c
|
||||
* Airspeed estimation
|
||||
*
|
||||
* @author Lorenz Meier <lm@inf.ethz.ch>
|
||||
*
|
||||
*/
|
||||
|
||||
#include "airspeed.h"
|
||||
|
||||
#include <px4_defines.h>
|
||||
#include <lib/ecl/geo/geo.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_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
|
||||
const float rho_air = get_air_density(pressure_ambient, temperature_celsius);
|
||||
|
||||
const float dp = fabsf(differential_pressure);
|
||||
float dp_tot = dp;
|
||||
|
||||
float dv = 0.0f;
|
||||
|
||||
switch (smodel) {
|
||||
|
||||
case AIRSPEED_SENSOR_MODEL_MEMBRANE: {
|
||||
// do nothing
|
||||
}
|
||||
break;
|
||||
|
||||
case AIRSPEED_SENSOR_MODEL_SDP3X: {
|
||||
// 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_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;
|
||||
|
||||
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 = M_PI_F * 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;
|
||||
}
|
||||
|
||||
}
|
||||
break;
|
||||
|
||||
default: {
|
||||
// do nothing
|
||||
}
|
||||
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;
|
||||
// }
|
||||
|
||||
// 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 result with correct sign
|
||||
return (differential_pressure > 0.0f) ? airspeed_corrected : -airspeed_corrected;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* 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(float differential_pressure)
|
||||
{
|
||||
|
||||
|
||||
if (differential_pressure > 0.0f) {
|
||||
return sqrtf((2.0f * differential_pressure) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C);
|
||||
|
||||
} else {
|
||||
return -sqrtf((2.0f * fabsf(differential_pressure)) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
/**
|
||||
* Calculate true airspeed from indicated airspeed.
|
||||
*
|
||||
* Note that the true airspeed is NOT the groundspeed, because of the effects of wind
|
||||
*
|
||||
* @param speed_indicated current indicated airspeed
|
||||
* @param pressure_ambient pressure at the side of the tube/airplane
|
||||
* @param temperature_celsius air temperature in degrees celcius
|
||||
* @return true airspeed in m/s
|
||||
*/
|
||||
float calc_true_airspeed_from_indicated(float speed_indicated, float pressure_ambient, float temperature_celsius)
|
||||
{
|
||||
return speed_indicated * sqrtf(CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C / get_air_density(pressure_ambient,
|
||||
temperature_celsius));
|
||||
}
|
||||
|
||||
/**
|
||||
* Directly calculate true airspeed
|
||||
*
|
||||
* Note that the true airspeed is NOT the groundspeed, because of the effects of wind
|
||||
*
|
||||
* @param total_pressure pressure inside the pitot/prandtl tube
|
||||
* @param static_pressure pressure at the side of the tube/airplane
|
||||
* @param temperature_celsius air temperature in degrees celcius
|
||||
* @return true airspeed in m/s
|
||||
*/
|
||||
float calc_true_airspeed(float total_pressure, float static_pressure, float temperature_celsius)
|
||||
{
|
||||
float density = get_air_density(static_pressure, temperature_celsius);
|
||||
|
||||
if (density < 0.0001f || !PX4_ISFINITE(density)) {
|
||||
density = CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C;
|
||||
}
|
||||
|
||||
float pressure_difference = total_pressure - static_pressure;
|
||||
|
||||
if (pressure_difference > 0) {
|
||||
return sqrtf((2.0f * (pressure_difference)) / density);
|
||||
|
||||
} else {
|
||||
return -sqrtf((2.0f * fabsf(pressure_difference)) / density);
|
||||
}
|
||||
}
|
||||
|
||||
float get_air_density(float static_pressure, float temperature_celsius)
|
||||
{
|
||||
return static_pressure / (CONSTANTS_AIR_GAS_CONST * (temperature_celsius - CONSTANTS_ABSOLUTE_NULL_CELSIUS));
|
||||
}
|
||||
@@ -1,124 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file airspeed.h
|
||||
* Airspeed estimation declarations
|
||||
*
|
||||
* @author Lorenz Meier <lorenz@px4.io>
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef AIRSPEED_H_
|
||||
#define AIRSPEED_H_
|
||||
|
||||
#include "math.h"
|
||||
#include "conversions.h"
|
||||
|
||||
__BEGIN_DECLS
|
||||
|
||||
enum AIRSPEED_SENSOR_MODEL {
|
||||
AIRSPEED_SENSOR_MODEL_MEMBRANE = 0,
|
||||
AIRSPEED_SENSOR_MODEL_SDP3X,
|
||||
};
|
||||
|
||||
enum AIRSPEED_COMPENSATION_MODEL {
|
||||
AIRSPEED_COMPENSATION_MODEL_PITOT = 0,
|
||||
AIRSPEED_COMPENSATION_MODEL_NO_PITOT = 1,
|
||||
AIRSPEED_COMPENSATION_TUBE_PRESSURE_LOSS = 2
|
||||
};
|
||||
|
||||
/**
|
||||
* 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_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.
|
||||
*
|
||||
* 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(float differential_pressure);
|
||||
|
||||
/**
|
||||
* Calculate true airspeed from indicated airspeed.
|
||||
*
|
||||
* Note that the true airspeed is NOT the groundspeed, because of the effects of wind
|
||||
*
|
||||
* @param speed_indicated current indicated airspeed
|
||||
* @param pressure_ambient pressure at the side of the tube/airplane
|
||||
* @param temperature_celsius air temperature in degrees celcius
|
||||
* @return true airspeed in m/s
|
||||
*/
|
||||
__EXPORT float calc_true_airspeed_from_indicated(float speed_indicated, float pressure_ambient,
|
||||
float temperature_celsius);
|
||||
|
||||
/**
|
||||
* Directly calculate true airspeed
|
||||
*
|
||||
* Note that the true airspeed is NOT the groundspeed, because of the effects of wind
|
||||
*
|
||||
* @param total_pressure pressure inside the pitot/prandtl tube
|
||||
* @param static_pressure pressure at the side of the tube/airplane
|
||||
* @param temperature_celsius air temperature in degrees celcius
|
||||
* @return true airspeed in m/s
|
||||
*/
|
||||
__EXPORT float calc_true_airspeed(float total_pressure, float static_pressure, float temperature_celsius);
|
||||
|
||||
/**
|
||||
* Calculates air density.
|
||||
*
|
||||
* @param static_pressure ambient pressure in millibar
|
||||
* @param temperature_celcius air / ambient temperature in celcius
|
||||
*/
|
||||
__EXPORT float get_air_density(float static_pressure, float temperature_celsius);
|
||||
|
||||
__END_DECLS
|
||||
|
||||
#endif
|
||||
@@ -130,11 +130,6 @@ union udid {
|
||||
|
||||
__BEGIN_DECLS
|
||||
|
||||
/**
|
||||
* s
|
||||
*/
|
||||
//__EXPORT float calc_indicated_airspeed(float differential_pressure);
|
||||
|
||||
__EXPORT void F_unlock(void);
|
||||
__EXPORT void F_lock(void);
|
||||
__EXPORT int val_read(void *dest, volatile const void *src, int bytes);
|
||||
|
||||
Reference in New Issue
Block a user