[modules] UAVCAN range sensor and cleanup (#3198)

* [modules] Cleanup airspeed uavcan

* [modules] Add UAVCAN range sensor

conf/airframes/examples/cube_orange.xml

@@ -24,10 +24,8 @@
         <configure name="SBUS_PORT" value="UART3"/>
       </module>
 
-      <module name="airspeed" type="ms45xx_i2c">
+      <module name="airspeed" type="uavcan"/>
-        <define name="USE_I2C2"/>
+      <module name="range_sensor" type="uavcan"/>
-        <define name="MS45XX_I2C_DEV" value="i2c2"/>
-      </module>
 
       <module name="scheduling_indi_simple"/>
 

conf/conf_tests.xml

@@ -7,7 +7,7 @@
    telemetry="telemetry/highspeed_rotorcraft.xml"
    flight_plan="flight_plans/rotorcraft_basic_geofence.xml"
    settings="settings/rotorcraft_basic.xml"
-   settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/electrical.xml modules/gps.xml modules/gps_ublox.xml modules/gps_ubx_ucenter.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/imu_heater.xml modules/ins_ekf2.xml modules/logger_sd_chibios.xml modules/nav_hybrid.xml modules/nav_rotorcraft.xml modules/preflight_checks.xml modules/scheduling_indi_simple.xml modules/stabilization_indi_simple.xml"
+   settings_modules="modules/air_data.xml modules/electrical.xml modules/gps.xml modules/gps_ublox.xml modules/gps_ubx_ucenter.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/imu_heater.xml modules/ins_ekf2.xml modules/logger_sd_chibios.xml modules/nav_hybrid.xml modules/nav_rotorcraft.xml modules/preflight_checks.xml modules/scheduling_indi_simple.xml modules/stabilization_indi_simple.xml"
    gui_color="blue"
   />
   <aircraft

conf/modules/airspeed_uavcan.xml

@@ -3,25 +3,21 @@
 <module name="airspeed_uavcan" dir="sensors" task="sensors">
   <doc>
     <description>
-      Airspeed sensor over the uavcan protocol. Currently only subscribes to the 
+      Airspeed sensor over the uavcan protocol and optionally publishes over ABI. 
     </description>
     <define name="AIRSPEED_UAVCAN_LOWPASS_FILTER" value="TRUE|FALSE" description="Enable the lowpass filter for the airspeed"/>
     <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="1" description="1 = Send ABI message, 0 = Log only"/>
+    <define name="AIRSPEED_UAVCAN_SEND_ABI" value="true" description="Send the uavcan airspeed sensor over ABI"/>
   </doc>
-
   <dep>
     <depends>uavcan</depends>
     <provides>airspeed</provides>
   </dep>
-
   <header>
     <file name="airspeed_uavcan.h"/>
   </header>
-
   <init fun="airspeed_uavcan_init()"/>
-
   <makefile target="ap">
     <file name="airspeed_uavcan.c"/>
   </makefile>

conf/modules/range_sensor_uavcan.xml

@@ -0,0 +1,20 @@
+<!DOCTYPE module SYSTEM "module.dtd">
+
+<module name="range_sensor_uavcan" dir="sensors" task="sensors">
+  <doc>
+    <description>
+      Range sensor over the uavcan protocol which publishes over ABI as AGL
+    </description>
+  </doc>
+  <dep>
+    <depends>uavcan</depends>
+    <provides>sonar</provides>
+  </dep>
+  <header>
+    <file name="range_sensor_uavcan.h"/>
+  </header>
+  <init fun="range_sensor_uavcan_init()"/>
+  <makefile target="ap">
+    <file name="range_sensor_uavcan.c"/>
+  </makefile>
+</module>

sw/airborne/modules/core/abi_sender_ids.h

@@ -197,6 +197,10 @@
 #define AGL_LIDAR_MATEKSYS_3901_L0X_ID 15
 #endif
 
+#ifndef AGL_UAVCAN_ID
+#define AGL_UAVCAN_ID 16
+#endif
+
 /*
  * IDs of magnetometer sensors (including IMUs with mag)
  */

sw/airborne/modules/sensors/airspeed_uavcan.c

@@ -26,15 +26,16 @@
 #include "airspeed_uavcan.h"
 #include "uavcan/uavcan.h"
 #include "core/abi.h"
-#include "filters/low_pass_filter.h"
 
-#include "pprzlink/messages.h"
+/* Enable ABI sending */
-#include "modules/datalink/downlink.h"
+#ifndef AIRSPEED_UAVCAN_SEND_ABI
-
+#define AIRSPEED_UAVCAN_SEND_ABI true
-#if PERIODIC_TELEMETRY
-#include "modules/datalink/telemetry.h"
 #endif
 
+/* Airspeed lowpass filter*/
+#ifdef USE_AIRSPEED_UAVCAN_LOWPASS_FILTER
+#include "filters/low_pass_filter.h"
+
 #ifndef AIRSPEED_UAVCAN_LOWPASS_TAU
 #define AIRSPEED_UAVCAN_LOWPASS_TAU 0.15
 #endif
@@ -43,21 +44,26 @@
 #define AIRSPEED_UAVCAN_LOWPASS_PERIOD 0.1
 #endif
 
-#ifndef AIRSPEED_UAVCAN_SEND_ABI
-#define AIRSPEED_UAVCAN_SEND_ABI 1
-#endif
-
-#ifdef USE_AIRSPEED_UAVCAN_LOWPASS_FILTER
 static Butterworth2LowPass airspeed_filter;
-#endif
+#endif  /* USE_AIRSPEED_UAVCAN_LOWPASS_FILTER */
-static uavcan_event airspeed_uavcan_ev;
 
 /* uavcan EQUIPMENT_ESC_STATUS message definition */
 #define UAVCAN_EQUIPMENT_AIR_DATA_RAWAIRDATA_ID            1027
 #define UAVCAN_EQUIPMENT_AIR_DATA_RAWAIRDATA_SIGNATURE     (0xC77DF38BA122F5DAULL)
 #define UAVCAN_EQUIPMENT_AIR_DATA_RAWAIRDATA_MAX_SIZE      ((397 + 7)/8)
 
-struct airspeed_uavcan_s airspeed_uavcan;
+/* 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};
+static uavcan_event airspeed_uavcan_ev;
+
+#if PERIODIC_TELEMETRY
+#include "modules/datalink/telemetry.h"
 
 static void airspeed_uavcan_downlink(struct transport_tx *trans, struct link_device *dev)
 {
@@ -77,6 +83,7 @@ static void airspeed_uavcan_downlink(struct transport_tx *trans, struct link_dev
                                 &airspeed_uavcan.temperature,
                                 &airspeed);
 }
+#endif /* PERIODIC_TELEMETRY */
 
 static void airspeed_uavcan_cb(struct uavcan_iface_t *iface __attribute__((unused)), CanardRxTransfer *transfer) {
   uint16_t tmp_float = 0;
@@ -118,18 +125,15 @@ static void airspeed_uavcan_cb(struct uavcan_iface_t *iface __attribute__((unuse
 
 void airspeed_uavcan_init(void)
 {
-  // Setup low pass filter with time constant and 100Hz sampling freq
+  // 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);
 #endif
 
-  airspeed_uavcan.diff_p = 0;
-  airspeed_uavcan.temperature = 0;
-
   // Bind uavcan RAWAIRDATA message from EQUIPMENT.AIR_DATA
   uavcan_bind(UAVCAN_EQUIPMENT_AIR_DATA_RAWAIRDATA_ID, UAVCAN_EQUIPMENT_AIR_DATA_RAWAIRDATA_SIGNATURE, &airspeed_uavcan_ev, &airspeed_uavcan_cb);
 
-  #if PERIODIC_TELEMETRY
+#if PERIODIC_TELEMETRY
-    register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AIRSPEED_RAW, airspeed_uavcan_downlink);
+  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AIRSPEED_RAW, airspeed_uavcan_downlink);
-  #endif
+#endif
 }

sw/airborne/modules/sensors/airspeed_uavcan.h

@@ -26,15 +26,7 @@
 #ifndef AIRSPEED_UAVCAN_H
 #define AIRSPEED_UAVCAN_H
 
-/* External variables */
-extern struct airspeed_uavcan_s {
-  float diff_p;
-  float temperature;
-} airspeed_uavcan;
-
 /* External functions */
 extern void airspeed_uavcan_init(void);
 
-
-
 #endif /* AIRSPEED_UAVCAN_H */

sw/airborne/modules/sensors/range_sensor_uavcan.c

@@ -0,0 +1,92 @@
+/*
+ * Copyright (C) 2023 Freek van Tienen <freek.v.tienen@gmail.com>
+ *
+ * This file is part of paparazzi
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING.  If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/** @file modules/sensors/range_sensor_uavcan.c
+ * Range sensor on the uavcan bus
+ */
+
+#include "range_sensor_uavcan.h"
+#include "uavcan/uavcan.h"
+#include "core/abi.h"
+
+
+/* uavcan EQUIPMENT_RANGE_SENSOR_MEASUREMENT message definition */
+#define UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_ID       1050
+#define UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_SIGNATURE (0x68FFFE70FC771952ULL)
+#define UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_MAX_SIZE ((120 + 7)/8)
+
+/* Local structure */
+struct range_sensor_uavcan_t {
+  float range;
+  uint8_t reading_type;
+};
+
+/* Local variables */
+static struct range_sensor_uavcan_t range_sensor_uavcan = {0};
+static uavcan_event range_sensor_uavcan_ev;
+
+#if PERIODIC_TELEMETRY
+#include "modules/datalink/telemetry.h"
+
+static void range_sensor_uavcan_send_lidar(struct transport_tx *trans, struct link_device *dev)
+{
+  uint8_t nul = 0;
+  pprz_msg_send_LIDAR(trans, dev, AC_ID,
+                      &range_sensor_uavcan.range,
+                      &range_sensor_uavcan.reading_type,
+                      &nul);
+}
+
+#endif
+
+static void range_sensor_uavcan_cb(struct uavcan_iface_t *iface __attribute__((unused)), CanardRxTransfer *transfer) {
+  uint16_t tmp_float = 0;
+
+  /* Decode the message */
+  //canardDecodeScalar(transfer, (uint32_t)0, 56, false, (void*)&dest->usec);
+  //canardDecodeScalar(transfer, (uint32_t)56, 8, false, (void*)&dest->sensor_id);
+  //canardDecodeScalar(transfer, (uint32_t)64, 5, true, (void*)(dest->fixed_axis_roll_pitch_yaw + 0));
+  //canardDecodeScalar(transfer, (uint32_t)69, 5, true, (void*)(dest->fixed_axis_roll_pitch_yaw + 1));
+  //canardDecodeScalar(transfer, (uint32_t)74, 5, true, (void*)(dest->fixed_axis_roll_pitch_yaw + 2));
+  //canardDecodeScalar(transfer, (uint32_t)79, 1, false, (void*)&dest->orientation_defined);
+  //canardDecodeScalar(transfer, (uint32_t)80, 16, false, (void*)&tmp_float);
+  //float fov = canardConvertFloat16ToNativeFloat(tmp_float);
+  //canardDecodeScalar(transfer, (uint32_t)96, 5, false, (void*)&dest->sensor_type);
+  canardDecodeScalar(transfer, (uint32_t)101, 3, false, (void*)&range_sensor_uavcan.reading_type);
+  canardDecodeScalar(transfer, (uint32_t)104, 16, false, (void*)&tmp_float);
+  range_sensor_uavcan.range = canardConvertFloat16ToNativeFloat(tmp_float);
+
+  // Send the range over ABI
+  if(!isnan(range_sensor_uavcan.range)) {
+    uint32_t now_ts = get_sys_time_usec();
+    AbiSendMsgAGL(AGL_UAVCAN_ID, now_ts, range_sensor_uavcan.range);
+  }
+}
+
+void range_sensor_uavcan_init(void)
+{
+  // Bind uavcan MEASUREMENT message from EQUIPMENT.RANGE_SENSOR
+  uavcan_bind(UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_ID, UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_SIGNATURE, &range_sensor_uavcan_ev, &range_sensor_uavcan_cb);
+
+#if PERIODIC_TELEMETRY
+  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_LIDAR, range_sensor_uavcan_send_lidar);
+#endif
+}

sw/airborne/modules/sensors/range_sensor_uavcan.h

@@ -0,0 +1,32 @@
+/*
+ * Copyright (C) 2023 Freek van Tienen <freek.v.tienen@gmail.com>
+ *
+ * This file is part of Paparazzi.
+ *
+ * Paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * Paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Paparazzi; see the file COPYING.  If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/** @file modules/sensors/range_sensor_uavcan.h
+ * Range sensor sensor on the uavcan bus
+ */
+
+#ifndef RANGE_SENSOR_UAVCAN_H
+#define RANGE_SENSOR_UAVCAN_H
+
+/* External functions */
+extern void range_sensor_uavcan_init(void);
+
+#endif /* RANGE_SENSOR_UAVCAN_H */