differential pressure remove filters from drivers and average in sensors/airspeed

src/modules/commander/airspeed_calibration.cpp

@@ -76,38 +76,38 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub)
 
 	const unsigned calibration_count = (maxcount * 2) / 3;
 
-	uORB::SubscriptionBlocking<differential_pressure_s> diff_pres_sub{ORB_ID(differential_pressure)};
-
 	float diff_pres_offset = 0.0f;
 
 	if (param_set(param_find("SENS_DPRES_OFF"), &diff_pres_offset) != PX4_OK) {
 		calibration_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG);
-		goto error_return;
+		return PX4_ERROR;
 	}
 
 	calibration_log_critical(mavlink_log_pub, "[cal] Ensure sensor is not measuring wind");
 	px4_usleep(500 * 1000);
 
+	uORB::SubscriptionBlocking<differential_pressure_s> diff_pres_sub{ORB_ID(differential_pressure)};
+
 	while (calibration_counter < calibration_count) {
 
 		if (calibrate_cancel_check(mavlink_log_pub, calibration_started)) {
-			goto error_return;
+			return PX4_ERROR;
 		}
 
 		differential_pressure_s diff_pres;
 
 		if (diff_pres_sub.updateBlocking(diff_pres, 1000000)) {
 
-			diff_pres_offset += diff_pres.differential_pressure_raw_pa;
+			diff_pres_offset += diff_pres.differential_pressure_pa;
 			calibration_counter++;
 
 			/* any differential pressure failure a reason to abort */
 			if (diff_pres.error_count != 0) {
-				calibration_log_critical(mavlink_log_pub, "[cal] Airspeed sensor is reporting errors (%" PRIu64 ")",
+				calibration_log_critical(mavlink_log_pub, "[cal] Airspeed sensor is reporting errors (%" PRIu32 ")",
 							 diff_pres.error_count);
 				calibration_log_critical(mavlink_log_pub, "[cal] Check wiring, reboot vehicle, and try again");
 				feedback_calibration_failed(mavlink_log_pub);
-				goto error_return;
+				return PX4_ERROR;
 			}
 
 			if (calibration_counter % (calibration_count / 20) == 0) {
@@ -117,7 +117,7 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub)
 		} else {
 			/* any poll failure for 1s is a reason to abort */
 			feedback_calibration_failed(mavlink_log_pub);
-			goto error_return;
+			return PX4_ERROR;
 		}
 	}
 
@@ -134,12 +134,12 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub)
 
 		if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
 			calibration_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG);
-			goto error_return;
+			return PX4_ERROR;
 		}
 
 	} else {
 		feedback_calibration_failed(mavlink_log_pub);
-		goto error_return;
+		return PX4_ERROR;
 	}
 
 	calibration_log_info(mavlink_log_pub, "[cal] Offset of %d Pascal", (int)diff_pres_offset);
@@ -149,28 +149,35 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub)
 
 	calibration_log_critical(mavlink_log_pub, "[cal] Blow into front of pitot without touching");
 
+	float differential_pressure_sum = 0.f;
+	int differential_pressure_sum_count = 0;
+
 	calibration_counter = 0;
 
 	/* just take a few samples and make sure pitot tubes are not reversed, timeout after ~30 seconds */
 	while (calibration_counter < maxcount) {
 
 		if (calibrate_cancel_check(mavlink_log_pub, calibration_started)) {
-			goto error_return;
+			return PX4_ERROR;
 		}
 
 		differential_pressure_s diff_pres;
 
 		if (diff_pres_sub.updateBlocking(diff_pres, 1000000)) {
-			if (fabsf(diff_pres.differential_pressure_filtered_pa) > 50.0f) {
-				if (diff_pres.differential_pressure_filtered_pa > 0) {
-					calibration_log_info(mavlink_log_pub, "[cal] Positive pressure: OK (%d Pa)",
-							     (int)diff_pres.differential_pressure_filtered_pa);
+			differential_pressure_sum += diff_pres.differential_pressure_pa;
+			differential_pressure_sum_count++;
+
+			const float differential_pressure_pa = (differential_pressure_sum / differential_pressure_sum_count) - diff_pres_offset;
+
+			if ((differential_pressure_sum_count > 10) && (fabsf(differential_pressure_pa) > 50.f)) {
+				if (differential_pressure_pa > 0) {
+					calibration_log_info(mavlink_log_pub, "[cal] Positive pressure: OK (%d Pa)", (int)differential_pressure_pa);
 					break;
 
 				} else {
 					/* do not allow negative values */
 					calibration_log_critical(mavlink_log_pub, "[cal] Negative pressure difference detected (%d Pa)",
-								 (int)diff_pres.differential_pressure_filtered_pa);
+								 (int)differential_pressure_pa);
 					calibration_log_critical(mavlink_log_pub, "[cal] Swap static and dynamic ports!");
 
 					/* the user setup is wrong, wipe the calibration to force a proper re-calibration */
@@ -178,7 +185,7 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub)
 
 					if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
 						calibration_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG);
-						goto error_return;
+						return PX4_ERROR;
 					}
 
 					/* save */
@@ -186,14 +193,18 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub)
 					param_save_default();
 
 					feedback_calibration_failed(mavlink_log_pub);
-					goto error_return;
+					return PX4_ERROR;
 				}
 			}
 
 			if (calibration_counter % 500 == 0) {
 				calibration_log_info(mavlink_log_pub, "[cal] Create air pressure! (got %d, wanted: 50 Pa)",
-						     (int)diff_pres.differential_pressure_filtered_pa);
+						     (int)differential_pressure_pa);
 				tune_neutral(true);
