/**
* 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 || !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);
}
}
/**
* 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: {
// 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;
}
switch (pmodel) {
case AIRSPEED_PITOT_MODEL_HB:
dp_pitot = 28557670.0 - 28557670.0 / (1 + pow((flow_SDP33 / 5027611.0), 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 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));
}
/**
* @brief Directly calculate true airspeed
*
* Please note that the true airspeed is NOT the groundspeed, because of the effects of wind
*
* @parem pressure_front pressure inside the pitot/prandl tube
* @param pressure_ambient pressure at the side of the tube/airplane
* @param temperature air temperature in degrees celcius
* @return true airspeed in m/s
*/
float calc_true_airspeed(float pressure_front, float pressure_ambient, float temperature)
{
return sqrt((2.0f*(pressure_front - pressure_ambient))/get_air_density(pressure_ambient, temperature));
}
/**
* @brief Calculate true airspeed from indicated airspeed
*
* Please note that the true airspeed is NOT the groundspeed, because of the effects of wind
*
* @parem speed current indicated airspeed
* @param pressure_ambient pressure at the side of the tube/airplane
* @param temperature air temperature in degrees celcius
* @return true airspeed in m/s
*/
float calc_true_airspeed_from_indicated(float speed, float pressure_ambient, float temperature)
{
return speed * sqrt(air_density_sea_level/get_air_density(pressure_ambient, temperature));
}
