Merge branch 'mpu6k_queue' of github.com:PX4/Firmware into fmuv2_bringup

1 files changed, 22 insertions, 12 deletions

diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp
index b19a1a0e6..3dc6159ef 100644
--- a/src/drivers/meas_airspeed/meas_airspeed.cpp
+++ b/src/drivers/meas_airspeed/meas_airspeed.cpp
@@ -34,6 +34,7 @@
 /**
  * @file meas_airspeed.cpp
  * @author Lorenz Meier
+ * @author Sarthak Kaingade
  * @author Simon Wilks
  *
  * Driver for the MEAS Spec series connected via I2C.
@@ -92,9 +93,6 @@
 
 /* Register address */
 #define ADDR_READ_MR			0x00	/* write to this address to start conversion */
-#define ADDR_READ_DF2			0x00	/* read from this address to read pressure only */
-#define ADDR_READ_DF3			0x01
-#define ADDR_READ_DF4			0x02	/* read from this address to read pressure and temp */
 
 /* Measurement rate is 100Hz */
 #define CONVERSION_INTERVAL	(1000000 / 100)	/* microseconds */
@@ -122,7 +120,7 @@ protected:
 extern "C" __EXPORT int meas_airspeed_main(int argc, char *argv[]);
 
 MEASAirspeed::MEASAirspeed(int bus, int address) : Airspeed(bus, address,
-	CONVERSION_INTERVAL)
+			CONVERSION_INTERVAL)
 {
 
 }
@@ -160,7 +158,7 @@ MEASAirspeed::collect()
 
 	perf_begin(_sample_perf);
 
-	ret = transfer(nullptr, 0, &val[0], 2);
+	ret = transfer(nullptr, 0, &val[0], 4);
 
 	if (ret < 0) {
 		log("error reading from sensor: %d", ret);
@@ -171,25 +169,37 @@ MEASAirspeed::collect()
 
 	if (status == 2) {
 		log("err: stale data");
+
 	} else if (status == 3) {
 		log("err: fault");
 	}
 
-	uint16_t diff_pres_pa = (val[1]) | ((val[0] & ~(0xC0)) << 8);
+	//uint16_t diff_pres_pa = (val[1]) | ((val[0] & ~(0xC0)) << 8);
 	uint16_t temp = (val[3] & 0xE0) << 8 | val[2];
 
-	diff_pres_pa = abs(diff_pres_pa - (16384 / 2.0f));
-	diff_pres_pa -= _diff_pres_offset;
+	// XXX leaving this in until new calculation method has been cross-checked
+	//diff_pres_pa = abs(diff_pres_pa - (16384 / 2.0f));
+	//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;
+	dT_raw = (val[2] << 8) + val[3];
+	dT_raw = (0xFFE0 & dT_raw) >> 5;
+	float temperature = ((200 * dT_raw) / 2047) - 50;
 
 	// XXX we may want to smooth out the readings to remove noise.
 
+	// 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));
+
 	_reports[_next_report].timestamp = hrt_absolute_time();
-	_reports[_next_report].temperature = temp;
-	_reports[_next_report].differential_pressure_pa = diff_pres_pa;
+	_reports[_next_report].temperature = temperature;
+	_reports[_next_report].differential_pressure_pa = diff_press_pa;
 
 	// Track maximum differential pressure measured (so we can work out top speed).
-	if (diff_pres_pa > _reports[_next_report].max_differential_pressure_pa) {
-		_reports[_next_report].max_differential_pressure_pa = diff_pres_pa;
+	if (diff_press_pa > _reports[_next_report].max_differential_pressure_pa) {
+		_reports[_next_report].max_differential_pressure_pa = diff_press_pa;
 	}
 
 	/* announce the airspeed if needed, just publish else */