Merge pull request #1308 from PX4/airspeedfabs

No absolute value of airspeed

5 files changed, 31 insertions, 59 deletions

diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp
index f98d615a2..0f77bb805 100644
--- a/src/drivers/ets_airspeed/ets_airspeed.cpp
+++ b/src/drivers/ets_airspeed/ets_airspeed.cpp
@@ -155,7 +155,6 @@ ETSAirspeed::collect()
 	}
 
 	uint16_t diff_pres_pa_raw = val[1] << 8 | val[0];
-	uint16_t diff_pres_pa;
         if (diff_pres_pa_raw == 0) {
 		// a zero value means the pressure sensor cannot give us a
 		// value. We need to return, and not report a value or the
@@ -166,28 +165,21 @@ ETSAirspeed::collect()
 		return -1;
         }
 
-	if (diff_pres_pa_raw < _diff_pres_offset + MIN_ACCURATE_DIFF_PRES_PA) {
-		diff_pres_pa = 0;
-	} else {
-		diff_pres_pa = diff_pres_pa_raw - _diff_pres_offset;
-	}
-
 	// The raw value still should be compensated for the known offset
 	diff_pres_pa_raw -= _diff_pres_offset;
 
 	// Track maximum differential pressure measured (so we can work out top speed).
-	if (diff_pres_pa > _max_differential_pressure_pa) {
-		_max_differential_pressure_pa = diff_pres_pa;
+	if (diff_pres_pa_raw > _max_differential_pressure_pa) {
+		_max_differential_pressure_pa = diff_pres_pa_raw;
 	}
 
 	differential_pressure_s report;
 	report.timestamp = hrt_absolute_time();
         report.error_count = perf_event_count(_comms_errors);
-	report.differential_pressure_pa = (float)diff_pres_pa;
 
 	// XXX we may want to smooth out the readings to remove noise.
-	report.differential_pressure_filtered_pa = (float)diff_pres_pa;
-	report.differential_pressure_raw_pa = (float)diff_pres_pa_raw;
+	report.differential_pressure_filtered_pa = diff_pres_pa_raw;
+	report.differential_pressure_raw_pa = diff_pres_pa_raw;
 	report.temperature = -1000.0f;
 	report.max_differential_pressure_pa = _max_differential_pressure_pa;
 
@@ -369,7 +361,7 @@ test()
 		err(1, "immediate read failed");
 
 	warnx("single read");
-	warnx("diff pressure: %f pa", (double)report.differential_pressure_pa);
+	warnx("diff pressure: %f pa", (double)report.differential_pressure_filtered_pa);
 
 	/* start the sensor polling at 2Hz */
 	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2))
@@ -394,7 +386,7 @@ test()
 			err(1, "periodic read failed");
 
 		warnx("periodic read %u", i);
-		warnx("diff pressure: %f pa", (double)report.differential_pressure_pa);
+		warnx("diff pressure: %f pa", (double)report.differential_pressure_filtered_pa);
 	}
 
 	/* reset the sensor polling to its default rate */
diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp
index 159706278..1d9a463ad 100644
--- a/src/drivers/meas_airspeed/meas_airspeed.cpp
+++ b/src/drivers/meas_airspeed/meas_airspeed.cpp
@@ -228,44 +228,23 @@ MEASAirspeed::collect()
 	// the raw value still should be compensated for the known offset
 	diff_press_pa_raw -= _diff_pres_offset;
 
-	float diff_press_pa = fabsf(diff_press_pa_raw);
-	
 	/*
-	  note that we return both the absolute value with offset
-	  applied and a raw value without the offset applied. This
-	  makes it possible for higher level code to detect if the
-	  user has the tubes connected backwards, and also makes it
-	  possible to correctly use offsets calculated by a higher
-	  level airspeed driver.
-
 	  With the above calculation the MS4525 sensor will produce a
 	  positive number when the top port is used as a dynamic port
 	  and bottom port is used as the static port
-
-	  Also note that the _diff_pres_offset is applied before the
-	  fabsf() not afterwards. It needs to be done this way to
-	  prevent a bias at low speeds, but this also means that when
-	  setting a offset you must set it based on the raw value, not
-	  the offset value
 	 */
-	
+
 	struct differential_pressure_s report;
 
 	/* track maximum differential pressure measured (so we can work out top speed). */
