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

2 files changed, 23 insertions, 3 deletions

diff --git a/src/drivers/drv_airspeed.h b/src/drivers/drv_airspeed.h
index bffc35c62..7bb9ee2af 100644
--- a/src/drivers/drv_airspeed.h
+++ b/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 */
diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp
index b34d3fa5d..da449c195 100644
--- a/src/drivers/ets_airspeed/ets_airspeed.cpp
+++ b/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;