AP_Periph: add support for multiple protocols on AP_Periph

Tools/AP_Periph/AP_Periph.cpp

@@ -68,7 +68,12 @@ AP_Periph_FW::AP_Periph_FW()
 #if HAL_LOGGING_ENABLED
     : logger(g.log_bitmask)
 #endif
-{}
+{
+    if (_singleton != nullptr) {
+        AP_HAL::panic("AP_Periph_FW must be singleton");
+    }
+    _singleton = this;
+}
 
 #if HAL_LOGGING_ENABLED
 const struct LogStructure AP_Periph_FW::log_structure[] = {
@@ -450,4 +455,11 @@ void AP_Periph_FW::prepare_reboot()
         hal.scheduler->delay(40);
 }
 
+AP_Periph_FW *AP_Periph_FW::_singleton;
+
+AP_Periph_FW& AP::periph()
+{
+    return *AP_Periph_FW::get_singleton();
+}
+
 AP_HAL_MAIN();

Tools/AP_Periph/AP_Periph.h

@@ -15,6 +15,13 @@
 #include <AP_MSP/msp.h>
 #include "../AP_Bootloader/app_comms.h"
 #include "hwing_esc.h"
+#include <AP_CANManager/AP_CANManager.h>
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
+#include <AP_HAL_ChibiOS/CANIface.h>
+#elif CONFIG_HAL_BOARD == HAL_BOARD_SITL
+#include <AP_HAL_SITL/CANSocketIface.h>
+#endif
 
 #if defined(HAL_PERIPH_NEOPIXEL_COUNT_WITHOUT_NOTIFY) || defined(HAL_PERIPH_ENABLE_NCP5623_LED_WITHOUT_NOTIFY) || defined(HAL_PERIPH_ENABLE_NCP5623_BGR_LED_WITHOUT_NOTIFY) || defined(HAL_PERIPH_ENABLE_TOSHIBA_LED_WITHOUT_NOTIFY)
 #define AP_PERIPH_HAVE_LED_WITHOUT_NOTIFY
@@ -55,6 +62,16 @@ class AP_Periph_FW {
 public:
     AP_Periph_FW();
 
+    CLASS_NO_COPY(AP_Periph_FW);
+
+    static AP_Periph_FW* get_singleton()
+    {
+        if (_singleton == nullptr) {
+            AP_HAL::panic("AP_Periph_FW used before allocation.");
+        }
+        return _singleton;
+    }
+
     void init();
     void update();
 
@@ -76,6 +93,12 @@ public:
     void check_for_serial_reboot_cmd(const int8_t serial_index);
 #endif
 
+#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
+    static ChibiOS::CANIface* can_iface_periph[HAL_NUM_CAN_IFACES];
+#elif CONFIG_HAL_BOARD == HAL_BOARD_SITL
+    static HALSITL::CANIface* can_iface_periph[HAL_NUM_CAN_IFACES];
+#endif
+
     AP_SerialManager serial_manager;
 
 #ifdef HAL_PERIPH_ENABLE_GPS
@@ -100,6 +123,12 @@ public:
     } battery;
 #endif
 
+#if HAL_NUM_CAN_IFACES >= 2
+    // This allows you to change the protocol and it continues to use the one at boot.
+    // Without this, changing away from UAVCAN causes loss of comms and you can't
+    // change the rest of your params or veryofy it suceeded.
+    AP_CANManager::Driver_Type can_protocol_cached[HAL_NUM_CAN_IFACES];
+#endif
 
 #ifdef HAL_PERIPH_ENABLE_MSP
     struct {
@@ -191,10 +220,17 @@ public:
     uint32_t last_baro_update_ms;
     uint32_t last_airspeed_update_ms;
 
+    static AP_Periph_FW *_singleton;
+
     // show stack as DEBUG msgs
     void show_stack_free();
 };
 