+
+				// reset average
+				differential_pressure_sum = 0.f;
+				differential_pressure_sum_count = 0;
 			}
 
 			calibration_counter++;
@@ -201,13 +212,13 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub)
 		} else {
 			/* any poll failure for 1s is a reason to abort */
 			feedback_calibration_failed(mavlink_log_pub);
-			goto error_return;
+			return PX4_ERROR;
 		}
 	}
 
 	if (calibration_counter == maxcount) {
 		feedback_calibration_failed(mavlink_log_pub);
-		goto error_return;
+		return PX4_ERROR;
 	}
 
 	calibration_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, 100);
@@ -215,13 +226,8 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub)
 	calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, sensor_name);
 	tune_neutral(true);
 
-normal_return:
 	// This give a chance for the log messages to go out of the queue before someone else stomps on then
 	px4_sleep(1);
 
 	return result;
-
-error_return:
-	result = PX4_ERROR;
-	goto normal_return;
 }

src/modules/ekf2/EKF2.cpp

@@ -1483,7 +1483,7 @@ void EKF2::UpdateAirspeedSample(ekf2_timestamps_s &ekf2_timestamps)
 			    && (airspeed.indicated_airspeed_m_s > 0.f)
 			   ) {
 				airspeedSample airspeed_sample {
-					.time_us = airspeed.timestamp,
+					.time_us = airspeed.timestamp_sample,
 					.true_airspeed = true_airspeed_m_s,
 					.eas2tas = airspeed.true_airspeed_m_s / airspeed.indicated_airspeed_m_s,
 				};

src/modules/mavlink/mavlink_receiver.cpp

@@ -2322,11 +2322,11 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 	// differential pressure
 	if ((hil_sensor.fields_updated & SensorSource::DIFF_PRESS) == SensorSource::DIFF_PRESS) {
 		differential_pressure_s report{};
-		report.timestamp = timestamp;
+		report.timestamp_sample = timestamp;
+		report.device_id = 0; // TODO
 		report.temperature = hil_sensor.temperature;
-		report.differential_pressure_filtered_pa = hil_sensor.diff_pressure * 100.0f; // convert from millibar to bar;
-		report.differential_pressure_raw_pa = hil_sensor.diff_pressure * 100.0f; // convert from millibar to bar;
-
+		report.differential_pressure_pa = hil_sensor.diff_pressure * 100.0f; // convert from millibar to bar;
+		report.timestamp = hrt_absolute_time();
 		_differential_pressure_pub.publish(report);
 	}
 

src/modules/mavlink/streams/HIGHRES_IMU.hpp

@@ -191,7 +191,7 @@ private:
 			msg.ymag = mag(1);
 			msg.zmag = mag(2);
 			msg.abs_pressure = air_data.baro_pressure_pa;
-			msg.diff_pressure = differential_pressure.differential_pressure_raw_pa;
+			msg.diff_pressure = differential_pressure.differential_pressure_pa;
 			msg.pressure_alt = air_data.baro_alt_meter;
 			msg.temperature = air_data.baro_temp_celcius;
 			msg.fields_updated = fields_updated;

src/modules/mavlink/streams/SCALED_PRESSURE.hpp

@@ -83,7 +83,7 @@ private:
 					msg.time_boot_ms = differential_pressure.timestamp / 1000;
 				}
 
