more fixes for the airspeed readout

4 files changed, 77 insertions, 61 deletions

diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp
index d8d200ea9..c29910dcc 100644
--- a/apps/sensors/sensors.cpp
+++ b/apps/sensors/sensors.cpp
@@ -993,7 +993,7 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
 		/* look for battery channel */
 
 		for (unsigned i = 0; i < sizeof(buf_adc) / sizeof(buf_adc[0]); i++) {
-
+			
 			if (ret >= (int)sizeof(buf_adc[0])) {
 
 				if (ADC_BATTERY_VOLTAGE_CHANNEL == buf_adc[i].am_channel) {
@@ -1025,8 +1025,7 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
 				} else if (ADC_AIRSPEED_VOLTAGE_CHANNEL == buf_adc[i].am_channel) {
 
 					/* calculate airspeed, raw is the difference from */
-
-					float voltage = buf_adc[i].am_data / 4096.0f;
+					float voltage = (float)(buf_adc[i].am_data ) * 3.3f / 4096.0f * 2.0f; //V_ref/4096 * (voltage divider factor)
 
 					/**
 					 * The voltage divider pulls the signal down, only act on
@@ -1034,21 +1033,23 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
 					 */
 					if (voltage > 0.4f) {
 
-						float pres_raw = fabsf(voltage - (3.3f / 2.0f));
-						float pres_mbar = pres_raw * (3.3f / 5.0f) * 10.0f;
+//						float pres_raw = fabsf(voltage - (3.3f / 2.0f));
+//						float pres_mbar = pres_raw * (3.3f / 5.0f) * 10.0f;
+						//XXX depends on sensor used..., where are the above numbers from?
+						float diff_pres_pa = fabsf(voltage - 2.5f) * 1000.0f; //for MPXV7002DP //xxx: need an offset calibration
 
-						float airspeed_true = calc_true_airspeed(pres_mbar + _barometer.pressure,
-							_barometer.pressure, _barometer.temperature - 5.0f);
+						float airspeed_true = calc_true_airspeed(diff_pres_pa + _barometer.pressure*1e2f,
+							_barometer.pressure*1e2f, _barometer.temperature - 5.0f); //factor 1e2 for conversion from mBar to Pa
 						// XXX HACK - true temperature is much less than indicated temperature in baro,
 						// subtract 5 degrees in an attempt to account for the electrical upheating of the PCB
 
-						float airspeed_indicated = calc_indicated_airspeed(pres_mbar + _barometer.pressure,
-							_barometer.pressure, _barometer.temperature - 5.0f);
-						// XXX HACK
+						float airspeed_indicated = calc_indicated_airspeed(diff_pres_pa);
+
+						//printf("voltage: %.4f, diff_pres_pa %.4f, v_ind %.4f, v_true %.4f\n", (double)voltage, (double)diff_pres_pa, (double)airspeed_indicated, (double)airspeed_true);
 
 						_differential_pressure.timestamp = hrt_absolute_time();
 						_differential_pressure.static_pressure_mbar = _barometer.pressure;
-						_differential_pressure.differential_pressure_mbar = pres_mbar;
+						_differential_pressure.differential_pressure_mbar = diff_pres_pa*1e-2f;
 						_differential_pressure.temperature_celcius = _barometer.temperature;
 						_differential_pressure.indicated_airspeed_m_s = airspeed_indicated;
 						_differential_pressure.true_airspeed_m_s = airspeed_true;
@@ -1064,7 +1065,7 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
 				}
 
 				_last_adc = hrt_absolute_time();
-				break;
+				//break;
 			}
 		}
 	}
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
 
diff --git a/apps/uORB/topics/differential_pressure.h b/apps/uORB/topics/differential_pressure.h
index fd7670cbc..78a37fdf4 100644
--- a/apps/uORB/topics/differential_pressure.h
+++ b/apps/uORB/topics/differential_pressure.h
@@ -49,7 +49,7 @@
  */
 
 /**
- * Battery voltages and status
+ * Differential pressure and airspeed
  */
 struct differential_pressure_s {
 	uint64_t	timestamp;			/**< microseconds since system boot, needed to integrate */
@@ -67,4 +67,4 @@ struct differential_pressure_s {
 /* register this as object request broker structure */
 ORB_DECLARE(differential_pressure);
 
-#endif
\ No newline at end of file
+#endif