-	if (diff_press_pa > _max_differential_pressure_pa) {
-		_max_differential_pressure_pa = diff_press_pa;
+	if (diff_press_pa_raw > _max_differential_pressure_pa) {
+		_max_differential_pressure_pa = diff_press_pa_raw;
 	}
 
 	report.timestamp = hrt_absolute_time();
 	report.error_count = perf_event_count(_comms_errors);
 	report.temperature = temperature;
-	report.differential_pressure_pa = diff_press_pa;
-	report.differential_pressure_filtered_pa =  _filter.apply(diff_press_pa);
-
-	/* the dynamics of the filter can make it overshoot into the negative range */
-	if (report.differential_pressure_filtered_pa < 0.0f) {
-		report.differential_pressure_filtered_pa = _filter.reset(diff_press_pa);
-	}
+	report.differential_pressure_filtered_pa =  _filter.apply(diff_press_pa_raw);
 
 	report.differential_pressure_raw_pa = diff_press_pa_raw;
 	report.max_differential_pressure_pa = _max_differential_pressure_pa;
@@ -345,7 +324,7 @@ MEASAirspeed::cycle()
 /**
    correct for 5V rail voltage if the system_power ORB topic is
    available
-   
+
    See http://uav.tridgell.net/MS4525/MS4525-offset.png for a graph of
    offset versus voltage for 3 sensors
  */
@@ -394,7 +373,7 @@ MEASAirspeed::voltage_correction(float &diff_press_pa, float &temperature)
 	if (voltage_diff < -1.0f) {
 		voltage_diff = -1.0f;
 	}
-	temperature -= voltage_diff * temp_slope;	
+	temperature -= voltage_diff * temp_slope;
 #endif // CONFIG_ARCH_BOARD_PX4FMU_V2
 }
 
@@ -523,7 +502,7 @@ test()
 	}
 
 	warnx("single read");
-	warnx("diff pressure: %d pa", (int)report.differential_pressure_pa);
+	warnx("diff pressure: %d pa", (int)report.differential_pressure_filtered_pa);
 
 	/* start the sensor polling at 2Hz */
 	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
@@ -551,7 +530,7 @@ test()
 		}
 
 		warnx("periodic read %u", i);
-		warnx("diff pressure: %d pa", (int)report.differential_pressure_pa);
+		warnx("diff pressure: %d pa", (int)report.differential_pressure_filtered_pa);
 		warnx("temperature: %d C (0x%02x)", (int)report.temperature, (unsigned) report.temperature);
 	}
 
diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp
index 0e58c68b6..339b11bbe 100644
--- a/src/modules/commander/airspeed_calibration.cpp
+++ b/src/modules/commander/airspeed_calibration.cpp
@@ -180,11 +180,13 @@ int do_airspeed_calibration(int mavlink_fd)
 		return ERROR;
 	}
 
-	mavlink_log_critical(mavlink_fd, "Offset of %d Pa, create airflow now!", (int)diff_pres_offset);
+	mavlink_log_critical(mavlink_fd, "Offset of %d Pascal", (int)diff_pres_offset);
 
 	/* wait 500 ms to ensure parameter propagated through the system */
 	usleep(500 * 1000);
 
+	mavlink_log_critical(mavlink_fd, "Create airflow now");
+
 	calibration_counter = 0;
 	const unsigned maxcount = 3000;
 
