Filtering option added to SDP3X airspeed sensor (#2436)

* Filtering option added to SDP3X airspeed sensor
* Offset and Scale converted to 0 and 1
* AIRSPEED_SDP3X_ID abi msg id added
* AIRSPEED sent as ABI msg instead of directly setting the state interface
* Initialization of the autoset offset to false

sw/airborne/modules/sensors/airspeed_sdp3x.c

@@ -25,6 +25,7 @@
 #include "std.h"
 #include "mcu_periph/i2c.h"
 #include "modules/sensors/airspeed_sdp3x.h"
+#include "filters/low_pass_filter.h"
 #include "subsystems/abi.h"
 
 #include "mcu_periph/uart.h"
@@ -35,6 +36,19 @@
 #include "subsystems/datalink/telemetry.h"
 #endif
 
+#ifndef USE_AIRSPEED_SDP3X
+#if USE_AIRSPEED
+#define USE_AIRSPEED_SDP3X TRUE
+PRINT_CONFIG_MSG("USE_AIRSPEED_SDP3X set to TRUE since this is set USE_AIRSPEED")
+#endif
+#endif
+
+/** Use low pass filter on pressure values
+ */
+#ifndef USE_AIRSPEED_LOWPASS_FILTER
+#define USE_AIRSPEED_LOWPASS_FILTER TRUE
+#endif
+
 /** Commands and scales
  */
 #define SDP3X_SCALE_TEMPERATURE   200.0f
@@ -91,9 +105,20 @@ PRINT_CONFIG_VAR(SDP3X_PRESSURE_OFFSET)
 #define SDP3X_AIRSPEED_SCALE 1.6327
 #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 SDP3X_LOWPASS_TAU
+#define SDP3X_LOWPASS_TAU 0.15
+#endif
+
 struct AirspeedSdp3x sdp3x;
 static struct i2c_transaction sdp3x_trans;
 
+#ifdef USE_AIRSPEED_LOWPASS_FILTER
+static Butterworth2LowPass sdp3x_filter;
+#endif
+
 static bool sdp3x_crc(const uint8_t data[], unsigned size, uint8_t checksum)
 {
   uint8_t crc_value = 0xff;
@@ -127,17 +152,22 @@ static void sdp3x_downlink(struct transport_tx *trans, struct link_device *dev)
 
 void sdp3x_init(void)
 {
-  sdp3x.pressure = 0.;
-  sdp3x.temperature = 0;
-  sdp3x.airspeed = 0.;
+  sdp3x.pressure = 0.f;
+  sdp3x.temperature = 0.f;
+  sdp3x.airspeed = 0.f;
   sdp3x.pressure_scale = SDP3X_PRESSURE_SCALE;
   sdp3x.pressure_offset = SDP3X_PRESSURE_OFFSET;
   sdp3x.airspeed_scale = SDP3X_AIRSPEED_SCALE;
+  sdp3x.autoset_offset = false;
   sdp3x.sync_send = SDP3X_SYNC_SEND;
   sdp3x.initialized = false;
 
   sdp3x_trans.status = I2CTransDone;
   // setup low pass filter with time constant and 100Hz sampling freq
+#ifdef USE_AIRSPEED_LOWPASS_FILTER
+  init_butterworth_2_low_pass(&sdp3x_filter, SDP3X_LOWPASS_TAU,
+                              SDP3X_PERIODIC_PERIOD, 0);
+#endif
 
 #if PERIODIC_TELEMETRY
   register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AIRSPEED_MS45XX, sdp3x_downlink); // FIXME
@@ -163,13 +193,16 @@ void sdp3x_periodic(void)
   }
 }
 
+#define AUTOSET_NB_MAX 20
+
 void sdp3x_event(void)
 {
   /* Check if transaction is succesfull */
   if (sdp3x_trans.status == I2CTransSuccess) {
 
     if (sdp3x.initialized) {
-      // make local copy of buffer
+      static int autoset_nb = 0;
+      static float autoset_offset = 0.f;
       uint8_t buf[6];
       for (uint8_t i = 0; i < 6; i++) {
         buf[i] = sdp3x_trans.buf[i];
@@ -183,7 +216,26 @@ void sdp3x_event(void)
       }
 
       int16_t p_raw = ((int16_t)(buf[0]) << 8) | (int16_t)(buf[1]);
-      sdp3x.pressure = ((float)p_raw / sdp3x.pressure_scale) - sdp3x.pressure_offset;
+
+      float p_out = ((float)p_raw / sdp3x.pressure_scale) - sdp3x.pressure_offset;
+
+#ifdef USE_AIRSPEED_LOWPASS_FILTER
+      sdp3x.pressure = update_butterworth_2_low_pass(&sdp3x_filter, p_out);
+#else
+      sdp3x.pressure = p_out;
+#endif
+
+      if (sdp3x.autoset_offset) {
+        if (autoset_nb < AUTOSET_NB_MAX) {
+          autoset_offset += p_raw * sdp3x.pressure_scale;
+          autoset_nb++;
+        } else {
+          sdp3x.pressure_offset = autoset_offset / (float)autoset_nb;
+          autoset_offset = 0.f;
+          autoset_nb = 0;
+          sdp3x.autoset_offset = false;
+        }
+      }
 
       int16_t t_raw = ((int16_t)(buf[3]) << 8) | (int16_t)(buf[4]);
       sdp3x.temperature = (float)t_raw / SDP3X_SCALE_TEMPERATURE;
@@ -195,6 +247,9 @@ void sdp3x_event(void)
       // Compute airspeed
       sdp3x.airspeed = sqrtf(Max(sdp3x.pressure * sdp3x.airspeed_scale, 0));
 
+#if USE_AIRSPEED_SDP3X
+      AbiSendMsgAIRSPEED(AIRSPEED_SDP3X_ID, sdp3x.airspeed);
+#endif
       if (sdp3x.sync_send) {
         sdp3x_downlink(&(DefaultChannel).trans_tx, &(DefaultDevice).device);
       }

sw/airborne/modules/sensors/airspeed_sdp3x.h

@@ -34,6 +34,7 @@ struct AirspeedSdp3x {
   float airspeed_scale;        ///< Quadratic scale factor to convert (differential) pressure to airspeed
   float pressure_scale;        ///< Scaling factor from raw measurement to Pascal
   float pressure_offset;       ///< Offset in Pascal
+  bool autoset_offset;         ///< Set offset value from current filtered value
   bool sync_send;              ///< Flag to enable sending every new measurement via telemetry for debugging purpose
   bool initialized;            ///< init flag
 };

sw/airborne/subsystems/abi_sender_ids.h

@@ -103,6 +103,10 @@
 #define AIRSPEED_ADC_ID 2
 #endif
 
+#ifndef AIRSPEED_SDP3X_ID
+#define AIRSPEED_SDP3X_ID 3
+#endif
+
 /*
  * IDs of Incidence angles (message 24)
  */