Networking: added CAN multicast UDP network gateway

Tools/AP_Bootloader/can.cpp

@@ -50,7 +50,7 @@ static CANConfig cancfg = {
 // pipelining is not faster when using ChibiOS CAN driver
 #define FW_UPDATE_PIPELINE_LEN 1
 #else
-static ChibiOS::CANIface can_iface[HAL_NUM_CAN_IFACES];
+ChibiOS::CANIface can_iface[HAL_NUM_CAN_IFACES];
 #endif
 
 #ifndef CAN_APP_VERSION_MAJOR
@@ -863,16 +863,13 @@ void can_update()
 }
 
 // printf to CAN LogMessage for debugging
-void can_printf(const char *fmt, ...)
+void can_vprintf(const char *fmt, va_list ap)
 {
     // only on H7 for now, where we have plenty of flash
 #if defined(STM32H7)
     uavcan_protocol_debug_LogMessage pkt {};
     uint8_t buffer[UAVCAN_PROTOCOL_DEBUG_LOGMESSAGE_MAX_SIZE];
-    va_list ap;
-    va_start(ap, fmt);
     uint32_t n = vsnprintf((char*)pkt.text.data, sizeof(pkt.text.data), fmt, ap);
-    va_end(ap);
     pkt.text.len = MIN(n, sizeof(pkt.text.data));
 
     uint32_t len = uavcan_protocol_debug_LogMessage_encode(&pkt, buffer, true);
@@ -887,5 +884,28 @@ void can_printf(const char *fmt, ...)
 #endif // defined(STM32H7)
 }
 
+// printf to CAN LogMessage for debugging
+void can_printf(const char *fmt, ...)
+{
+    // only on H7 for now, where we have plenty of flash
+#if defined(STM32H7)
+    va_list ap;
+    va_start(ap, fmt);
+    can_vprintf(fmt, ap);
+    va_end(ap);
+#endif // defined(STM32H7)
+}
+
+void can_printf_severity(uint8_t severity, const char *fmt, ...)
+{
+    // only on H7 for now, where we have plenty of flash
+#if defined(STM32H7)
+    va_list ap;
+    va_start(ap, fmt);
+    can_vprintf(fmt, ap);
+    va_end(ap);
+#endif
+}
+
 
 #endif // HAL_USE_CAN

Tools/AP_Bootloader/can.h

@@ -2,4 +2,8 @@ void can_start();
 void can_update();
 void can_set_node_id(uint8_t node_id);
 bool can_check_update(void);
+extern "C" {
+void can_vprintf(const char *fmt, va_list ap);
 void can_printf(const char *fmt, ...);
+void can_printf_severity(uint8_t severity, const char *fmt, ...);
+};

Tools/AP_Bootloader/network.cpp

@@ -8,6 +8,7 @@
 
 #include <AP_Networking/AP_Networking.h>
 #include <AP_Networking/AP_Networking_ChibiOS.h>
+#include <AP_Networking/AP_Networking_CAN.h>
 
 #include <lwip/ip_addr.h>
 #include <lwip/tcpip.h>
@@ -29,6 +30,7 @@
 #include <stdio.h>
 #include <stdarg.h>
 #include <AP_HAL_ChibiOS/hwdef/common/flash.h>
+#include <AP_HAL_ChibiOS/CANIface.h>
 
 #ifndef AP_NETWORKING_BOOTLOADER_DEFAULT_MAC_ADDR
 #define AP_NETWORKING_BOOTLOADER_DEFAULT_MAC_ADDR "C2:AF:51:03:CF:46"
@@ -59,6 +61,8 @@
 
 #define MIN(a,b) ((a)<(b)?(a):(b))
 
+static AP_Networking_CAN mcast_server;
+
 void BL_Network::link_up_cb(void *p)
 {
     auto *driver = (BL_Network *)p;
@@ -67,6 +71,9 @@ void BL_Network::link_up_cb(void *p)
 #endif
     char ipstr[IP4_STR_LEN];
     can_printf("IP %s", SocketAPM::inet_addr_to_str(ntohl(driver->thisif->ip_addr.addr), ipstr, sizeof(ipstr)));
+
+    // start mcast CAN server
+    mcast_server.start((1U<<HAL_NUM_CAN_IFACES)-1);
 }
 
 void BL_Network::link_down_cb(void *p)

Tools/AP_Bootloader/support.cpp

@@ -364,6 +364,17 @@ void thread_sleep_ms(uint32_t ms)
     }
 }
 