+namespace AP
+{
+    AP_Periph_FW& periph();
+}
+
 extern AP_Periph_FW periph;
 
 extern "C" {

Tools/AP_Periph/Parameters.cpp

@@ -35,6 +35,7 @@ extern const AP_HAL::HAL &hal;
  */
 
 #define GSCALAR(v, name, def) { periph.g.v.vtype, name, Parameters::k_param_ ## v, &periph.g.v, {def_value : def} }
+#define GARRAY(v, index, name, def) { periph.g.v[index].vtype, name, Parameters::k_param_ ## v ## index, &periph.g.v[index], {def_value : def} }
 #define ASCALAR(v, name, def) { periph.aparm.v.vtype, name, Parameters::k_param_ ## v, (const void *)&periph.aparm.v, {def_value : def} }
 #define GGROUP(v, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## v, &periph.g.v, {group_info : class::var_info} }
 #define GOBJECT(v, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## v, (const void *)&periph.v, {group_info : class::var_info} }
@@ -61,7 +62,51 @@ const AP_Param::Info AP_Periph_FW::var_info[] = {
     // @Range: 10000 1000000
     // @User: Advanced
     // @RebootRequired: True
-    GSCALAR(can_baudrate,     "CAN_BAUDRATE", 1000000),
+    GARRAY(can_baudrate,     0, "CAN_BAUDRATE", 1000000),
+
+#if HAL_NUM_CAN_IFACES >= 2
+    // @Param: CAN_PROTOCOL
+    // @DisplayName: Enable use of specific protocol to be used on this port
+    // @Description: Enabling this option starts selected protocol that will use this virtual driver. At least one CAN port must be UAVCAN or else CAN1 gets set to UAVCAN
+    // @Values: 0:Disabled,1:UAVCAN,3:ToshibaCAN,4:PiccoloCAN,5:CANTester,6:EFI_NWPMU,7:USD1,8:KDECAN,9:PacketDigital
+    // @User: Advanced
+    // @RebootRequired: True
+    GARRAY(can_protocol,     0, "CAN_PROTOCOL", AP_CANManager::Driver_Type_UAVCAN),
+    
+    // @Param: CAN2_BAUDRATE
+    // @DisplayName: Bitrate of CAN2 interface
+    // @Description: Bit rate can be set up to from 10000 to 1000000
+    // @Range: 10000 1000000
+    // @User: Advanced
+    // @RebootRequired: True
+    GARRAY(can_baudrate,     1, "CAN2_BAUDRATE", 1000000),
+
+    // @Param: CAN2_PROTOCOL
+    // @DisplayName: Enable use of specific protocol to be used on this port
+    // @Description: Enabling this option starts selected protocol that will use this virtual driver. At least one CAN port must be UAVCAN or else CAN1 gets set to UAVCAN
+    // @Values: 0:Disabled,1:UAVCAN,3:ToshibaCAN,4:PiccoloCAN,5:CANTester,6:EFI_NWPMU,7:USD1,8:KDECAN,9:PacketDigital
+    // @User: Advanced
+    // @RebootRequired: True
+    GARRAY(can_protocol,     1, "CAN2_PROTOCOL", AP_CANManager::Driver_Type_UAVCAN),
+#endif
+
+#if HAL_NUM_CAN_IFACES >= 3
+    // @Param: CAN3_BAUDRATE
+    // @DisplayName: Bitrate of CAN3 interface
+    // @Description: Bit rate can be set up to from 10000 to 1000000
+    // @Range: 10000 1000000
+    // @User: Advanced
+    // @RebootRequired: True
+    GARRAY(can_baudrate,    2, "CAN3_BAUDRATE", 1000000),
+
+    // @Param: CAN3_PROTOCOL
+    // @DisplayName: Enable use of specific protocol to be used on this port
+    // @Description: Enabling this option starts selected protocol that will use this virtual driver. At least one CAN port must be UAVCAN or else CAN1 gets set to UAVCAN
+    // @Values: 0:Disabled,1:UAVCAN,3:ToshibaCAN,4:PiccoloCAN,5:CANTester,6:EFI_NWPMU,7:USD1,8:KDECAN,9:PacketDigital
+    // @User: Advanced
+    // @RebootRequired: True
+    GARRAY(can_protocol,    2, "CAN3_PROTOCOL", AP_CANManager::Driver_Type_UAVCAN),
+#endif
 
 #if !defined(HAL_NO_FLASH_SUPPORT) && !defined(HAL_NO_ROMFS_SUPPORT)
     // @Param: FLASH_BOOTLOADER

Tools/AP_Periph/Parameters.h

@@ -15,7 +15,7 @@ public:
         k_param_gps,
         k_param_compass,
         k_param_can_node,
-        k_param_can_baudrate,
+        k_param_can_baudrate0,
         k_param_baro,
         k_param_buzz_volume,
         k_param_led_brightness,
@@ -40,11 +40,21 @@ public:
         k_param_esc_pwm_type,
         k_param_logger,
         k_param_log_bitmask,
+        k_param_can_baudrate1,
+        k_param_can_baudrate2,
+        k_param_can_protocol0,
+        k_param_can_protocol1,
+        k_param_can_protocol2,
     };
 
     AP_Int16 format_version;
     AP_Int16 can_node;
-    AP_Int32 can_baudrate;
+    
+    AP_Int32 can_baudrate[HAL_NUM_CAN_IFACES];
+#if HAL_NUM_CAN_IFACES >= 2
+    AP_Enum<AP_CANManager::Driver_Type> can_protocol[HAL_NUM_CAN_IFACES];
+#endif
+
 #ifdef HAL_PERIPH_ENABLE_BUZZER_WITHOUT_NOTIFY
     AP_Int8 buzz_volume;
 #endif

Tools/AP_Periph/can.cpp

@@ -68,6 +68,10 @@
 #include "i2c.h"
 #include <utility>
 
+#if HAL_NUM_CAN_IFACES >= 2
+#include <AP_CANManager/AP_CANSensor.h>
+#endif
+
 extern const AP_HAL::HAL &hal;
 extern AP_Periph_FW periph;
 
@@ -95,11 +99,16 @@ static uint8_t transfer_id;
 #define CAN_PROBE_CONTINUOUS 0
 #endif
 
+#ifndef AP_PERIPH_ENFORCE_AT_LEAST_ONE_PORT_IS_UAVCAN_1MHz
+#define AP_PERIPH_ENFORCE_AT_LEAST_ONE_PORT_IS_UAVCAN_1MHz 1
+#endif
+
 #if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
-static ChibiOS::CANIface can_iface[HAL_NUM_CAN_IFACES];
+ChibiOS::CANIface* AP_Periph_FW::can_iface_periph[HAL_NUM_CAN_IFACES];
 #elif CONFIG_HAL_BOARD == HAL_BOARD_SITL
-static HALSITL::CANIface can_iface[HAL_NUM_CAN_IFACES];
+HALSITL::CANIface* AP_Periph_FW::can_iface_periph[HAL_NUM_CAN_IFACES];
 #endif
+
 /*
  * Variables used for dynamic node ID allocation.
  * RTFM at http://uavcan.org/Specification/6._Application_level_functions/#dynamic-node-id-allocation
@@ -980,7 +989,12 @@ static void processTx(void)
         bool sent_ok = false;
         const uint64_t deadline = AP_HAL::native_micros64() + 1000000;
         for (uint8_t i=0; i<HAL_NUM_CAN_IFACES; i++) {
-            sent_ok |= (can_iface[i].send(txmsg, deadline, 0) > 0);
+#if HAL_NUM_CAN_IFACES >= 2
+            if (periph.can_protocol_cached[i] != AP_CANManager::Driver_Type_UAVCAN) {
+                continue;
+            }
+#endif
+            sent_ok |= (AP::periph().can_iface_periph[i]->send(txmsg, deadline, 0) > 0);
         }
         if (sent_ok) {
             canardPopTxQueue(&canard);
@@ -1004,9 +1018,14 @@ static void processRx(void)
     while (true) {
         bool got_pkt = false;
         for (uint8_t i=0; i<HAL_NUM_CAN_IFACES; i++) {
+#if HAL_NUM_CAN_IFACES >= 2
+            if (periph.can_protocol_cached[i] != AP_CANManager::Driver_Type_UAVCAN) {
+                continue;
+            }
+#endif
             bool read_select = true;
             bool write_select = false;
-            can_iface[i].select(read_select, write_select, nullptr, 0);
+            AP::periph().can_iface_periph[i]->select(read_select, write_select, nullptr, 0);
             if (!read_select) {
                 continue;
             }
@@ -1015,7 +1034,7 @@ static void processRx(void)
             //palToggleLine(HAL_GPIO_PIN_LED);
             uint64_t timestamp;
             AP_HAL::CANIface::CanIOFlags flags;
-            can_iface[i].receive(rxmsg, timestamp, flags);
+            AP::periph().can_iface_periph[i]->receive(rxmsg, timestamp, flags);
             memcpy(rx_frame.data, rxmsg.data, 8);
             rx_frame.data_len = rxmsg.dlc;
             rx_frame.id = rxmsg.id;
@@ -1253,8 +1272,32 @@ void AP_Periph_FW::can_start()
     periph.g.flash_bootloader.set_and_save_ifchanged(0);
 #endif
 
-    for (int8_t i=0; i<HAL_NUM_CAN_IFACES; i++) {
-        can_iface[i].init(1000000, AP_HAL::CANIface::NormalMode);
+#if AP_PERIPH_ENFORCE_AT_LEAST_ONE_PORT_IS_UAVCAN_1MHz && HAL_NUM_CAN_IFACES >= 2
+    bool has_uavcan_at_1MHz = false;
+    for (uint8_t i=0; i<HAL_NUM_CAN_IFACES; i++) {
+        if (g.can_protocol[i] == AP_CANManager::Driver_Type_UAVCAN && g.can_baudrate[i] == 1000000) {
+            has_uavcan_at_1MHz = true;
+        }
+    }
+    if (!has_uavcan_at_1MHz) {
+        g.can_protocol[0].set_and_save(AP_CANManager::Driver_Type_UAVCAN);
+        g.can_baudrate[0].set_and_save(1000000);
+    }
+#endif // HAL_PERIPH_ENFORCE_AT_LEAST_ONE_PORT_IS_UAVCAN_1MHz
+
+    for (uint8_t i=0; i<HAL_NUM_CAN_IFACES; i++) {
+#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
+        can_iface_periph[i] = new ChibiOS::CANIface();
+#elif CONFIG_HAL_BOARD == HAL_BOARD_SITL
+        can_iface_periph[i] = new HALSITL::CANIface();
+#endif
+#if HAL_NUM_CAN_IFACES >= 2
+        can_protocol_cached[i] = g.can_protocol[i];
+        CANSensor::set_periph(i, can_protocol_cached[i], can_iface_periph[i]);
+#endif
+        if (can_iface_periph[i] != nullptr) {
+            can_iface_periph[i]->init(g.can_baudrate[i], AP_HAL::CANIface::NormalMode);
+        }
     }
 
     canardInit(&canard, (uint8_t *)canard_memory_pool, sizeof(canard_memory_pool),

Tools/AP_Periph/wscript

@@ -22,6 +22,7 @@ def build(bld):
         'AP_Math',
         'AP_BoardConfig',
         'AP_BattMonitor',
+        'AP_CANManager',
         'AP_Param',
         'StorageManager',
         'AP_FlashStorage',