-				msg.press_diff = differential_pressure.differential_pressure_raw_pa * 100.f; // Pa to hPa
+				msg.press_diff = differential_pressure.differential_pressure_pa * 100.f; // Pa to hPa
 				msg.temperature_press_diff = roundf(differential_pressure.temperature * 100.f); // centidegrees
 			}
 

src/modules/mavlink/streams/SCALED_PRESSURE2.hpp

@@ -83,7 +83,7 @@ private:
 					msg.time_boot_ms = differential_pressure.timestamp / 1000;
 				}
 
-				msg.press_diff = differential_pressure.differential_pressure_raw_pa * 100.f; // Pa to hPa
+				msg.press_diff = differential_pressure.differential_pressure_pa * 100.f; // Pa to hPa
 				msg.temperature_press_diff = roundf(differential_pressure.temperature * 100.f); // centidegrees
 			}
 

src/modules/mavlink/streams/SCALED_PRESSURE3.hpp

@@ -83,7 +83,7 @@ private:
 					msg.time_boot_ms = differential_pressure.timestamp / 1000;
 				}
 
-				msg.press_diff = differential_pressure.differential_pressure_raw_pa * 100.f; // Pa to hPa
+				msg.press_diff = differential_pressure.differential_pressure_pa * 100.f; // Pa to hPa
 				msg.temperature_press_diff = roundf(differential_pressure.temperature * 100.f); // centidegrees
 			}
 

src/modules/sensors/sensors.cpp

@@ -139,13 +139,16 @@ private:
 
 	DataValidator	_airspeed_validator;		/**< data validator to monitor airspeed */
 
+	uint64_t _airspeed_last_publish{0};
+	uint64_t _diff_pres_timestamp_sum{0};
+	float _diff_pres_pressure_sum{0.f};
+	float _diff_pres_temperature_sum{0.f};
+	float _baro_pressure_sum{0.f};
+	int _diff_pres_count{0};
+
 #ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL
-
-	hrt_abstime	_last_adc{0};			/**< last time we took input from the ADC */
-
-	uORB::Subscription	_adc_report_sub{ORB_ID(adc_report)};		/**< adc_report sub */
-	differential_pressure_s	_diff_pres {};
-	uORB::PublicationMulti<differential_pressure_s>	_diff_pres_pub{ORB_ID(differential_pressure)};		/**< differential_pressure */
+	uORB::Subscription _adc_report_sub {ORB_ID(adc_report)};
+	uORB::PublicationMulti<differential_pressure_s> _diff_pres_pub{ORB_ID(differential_pressure)};
 #endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */
 
 
@@ -393,6 +396,18 @@ void Sensors::diff_pres_poll()
 
 	if (_diff_pres_sub.update(&diff_pres)) {
 
+		if (!PX4_ISFINITE(diff_pres.differential_pressure_pa)) {
+			// ignore invalid data and reset accumulated
+
+			// reset
+			_diff_pres_timestamp_sum = 0;
+			_diff_pres_pressure_sum = 0;
+			_diff_pres_temperature_sum = 0;
+			_baro_pressure_sum = 0;
+			_diff_pres_count = 0;
+			return;
+		}
+
 		vehicle_air_data_s air_data{};
 		_vehicle_air_data_sub.copy(&air_data);
 
@@ -413,54 +428,71 @@ void Sensors::diff_pres_poll()
 			}
 		}
 
-		airspeed_s airspeed{};
-		airspeed.timestamp = diff_pres.timestamp;
+		// push raw data into validator
+		float airspeed_input[3] { diff_pres.differential_pressure_pa, air_temperature_celsius, 0.0f };
+		_airspeed_validator.put(diff_pres.timestamp_sample, airspeed_input, diff_pres.error_count, 100); // TODO: real priority?
 
-		// apply calibration offset (SENS_DPRES_OFF)
-		const float differential_pressure_raw_pa = diff_pres.differential_pressure_raw_pa - _parameters.diff_pres_offset_pa;
-		const float differential_pressure_filtered_pa = diff_pres.differential_pressure_filtered_pa -
-				_parameters.diff_pres_offset_pa;
+		// accumulate average for publication
+		_diff_pres_timestamp_sum += diff_pres.timestamp_sample;
+		_diff_pres_pressure_sum += diff_pres.differential_pressure_pa;
+		_diff_pres_temperature_sum += air_temperature_celsius;
+		_baro_pressure_sum += air_data.baro_pressure_pa;
+		_diff_pres_count++;
 
