Added and option to get flyble values out of this old ETS sensor (#3534)

* Added and option to get flyble values out of the old sensor with decent filtering

* Extended the test block in module XML

conf/modules/airspeed_ets.xml

@@ -4,27 +4,37 @@
   <doc>
     <description>
       Airspeed ETS (I2C).
-      Driver for the EagleTree Systems Airspeed Sensor.
-      Has only been tested with V3 of the sensor hardware.
+      Driver for the EagleTree Systems Airspeed Sensor v3 connected via an I2C port. 
+      This device measures airspeed from 4m/s to 156m/s with about 0,5 m/s resolution
 
       Notes:
-      Connect directly to TWOG/Tiny/Lisa I2C port. Multiple sensors can be chained together.
-      Sensor may be in the proprietary mode (default) or in 3rd-party mode. In 3rd-party mode you must define AIRSPEED_ETS_3RD_PARTY_MODE.
-      A eLogger is needed to set sensor to 3rd-party mode, which is supposed to be more reliable than the default mode.
+      + Connects directly to a flightcontroller board I2C port. 
+      + Has I2C signal level conversion build in. 
+      + Multiple sensors can be chained together.
+      + Sensor may be in the default or in 3rd-party mode. 
+      
+      Per default the sensor is in Eagltreemode, but if you have set your sensor to 3rd-party mode
+      only then must you add the define AIRSPEED_ETS_3RD_PARTY_MODE to your airframe
+      
+      This driver has only been tested with v3 of the sensor hardware, older versions might or might not work.
 
       Sensor module wire assignments:
-      - Red wire: 5V
+      - Red wire: 3V-16V
       - White wire: Ground
       - Yellow wire: SDA
       - Brown wire: SCL
+
     </description>
     <configure name="AIRSPEED_ETS_I2C_DEV" value="i2cX" description="set i2c peripheral (default: i2c0)"/>
-    <define name="AIRSPEED_ETS_OFFSET" value="offset" description="sensor reading offset for sensor in proprietary mode (default: 0)"/>
-    <define name="AIRSPEED_ETS_SCALE" value="scale" description="sensor scale factor for sensor in proprietary mode  (default: 1.8)"/>
-    <define name="AIRSPEED_ETS_START_DELAY" value="delay" description="set initial start delay in seconds"/>
-    <define name="USE_AIRSPEED_ETS" value="TRUE|FALSE" description="set airspeed in state interface"/>
-    <define name="AIRSPEED_ETS_3RD_PARTY_MODE"  description="read raw value for sensor in third-party mode"/>
-    <define name="AIRSPEED_ETS_SDLOG" value="TRUE|FALSE" description="start logging to SD card"/>
+    <define name="AIRSPEED_ETS_OFFSET" value="0" description="sensor reading offset for sensor in proprietary mode (default: 0)"/>
+    <define name="AIRSPEED_ETS_SCALE" value="1.76" description="sensor scale factor for sensor in proprietary mode  (default: 1.8)"/>
+    <define name="AIRSPEED_ETS_START_DELAY" value="1.5" description="set initial start delay in seconds(float), somtimes needed if sensor initialization is not OK, default 0.2s"/>
+    <define name="AIRSPEED_ETS_SYNC_SEND" description="flag to transmit the data as it is acquired"/>
+    <define name="AIRSPEED_ETS_3RD_PARTY_MODE"  description="Read raw value for sensor in third-party mode"/>
+    <define name="AIRSPEED_ETS_USE_FILTER" value="TRUE" description="Enable or disable a filter on the sensor output values"/>
+    <define name="AIRSPEED_ETS_LOWPASS_TAU" value="0.15" description="Time constant for second order Butterworth low pass filter .15 is about 1hz"/>
+    <define name="USE_AIRSPEED_ETS" value="TRUE|FALSE" description="Set airspeed in state interface for this sensor"/>
+    <define name="AIRSPEED_ETS_SDLOG" value="TRUE|FALSE" description="start logging to SD card for Chibios only"/>
   </doc>
   <dep>
     <depends>i2c</depends>
@@ -44,6 +54,7 @@
     <define name="AIRSPEED_ETS_I2C_DEV" value="$(AIRSPEED_ETS_I2C_DEV_LOWER)"/>
     <file name="airspeed_ets.c"/>
     <test>
+      <define name="AIRSPEED_ETS_READ_PERIODIC_PERIOD" value="0.001"/>
       <define name="USE_I2C0"/>
       <define name="USE_AIRSPEED_ETS"/>
       <define name="DOWNLINK_TRANSPORT" value="pprz_tp"/>
@@ -53,4 +64,3 @@
   </makefile>
 
 </module>
-

sw/airborne/modules/sensors/airspeed_ets.c

@@ -23,14 +23,14 @@
 
 /**
  * @file modules/sensors/airspeed_ets.c
- *
- * Driver for the EagleTree Systems Airspeed Sensor.
- * Has only been tested with V3 of the sensor hardware.
+ * @brief Driver for the EagleTree Systems Airspeed Sensor v3 connected via an I2C port. 
+ * The device Measures airspeed from 4m/s to 156m/s with about 0,5 m/s resolution
  *
  * Notes:
- * Connect directly to TWOG/Tiny I2C port. Multiple sensors can be chained together.
- * Sensor should be in the proprietary mode (default) and not in 3rd party mode.
- * Define AIRSPEED_ETS_3RD_PARTY_MODE to run it in 3rd party mode.
+ * Connects directly to a flightcontroller board I2C port. Has signal level conversion build in. Multiple sensors can be chained together.
+ * Sensor may be in the default or in 3rd-party mode. If set to 3rd-party mode you must define AIRSPEED_ETS_3RD_PARTY_MODE.
+ *
+ * To set sensor to 3rd-party mode one can use the supplied Jumper, read Eagletree original manual on how to do this.
  *
  * Sensor module wire assignments:
  * Red wire: 5V
@@ -65,6 +65,7 @@ PRINT_CONFIG_MSG("AIRSPEED_ETS not used")
 #endif
 
 #define AIRSPEED_ETS_ADDR 0xEA
+
 #ifndef AIRSPEED_ETS_SCALE
 #define AIRSPEED_ETS_SCALE 1.8
 #endif
@@ -75,7 +76,23 @@ PRINT_CONFIG_MSG("AIRSPEED_ETS not used")
 #define AIRSPEED_ETS_OFFSET_MIN 1450
 #define AIRSPEED_ETS_OFFSET_NBSAMPLES_INIT 40
 #define AIRSPEED_ETS_OFFSET_NBSAMPLES_AVRG 60
+
+#ifndef AIRSPEED_ETS_USE_FILTER
+#define AIRSPEED_ETS_USE_FILTER FALSE
+#endif
+
+#ifdef AIRSPEED_ETS_USE_FILTER
+#define AIRSPEED_ETS_NBSAMPLES_AVRG 20
+#else
 #define AIRSPEED_ETS_NBSAMPLES_AVRG 10
+#endif
+
+/** Time constant for second order Butterworth low pass filter
+ * Default of 0.15 should give cut-off freq of 1/(2*pi*tau) ~= 1Hz
+ */
+#ifndef AIRSPEED_ETS_LOWPASS_TAU
+#define AIRSPEED_ETS_LOWPASS_TAU 0.15
+#endif
 
 #ifndef AIRSPEED_ETS_I2C_DEV
 #define AIRSPEED_ETS_I2C_DEV i2c0
@@ -88,6 +105,10 @@ PRINT_CONFIG_VAR(AIRSPEED_ETS_I2C_DEV)
 #endif
 PRINT_CONFIG_VAR(AIRSPEED_ETS_START_DELAY)
 
+#ifdef AIRSPEED_ETS_USE_FILTER
+#include "filters/low_pass_filter.h"
+#endif
+
 #ifndef SITL
 #if AIRSPEED_ETS_SDLOG
 #include "modules/loggers/sdlog_chibios.h"
@@ -108,6 +129,10 @@ float airspeed_ets_buffer[AIRSPEED_ETS_NBSAMPLES_AVRG];
 
 struct i2c_transaction airspeed_ets_i2c_trans;
 
+#ifdef AIRSPEED_ETS_USE_FILTER
+static Butterworth2LowPass airspeed_filter;
+#endif
+
 // Local variables
 volatile bool airspeed_ets_i2c_done;
 bool airspeed_ets_offset_init;
@@ -148,6 +173,11 @@ void airspeed_ets_init(void)
     airspeed_ets_buffer[n] = 0.0;
   }
 
