apply differential pressure calibration (SENS_DPRES_OFF) centrally

- remove drv_airspeed.h and ioctls

src/drivers/differential_pressure/ets/ets_airspeed.cpp

@@ -150,9 +150,6 @@ ETSAirspeed::collect()
 		diff_pres_pa_raw = 0.001f * (report.timestamp & 0x01);
 	}
 
-	// The raw value still should be compensated for the known offset
-	diff_pres_pa_raw -= _diff_pres_offset;
-
 	report.error_count = perf_event_count(_comms_errors);
 
 	// XXX we may want to smooth out the readings to remove noise.

src/drivers/differential_pressure/ms4525/ms4525_airspeed.cpp

@@ -212,8 +212,8 @@ MEASAirspeed::collect()
 
 		report.error_count = perf_event_count(_comms_errors);
 		report.temperature = temperature;
-		report.differential_pressure_filtered_pa = _filter.apply(diff_press_pa_raw) - _diff_pres_offset;
-		report.differential_pressure_raw_pa = diff_press_pa_raw - _diff_pres_offset;
+		report.differential_pressure_filtered_pa = _filter.apply(diff_press_pa_raw);
+		report.differential_pressure_raw_pa = diff_press_pa_raw;
 		report.device_id = _device_id.devid;
 		report.timestamp = hrt_absolute_time();
 

src/drivers/differential_pressure/ms5525/MS5525.cpp

@@ -265,8 +265,8 @@ MS5525::collect()
 		differential_pressure_s diff_pressure{};
 
 		diff_pressure.error_count = perf_event_count(_comms_errors);
-		diff_pressure.differential_pressure_raw_pa = diff_press_pa_raw - _diff_pres_offset;
-		diff_pressure.differential_pressure_filtered_pa = _filter.apply(diff_press_pa_raw) - _diff_pres_offset;
+		diff_pressure.differential_pressure_raw_pa = diff_press_pa_raw;
+		diff_pressure.differential_pressure_filtered_pa = _filter.apply(diff_press_pa_raw);
 		diff_pressure.temperature = temperature_c;
 		diff_pressure.device_id = _device_id.devid;
 		diff_pressure.timestamp = hrt_absolute_time();

src/drivers/differential_pressure/sdp3x/SDP3X.cpp

@@ -172,8 +172,8 @@ SDP3X::collect()
 
 		report.error_count = perf_event_count(_comms_errors);
 		report.temperature = temperature_c;
-		report.differential_pressure_filtered_pa = _filter.apply(diff_press_pa_raw) - _diff_pres_offset;
-		report.differential_pressure_raw_pa = diff_press_pa_raw - _diff_pres_offset;
+		report.differential_pressure_filtered_pa = _filter.apply(diff_press_pa_raw);
+		report.differential_pressure_raw_pa = diff_press_pa_raw;
 		report.device_id = _device_id.devid;
 		report.timestamp = hrt_absolute_time();
 

src/drivers/drv_airspeed.h

@@ -1,72 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file drv_airspeed.h
- *
- * Airspeed driver interface.
- *
- * @author Simon Wilks
- */
-
-#ifndef _DRV_AIRSPEED_H
-#define _DRV_AIRSPEED_H
-
-#include <stdint.h>
-#include <sys/ioctl.h>
-
-#include "drv_sensor.h"
-#include "drv_orb_dev.h"
-
-#define AIRSPEED_BASE_DEVICE_PATH "/dev/airspeed"
-#define AIRSPEED0_DEVICE_PATH	"/dev/airspeed0"
-
-/*
- * ioctl() definitions
- *
- * Airspeed drivers also implement the generic sensor driver
- * interfaces from drv_sensor.h
- */
-
-#define _AIRSPEEDIOCBASE		(0x7700)
-#define __AIRSPEEDIOC(_n)		(_PX4_IOC(_AIRSPEEDIOCBASE, _n))
-
-#define AIRSPEEDIOCSSCALE		__AIRSPEEDIOC(0)
-
-/** airspeed scaling factors; out = (in * Vscale) + offset */
-struct airspeed_scale {
-	float	offset_pa;
-	float	scale;
-};
-
-#endif /* _DRV_AIRSPEED_H */

src/drivers/uavcan/sensors/airspeed.hpp

@@ -38,7 +38,6 @@
 #pragma once
 
 #include "sensor_bridge.hpp"