-		/* push data into validator */
-		float airspeed_input[3] = { differential_pressure_raw_pa, diff_pres.temperature, 0.0f };
+		if ((_diff_pres_count > 0) && hrt_elapsed_time(&_airspeed_last_publish) >= 100_ms) {
 
-		_airspeed_validator.put(airspeed.timestamp, airspeed_input, diff_pres.error_count, 100); // TODO: real priority?
+			// average data and apply calibration offset (SENS_DPRES_OFF)
+			const uint64_t timestamp_sample = _diff_pres_timestamp_sum / _diff_pres_count;
+			const float differential_pressure_pa = _diff_pres_pressure_sum / _diff_pres_count - _parameters.diff_pres_offset_pa;
+			const float baro_pressure_pa = _baro_pressure_sum / _diff_pres_count;
+			const float temperature = _diff_pres_temperature_sum / _diff_pres_count;
 
-		airspeed.confidence = _airspeed_validator.confidence(hrt_absolute_time());
+			// reset
+			_diff_pres_timestamp_sum = 0;
+			_diff_pres_pressure_sum = 0;
+			_diff_pres_temperature_sum = 0;
+			_baro_pressure_sum = 0;
+			_diff_pres_count = 0;
 
-		enum AIRSPEED_SENSOR_MODEL smodel;
 
-		switch ((diff_pres.device_id >> 16) & 0xFF) {
-		case DRV_DIFF_PRESS_DEVTYPE_SDP31:
+			enum AIRSPEED_SENSOR_MODEL smodel;
 
-		/* fallthrough */
-		case DRV_DIFF_PRESS_DEVTYPE_SDP32:
+			switch ((diff_pres.device_id >> 16) & 0xFF) {
+			case DRV_DIFF_PRESS_DEVTYPE_SDP31:
 
-		/* fallthrough */
-		case DRV_DIFF_PRESS_DEVTYPE_SDP33:
-			/* fallthrough */
-			smodel = AIRSPEED_SENSOR_MODEL_SDP3X;
-			break;
+			// fallthrough
+			case DRV_DIFF_PRESS_DEVTYPE_SDP32:
 
-		default:
-			smodel = AIRSPEED_SENSOR_MODEL_MEMBRANE;
-			break;
-		}
+			// fallthrough
+			case DRV_DIFF_PRESS_DEVTYPE_SDP33:
+				smodel = AIRSPEED_SENSOR_MODEL_SDP3X;
+				break;
 
-		/* don't risk to feed negative airspeed into the system */
-		airspeed.indicated_airspeed_m_s = calc_IAS_corrected((enum AIRSPEED_COMPENSATION_MODEL)
-						  _parameters.air_cmodel,
-						  smodel, _parameters.air_tube_length, _parameters.air_tube_diameter_mm,
-						  differential_pressure_filtered_pa, air_data.baro_pressure_pa,
-						  air_temperature_celsius);
+			default:
+				smodel = AIRSPEED_SENSOR_MODEL_MEMBRANE;
+				break;
+			}
 
-		airspeed.true_airspeed_m_s = calc_TAS_from_CAS(airspeed.indicated_airspeed_m_s, air_data.baro_pressure_pa,
-					     air_temperature_celsius); // assume that CAS = IAS as we don't have an CAS-scale here
+			float indicated_airspeed_m_s = calc_IAS_corrected((enum AIRSPEED_COMPENSATION_MODEL)_parameters.air_cmodel,
+						       smodel, _parameters.air_tube_length, _parameters.air_tube_diameter_mm,
+						       differential_pressure_pa, baro_pressure_pa, temperature);
 
-		airspeed.air_temperature_celsius = air_temperature_celsius;
+			// assume that CAS = IAS as we don't have an CAS-scale here
+			float true_airspeed_m_s = calc_TAS_from_CAS(indicated_airspeed_m_s, baro_pressure_pa, temperature);
 
-		if (PX4_ISFINITE(airspeed.indicated_airspeed_m_s) && PX4_ISFINITE(airspeed.true_airspeed_m_s)) {
-			_airspeed_pub.publish(airspeed);
+			if (PX4_ISFINITE(indicated_airspeed_m_s) && PX4_ISFINITE(true_airspeed_m_s)) {
+
+				airspeed_s airspeed;
+				airspeed.timestamp_sample = timestamp_sample;
+				airspeed.indicated_airspeed_m_s = indicated_airspeed_m_s;
+				airspeed.true_airspeed_m_s = true_airspeed_m_s;
+				airspeed.air_temperature_celsius = temperature;
+				airspeed.confidence = _airspeed_validator.confidence(hrt_absolute_time());
+				airspeed.timestamp = hrt_absolute_time();
+				_airspeed_pub.publish(airspeed);
+
+				_airspeed_last_publish = airspeed.timestamp;
+			}
 		}
 	}
 }
