Merge branch 'master' of https://github.com/PX4/Firmware into bottledrop

4 files changed, 19 insertions, 12 deletions

diff --git a/src/drivers/airspeed/airspeed.h b/src/drivers/airspeed/airspeed.h
index 048784813..62c0d1f17 100644
--- a/src/drivers/airspeed/airspeed.h
+++ b/src/drivers/airspeed/airspeed.h
@@ -119,7 +119,7 @@ protected:
 	virtual int	collect() = 0;
 
 	work_s			_work;
-	uint16_t		_max_differential_pressure_pa;
+	float		_max_differential_pressure_pa;
 	bool			_sensor_ok;
 	int			_measure_ticks;
 	bool			_collect_phase;
diff --git a/src/drivers/boards/px4fmu-v1/px4fmu_init.c b/src/drivers/boards/px4fmu-v1/px4fmu_init.c
index 964f5069c..4b12b75f9 100644
--- a/src/drivers/boards/px4fmu-v1/px4fmu_init.c
+++ b/src/drivers/boards/px4fmu-v1/px4fmu_init.c
@@ -222,14 +222,9 @@ __EXPORT int nsh_archinitialize(void)
 	 * If SPI2 is enabled in the defconfig, we loose some ADC pins as chip selects.
 	 * Keep the SPI2 init optional and conditionally initialize the ADC pins
 	 */
-	spi2 = up_spiinitialize(2);
 
-	if (!spi2) {
-		message("[boot] Enabling IN12/13 instead of SPI2\n");
-		/* no SPI2, use pins for ADC */
-		stm32_configgpio(GPIO_ADC1_IN12);
-		stm32_configgpio(GPIO_ADC1_IN13);	// jumperable to MPU6000 DRDY on some boards
-	} else {
+	#ifdef CONFIG_STM32_SPI2
+		spi2 = up_spiinitialize(2);
 		/* Default SPI2 to 1MHz and de-assert the known chip selects. */
 		SPI_SETFREQUENCY(spi2, 10000000);
 		SPI_SETBITS(spi2, 8);
@@ -238,7 +233,13 @@ __EXPORT int nsh_archinitialize(void)
 		SPI_SELECT(spi2, PX4_SPIDEV_ACCEL_MAG, false);
 
 		message("[boot] Initialized SPI port2 (ADC IN12/13 blocked)\n");
-	}
+	#else
+		spi2 = NULL;
+		message("[boot] Enabling IN12/13 instead of SPI2\n");
+		/* no SPI2, use pins for ADC */
+		stm32_configgpio(GPIO_ADC1_IN12);
+		stm32_configgpio(GPIO_ADC1_IN13);	// jumperable to MPU6000 DRDY on some boards
+	#endif
 
 	/* Get the SPI port for the microSD slot */
 
diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp
index 9d8ad084e..de371bf32 100644
--- a/src/drivers/ets_airspeed/ets_airspeed.cpp
+++ b/src/drivers/ets_airspeed/ets_airspeed.cpp
@@ -180,7 +180,7 @@ ETSAirspeed::collect()
 	differential_pressure_s report;
 	report.timestamp = hrt_absolute_time();
         report.error_count = perf_event_count(_comms_errors);
-	report.differential_pressure_pa = diff_pres_pa;
+	report.differential_pressure_pa = (float)diff_pres_pa;
 	report.voltage = 0;
 	report.max_differential_pressure_pa = _max_differential_pressure_pa;
 
diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp
index b3003fc1b..3cd6d6720 100644
--- a/src/drivers/meas_airspeed/meas_airspeed.cpp
+++ b/src/drivers/meas_airspeed/meas_airspeed.cpp
@@ -36,6 +36,7 @@
  * @author Lorenz Meier
  * @author Sarthak Kaingade
  * @author Simon Wilks
+ * @author Thomas Gubler
  *
  * Driver for the MEAS Spec series connected via I2C.
  *
@@ -76,6 +77,7 @@
 #include <systemlib/err.h>
 #include <systemlib/param/param.h>
 #include <systemlib/perf_counter.h>
+#include <mathlib/mathlib.h>
 
 #include <drivers/drv_airspeed.h>
 #include <drivers/drv_hrt.h>
@@ -184,7 +186,7 @@ MEASAirspeed::collect()
 	//diff_pres_pa -= _diff_pres_offset;
 	int16_t dp_raw = 0, dT_raw = 0;
 	dp_raw = (val[0] << 8) + val[1];
-	dp_raw = 0x3FFF & dp_raw;
+	dp_raw = 0x3FFF & dp_raw; //mask the used bits
 	dT_raw = (val[2] << 8) + val[3];
 	dT_raw = (0xFFE0 & dT_raw) >> 5;
 	float temperature = ((200 * dT_raw) / 2047) - 50;
@@ -193,7 +195,11 @@ MEASAirspeed::collect()
 
 	// Calculate differential pressure. As its centered around 8000
 	// and can go positive or negative, enforce absolute value
-	uint16_t diff_press_pa = abs(dp_raw - (16384 / 2.0f));
+//	uint16_t diff_press_pa = abs(dp_raw - (16384 / 2.0f));
+	const float P_min = -1.0f;
+	const float P_max = 1.0f;
+	float diff_press_pa = math::max(0.0f, fabsf( ( ((float)dp_raw - 0.1f*16383.0f) * (P_max-P_min)/(0.8f*16383.0f) + P_min) * 6894.8f)   - _diff_pres_offset);
+
 	struct differential_pressure_s report;
 
 	// Track maximum differential pressure measured (so we can work out top speed).