-#include <drivers/drv_airspeed.h>
 #include <uORB/topics/airspeed.h>
 
 #include <uavcan/equipment/air_data/IndicatedAirspeed.hpp>

src/drivers/uavcan/sensors/differential_pressure.cpp

@@ -37,7 +37,6 @@
 
 #include "differential_pressure.hpp"
 
-#include <drivers/drv_airspeed.h>
 #include <drivers/drv_hrt.h>
 #include <lib/geo/geo.h>
 #include <parameters/param.h>
@@ -53,9 +52,6 @@ UavcanDifferentialPressureBridge::UavcanDifferentialPressureBridge(uavcan::INode
 
 int UavcanDifferentialPressureBridge::init()
 {
-	// Initialize the calibration offset
-	param_get(param_find("SENS_DPRES_OFF"), &_diff_pres_offset);
-
 	int res = _sub_air.start(AirCbBinder(this, &UavcanDifferentialPressureBridge::air_sub_cb));
 
 	if (res < 0) {
@@ -78,8 +74,8 @@ void UavcanDifferentialPressureBridge::air_sub_cb(const
 	if (PX4_ISFINITE(diff_press_pa)) {
 		differential_pressure_s report{};
 
-		report.differential_pressure_raw_pa = diff_press_pa - _diff_pres_offset;
-		report.differential_pressure_filtered_pa = _filter.apply(diff_press_pa) - _diff_pres_offset;
+		report.differential_pressure_raw_pa = diff_press_pa;
+		report.differential_pressure_filtered_pa = _filter.apply(diff_press_pa);
 		report.temperature = temperature_c;
 		report.device_id = _device_id.devid;
 		report.timestamp = hrt_absolute_time();

src/drivers/uavcan/sensors/differential_pressure.hpp

@@ -37,9 +37,8 @@
 
 #pragma once
 
-#include <uORB/uORB.h>
-#include <uORB/topics/differential_pressure.h>
 #include <mathlib/math/filter/LowPassFilter2p.hpp>
+#include <uORB/topics/differential_pressure.h>
 
 #include "sensor_bridge.hpp"
 
@@ -57,8 +56,6 @@ public:
 	int init() override;
 
 private:
-	float _diff_pres_offset{0.f};
-
 	math::LowPassFilter2p<float> _filter{10.f, 1.1f}; /// Adapted from MS5525 driver
 
 	void air_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::RawAirData> &msg);

src/lib/drivers/airspeed/airspeed.cpp

@@ -47,10 +47,8 @@
 #include <parameters/param.h>
 #include <perf/perf_counter.h>
 
-#include <drivers/drv_airspeed.h>
 #include <drivers/drv_hrt.h>
 
-#include <uORB/uORB.h>
 #include <uORB/topics/differential_pressure.h>
 
 #include <drivers/airspeed/airspeed.h>
@@ -60,9 +58,6 @@ Airspeed::Airspeed(int bus, int bus_frequency, int address, unsigned conversion_
 	_sensor_ok(false),
 	_measure_interval(conversion_interval),
 	_collect_phase(false),
-	_diff_pres_offset(0.0f),
-	_airspeed_orb_class_instance(-1),
-	_class_instance(-1),
 	_conversion_interval(conversion_interval),
 	_sample_perf(perf_alloc(PC_ELAPSED, "aspd_read")),
 	_comms_errors(perf_alloc(PC_COUNT, "aspd_com_err"))
@@ -71,10 +66,6 @@ Airspeed::Airspeed(int bus, int bus_frequency, int address, unsigned conversion_
 
 Airspeed::~Airspeed()
 {
-	if (_class_instance != -1) {
-		unregister_class_devname(AIRSPEED_BASE_DEVICE_PATH, _class_instance);
-	}
-
 	// free perf counters
 	perf_free(_sample_perf);
 	perf_free(_comms_errors);
@@ -88,9 +79,6 @@ Airspeed::init()
 		return PX4_ERROR;
 	}
 
-	/* register alternate interfaces if we have to */
-	_class_instance = register_class_devname(AIRSPEED_BASE_DEVICE_PATH);
-
 	/* advertise sensor topic, measure manually to initialize valid report */
 	measure();
 
@@ -109,20 +97,3 @@ Airspeed::probe()
 
 	return ret;
 }
-
-int
-Airspeed::ioctl(device::file_t *filp, int cmd, unsigned long arg)
-{
-	switch (cmd) {
-	case AIRSPEEDIOCSSCALE: {
-			struct airspeed_scale *s = (struct airspeed_scale *)arg;
-			_diff_pres_offset = s->offset_pa;
-			return OK;
-		}
-
-	default:
-		/* give it to the superclass */
-		return I2C::ioctl(filp, cmd, arg);
-	}
-}
-

src/lib/drivers/airspeed/airspeed.h

@@ -35,7 +35,6 @@
 
 #include <string.h>
 #include <drivers/device/i2c.h>
-#include <drivers/drv_airspeed.h>
 #include <drivers/drv_hrt.h>
 #include <px4_platform_common/px4_config.h>
 #include <px4_platform_common/defines.h>
@@ -51,8 +50,6 @@ public:
 
 	int	init() override;
 
-	int	ioctl(device::file_t *filp, int cmd, unsigned long arg) override;
-
 private:
 	Airspeed(const Airspeed &) = delete;
 	Airspeed &operator=(const Airspeed &) = delete;
@@ -70,14 +67,9 @@ protected:
 	bool			_sensor_ok;
 	int				_measure_interval;
 	bool			_collect_phase;
-	float			_diff_pres_offset;
 
 	uORB::PublicationMulti<differential_pressure_s>	_airspeed_pub{ORB_ID(differential_pressure)};
 
-	int			_airspeed_orb_class_instance;
-
-	int			_class_instance;
-
 	unsigned		_conversion_interval;
 
 	perf_counter_t		_sample_perf;

src/modules/commander/airspeed_calibration.cpp

@@ -49,7 +49,7 @@
 #include <fcntl.h>
 #include <math.h>
 #include <drivers/drv_hrt.h>
-#include <drivers/drv_airspeed.h>
+#include <uORB/SubscriptionBlocking.hpp>
 #include <uORB/topics/differential_pressure.h>
 #include <systemlib/mavlink_log.h>
 #include <parameters/param.h>
@@ -76,47 +76,13 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub)
 
 	const unsigned calibration_count = (maxcount * 2) / 3;
 
-	int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
-	struct differential_pressure_s diff_pres;
+	uORB::SubscriptionBlocking<differential_pressure_s> diff_pres_sub{ORB_ID(differential_pressure)};
 
 	float diff_pres_offset = 0.0f;
 
-	/* Reset sensor parameters */
-	struct airspeed_scale airscale = {
-		diff_pres_offset,
-		1.0f,
-	};
-
-	bool paramreset_successful = false;
-	int  fd = px4_open(AIRSPEED0_DEVICE_PATH, 0);
-
-	if (fd >= 0) {
-		if (PX4_OK == px4_ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
-			paramreset_successful = true;
-
-		} else {
-			calibration_log_critical(mavlink_log_pub, "[cal] airspeed offset zero failed");
-		}
-
-		px4_close(fd);
-	}
-
-	if (!paramreset_successful) {
-
-		/* only warn if analog scaling is zero */
-		float analog_scaling = 0.0f;
-		param_get(param_find("SENS_DPRES_ANSC"), &(analog_scaling));
-
-		if (fabsf(analog_scaling) < 0.1f) {
-			calibration_log_critical(mavlink_log_pub, "[cal] No airspeed sensor found");
-			goto error_return;
-		}
-
-		/* set scaling offset parameter */
-		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;
-		}
+	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;
 	}
 
 	calibration_log_critical(mavlink_log_pub, "[cal] Ensure sensor is not measuring wind");
@@ -128,15 +94,9 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub)
 			goto error_return;
 		}
 
-		/* wait blocking for new data */
-		px4_pollfd_struct_t fds[1];
-		fds[0].fd = diff_pres_sub;
-		fds[0].events = POLLIN;
+		differential_pressure_s diff_pres;
 
-		int poll_ret = px4_poll(fds, 1, 1000);
-
-		if (poll_ret) {
-			orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
+		if (diff_pres_sub.updateBlocking(diff_pres, 1000000)) {
 
 			diff_pres_offset += diff_pres.differential_pressure_raw_pa;
 			calibration_counter++;
@@ -154,7 +114,7 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub)
 				calibration_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, (calibration_counter * 80) / calibration_count);
 			}
 
