AP_Periph: support rangefinder CAN nodes

Tools/AP_Periph/AP_Periph.cpp

@@ -82,6 +82,10 @@ void AP_Periph_FW::init()
 #ifdef HAL_PERIPH_ENABLE_AIRSPEED
     airspeed.init();
 #endif
+
+#ifdef HAL_PERIPH_ENABLE_RANGEFINDER
+    rangefinder.init(ROTATION_NONE);
+#endif
     
     start_ms = AP_HAL::millis();
 }

Tools/AP_Periph/AP_Periph.h

@@ -4,6 +4,7 @@
 #include <AP_Compass/AP_Compass.h>
 #include <AP_Baro/AP_Baro.h>
 #include <AP_Airspeed/AP_Airspeed.h>
+#include <AP_RangeFinder/AP_RangeFinder.h>
 
 #if defined(HAL_PERIPH_NEOPIXEL_COUNT) || defined(HAL_PERIPH_ENABLE_NCP5623_LED)
 #define AP_PERIPH_HAVE_LED
@@ -26,6 +27,7 @@ public:
     void can_gps_update();
     void can_baro_update();
     void can_airspeed_update();
+    void can_rangefinder_update();
 
     void load_parameters();
 
@@ -56,6 +58,10 @@ public:
 #ifdef HAL_PERIPH_ENABLE_AIRSPEED
     AP_Airspeed airspeed;
 #endif
+
+#ifdef HAL_PERIPH_ENABLE_RANGEFINDER
+    RangeFinder rangefinder;
+#endif
     
     // setup the var_info table
     AP_Param param_loader{var_info};

Tools/AP_Periph/Parameters.cpp

@@ -37,20 +37,20 @@ const AP_Param::Info AP_Periph_FW::var_info[] = {
 #ifdef HAL_PERIPH_ENABLE_GPS
     // GPS driver
     // @Group: GPS_
-    // @Path: ../libraries/AP_GPS/AP_GPS.cpp
+    // @Path: ../../libraries/AP_GPS/AP_GPS.cpp
     GOBJECT(gps, "GPS_", AP_GPS),
 #endif
 
 #ifdef HAL_PERIPH_ENABLE_MAG
     // @Group: COMPASS_
-    // @Path: ../libraries/AP_Compass/AP_Compass.cpp
+    // @Path: ../../libraries/AP_Compass/AP_Compass.cpp
     GOBJECT(compass,         "COMPASS_",     Compass),
 #endif
 
 #ifdef HAL_PERIPH_ENABLE_BARO
     // Baro driver
     // @Group: BARO_
-    // @Path: ../libraries/AP_Baro/AP_Baro.cpp
+    // @Path: ../../libraries/AP_Baro/AP_Baro.cpp
     GOBJECT(baro, "BARO_", AP_Baro),
 #endif
 
@@ -61,9 +61,16 @@ const AP_Param::Info AP_Periph_FW::var_info[] = {
 #ifdef HAL_PERIPH_ENABLE_AIRSPEED
     // Airspeed driver
     // @Group: ARSP
-    // @Path: ../libraries/AP_Airspeed/AP_Airspeed.cpp
+    // @Path: ../../libraries/AP_Airspeed/AP_Airspeed.cpp
     GOBJECT(airspeed, "ARSP", AP_Airspeed),
 #endif
+
+#ifdef HAL_PERIPH_ENABLE_RANGEFINDER
+    // Rangefinder driver
+    // @Group: RNGFND
+    // @Path: ../../libraries/AP_RangeFinder/Rangefinder.cpp
+    GOBJECT(rangefinder, "RNGFND", RangeFinder),
+#endif
     
     AP_VAREND
 };

Tools/AP_Periph/Parameters.h

@@ -20,6 +20,7 @@ public:
         k_param_buzz_volume,
         k_param_led_brightness,
         k_param_airspeed,
+        k_param_rangefinder,
     };
 
     AP_Int16 format_version;

Tools/AP_Periph/can.cpp

@@ -35,6 +35,7 @@
 #include <uavcan/equipment/air_data/RawAirData.h>
 #include <uavcan/equipment/indication/BeepCommand.h>
 #include <uavcan/equipment/indication/LightsCommand.h>
+#include <uavcan/equipment/range_sensor/Measurement.h>
 #include <ardupilot/indication/SafetyState.h>
 #include <ardupilot/indication/Button.h>
 #include <ardupilot/equipment/trafficmonitor/TrafficReport.h>
