AP_Periph: Add support for up to two rangefinders

Tools/AP_Periph/AP_Periph.cpp

@@ -227,15 +227,20 @@ void AP_Periph_FW::init()
 #endif
 
 #ifdef HAL_PERIPH_ENABLE_RANGEFINDER
-    if (rangefinder.get_type(0) != RangeFinder::Type::NONE) {
+    bool have_rangefinder = false;
-        if (g.rangefinder_port >= 0) {
+    for (uint8_t i=0; i<RANGEFINDER_MAX_INSTANCES; i++) {
+        if ((rangefinder.get_type(i) != RangeFinder::Type::NONE) && (g.rangefinder_port[i] >= 0)) {
             // init uart for serial rangefinders
-            auto *uart = hal.serial(g.rangefinder_port);
+            auto *uart = hal.serial(g.rangefinder_port[i]);
             if (uart != nullptr) {
-                uart->begin(g.rangefinder_baud);
+                uart->begin(g.rangefinder_baud[i]);
-                serial_manager.set_protocol_and_baud(g.rangefinder_port, AP_SerialManager::SerialProtocol_Rangefinder, g.rangefinder_baud);
+                serial_manager.set_protocol_and_baud(g.rangefinder_port[i], AP_SerialManager::SerialProtocol_Rangefinder, g.rangefinder_baud[i]);
+                have_rangefinder = true;
             }
         }
+    }
+    if (have_rangefinder) {
+        // Can only call rangefinder init once, subsequent inits are blocked
         rangefinder.init(ROTATION_NONE);
     }
 #endif
@@ -419,7 +424,14 @@ void AP_Periph_FW::update()
         hal.serial(0)->printf("BARO H=%u P=%.2f T=%.2f\n", baro.healthy(), baro.get_pressure(), baro.get_temperature());
 #endif
 #ifdef HAL_PERIPH_ENABLE_RANGEFINDER
-        hal.serial(0)->printf("RNG %u %ucm\n", rangefinder.num_sensors(), rangefinder.distance_cm_orient(ROTATION_NONE));
+        hal.serial(0)->printf("Num RNG sens %u\n", rangefinder.num_sensors());
+        for (uint8_t i=0; i<RANGEFINDER_MAX_INSTANCES; i++) {
+            AP_RangeFinder_Backend *backend = rangefinder.get_backend(i);
+            if (backend == nullptr) {
+                continue;
+            }
+            hal.serial(0)->printf("RNG %u %ucm\n", i, uint16_t(backend->distance()*100));
+        }
 #endif
         hal.scheduler->delay(1);
 #endif

Tools/AP_Periph/AP_Periph.h

@@ -12,6 +12,7 @@
 #include <AP_BattMonitor/AP_BattMonitor.h>
 #include <AP_Airspeed/AP_Airspeed.h>
 #include <AP_RangeFinder/AP_RangeFinder.h>
+#include <AP_RangeFinder/AP_RangeFinder_Backend.h>
 #include <AP_Proximity/AP_Proximity.h>
 #include <AP_EFI/AP_EFI.h>
 #include <AP_KDECAN/AP_KDECAN.h>
@@ -168,7 +169,9 @@ public:
     void send_relposheading_msg();
     void can_baro_update();
     void can_airspeed_update();
+#ifdef HAL_PERIPH_ENABLE_RANGEFINDER
     void can_rangefinder_update();
+#endif
     void can_battery_update();
     void can_battery_send_cells(uint8_t instance);
     void can_proximity_update();
@@ -279,7 +282,8 @@ public:
 
 #ifdef HAL_PERIPH_ENABLE_RANGEFINDER
     RangeFinder rangefinder;
-    uint32_t last_sample_ms;
+    uint32_t last_rangefinder_update_ms;
+    uint32_t last_rangefinder_sample_ms[RANGEFINDER_MAX_INSTANCES];
 #endif
 
 #ifdef HAL_PERIPH_ENABLE_PROXIMITY
@@ -452,6 +456,15 @@ public:
         return (uint8_t(g.debug.get()) & (1U<<uint8_t(option))) != 0;
     }
 
+    enum class PeriphOptions {
+        PROBE_CONTINUOUS = 1U<<0,
+    };
+
+    // check if a periph option is set
+    bool option_is_set(const PeriphOptions opt) const {
+        return (uint32_t(g.options.get()) & uint32_t(opt)) != 0;
+    }
+
     // show stack as DEBUG msgs
     void show_stack_free();
 

Tools/AP_Periph/Parameters.cpp

@@ -69,6 +69,10 @@ extern const AP_HAL::HAL &hal;
   #define APD_ESC_SERIAL_1 -1
 #endif
 
+#ifndef AP_PERIPH_PROBE_CONTINUOUS
+#define AP_PERIPH_PROBE_CONTINUOUS 0
+#endif
+
 /*
  *  AP_Periph parameter definitions
  *
@@ -305,7 +309,7 @@ const AP_Param::Info AP_Periph_FW::var_info[] = {
     // @Increment: 1
     // @User: Standard
     // @RebootRequired: True
-    GSCALAR(rangefinder_baud, "RNGFND_BAUDRATE", HAL_PERIPH_RANGEFINDER_BAUDRATE_DEFAULT),
+    GARRAY(rangefinder_baud, 0, "RNGFND_BAUDRATE", HAL_PERIPH_RANGEFINDER_BAUDRATE_DEFAULT),
 
     // @Param: RNGFND_PORT
     // @DisplayName: Rangefinder Serial Port
@@ -314,7 +318,27 @@ const AP_Param::Info AP_Periph_FW::var_info[] = {
     // @Increment: 1
     // @User: Advanced
     // @RebootRequired: True
-    GSCALAR(rangefinder_port, "RNGFND_PORT", AP_PERIPH_RANGEFINDER_PORT_DEFAULT),
+    GARRAY(rangefinder_port, 0, "RNGFND_PORT", AP_PERIPH_RANGEFINDER_PORT_DEFAULT),
+
+#if RANGEFINDER_MAX_INSTANCES > 1
+    // @Param: RNGFND2_BAUDRATE
+    // @DisplayName: Rangefinder serial baudrate
+    // @Description: Rangefinder serial baudrate.
+    // @Values: 1:1200,2:2400,4:4800,9:9600,19:19200,38:38400,57:57600,111:111100,115:115200,230:230400,256:256000,460:460800,500:500000,921:921600,1500:1500000
+    // @Increment: 1
+    // @User: Standard
+    // @RebootRequired: True
+    GARRAY(rangefinder_baud, 1, "RNGFND2_BAUDRATE", HAL_PERIPH_RANGEFINDER_BAUDRATE_DEFAULT),
+
+    // @Param: RNGFND2_PORT
+    // @DisplayName: Rangefinder Serial Port
+    // @Description: This is the serial port number where SERIALx_PROTOCOL will be set to Rangefinder.
+    // @Range: 0 10
+    // @Increment: 1
+    // @User: Advanced
+    // @RebootRequired: True
+    GARRAY(rangefinder_port, 1, "RNGFND2_PORT", -1),
+#endif
 
     // @Param: RNGFND_MAX_RATE
     // @DisplayName: Rangefinder max rate
@@ -661,6 +685,13 @@ const AP_Param::Info AP_Periph_FW::var_info[] = {
     GSCALAR(temperature_msg_rate, "TEMP_MSG_RATE", 0),
 #endif
 
+    // @Param: OPTIONS
+    // @DisplayName: AP Periph Options
+    // @Description: Bitmask of AP Periph Options
+    // @Bitmask: 0: Enable continuous sensor probe
+    // @User: Standard
+    GSCALAR(options, "OPTIONS", AP_PERIPH_PROBE_CONTINUOUS),
+
     AP_VAREND
 };
 

Tools/AP_Periph/Parameters.h

@@ -25,7 +25,7 @@ public:
         k_param_airspeed,
         k_param_rangefinder,
         k_param_flash_bootloader,
-        k_param_rangefinder_baud,
+        k_param_rangefinder_baud0,
         k_param_adsb_baudrate,
         k_param_hardpoint_id,
         k_param_hardpoint_rate,
@@ -36,7 +36,7 @@ public:
         k_param_serial_number,
         k_param_adsb_port,
         k_param_servo_channels,
-        k_param_rangefinder_port,
+        k_param_rangefinder_port0,
         k_param_gps_port,
         k_param_msp_port,
         k_param_notify,
@@ -91,6 +91,9 @@ public:
         k_param_serial_options,
         k_param_relay,
         k_param_temperature_msg_rate,
+        k_param_rangefinder_baud1,
+        k_param_rangefinder_port1,
+        k_param_options,
     };
 
     AP_Int16 format_version;
@@ -119,8 +122,8 @@ public:
 #endif
 
 #ifdef HAL_PERIPH_ENABLE_RANGEFINDER
-    AP_Int32 rangefinder_baud;
+    AP_Int32 rangefinder_baud[RANGEFINDER_MAX_INSTANCES];
-    AP_Int8 rangefinder_port;
+    AP_Int8 rangefinder_port[RANGEFINDER_MAX_INSTANCES];
     AP_Int16 rangefinder_max_rate;
 #endif
 
@@ -216,6 +219,8 @@ public:
     static constexpr uint8_t can_fdmode = 0;
 #endif
 
+    AP_Int32 options;
+
     AP_Int8 can_terminate[HAL_NUM_CAN_IFACES];
 
     AP_Int8 node_stats;

Tools/AP_Periph/rangefinder.cpp

@@ -12,6 +12,8 @@
 #define AP_PERIPH_PROBE_CONTINUOUS 0
 #endif
 
+extern const AP_HAL::HAL &hal;
+
 /*
   update CAN rangefinder
  */
@@ -21,7 +23,8 @@ void AP_Periph_FW::can_rangefinder_update(void)
         return;
     }
 #if AP_PERIPH_PROBE_CONTINUOUS