+void thread_sleep_us(uint32_t us)
+{
+    while (us > 0) {
+        // don't sleep more than 65 at a time, to cope with 16 bit
+        // timer
+        const uint32_t dt = us > 6500? 6500: us;
+        chThdSleepMicroseconds(dt);
+        us -= dt;
+    }
+}
+
 // generate a pulse sequence forever, for debugging
 void led_pulses(uint8_t npulses)
 {

Tools/AP_Bootloader/support.h

@@ -48,6 +48,7 @@ void led_off(unsigned led);
 void led_toggle(unsigned led);
 
 void thread_sleep_ms(uint32_t ms);
+void thread_sleep_us(uint32_t us);
 
 void custom_startup(void);
 

Tools/scripts/build_options.py

@@ -420,6 +420,7 @@ def config_option(self):
     Feature('Filesystem', 'APJ_TOOL_PARAMETERS', 'FORCE_APJ_DEFAULT_PARAMETERS', 'Enable apj_tool parameter area', 0, None),
 
     Feature('Networking', 'PPP', 'AP_NETWORKING_BACKEND_PPP', 'Enable PPP networking', 0, None),
+    Feature('Networking', 'CAN MCAST', 'AP_NETWORKING_CAN_MCAST_ENABLED', 'Enable CAN multicast bridge', 0, None),
 
     Feature('DroneCAN', 'DroneCAN', 'HAL_ENABLE_DRONECAN_DRIVERS', 'Enable DroneCAN support', 0, None),
 ]

Tools/scripts/extract_features.py

@@ -262,6 +262,7 @@ def __init__(self, filename, nm="arm-none-eabi-nm", strings="strings"):
             ('AP_DRONECAN_SERIAL_ENABLED', 'AP_DroneCAN_Serial::update'),
             ('AP_SERIALMANAGER_IMUOUT_ENABLED', 'AP_InertialSensor::send_uart_data'),
             ('AP_NETWORKING_BACKEND_PPP', 'AP_Networking_PPP::init'),
+            ('AP_NETWORKING_CAN_MCAST_ENABLED', 'AP_Networking_CAN::start'),
             ('FORCE_APJ_DEFAULT_PARAMETERS', 'AP_Param::param_defaults_data'),
             ('HAL_BUTTON_ENABLED', 'AP_Button::update'),
             ('HAL_LOGGING_ENABLED', 'AP_Logger::init'),

libraries/AP_CANManager/AP_CANManager.cpp

