airspeed (pitot) offset calibration

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);
+	}
 }