First stab at IOCTL driven offset handling (in PA) for all airspeed sensors. Untested

src/drivers/drv_airspeed.h

@@ -57,5 +57,14 @@
 #define _AIRSPEEDIOCBASE		(0x7700)
 #define __AIRSPEEDIOC(_n)		(_IOC(_AIRSPEEDIOCBASE, _n))
 
+#define AIRSPEEDIOCSSCALE		__AIRSPEEDIOC(0)
+#define AIRSPEEDIOCGSCALE		__AIRSPEEDIOC(1)
+
+
+/** airspeed scaling factors; out = (in * Vscale) + offset */
+struct airspeed_scale {
+	float	offset_pa;
+	float	scale;
+};
 
 #endif /* _DRV_AIRSPEED_H */

src/drivers/ets_airspeed/ets_airspeed.cpp

@@ -129,7 +129,7 @@ private:
 	bool			_sensor_ok;
 	int			_measure_ticks;
 	bool			_collect_phase;
-	int			_diff_pres_offset;
+	float			_diff_pres_offset;
 
 	orb_advert_t		_airspeed_pub;
 
@@ -358,6 +358,19 @@ ETSAirspeed::ioctl(struct file *filp, int cmd, unsigned long arg)
 		/* XXX implement this */
 		return -EINVAL;
 
+	case AIRSPEEDIOCSSCALE: {
+		struct airspeed_scale *s = (struct airspeed_scale*)arg;
+		_diff_pres_offset = s->offset_pa;
+		return OK;
+		}
+
+	case AIRSPEEDIOCGSCALE: {
+		struct airspeed_scale *s = (struct airspeed_scale*)arg;
+		s->offset_pa = _diff_pres_offset;
+		s->scale = 1.0f;
+		return OK;
+		}
+
 	default:
 		/* give it to the superclass */
 		return I2C::ioctl(filp, cmd, arg);
@@ -464,8 +477,6 @@ ETSAirspeed::collect()
 
 	uint16_t diff_pres_pa = val[1] << 8 | val[0];
 
-	// XXX move the parameter read out of the driver.
-	param_get(param_find("SENS_DPRES_OFF"), &_diff_pres_offset);
 	if (diff_pres_pa < _diff_pres_offset + MIN_ACCURATE_DIFF_PRES_PA) {
 		diff_pres_pa = 0;
 

src/modules/sensors/sensor_params.c

@@ -68,7 +68,7 @@ PARAM_DEFINE_FLOAT(SENS_ACC_XSCALE, 1.0f);
 PARAM_DEFINE_FLOAT(SENS_ACC_YSCALE, 1.0f);
 PARAM_DEFINE_FLOAT(SENS_ACC_ZSCALE, 1.0f);
 
-PARAM_DEFINE_INT32(SENS_DPRES_OFF, 1667);
+PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 1667);
 
 PARAM_DEFINE_FLOAT(RC1_MIN, 1000.0f);
 PARAM_DEFINE_FLOAT(RC1_TRIM, 1500.0f);

src/modules/sensors/sensors.cpp

@@ -60,6 +60,7 @@
 #include <drivers/drv_baro.h>
 #include <drivers/drv_rc_input.h>
 #include <drivers/drv_adc.h>
+#include <drivers/drv_airspeed.h>
 
 #include <systemlib/systemlib.h>
 #include <systemlib/param/param.h>
@@ -199,7 +200,7 @@ private:
 		float mag_scale[3];
 		float accel_offset[3];
 		float accel_scale[3];
-		int diff_pres_offset_pa;
+		float diff_pres_offset_pa;
 
 		int rc_type;
 
@@ -230,6 +231,8 @@ private:
 		float rc_scale_flaps;
 
 		float battery_voltage_scaling;
+
+		int airspeed_offset;
 	}		_parameters;			/**< local copies of interesting parameters */
 
 	struct {
@@ -278,6 +281,8 @@ private:
 		param_t rc_scale_flaps;
 
 		param_t battery_voltage_scaling;
+
+		param_t airspeed_offset;
 	}		_parameter_handles;		/**< handles for interesting parameters */
 
 
