diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2013-02-24 21:57:38 +0100 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2013-02-24 21:57:38 +0100 |
commit | c0a852dab48e55aa12c995adc6dc0c32aa9a7ac3 (patch) | |
tree | ec06d0ad411737cc56bc102f90e82680cf943018 /apps/systemlib | |
parent | 2707d2c1dde65dfb9ba48258994badb4b57f9627 (diff) | |
download | px4-firmware-c0a852dab48e55aa12c995adc6dc0c32aa9a7ac3.tar.gz px4-firmware-c0a852dab48e55aa12c995adc6dc0c32aa9a7ac3.tar.bz2 px4-firmware-c0a852dab48e55aa12c995adc6dc0c32aa9a7ac3.zip |
airspeed (pitot) offset calibration
Diffstat (limited to 'apps/systemlib')
-rw-r--r-- | apps/systemlib/airspeed.c | 20 |
1 files changed, 17 insertions, 3 deletions
diff --git a/apps/systemlib/airspeed.c b/apps/systemlib/airspeed.c index 32943b2f5..382df2ee4 100644 --- a/apps/systemlib/airspeed.c +++ b/apps/systemlib/airspeed.c @@ -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); + } } |