+#ifdef AIRSPEED_ETS_USE_FILTER
+  init_butterworth_2_low_pass(&airspeed_filter, AIRSPEED_ETS_LOWPASS_TAU,
+                              AIRSPEED_ETS_READ_PERIODIC_PERIOD, airspeed_ets);
+#endif
+
   airspeed_ets_i2c_trans.status = I2CTransDone;
 
   airspeed_ets_delay_done = false;
@@ -198,6 +228,7 @@ void airspeed_ets_read_event(void)
   // Continue only if a new airspeed value was received
   if (airspeed_ets_valid) {
 #if !AIRSPEED_ETS_3RD_PARTY_MODE
+    
     // Calculate offset average if not done already
     if (!airspeed_ets_offset_init) {
       --airspeed_ets_cnt;
@@ -219,14 +250,15 @@ void airspeed_ets_read_event(void)
         airspeed_ets_offset_tmp += airspeed_ets_raw;
       }
     }
+
     // Convert raw to m/s
 #ifdef AIRSPEED_ETS_REVERSE
     if (airspeed_ets_offset_init && airspeed_ets_raw < airspeed_ets_offset) {
-      airspeed_tmp = AIRSPEED_ETS_SCALE * sqrtf((float)(airspeed_ets_offset - airspeed_ets_raw)) - AIRSPEED_ETS_OFFSET;
+      airspeed_tmp = AIRSPEED_ETS_SCALE * sqrtf((float)(airspeed_ets_offset - airspeed_ets_raw)) - (float)AIRSPEED_ETS_OFFSET;
     }
 #else
     if (airspeed_ets_offset_init && airspeed_ets_raw > airspeed_ets_offset) {
-      airspeed_tmp = AIRSPEED_ETS_SCALE * sqrtf((float)(airspeed_ets_raw - airspeed_ets_offset)) - AIRSPEED_ETS_OFFSET;
+      airspeed_tmp = AIRSPEED_ETS_SCALE * sqrtf((float)(airspeed_ets_raw - airspeed_ets_offset)) - (float)AIRSPEED_ETS_OFFSET;
     }
 #endif
     else {
@@ -252,6 +284,20 @@ void airspeed_ets_read_event(void)
     }
     airspeed_ets = airspeed_ets / (float)AIRSPEED_ETS_NBSAMPLES_AVRG;
     
+#ifdef AIRSPEED_ETS_USE_FILTER
+    float a_out;
+    a_out = airspeed_tmp;
+    airspeed_ets = update_butterworth_2_low_pass(&airspeed_filter, a_out);
+#endif
+
+    //Sometimes offset is not correct enough calculated dynamically and yields to a small negative airspeed once in a while at certain samples
+    //Also with steep airspeed decline and a butterworth_2_low_pass filter on it could overshoot to a negative value this is also capped
+    //so force cap it to zero
+    if ((airspeed_ets < 0.0) || (airspeed_tmp < 0.0)) {
+      airspeed_tmp = 0.0; //for filter also
+      airspeed_ets = 0.0;   
+    }
+
     // Publish airspeed sensor
     AbiSendMsgAIRSPEED(AIRSPEED_ETS_ID, airspeed_ets);