@@ -551,25 +556,11 @@ Sensors::parameters_update()
 	/* rc values */
 	for (unsigned int i = 0; i < RC_CHANNELS_MAX; i++) {
 
-		if (param_get(_parameter_handles.min[i], &(_parameters.min[i])) != OK) {
+		param_get(_parameter_handles.min[i], &(_parameters.min[i]));
-			warnx("Failed getting min for chan %d", i);
+		param_get(_parameter_handles.trim[i], &(_parameters.trim[i]));
-		}
+		param_get(_parameter_handles.max[i], &(_parameters.max[i]));
-
+		param_get(_parameter_handles.rev[i], &(_parameters.rev[i]));
-		if (param_get(_parameter_handles.trim[i], &(_parameters.trim[i])) != OK) {
+		param_get(_parameter_handles.dz[i], &(_parameters.dz[i]));
-			warnx("Failed getting trim for chan %d", i);
-		}
-
-		if (param_get(_parameter_handles.max[i], &(_parameters.max[i])) != OK) {
-			warnx("Failed getting max for chan %d", i);
-		}
-
-		if (param_get(_parameter_handles.rev[i], &(_parameters.rev[i])) != OK) {
-			warnx("Failed getting rev for chan %d", i);
-		}
-
-		if (param_get(_parameter_handles.dz[i], &(_parameters.dz[i])) != OK) {
-			warnx("Failed getting dead zone for chan %d", i);
-		}
 
 		_parameters.scaling_factor[i] = (1.0f / ((_parameters.max[i] - _parameters.min[i]) / 2.0f) * _parameters.rev[i]);
 
@@ -659,21 +650,10 @@ Sensors::parameters_update()
 		warnx("Failed getting mode aux 5 index");
 	}
 
-	if (param_get(_parameter_handles.rc_scale_roll, &(_parameters.rc_scale_roll)) != OK) {
+	param_get(_parameter_handles.rc_scale_roll, &(_parameters.rc_scale_roll));
-		warnx("Failed getting rc scaling for roll");
+	param_get(_parameter_handles.rc_scale_pitch, &(_parameters.rc_scale_pitch));
-	}
+	param_get(_parameter_handles.rc_scale_yaw, &(_parameters.rc_scale_yaw));
-
+	param_get(_parameter_handles.rc_scale_flaps, &(_parameters.rc_scale_flaps));
-	if (param_get(_parameter_handles.rc_scale_pitch, &(_parameters.rc_scale_pitch)) != OK) {
-		warnx("Failed getting rc scaling for pitch");
-	}
-
-	if (param_get(_parameter_handles.rc_scale_yaw, &(_parameters.rc_scale_yaw)) != OK) {
-		warnx("Failed getting rc scaling for yaw");
-	}
-
-	if (param_get(_parameter_handles.rc_scale_flaps, &(_parameters.rc_scale_flaps)) != OK) {
-		warnx("Failed getting rc scaling for flaps");
-	}
 
 	/* update RC function mappings */
 	_rc.function[THROTTLE] = _parameters.rc_map_throttle - 1;
@@ -1033,6 +1013,20 @@ Sensors::parameter_update_poll(bool forced)
 
 		close(fd);
 
+		fd = open(AIRSPEED_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 != ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale))
+				warn("WARNING: failed to set scale / offsets for airspeed sensor");
+		}
+
 #if 0
 		printf("CH0: RAW MAX: %d MIN %d S: %d MID: %d FUNC: %d\n", (int)_parameters.max[0], (int)_parameters.min[0], (int)(_rc.chan[0].scaling_factor * 10000), (int)(_rc.chan[0].mid), (int)_rc.function[0]);
 		printf("CH1: RAW MAX: %d MIN %d S: %d MID: %d FUNC: %d\n", (int)_parameters.max[1], (int)_parameters.min[1], (int)(_rc.chan[1].scaling_factor * 10000), (int)(_rc.chan[1].mid), (int)_rc.function[1]);