-    if (rangefinder.num_sensors() == 0) {
+    // We only allow continuous probing for rangefinders while vehicle is disarmed. Probing is currently inefficient and leads to longer loop times.
+    if ((rangefinder.num_sensors() == 0) && !hal.util->get_soft_armed() && option_is_set(PeriphOptions::PROBE_CONTINUOUS)) {
         uint32_t now = AP_HAL::millis();
         static uint32_t last_probe_ms;
         if (now - last_probe_ms >= 1000) {
@@ -31,28 +34,44 @@ void AP_Periph_FW::can_rangefinder_update(void)
     }
 #endif
     uint32_t now = AP_HAL::millis();
-    static uint32_t last_update_ms;
     if (g.rangefinder_max_rate > 0 &&
-        now - last_update_ms < uint32_t(1000/g.rangefinder_max_rate)) {
+        now - last_rangefinder_update_ms < uint32_t(1000/g.rangefinder_max_rate)) {
         // limit to max rate
         return;
     }
-    last_update_ms = now;
-    rangefinder.update();
-    RangeFinder::Status status = rangefinder.status_orient(ROTATION_NONE);
-    if (status <= RangeFinder::Status::NoData) {
-        // don't send any data
-        return;
-    }
-    const uint32_t sample_ms = rangefinder.last_reading_ms(ROTATION_NONE);
-    if (last_sample_ms == sample_ms) {
-        return;
-    }
-    last_sample_ms = sample_ms;
 
-    uint16_t dist_cm = rangefinder.distance_cm_orient(ROTATION_NONE);
+    // update all rangefinder instances
+    rangefinder.update();
+
+    // cycle through each rangefinder instance to find one to send
+    // equipment.range_sensor only uses 3 CAN frames so we just send all available sensor measurements.
+    for (uint8_t i = 0; i <= rangefinder.num_sensors(); i++) {
+
+        if (rangefinder.get_type(i) == RangeFinder::Type::NONE) {
+            continue;
+        }
+
+        AP_RangeFinder_Backend *backend = rangefinder.get_backend(i);
+        if (backend == nullptr) {
+            continue;
+        }
+
+        RangeFinder::Status status = backend->status();
+        if (status <= RangeFinder::Status::NoData) {
+            // don't send any data for this instance
+            continue;
+        }
+
+        const uint32_t sample_ms = backend->last_reading_ms();
+        if (last_rangefinder_sample_ms[i] == sample_ms) {
+            // don't same the same reading again
+            continue;
+        }
+        last_rangefinder_sample_ms[i] = sample_ms;
+
         uavcan_equipment_range_sensor_Measurement pkt {};
-    pkt.sensor_id = rangefinder.get_address(0);
+        pkt.sensor_id = rangefinder.get_address(i);
+
         switch (status) {
         case RangeFinder::Status::OutOfRangeLow:
             pkt.reading_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_READING_TYPE_TOO_CLOSE;
@@ -67,7 +86,8 @@ void AP_Periph_FW::can_rangefinder_update(void)
             pkt.reading_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_READING_TYPE_UNDEFINED;
             break;
         }
-    switch (rangefinder.get_mav_distance_sensor_type_orient(ROTATION_NONE)) {
+
+        switch (backend->get_mav_distance_sensor_type()) {
         case MAV_DISTANCE_SENSOR_LASER:
             pkt.sensor_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_SENSOR_TYPE_LIDAR;
             break;
@@ -82,7 +102,8 @@ void AP_Periph_FW::can_rangefinder_update(void)
             break;
         }
 
-    pkt.range = dist_cm * 0.01;
+        float dist_m = backend->distance();
+        pkt.range = dist_m;
 
         uint8_t buffer[UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_MAX_SIZE] {};
         uint16_t total_size = uavcan_equipment_range_sensor_Measurement_encode(&pkt, buffer, !periph.canfdout());
@@ -92,6 +113,9 @@ void AP_Periph_FW::can_rangefinder_update(void)
                         CANARD_TRANSFER_PRIORITY_LOW,
                         &buffer[0],
                         total_size);
+
+        last_rangefinder_update_ms = now;
+    }
 }
 
 #endif // HAL_PERIPH_ENABLE_RANGEFINDER

Tools/AP_Periph/serial_tunnel.cpp

@@ -50,7 +50,7 @@ int8_t AP_Periph_FW::get_default_tunnel_serial_port(void) const
 #endif
 #ifdef HAL_PERIPH_ENABLE_RANGEFINDER
     if (uart_num == -1) {
-        uart_num = g.rangefinder_port;
+        uart_num = g.rangefinder_port[0];
     }
 #endif
 #ifdef HAL_PERIPH_ENABLE_ADSB