@@ -428,17 +428,18 @@ bool AP_CANManager::handle_can_forward(mavlink_channel_t chan, const mavlink_com
     const int8_t bus = int8_t(packet.param1)-1;
     if (bus == -1) {
         for (auto can_iface : hal.can) {
-            if (can_iface) {
-                can_iface->register_frame_callback(nullptr);
+            if (can_iface && can_forward.callback_id != 0) {
+                can_iface->unregister_frame_callback(can_forward.callback_id);
             }
         }
         return true;
     }
     if (bus >= HAL_NUM_CAN_IFACES || hal.can[bus] == nullptr) {
         return false;
     }
-    if (!hal.can[bus]->register_frame_callback(
-            FUNCTOR_BIND_MEMBER(&AP_CANManager::can_frame_callback, void, uint8_t, const AP_HAL::CANFrame &))) {
+    if (can_forward.callback_id == 0 &&
+        !hal.can[bus]->register_frame_callback(
+            FUNCTOR_BIND_MEMBER(&AP_CANManager::can_frame_callback, void, uint8_t, const AP_HAL::CANFrame &), can_forward.callback_id)) {
         return false;
     }
     can_forward.last_callback_enable_ms = AP_HAL::millis();
@@ -448,8 +449,8 @@ bool AP_CANManager::handle_can_forward(mavlink_channel_t chan, const mavlink_com
 
     // remove registration on other buses, allowing for bus change in the GUI tool
     for (uint8_t i=0; i<HAL_NUM_CAN_IFACES; i++) {
-        if (i != bus && hal.can[i] != nullptr) {
-            hal.can[i]->register_frame_callback(nullptr);
+        if (i != bus && hal.can[i] != nullptr && can_forward.callback_id != 0) {
+            hal.can[i]->unregister_frame_callback(can_forward.callback_id);
         }
     }
 
@@ -643,8 +644,9 @@ void AP_CANManager::can_frame_callback(uint8_t bus, const AP_HAL::CANFrame &fram
         // check every 100 frames for disabling CAN_FRAME send
         // we stop sending after 5s if the client stops
         // sending MAV_CMD_CAN_FORWARD requests
-        if (AP_HAL::millis() - can_forward.last_callback_enable_ms > 5000) {
-            hal.can[bus]->register_frame_callback(nullptr);
+        if (can_forward.callback_id != 0 &&
+            AP_HAL::millis() - can_forward.last_callback_enable_ms > 5000) {
+            hal.can[bus]->unregister_frame_callback(can_forward.callback_id);
             return;
         }
         can_forward.frame_counter = 0;

libraries/AP_CANManager/AP_CANManager.h

@@ -185,6 +185,7 @@ class AP_CANManager
         HAL_Semaphore sem;
         uint16_t num_filter_ids;
         uint16_t *filter_ids;
+        uint8_t callback_id;
     } can_forward;
 
     // buffer for MAVCAN frames

libraries/AP_HAL/CANIface.cpp

@@ -58,9 +58,16 @@ bool AP_HAL::CANFrame::priorityHigherThan(const CANFrame& rhs) const
  */
 int16_t AP_HAL::CANIface::receive(CANFrame& out_frame, uint64_t& out_ts_monotonic, CanIOFlags& out_flags)
 {
-    auto cb = frame_callback;
-    if (cb && (out_flags & IsMAVCAN)==0) {
-        cb(get_iface_num(), out_frame);
+    if ((out_flags & IsMAVCAN) != 0) {
+        return 1;
+    }
+#ifndef HAL_BOOTLOADER_BUILD
+    WITH_SEMAPHORE(callbacks.sem);
+#endif
+    for (auto &cb : callbacks.cb) {
+        if (cb != nullptr) {
+            cb(get_iface_num(), out_frame);
+        }
     }
     return 1;
 }
@@ -70,28 +77,59 @@ int16_t AP_HAL::CANIface::receive(CANFrame& out_frame, uint64_t& out_ts_monotoni
  */
 int16_t AP_HAL::CANIface::send(const CANFrame& frame, uint64_t tx_deadline, CanIOFlags flags)
 {
-    auto cb = frame_callback;
-    if (cb) {
+#ifndef HAL_BOOTLOADER_BUILD
+    WITH_SEMAPHORE(callbacks.sem);
+#endif
+    bool added_to_rx_queue = false;
+    for (auto &cb : callbacks.cb) {
+        if (cb == nullptr) {
+            continue;
+        }
         if ((flags & IsMAVCAN) == 0) {
             cb(get_iface_num(), frame);
-        } else {
+        } else if (!added_to_rx_queue) {
             CanRxItem rx_item;
             rx_item.frame = frame;
             rx_item.timestamp_us = AP_HAL::micros64();
             rx_item.flags = AP_HAL::CANIface::IsMAVCAN;
             add_to_rx_queue(rx_item);
+            added_to_rx_queue = true;
         }
     }
     return 1;
 }
 
 /*
-  register a callback for for sending CAN_FRAME messages
+  register a callback for for sending CAN_FRAME messages.
+  On success the returned callback_id can be used to unregister the callback
+ */
+bool AP_HAL::CANIface::register_frame_callback(FrameCb cb, uint8_t &callback_id)
+{
+#ifndef HAL_BOOTLOADER_BUILD
+    WITH_SEMAPHORE(callbacks.sem);
+#endif
+    for (uint8_t i=0; i<ARRAY_SIZE(callbacks.cb); i++) {
+        if (callbacks.cb[i] == nullptr) {
+            callbacks.cb[i] = cb;
+            callback_id = i+1;
+            return true;
+        }
+    }
+    return false;
+}
+
+/*
+  unregister a callback for for sending CAN_FRAME messages
  */
-bool AP_HAL::CANIface::register_frame_callback(FrameCb cb)
+void AP_HAL::CANIface::unregister_frame_callback(uint8_t callback_id)
 {
-    frame_callback = cb;
-    return true;
+#ifndef HAL_BOOTLOADER_BUILD
+    WITH_SEMAPHORE(callbacks.sem);
+#endif
+    const uint8_t idx = callback_id - 1;
+    if (idx < ARRAY_SIZE(callbacks.cb)) {
+        callbacks.cb[idx] = nullptr;
+    }
 }
 
 AP_HAL::CANFrame::CANFrame(uint32_t can_id, const uint8_t* can_data, uint8_t data_len, bool canfd_frame) :

libraries/AP_HAL/CANIface.h

@@ -262,13 +262,21 @@ class AP_HAL::CANIface
     FUNCTOR_TYPEDEF(FrameCb, void, uint8_t, const AP_HAL::CANFrame &);
 
     // register a frame callback function
-    virtual bool register_frame_callback(FrameCb cb);
+    virtual bool register_frame_callback(FrameCb cb, uint8_t &cb_id);
+    virtual void unregister_frame_callback(uint8_t cb_id);
 
 protected:
     virtual int8_t get_iface_num() const = 0;
     virtual bool add_to_rx_queue(const CanRxItem &rx_item) = 0;
 
-    FrameCb frame_callback;
+    struct {
+#ifndef HAL_BOOTLOADER_BUILD
+        HAL_Semaphore sem;
+#endif
+        // allow up to 2 callbacks per interface
+        FrameCb cb[2];
+    } callbacks;
+
     uint32_t bitrate_;
     OperatingMode mode_;
 };

libraries/AP_HAL/utility/Socket.cpp

@@ -100,7 +100,7 @@ void SOCKET_CLASS_NAME::make_sockaddr(const char *address, uint16_t port, struct
     sockaddr.sin_addr.s_addr = htonl(inet_str_to_addr(address));
 }
 
-#if !defined(HAL_BOOTLOADER_BUILD)
+#if !defined(HAL_BOOTLOADER_BUILD) || AP_NETWORKING_CAN_MCAST_ENABLED
 /*
   connect the socket
  */
@@ -187,7 +187,7 @@ bool SOCKET_CLASS_NAME::connect(const char *address, uint16_t port)
     fd_in = -1;
     return false;
 }
-#endif // HAL_BOOTLOADER_BUILD
+#endif // !defined(HAL_BOOTLOADER_BUILD) || AP_NETWORKING_CAN_MCAST_ENABLED
 
 /*
   connect the socket with a timeout

libraries/AP_HAL_ChibiOS/CANFDIface.cpp

@@ -379,6 +379,17 @@ int16_t CANIface::send(const AP_HAL::CANFrame& frame, uint64_t tx_deadline,
 
         if ((can_->TXFQS & FDCAN_TXFQS_TFQF) != 0) {
             stats.tx_overflow++;
+            if (stats.tx_success == 0) {
+                /*
+                  if we have never successfully transmitted a frame
+                  then we may be operating with just MAVCAN or UDP
+                  MCAST. Consider the frame sent if the send
+                  succeeds. This allows for UDP MCAST and MAVCAN to
+                  operate fully when the CAN bus has no cable plugged
+                  in
+                 */
+                return AP_HAL::CANIface::send(frame, tx_deadline, flags);
+            }
             return 0;    //we don't have free space
         }
         index = ((can_->TXFQS & FDCAN_TXFQS_TFQPI) >> FDCAN_TXFQS_TFQPI_Pos);

libraries/AP_HAL_ChibiOS/CanIface.cpp

@@ -325,6 +325,19 @@ int16_t CANIface::send(const AP_HAL::CANFrame& frame, uint64_t tx_deadline,
             txmailbox = 2;
         } else {
             PERF_STATS(stats.tx_overflow);
+#if !defined(HAL_BOOTLOADER_BUILD)
+            if (stats.tx_success == 0) {
+                /*
+                  if we have never successfully transmitted a frame
+                  then we may be operating with just MAVCAN or UDP
+                  MCAST. Consider the frame sent if the send
+                  succeeds. This allows for UDP MCAST and MAVCAN to
+                  operate fully when the CAN bus has no cable plugged
+                  in
+                 */
+                return AP_HAL::CANIface::send(frame, tx_deadline, flags);
+            }
+#endif
             return 0;       // No transmission for you.
         }
 

libraries/AP_HAL_ChibiOS/hwdef/BotBloxDroneNet/defaults.parm

@@ -1,5 +1,5 @@
 NET_ENABLE 1
-NET_OPTIONS 1
+NET_OPTIONS 3
 
 # enable hw flow control
 UART1_RTSCTS 1

libraries/AP_HAL_ChibiOS/hwdef/CubePilot-PPPGW/defaults.parm

@@ -1,5 +1,5 @@
 NET_ENABLE 1
-NET_OPTIONS 1
+NET_OPTIONS 3
 
 # enable hw flow control
 UART1_RTSCTS 1

libraries/AP_HAL_ChibiOS/hwdef/CubeRedPrimary-PPPGW/defaults.parm

@@ -1,5 +1,5 @@
 NET_ENABLE 1
-NET_OPTIONS 1
+NET_OPTIONS 3
 
 # enable hw flow control
 UART1_RTSCTS 1

libraries/AP_HAL_ChibiOS/hwdef/Pixhawk6X-PPPGW/defaults.parm

@@ -1,5 +1,5 @@
 NET_ENABLED 1
-NET_OPTIONS 1
+NET_OPTIONS 3
 
 # enable hw flow control
 UART1_RTSCTS 1

libraries/AP_HAL_ChibiOS/hwdef/include/network_PPPGW.inc

@@ -5,6 +5,7 @@ define HAL_PERIPH_ENABLE_RTC
 
 define HAL_PERIPH_ENABLE_SERIAL_OPTIONS
 define AP_NETWORKING_BACKEND_PPP 1
+define AP_NETWORKING_CAN_MCAST_ENABLED 1
 
 define AP_PERIPH_NET_PPP_PORT_DEFAULT 1
 define AP_PERIPH_NET_PPP_BAUD_DEFAULT 12500000