-		} else if (poll_ret == 0) {
+		} else {
 			/* any poll failure for 1s is a reason to abort */
 			feedback_calibration_failed(mavlink_log_pub);
 			goto error_return;
@@ -164,18 +124,6 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub)
 	diff_pres_offset = diff_pres_offset / calibration_count;
 
 	if (PX4_ISFINITE(diff_pres_offset)) {
-
-		int fd_scale = px4_open(AIRSPEED0_DEVICE_PATH, 0);
-		airscale.offset_pa = diff_pres_offset;
-
-		if (fd_scale >= 0) {
-			if (PX4_OK != px4_ioctl(fd_scale, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
-				calibration_log_critical(mavlink_log_pub, "[cal] airspeed offset update failed");
-			}
-
-			px4_close(fd_scale);
-		}
-
 		// Prevent a completely zero param
 		// since this is used to detect a missing calibration
 		// This value is numerically down in the noise and has
@@ -210,16 +158,9 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub)
 			goto error_return;
 		}
 
-		/* wait blocking for new data */
-		px4_pollfd_struct_t fds[1];
-		fds[0].fd = diff_pres_sub;
-		fds[0].events = POLLIN;
-
-		int poll_ret = px4_poll(fds, 1, 1000);
-
-		if (poll_ret) {
-			orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
+		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)",
@@ -257,7 +198,7 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub)
 
 			calibration_counter++;
 
