AP_CANManager: support CAN_Dn_PROTOCOL2 for an aux 11 bit protocol

libraries/AP_CANManager/AP_CANDriver.cpp

@@ -51,6 +51,14 @@ const AP_Param::GroupInfo AP_CANManager::CANDriver_Params::var_info[] = {
     AP_SUBGROUPPTR(_piccolocan, "PC_", 5, AP_CANManager::CANDriver_Params, AP_PiccoloCAN),
 #endif
 
+    // @Param: PROTOCOL2
+    // @DisplayName: Secondary protocol with 11 bit CAN addressing
+    // @Description: Secondary protocol with 11 bit CAN addressing
+    // @Values: 0:Disabled,7:USD1,10:Scripting,11:Benewake,12:Scripting2,13:TOFSenseP,14:NanoRadar_NRA24
+    // @User: Advanced
+    // @RebootRequired: True
+    AP_GROUPINFO("PROTOCOL2", 6, AP_CANManager::CANDriver_Params, _driver_type_11bit, float(AP_CAN::Protocol::None)),
+
     AP_GROUPEND
 };
 #endif

libraries/AP_CANManager/AP_CANDriver.h

@@ -21,6 +21,8 @@
 #include <AP_HAL/AP_HAL.h>
 
 class AP_CANManager;
+class CANSensor;
+
 class AP_CANDriver
 {
 public:
@@ -34,4 +36,9 @@ class AP_CANDriver
     // link protocol drivers with interfaces by adding reference to CANIface
     virtual bool add_interface(AP_HAL::CANIface* can_iface) = 0;
 
+    // add an 11 bit auxillary driver
+    virtual bool add_11bit_driver(CANSensor *sensor) { return false; }
+
+    // handler for outgoing frames for auxillary drivers
+    virtual bool write_aux_frame(AP_HAL::CANFrame &out_frame, const uint64_t timeout_us) { return false; }
 };

libraries/AP_CANManager/AP_CANManager.cpp

@@ -277,6 +277,7 @@ void AP_CANManager::init()
     }
 }
 #endif
+
 /*
   register a new CAN driver
  */
@@ -328,6 +329,32 @@ bool AP_CANManager::register_driver(AP_CAN::Protocol dtype, AP_CANDriver *driver
     return false;
 }
 
+// register a new auxillary sensor driver for 11 bit address frames
+bool AP_CANManager::register_11bit_driver(AP_CAN::Protocol dtype, CANSensor *sensor, uint8_t &driver_index)
+{
+    WITH_SEMAPHORE(_sem);
+
+    for (uint8_t i = 0; i < HAL_NUM_CAN_IFACES; i++) {
+        uint8_t drv_num = _interfaces[i]._driver_number;
+        if (drv_num == 0 || drv_num > HAL_MAX_CAN_PROTOCOL_DRIVERS) {
+            continue;
+        }
+        // from 1 based to 0 based
+        drv_num--;
+
+        if (dtype != (AP_CAN::Protocol)_drv_param[drv_num]._driver_type_11bit.get()) {
+            continue;
+        }
+        if (_drivers[drv_num] != nullptr &&
+            _drivers[drv_num]->add_11bit_driver(sensor)) {
+            driver_index = drv_num;
+            return true;
+        }
+    }
+    return false;
+
+}
+
 // Method used by CAN related library methods to report status and debug info
 // The result of this method can be accessed via ftp get @SYS/can_log.txt
 void AP_CANManager::log_text(AP_CANManager::LogLevel loglevel, const char *tag, const char *fmt, ...)

libraries/AP_CANManager/AP_CANManager.h

@@ -34,6 +34,8 @@
 
 #include "AP_CAN.h"
 
+class CANSensor;
+
 class AP_CANManager
 {
 public:
@@ -63,6 +65,9 @@ class AP_CANManager
     // register a new driver
     bool register_driver(AP_CAN::Protocol dtype, AP_CANDriver *driver);
 
+    // register a new auxillary sensor driver for 11 bit address frames
+    bool register_11bit_driver(AP_CAN::Protocol dtype, CANSensor *sensor, uint8_t &driver_index);
+
     // returns number of active CAN Drivers
     uint8_t get_num_drivers(void) const
     {
@@ -141,6 +146,7 @@ class AP_CANManager
 
     private:
         AP_Int8 _driver_type;
+        AP_Int8 _driver_type_11bit;
         AP_CANDriver* _uavcan;
         AP_CANDriver* _piccolocan;
     };

libraries/AP_CANManager/AP_CANSensor.cpp

@@ -40,7 +40,13 @@ void CANSensor::register_driver(AP_CAN::Protocol dtype)
 {
 #if HAL_CANMANAGER_ENABLED
     if (!AP::can().register_driver(dtype, this)) {
-        debug_can(AP_CANManager::LOG_ERROR, "Failed to register CANSensor %s", _driver_name);
+        if (AP::can().register_11bit_driver(dtype, this, _driver_index)) {
+            is_aux_11bit_driver = true;
+            _can_driver = AP::can().get_driver(_driver_index);
+            _initialized = true;
+        } else {
+            debug_can(AP_CANManager::LOG_ERROR, "Failed to register CANSensor %s", _driver_name);
+        }
     } else {
         debug_can(AP_CANManager::LOG_INFO, "%s: constructed", _driver_name);
     }
@@ -135,6 +141,10 @@ bool CANSensor::write_frame(AP_HAL::CANFrame &out_frame, const uint64_t timeout_
         return false;
     }
 
+    if (is_aux_11bit_driver && _can_driver != nullptr) {
+        return _can_driver->write_aux_frame(out_frame, timeout_us);
+    }
+
     bool read_select = false;
     bool write_select = true;
     bool ret = _can_iface->select(read_select, write_select, &out_frame, AP_HAL::micros64() + timeout_us);

libraries/AP_CANManager/AP_CANSensor.h

@@ -72,6 +72,10 @@ class CANSensor : public AP_CANDriver {
     const uint16_t _stack_size;
     bool _initialized;
     uint8_t _driver_index;
+
+    // this is true when we are setup as an auxillary driver using CAN_Dn_PROTOCOL2
+    bool is_aux_11bit_driver;
+
     AP_CANDriver *_can_driver;
     HAL_EventHandle _event_handle;
     AP_HAL::CANIface* _can_iface;