@@ -204,18 +206,18 @@ int do_airspeed_calibration(int mavlink_fd)
 			calibration_counter++;
 
 			if (fabsf(diff_pres.differential_pressure_raw_pa) < 50.0f) {
-				if (calibration_counter % 100 == 0) {
-					mavlink_log_critical(mavlink_fd, "Missing airflow! (%d, wanted: 50 Pa, #h101)",
-					(int)diff_pres.differential_pressure_raw_pa);
+				if (calibration_counter % 500 == 0) {
+					mavlink_log_info(mavlink_fd, "Create airflow! (%d, wanted: 50 Pa)",
+						(int)diff_pres.differential_pressure_raw_pa);
 				}
 				continue;
 			}
 
 			/* do not allow negative values */
 			if (diff_pres.differential_pressure_raw_pa < 0.0f) {
-				mavlink_log_info(mavlink_fd, "negative pressure: ERROR (%d Pa)",
-					(int)diff_pres.differential_pressure_raw_pa);
-				mavlink_log_critical(mavlink_fd, "%d Pa: swap static and dynamic ports!", (int)diff_pres.differential_pressure_raw_pa);
+				mavlink_log_critical(mavlink_fd, "Swap static and dynamic ports!");
+				mavlink_log_info(mavlink_fd, "ERROR: Negative pressure difference detected! (%d Pa)",
+						(int)diff_pres.differential_pressure_raw_pa);
 				close(diff_pres_sub);
 
 				/* the user setup is wrong, wipe the calibration to force a proper re-calibration */
@@ -236,7 +238,7 @@ int do_airspeed_calibration(int mavlink_fd)
 				mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
 				return ERROR;
 			} else {
-				mavlink_log_info(mavlink_fd, "positive pressure: OK (%d Pa)",
+				mavlink_log_info(mavlink_fd, "Positive pressure: OK (%d Pa)",
 					(int)diff_pres.differential_pressure_raw_pa);
 				break;
 			}
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index f40034d79..cdcb428dd 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -1229,16 +1229,18 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw)
 	if (updated) {
 		orb_copy(ORB_ID(differential_pressure), _diff_pres_sub, &_diff_pres);
 
-		raw.differential_pressure_pa = _diff_pres.differential_pressure_pa;
+		raw.differential_pressure_pa = _diff_pres.differential_pressure_raw_pa;
 		raw.differential_pressure_timestamp = _diff_pres.timestamp;
 		raw.differential_pressure_filtered_pa = _diff_pres.differential_pressure_filtered_pa;
 
 		float air_temperature_celsius = (_diff_pres.temperature > -300.0f) ? _diff_pres.temperature : (raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG);
 
 		_airspeed.timestamp = _diff_pres.timestamp;
-		_airspeed.indicated_airspeed_m_s = calc_indicated_airspeed(_diff_pres.differential_pressure_filtered_pa);
-		_airspeed.true_airspeed_m_s = calc_true_airspeed(_diff_pres.differential_pressure_filtered_pa + raw.baro_pres_mbar * 1e2f,
-					      raw.baro_pres_mbar * 1e2f, air_temperature_celsius);
+
+		/* don't risk to feed negative airspeed into the system */
+		_airspeed.indicated_airspeed_m_s = math::max(0.0f, calc_indicated_airspeed(_diff_pres.differential_pressure_filtered_pa));
+		_airspeed.true_airspeed_m_s = math::max(0.0f, calc_true_airspeed(_diff_pres.differential_pressure_filtered_pa + raw.baro_pres_mbar * 1e2f,
+					      raw.baro_pres_mbar * 1e2f, air_temperature_celsius));
 		_airspeed.air_temperature_celsius = air_temperature_celsius;
 
 		/* announce the airspeed if needed, just publish else */
@@ -1457,12 +1459,10 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
 					if (voltage > 0.4f && (_parameters.diff_pres_analog_scale > 0.0f)) {
 
 						float diff_pres_pa_raw = voltage * _parameters.diff_pres_analog_scale - _parameters.diff_pres_offset_pa;
-						float diff_pres_pa = (diff_pres_pa_raw > 0.0f) ? diff_pres_pa_raw : 0.0f;
 
 						_diff_pres.timestamp = t;
-						_diff_pres.differential_pressure_pa = diff_pres_pa;
 						_diff_pres.differential_pressure_raw_pa = diff_pres_pa_raw;
-						_diff_pres.differential_pressure_filtered_pa = (_diff_pres.differential_pressure_filtered_pa * 0.9f) + (diff_pres_pa * 0.1f);
+						_diff_pres.differential_pressure_filtered_pa = (_diff_pres.differential_pressure_filtered_pa * 0.9f) + (diff_pres_pa_raw * 0.1f);
 						_diff_pres.temperature = -1000.0f;
 
 						/* announce the airspeed if needed, just publish else */
diff --git a/src/modules/uORB/topics/differential_pressure.h b/src/modules/uORB/topics/differential_pressure.h
index cd48d3cb2..7342fcf04 100644
--- a/src/modules/uORB/topics/differential_pressure.h
+++ b/src/modules/uORB/topics/differential_pressure.h
@@ -54,7 +54,6 @@
 struct differential_pressure_s {
 	uint64_t	timestamp;			/**< Microseconds since system boot, needed to integrate */
 	uint64_t	error_count;			/**< Number of errors detected by driver */
-	float	differential_pressure_pa;		/**< Differential pressure reading */
 	float	differential_pressure_raw_pa;		/**< Raw differential pressure reading (may be negative) */
 	float	differential_pressure_filtered_pa;	/**< Low pass filtered differential pressure reading */
 	float	max_differential_pressure_pa;		/**< Maximum differential pressure reading */