-		} else if (poll_ret == 0) {
+		} else {
 			/* any poll failure for 1s is a reason to abort */
 			feedback_calibration_failed(mavlink_log_pub);
 			goto error_return;
@@ -274,13 +215,7 @@ 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);
 
-	/* Wait 2sec for the airflow to stop and ensure the driver filter has caught up, otherwise
-	 * the followup preflight checks might fail. */
-	px4_usleep(2e6);
-
 normal_return:
-	px4_close(diff_pres_sub);
-
 	// This give a chance for the log messages to go out of the queue before someone else stomps on then
 	px4_sleep(1);
 

src/modules/sensors/sensors.cpp

@@ -42,8 +42,8 @@
  */
 
 #include <drivers/drv_adc.h>
-#include <drivers/drv_airspeed.h>
 #include <drivers/drv_hrt.h>
+#include <drivers/drv_sensor.h>
 #include <lib/airspeed/airspeed.h>
 #include <lib/mathlib/mathlib.h>
 #include <lib/parameters/param.h>
@@ -416,8 +416,13 @@ void Sensors::diff_pres_poll()
 		airspeed_s airspeed{};
 		airspeed.timestamp = diff_pres.timestamp;
 
+		// 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;
+
 		/* push data into validator */
-		float airspeed_input[3] = { diff_pres.differential_pressure_raw_pa, diff_pres.temperature, 0.0f };
+		float airspeed_input[3] = { differential_pressure_raw_pa, diff_pres.temperature, 0.0f };
 
 		_airspeed_validator.put(airspeed.timestamp, airspeed_input, diff_pres.error_count, 100); // TODO: real priority?
 
@@ -446,7 +451,7 @@ void Sensors::diff_pres_poll()
 		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,
-						  diff_pres.differential_pressure_filtered_pa, air_data.baro_pressure_pa,
+						  differential_pressure_filtered_pa, air_data.baro_pressure_pa,
 						  air_temperature_celsius);
 
 		airspeed.true_airspeed_m_s = calc_TAS_from_CAS(airspeed.indicated_airspeed_m_s, air_data.baro_pressure_pa,
@@ -472,23 +477,6 @@ Sensors::parameter_update_poll(bool forced)
 		// update parameters from storage
 		parameters_update();
 		updateParams();
-
-		/* update airspeed scale */
-		int fd = px4_open(AIRSPEED0_DEVICE_PATH, 0);
-
-		/* this sensor is optional, abort without error */
-		if (fd >= 0) {
-			struct airspeed_scale airscale = {
-				_parameters.diff_pres_offset_pa,
-				1.0f,
-			};
-
-			if (OK != px4_ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
-				warn("WARNING: failed to set scale / offsets for airspeed sensor");
-			}
-
-			px4_close(fd);
-		}
 	}
 }
 
@@ -530,7 +518,7 @@ void Sensors::adc_poll()
 						 * vref. Those devices require no divider at all.
 						 */
 						if (voltage > 0.4f) {
-							const float diff_pres_pa_raw = voltage * _parameters.diff_pres_analog_scale - _parameters.diff_pres_offset_pa;
+							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;