fix MEAS airspeed and airspeed calibration

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;

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;
 

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).

src/modules/commander/airspeed_calibration.cpp

@@ -37,12 +37,15 @@
  */
 
 #include "airspeed_calibration.h"
+#include "calibration_messages.h"
 #include "commander_helper.h"
 
 #include <stdio.h>
+#include <fcntl.h>
 #include <poll.h>
 #include <math.h>
 #include <drivers/drv_hrt.h>
+#include <drivers/drv_airspeed.h>
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/differential_pressure.h>
 #include <mavlink/mavlink_log.h>
@@ -55,10 +58,13 @@
 #endif
 static const int ERROR = -1;
 
+static const char *sensor_name = "dpress";
+
 int do_airspeed_calibration(int mavlink_fd)
 {
 	/* give directions */
-	mavlink_log_info(mavlink_fd, "airspeed calibration starting, keep it still");
+	mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
+	mavlink_log_info(mavlink_fd, "don't move system");
 
 	const int calibration_count = 2500;
 
@@ -68,6 +74,28 @@ int do_airspeed_calibration(int mavlink_fd)
 	int calibration_counter = 0;
 	float diff_pres_offset = 0.0f;
 
+	/* Reset sensor parameters */
+	struct airspeed_scale airscale = {
+		0.0f,
+		1.0f,
+	};
+
+	bool paramreset_successful = false;
+	int  fd = open(AIRSPEED_DEVICE_PATH, 0);
+	if (fd > 0) {
+		if (OK == ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
+			paramreset_successful = true;
+		}
+		close(fd);
+	}
+
+	if (!paramreset_successful) {
+		warn("WARNING: failed to set scale / offsets for airspeed sensor");
+		mavlink_log_critical(mavlink_fd, "could not reset dpress sensor");
+		mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
+		return ERROR;
+	}
+
 	while (calibration_counter < calibration_count) {
 
 		/* wait blocking for new data */
@@ -82,9 +110,12 @@ int do_airspeed_calibration(int mavlink_fd)
 			diff_pres_offset += diff_pres.differential_pressure_pa;
 			calibration_counter++;
 
+			if (calibration_counter % (calibration_count / 20) == 0)
+				mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 100) / calibration_count);
+
 		} else if (poll_ret == 0) {
 			/* any poll failure for 1s is a reason to abort */
-			mavlink_log_info(mavlink_fd, "airspeed calibration aborted");
+			mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name);
 			close(diff_pres_sub);
 			return ERROR;
 		}
@@ -95,7 +126,7 @@ int do_airspeed_calibration(int mavlink_fd)
 	if (isfinite(diff_pres_offset)) {
 
 		if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
-			mavlink_log_critical(mavlink_fd, "Setting offs failed!");
+			mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
 			close(diff_pres_sub);
 			return ERROR;
 		}
@@ -105,17 +136,18 @@ int do_airspeed_calibration(int mavlink_fd)
 
 		if (save_ret != 0) {
 			warn("WARNING: auto-save of params to storage failed");
-			mavlink_log_info(mavlink_fd, "FAILED storing calibration");
+			mavlink_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG);
 			close(diff_pres_sub);
 			return ERROR;
 		}
 
-		mavlink_log_info(mavlink_fd, "airspeed calibration done");
+		mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name);
+		tune_neutral();
 		close(diff_pres_sub);
 		return OK;
 
 	} else {
-		mavlink_log_info(mavlink_fd, "airspeed calibration FAILED (NaN)");
+		mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
 		close(diff_pres_sub);
 		return ERROR;
 	}

src/modules/sensors/sensors.cpp

@@ -1121,6 +1121,7 @@ Sensors::parameter_update_poll(bool forced)
 
 			if (OK != ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale))
 				warn("WARNING: failed to set scale / offsets for airspeed sensor");
+			close(fd);
 		}
 
 #if 0

src/modules/systemlib/airspeed.c

@@ -59,7 +59,7 @@
 float calc_indicated_airspeed(float differential_pressure)
 {
 
-	if (differential_pressure > 0) {
+	if (differential_pressure > 0.0f) {
 		return sqrtf((2.0f*differential_pressure) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C);
 	} else {
 		return -sqrtf((2.0f*fabsf(differential_pressure)) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C);

src/modules/uORB/topics/differential_pressure.h

@@ -54,8 +54,8 @@
 struct differential_pressure_s {
 	uint64_t	timestamp;			/**< microseconds since system boot, needed to integrate */
 	uint64_t	error_count;
-	uint16_t	differential_pressure_pa;	/**< Differential pressure reading */
+	float	differential_pressure_pa;	/**< Differential pressure reading */
-	uint16_t	max_differential_pressure_pa;	/**< Maximum differential pressure reading */
+	float	max_differential_pressure_pa;	/**< Maximum differential pressure reading */
 	float		voltage;			/**< Voltage from analog airspeed sensors (voltage divider already compensated) */
 	float		temperature;			/**< Temperature provided by sensor */