@@ -265,6 +266,9 @@ static void handle_param_executeopcode(CanardInstance* ins, CanardRxTransfer* tr
 #endif
 #ifdef HAL_PERIPH_ENABLE_AIRSPEED
         AP_Param::setup_object_defaults(&periph.airspeed, periph.airspeed.var_info);
+#endif
+#ifdef HAL_PERIPH_ENABLE_RANGEFINDER
+        AP_Param::setup_object_defaults(&periph.rangefinder, periph.rangefinder.var_info);
 #endif
     }
 
@@ -899,6 +903,7 @@ void AP_Periph_FW::can_update()
     can_gps_update();
     can_baro_update();
     can_airspeed_update();
+    can_rangefinder_update();
 #ifdef HAL_PERIPH_ENABLE_BUZZER
     can_buzzer_update();
 #endif
@@ -1189,6 +1194,82 @@ void AP_Periph_FW::can_airspeed_update(void)
 #endif // HAL_PERIPH_ENABLE_AIRSPEED
 }
 
+
+/*
+  update CAN rangefinder
+ */
+void AP_Periph_FW::can_rangefinder_update(void)
+{
+#ifdef HAL_PERIPH_ENABLE_RANGEFINDER
+    if (rangefinder.num_sensors() == 0) {
+        uint32_t now = AP_HAL::millis();
+        static uint32_t last_probe_ms;
+        if (now - last_probe_ms >= 1000) {
+            last_probe_ms = now;
+            rangefinder.init(ROTATION_NONE);
+        }
+    }
+    uint32_t now = AP_HAL::millis();
+    static uint32_t last_update_ms;
+    if (now - last_update_ms < 20) {
+        // max 50Hz data
+        return;
+    }
+    last_update_ms = now;
+    rangefinder.update();
+    RangeFinder::RangeFinder_Status status = rangefinder.status_orient(ROTATION_NONE);
+    if (status <= RangeFinder::RangeFinder_NoData) {
+        // don't send any data
+        return;
+    }
+    uint16_t dist_cm = rangefinder.distance_cm_orient(ROTATION_NONE);
+    uavcan_equipment_range_sensor_Measurement pkt {};
+    switch (status) {
+    case RangeFinder::RangeFinder_OutOfRangeLow:
+        pkt.reading_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_READING_TYPE_TOO_CLOSE;
+        break;
+    case RangeFinder::RangeFinder_OutOfRangeHigh:
+        pkt.reading_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_READING_TYPE_TOO_FAR;
+        break;
+    case RangeFinder::RangeFinder_Good:
+        pkt.reading_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_READING_TYPE_VALID_RANGE;
+        break;
+    default:
+        pkt.reading_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_READING_TYPE_UNDEFINED;
+        break;
+    }
+    switch (rangefinder.get_mav_distance_sensor_type_orient(ROTATION_NONE)) {
+    case MAV_DISTANCE_SENSOR_LASER:
+        pkt.sensor_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_SENSOR_TYPE_LIDAR;
+        break;
+    case MAV_DISTANCE_SENSOR_ULTRASOUND:
+        pkt.sensor_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_SENSOR_TYPE_SONAR;
+        break;
+    case MAV_DISTANCE_SENSOR_RADAR:
+        pkt.sensor_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_SENSOR_TYPE_RADAR;
+        break;
+    default:
+        pkt.sensor_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_SENSOR_TYPE_UNDEFINED;
+        break;
+    }
+
+    pkt.range = dist_cm * 0.01;
+    fix_float16(pkt.range);
+
+    uint8_t buffer[UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_MAX_SIZE];
+    uint16_t total_size = uavcan_equipment_range_sensor_Measurement_encode(&pkt, buffer);
+
+    canardBroadcast(&canard,
+                    UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_SIGNATURE,
+                    UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_ID,
+                    &transfer_id,
+                    CANARD_TRANSFER_PRIORITY_LOW,
+                    &buffer[0],
+                    total_size);
+#endif // HAL_PERIPH_ENABLE_RANGEFINDER
+}
+
+
 #ifdef HAL_PERIPH_ENABLE_ADSB
 /*
   map an ADSB_VEHICLE MAVLink message to a UAVCAN TrafficReport message

Tools/AP_Periph/wscript

@@ -33,6 +33,7 @@ def build(bld):
         'AP_InternalError',
         'GCS_MAVLink',
         'AP_Airspeed',
+        'AP_RangeFinder',
         ],
         exclude_src=[
             'libraries/AP_HAL_ChibiOS/Storage.cpp'