Merge remote-tracking branch 'upstream/master' into new_state_machine

Conflicts:
	apps/commander/commander.c
	apps/uORB/topics/vehicle_status.h

7 files changed, 84 insertions, 53 deletions

diff --git a/apps/systemlib/airspeed.c b/apps/systemlib/airspeed.c
index 5c68f8ea5..264287b10 100644
--- a/apps/systemlib/airspeed.c
+++ b/apps/systemlib/airspeed.c
@@ -40,14 +40,31 @@
  *
  */
 
-#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);
+
+	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);
+	}
+
 }
  
 /**
@@ -55,14 +72,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 +87,25 @@ 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/prandtl 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 || !isfinite(density)) {
+	 density = CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C;
+	 printf("[airspeed] Invalid air density, using density at sea level\n");
+	}
+
+	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);
+	}
 }
diff --git a/apps/systemlib/airspeed.h b/apps/systemlib/airspeed.h
index b1beb79ae..def53f0c1 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/prandtl 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/systemlib/conversions.c b/apps/systemlib/conversions.c
index 2b8003e45..ac94252c5 100644
--- a/apps/systemlib/conversions.c
+++ b/apps/systemlib/conversions.c
@@ -150,5 +150,5 @@ void quat2rot(const float Q[4], float R[9])
 
 float get_air_density(float static_pressure, float temperature_celsius)
 {
-	return static_pressure / (CONSTANTS_AIR_GAS_CONST * (temperature_celsius + CONSTANTS_ABSOLUTE_NULL_KELVIN));
+	return static_pressure / (CONSTANTS_AIR_GAS_CONST * (temperature_celsius - CONSTANTS_ABSOLUTE_NULL_CELSIUS));
 }
diff --git a/apps/systemlib/conversions.h b/apps/systemlib/conversions.h
index c2987709b..5d485b01f 100644
--- a/apps/systemlib/conversions.h
+++ b/apps/systemlib/conversions.h
@@ -44,10 +44,10 @@
 #include <float.h>
 #include <stdint.h>
 
-#define CONSTANTS_ONE_G				9.80665f
-#define CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C	1.225f
-#define CONSTANTS_AIR_GAS_CONST 8.31432f
-#define CONSTANTS_ABSOLUTE_NULL_KELVIN 273.15f
+#define CONSTANTS_ONE_G						9.80665f		// m/s^2
+#define CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C	1.225f			// kg/m^3
+#define CONSTANTS_AIR_GAS_CONST				287.1f 			// J/(kg * K)
+#define CONSTANTS_ABSOLUTE_NULL_CELSIUS		-273.15f		// °C
 
 __BEGIN_DECLS
 
diff --git a/apps/systemlib/mixer/mixer.cpp b/apps/systemlib/mixer/mixer.cpp
index 72f765d90..df0dfe838 100644
--- a/apps/systemlib/mixer/mixer.cpp
+++ b/apps/systemlib/mixer/mixer.cpp
@@ -54,6 +54,7 @@
 #include "mixer.h"
 
 Mixer::Mixer(ControlCallback control_cb, uintptr_t cb_handle) :
+	_next(nullptr),
 	_control_cb(control_cb),
 	_cb_handle(cb_handle)
 {
diff --git a/apps/systemlib/mixer/mixer.h b/apps/systemlib/mixer/mixer.h
index 00ddf1581..71386cba7 100644
--- a/apps/systemlib/mixer/mixer.h
+++ b/apps/systemlib/mixer/mixer.h
@@ -160,7 +160,7 @@ public:
 	 * @param control_cb		Callback invoked when reading controls.
 	 */
 	Mixer(ControlCallback control_cb, uintptr_t cb_handle);
-	~Mixer() {};
+	virtual ~Mixer() {};
 
 	/**
 	 * Perform the mixing function.
diff --git a/apps/systemlib/mixer/mixer_group.cpp b/apps/systemlib/mixer/mixer_group.cpp
index 60eeff225..1dbc512cd 100644
--- a/apps/systemlib/mixer/mixer_group.cpp
+++ b/apps/systemlib/mixer/mixer_group.cpp
@@ -93,6 +93,7 @@ MixerGroup::reset()
 		mixer = _first;
 		_first = mixer->_next;
 		delete mixer;
+		mixer = nullptr;
 	}
 }