fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-18 02:43:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

airspeed (pitot) offset calibration

Browse Source
This commit is contained in:
Thomas Gubler
2013-02-24 21:57:38 +01:00
parent 2707d2c1dd
commit c0a852dab4
6 changed files with 130 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/systemlib/airspeed.c
+17 -3
View File
@@ -58,7 +58,13 @@
*/
float calc_indicated_airspeed(float differential_pressure)
{
return sqrtf((2.0f*differential_pressure) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C);
if (differential_pressure > 0) {
return sqrtf((2.0f*differential_pressure) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C);
} else {
return -sqrtf((2.0f*fabs(differential_pressure)) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C);
}
}
/**
@@ -89,9 +95,17 @@ float calc_true_airspeed_from_indicated(float speed_indicated, float pressure_am
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 || isnan(density)) {
if (density < 0.0001f || !isfinite(density)) {
density = CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C;
printf("Invalid air density, using density at sea level\n");
}
return sqrtf((2.0f*(total_pressure - static_pressure)) / density);
float pressure_difference = total_pressure - static_pressure;
if(pressure_difference > 0) {
return sqrtf((2.0f*(pressure_difference)) / density);
} else
{
return -sqrtf((2.0f*fabs(pressure_difference)) / density);
}
}