more fixes for the airspeed readout

2 files changed, 62 insertions, 47 deletions

diff --git a/apps/systemlib/airspeed.c b/apps/systemlib/airspeed.c
index 5c68f8ea5..32943b2f5 100644
--- a/apps/systemlib/airspeed.c
+++ b/apps/systemlib/airspeed.c
@@ -40,14 +40,25 @@
  *
  */
 
-#include "math.h"
+#include <stdio.h>
+#include <math.h>
 #include "conversions.h"
 #include "airspeed.h"
 
 
-float calc_indicated_airspeed(float pressure_front, float pressure_ambient, float temperature)
+/**
+ * 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)
 {
-	return sqrtf((2.0f*(pressure_front - pressure_ambient)) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C);
+	return sqrtf((2.0f*differential_pressure) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C);
 }
  
 /**
@@ -55,14 +66,14 @@ float calc_indicated_airspeed(float pressure_front, float pressure_ambient, floa
  *
  * Note that the true airspeed is NOT the groundspeed, because of the effects of wind
  *
- * @param speed current indicated airspeed
+ * @param speed_indicated current indicated airspeed
  * @param pressure_ambient pressure at the side of the tube/airplane
- * @param temperature air temperature in degrees celcius
+ * @param temperature_celsius air temperature in degrees celcius
  * @return true airspeed in m/s
  */
-float calc_true_airspeed_from_indicated(float speed, float pressure_ambient, float temperature)
+float calc_true_airspeed_from_indicated(float speed_indicated, float pressure_ambient, float temperature_celsius)
 {
-	return speed * sqrtf(CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C / get_air_density(pressure_ambient, temperature));
+	return speed_indicated * sqrtf(CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C / get_air_density(pressure_ambient, temperature_celsius));
 }
  
 /**
@@ -70,12 +81,17 @@ float calc_true_airspeed_from_indicated(float speed, float pressure_ambient, flo
  *
  * Note that the true airspeed is NOT the groundspeed, because of the effects of wind
  *
- * @param 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
+ * @param total_pressure pressure inside the pitot/prandl 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 pressure_front, float pressure_ambient, float temperature)
+float calc_true_airspeed(float total_pressure, float static_pressure, float temperature_celsius)
 {
-	return sqrtf((2.0f*(pressure_front - pressure_ambient)) / get_air_density(pressure_ambient, temperature));
+	float density = get_air_density(static_pressure, temperature_celsius);
+	if (density < 0.0001f || isnan(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);
 }
diff --git a/apps/systemlib/airspeed.h b/apps/systemlib/airspeed.h
index b1beb79ae..088498943 100644
--- a/apps/systemlib/airspeed.h
+++ b/apps/systemlib/airspeed.h
@@ -48,43 +48,42 @@
 
  __BEGIN_DECLS
  
-/**
- * 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 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 indicated airspeed in m/s
- */
-__EXPORT float calc_indicated_airspeed(float pressure_front, float pressure_ambient, float temperature);
+ /**
+  * 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 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
- */
-__EXPORT float calc_true_airspeed_from_indicated(float speed, float pressure_ambient, float temperature);
+ /**
+  * 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 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
- */
-__EXPORT float calc_true_airspeed(float pressure_front, float pressure_ambient, float temperature);
+ /**
+  * 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/prandl 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);
 
 __END_DECLS