[modules] Add offset and scaling to UAVCAN airspeed sensor (#3212)

Updated airspeed and servo endpoints

Airspeed debugging

Co-authored-by: Freek van Tienen <freek.v.tienen@gmail.com>

conf/airframes/tudelft/rot_wing_v3b.xml

@@ -74,11 +74,13 @@
             <define name="MS45XX_PRESSURE_SCALE"        value="1.9077609"/>
             <define name="USE_AIRSPEED_LOWPASS_FILTER"  value="TRUE"/>
             <define name="MS45XX_LOWPASS_TAU"           value="0.15"/>
+            <define name="AIRSPEED_MS45XX_SEND_ABI"     value="0"/>
         </module>
         <module name="airspeed" type="uavcan">
             <define name="AIRSPEED_UAVCAN_LOWPASS_FILTER"   value="TRUE" />
             <define name="AIRSPEED_UAVCAN_LOWPASS_PERIOD"   value="0.1" />
-            <define name="AIRSPEED_UAVCAN_SEND_ABI"         value="0" /> <!-- Read Airspeed for logging but do not use it -->
+            <define name="AIRSPEED_UAVCAN_LOWPASS_TAU"      value="0.15" />
+            <define name="AIRSPEED_UAVCAN_SEND_ABI"         value="1" /> <!-- Read Airspeed for logging but do not use it -->
         </module>
         <module name="air_data"/>
         <module name="gps" type="ublox">

conf/airframes/tudelft/rot_wing_v3d.xml

@@ -74,10 +74,12 @@
             <define name="MS45XX_PRESSURE_SCALE"        value="1.9077609"/>
             <define name="USE_AIRSPEED_LOWPASS_FILTER"  value="TRUE"/>
             <define name="MS45XX_LOWPASS_TAU"           value="0.15"/>
+            <define name="AIRSPEED_MS45XX_SEND_ABI"     value="1"/>
         </module>
         <module name="airspeed" type="uavcan">
             <define name="AIRSPEED_UAVCAN_LOWPASS_FILTER"   value="TRUE" />
             <define name="AIRSPEED_UAVCAN_LOWPASS_PERIOD"   value="0.1" />
+            <define name="AIRSPEED_UAVCAN_LOWPASS_TAU"      value="0.15" />
             <define name="AIRSPEED_UAVCAN_SEND_ABI"         value="0" /> <!-- Read Airspeed for logging but do not use it -->
         </module>
         <module name="air_data"/>

conf/modules/airspeed_uavcan.xml

@@ -9,7 +9,17 @@
     <define name="AIRSPEED_UAVCAN_LOWPASS_TAU" value="0.15" description="Time constant for second order Butterworth low pass filter"/>
     <define name="AIRSPEED_UAVCAN_LOWPASS_PERIOD" value="0.1" description="Period at which the sensor is sending airspeed"/>
     <define name="AIRSPEED_UAVCAN_SEND_ABI" value="true" description="Send the uavcan airspeed sensor over ABI"/>
+    <define name="AIRSPEED_UAVCAN_DIFF_P_SCALE" value="1.0" description="Pressure scaling correcting factor"/>
   </doc>
+  <settings>
+    <dl_settings>
+      <dl_settings name="Airspeed UAVCAN">
+        <dl_setting shortname="autoset offset" var="autoset_offset" handler="autoset_offset" min="0" max="1" step="1" values="FALSE|TRUE" type="fun" module="modules/sensors/airspeed_uavcan"/>
+        <dl_setting shortname="diff_p offset" var="airspeed_uavcan.diff_p_offset" min="-2000" max="1000" step="0.1" type="float"/>
+        <dl_setting shortname="diff_p scale" var="airspeed_uavcan.diff_p_scale" min="0" max="1" step="0.01" type="float" param="AIRSPEED_UAVCAN_DIFF_P_SCALE"/>
+      </dl_settings>
+    </dl_settings>
+  </settings>
   <dep>
     <depends>uavcan</depends>
     <provides>airspeed</provides>

conf/telemetry/highspeed_rotorcraft.xml

@@ -24,7 +24,7 @@
       <message name="ENERGY"                   period="0.5"/>
       <message name="DATALINK_REPORT"          period="5.1"/>
       <message name="STATE_FILTER_STATUS"      period="3.2"/>
-      <message name="AIR_DATA"                 period="0.6"/>
+      <message name="AIR_DATA"                 period="0.1"/>
       <message name="SURVEY"                   period="2.5"/>
       <message name="OPTIC_FLOW_EST"           period="0.05"/>
       <message name="VECTORNAV_INFO"           period="0.5"/>

sw/airborne/modules/sensors/airspeed_ms45xx_i2c.c

@@ -39,6 +39,11 @@
 #include "modules/datalink/telemetry.h"
 #endif
 
+/* Enable ABI sending */
+#ifndef AIRSPEED_MS45XX_SEND_ABI
+#define AIRSPEED_MS45XX_SEND_ABI true
+#endif
+
 /** Default I2C device
  */
 #ifndef MS45XX_I2C_DEV
@@ -286,10 +291,16 @@ void ms45xx_i2c_event(void)
       ms45xx.temperature = ((uint32_t)temp_raw * 2000) / 2047 - 500;
 
       // Send (differential) pressure via ABI
+
+      #if AIRSPEED_MS45XX_SEND_ABI
       AbiSendMsgBARO_DIFF(MS45XX_SENDER_ID, ms45xx.pressure);
+
       // Send temperature as float in deg Celcius via ABI
       float temp = ms45xx.temperature / 10.0f;
+      
       AbiSendMsgTEMPERATURE(MS45XX_SENDER_ID, temp);
+      #endif
+
       // Compute airspeed
       float sign = 1.0f;
       if (ms45xx.pressure < 0.0f) {

sw/airborne/modules/sensors/airspeed_uavcan.c

@@ -32,6 +32,11 @@
 #define AIRSPEED_UAVCAN_SEND_ABI true
 #endif
 
+/* Default pressure scaling */
+#ifndef AIRSPEED_UAVCAN_DIFF_P_SCALE
+#define AIRSPEED_UAVCAN_DIFF_P_SCALE 1.0f
+#endif
+
 /* Airspeed lowpass filter*/
 #ifdef USE_AIRSPEED_UAVCAN_LOWPASS_FILTER
 #include "filters/low_pass_filter.h"
@@ -52,14 +57,8 @@ static Butterworth2LowPass airspeed_filter;
 #define UAVCAN_EQUIPMENT_AIR_DATA_RAWAIRDATA_SIGNATURE     (0xC77DF38BA122F5DAULL)
 #define UAVCAN_EQUIPMENT_AIR_DATA_RAWAIRDATA_MAX_SIZE      ((397 + 7)/8)
 
-/* Local structure */
-struct airspeed_uavcan_t {
-  float diff_p;       ///< Differential pressure
-  float temperature;  ///< Temperature in Celsius
-};
-
 /* Local variables */
-static struct airspeed_uavcan_t airspeed_uavcan = {0};
+struct airspeed_uavcan_t airspeed_uavcan = {0};
 static uavcan_event airspeed_uavcan_ev;
 
 #if PERIODIC_TELEMETRY
@@ -103,6 +102,11 @@ static void airspeed_uavcan_cb(struct uavcan_iface_t *iface __attribute__((unuse
   //float pitot_temp = canardConvertFloat16ToNativeFloat(tmp_float);
 
   if(!isnan(diff_p)) {
+    // Remove the offset and apply a scaling factor
+    diff_p -= airspeed_uavcan.diff_p_offset;
+    diff_p *= airspeed_uavcan.diff_p_scale;
+
+    // Filtering
 #ifdef USE_AIRSPEED_UAVCAN_LOWPASS_FILTER
     float diff_p_filt = update_butterworth_2_low_pass(&airspeed_filter, diff_p);
     airspeed_uavcan.diff_p = diff_p_filt;
@@ -110,6 +114,7 @@ static void airspeed_uavcan_cb(struct uavcan_iface_t *iface __attribute__((unuse
     airspeed_uavcan.diff_p = diff_p;
 #endif
 
+    // Send the ABI message
 #if AIRSPEED_UAVCAN_SEND_ABI
     AbiSendMsgBARO_DIFF(UAVCAN_SENDER_ID, airspeed_uavcan.diff_p);
 #endif
@@ -125,6 +130,9 @@ static void airspeed_uavcan_cb(struct uavcan_iface_t *iface __attribute__((unuse
 
 void airspeed_uavcan_init(void)
 {
+  // Set the default values
+  airspeed_uavcan.diff_p_scale = AIRSPEED_UAVCAN_DIFF_P_SCALE;
+
   // Setup the low pass filter
 #ifdef USE_AIRSPEED_UAVCAN_LOWPASS_FILTER
   init_butterworth_2_low_pass(&airspeed_filter, AIRSPEED_UAVCAN_LOWPASS_TAU, AIRSPEED_UAVCAN_LOWPASS_PERIOD, 0);
@@ -137,3 +145,9 @@ void airspeed_uavcan_init(void)
   register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AIRSPEED_RAW, airspeed_uavcan_downlink);
 #endif
 }
+
+void airspeed_uavcan_autoset_offset(bool set) {
+  if(set) {
+    airspeed_uavcan.diff_p_offset = airspeed_uavcan.diff_p;
+  }
+}

sw/airborne/modules/sensors/airspeed_uavcan.h

@@ -26,7 +26,20 @@
 #ifndef AIRSPEED_UAVCAN_H
 #define AIRSPEED_UAVCAN_H
 
+#include "std.h"
+
+/* Airspeed UAVCAN structure */
+struct airspeed_uavcan_t {
+  float diff_p;       ///< Differential pressure
+  float temperature;  ///< Temperature in Celsius
+
+  float diff_p_offset;  ///< Differential pressure offset
+  float diff_p_scale;   ///< Differential pressure scale
+};
+extern struct airspeed_uavcan_t airspeed_uavcan;
+
 /* External functions */
 extern void airspeed_uavcan_init(void);
+extern void airspeed_uavcan_autoset_offset(bool set);
 
 #endif /* AIRSPEED_UAVCAN_H */