@@ -490,49 +522,41 @@ void Sensors::adc_poll()
 #ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL
 
 	if (_parameters.diff_pres_analog_scale > 0.0f) {
+		adc_report_s adc;
 
-		hrt_abstime t = hrt_absolute_time();
+		if (_adc_report_sub.update(&adc)) {
+			/* Read add channels we got */
+			for (unsigned i = 0; i < PX4_MAX_ADC_CHANNELS; i++) {
+				if (adc.channel_id[i] == -1) {
+					continue;	// skip non-exist channels
+				}
 
-		/* rate limit to 100 Hz */
-		if (t - _last_adc >= 10000) {
-			adc_report_s adc;
+				if (ADC_AIRSPEED_VOLTAGE_CHANNEL == adc.channel_id[i]) {
 
-			if (_adc_report_sub.update(&adc)) {
-				/* Read add channels we got */
-				for (unsigned i = 0; i < PX4_MAX_ADC_CHANNELS; i++) {
-					if (adc.channel_id[i] == -1) {
-						continue;	// skip non-exist channels
-					}
+					/* calculate airspeed, raw is the difference from */
+					const float voltage = (float)(adc.raw_data[i]) * adc.v_ref / adc.resolution * ADC_DP_V_DIV;
 
-					if (ADC_AIRSPEED_VOLTAGE_CHANNEL == adc.channel_id[i]) {
+					/**
+					 * The voltage divider pulls the signal down, only act on
+					 * a valid voltage from a connected sensor. Also assume a non-
+					 * zero offset from the sensor if its connected.
+					 *
+					 * Notice: This won't work on devices which have PGA controlled
+					 * vref. Those devices require no divider at all.
+					 */
+					if (voltage > 0.4f) {
+						const float diff_pres_pa_raw = voltage * _parameters.diff_pres_analog_scale;
 
-						/* calculate airspeed, raw is the difference from */
-						const float voltage = (float)(adc.raw_data[i]) * adc.v_ref / adc.resolution * ADC_DP_V_DIV;
+						differential_pressure_s diff_pres{};
+						diff_pres.timestamp_sample = adc.timestamp;
+						diff_pres.differential_pressure_pa = diff_pres_pa_raw;
+						diff_pres.temperature = NAN;
+						diff_pres.timestamp = hrt_absolute_time();
 
-						/**
-						 * The voltage divider pulls the signal down, only act on
-						 * a valid voltage from a connected sensor. Also assume a non-
-						 * zero offset from the sensor if its connected.
-						 *
-						 * Notice: This won't work on devices which have PGA controlled
-						 * vref. Those devices require no divider at all.
-						 */
-						if (voltage > 0.4f) {
-							const float diff_pres_pa_raw = voltage * _parameters.diff_pres_analog_scale;
-
-							_diff_pres.timestamp = t;
-							_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_raw * 0.1f);
-							_diff_pres.temperature = -1000.0f;
-
-							_diff_pres_pub.publish(_diff_pres);
-						}
+						_diff_pres_pub.publish(diff_pres);
 					}
 				}
 			}
-
-			_last_adc = t;
 		}
 	}
 

src/modules/simulator/simulator_mavlink.cpp

@@ -352,11 +352,11 @@ void Simulator::update_sensors(const hrt_abstime &time, const mavlink_hil_sensor
 	// differential pressure
 	if ((sensors.fields_updated & SensorSource::DIFF_PRESS) == SensorSource::DIFF_PRESS && !_airspeed_blocked) {
 		differential_pressure_s report{};
-		report.timestamp = time;
+		report.timestamp_sample = time;
+		report.device_id = 0; // TODO
+		report.differential_pressure_pa = sensors.diff_pressure * hPa2Pa; // convert hPa to Pa;
 		report.temperature = _sensors_temperature;
-		report.differential_pressure_filtered_pa = sensors.diff_pressure * hPa2Pa; // convert hPa to Pa;
-		report.differential_pressure_raw_pa = sensors.diff_pressure * hPa2Pa; // convert hPa to Pa;
-
+		report.timestamp = hrt_absolute_time();
 		_differential_pressure_pub.publish(report);
 	}
 }