diff --git a/libraries/AP_Networking/AP_Networking.cpp b/libraries/AP_Networking/AP_Networking.cpp index 12b0fb1ae3f55b..08fe7b993a448a 100644 --- a/libraries/AP_Networking/AP_Networking.cpp +++ b/libraries/AP_Networking/AP_Networking.cpp @@ -90,7 +90,7 @@ const AP_Param::GroupInfo AP_Networking::var_info[] = { // @Param: OPTIONS // @DisplayName: Networking options // @Description: Networking options - // @Bitmask: 0:EnablePPP Ethernet gateway, 1:Enable CAN1 multicast gateway, 2:Enable CAN2 multicast gateway + // @Bitmask: 0:EnablePPP Ethernet gateway, 1:Enable CAN1 multicast endpoint, 2:Enable CAN2 multicast endpoint, 3:Enable CAN1 multicast bridged, 4:Enable CAN2 multicast bridged // @RebootRequired: True // @User: Advanced AP_GROUPINFO("OPTIONS", 9, AP_Networking, param.options, 0), @@ -200,13 +200,13 @@ void AP_Networking::init() #endif #if AP_NETWORKING_CAN_MCAST_ENABLED && !defined(HAL_BOOTLOADER_BUILD) - if (option_is_set(OPTION::CAN1_MCAST_GATEWAY) || option_is_set(OPTION::CAN2_MCAST_GATEWAY)) { + if (option_is_set(OPTION::CAN1_MCAST_ENDPOINT) || option_is_set(OPTION::CAN1_MCAST_ENDPOINT)) { // get mask of enabled buses uint8_t bus_mask = 0; - if (option_is_set(OPTION::CAN1_MCAST_GATEWAY)) { + if (option_is_set(OPTION::CAN1_MCAST_ENDPOINT)) { bus_mask |= (1U<<0); } - if (option_is_set(OPTION::CAN2_MCAST_GATEWAY)) { + if (option_is_set(OPTION::CAN1_MCAST_ENDPOINT)) { bus_mask |= (1U<<1); } mcast_server.start(bus_mask); diff --git a/libraries/AP_Networking/AP_Networking.h b/libraries/AP_Networking/AP_Networking.h index 5c7e8ebca18d62..2a35257f16d393 100644 --- a/libraries/AP_Networking/AP_Networking.h +++ b/libraries/AP_Networking/AP_Networking.h @@ -157,14 +157,21 @@ class AP_Networking enum class OPTION { PPP_ETHERNET_GATEWAY=(1U<<0), #if AP_NETWORKING_CAN_MCAST_ENABLED - CAN1_MCAST_GATEWAY=(1U<<1), - CAN2_MCAST_GATEWAY=(1U<<2), + CAN1_MCAST_ENDPOINT=(1U<<1), + CAN2_MCAST_ENDPOINT=(1U<<2), + CAN1_MCAST_BRIDGED=(1U<<3), + CAN2_MCAST_BRIDGED=(1U<<4), #endif }; bool option_is_set(OPTION option) const { return (param.options.get() & int32_t(option)) != 0; } + bool is_can_mcast_ep_bridged(uint8_t bus) const { + return (option_is_set(OPTION::CAN1_MCAST_BRIDGED) && bus == 0) || + (option_is_set(OPTION::CAN2_MCAST_BRIDGED) && bus == 1); + } + private: static AP_Networking *singleton; diff --git a/libraries/AP_Networking/AP_Networking_CAN.cpp b/libraries/AP_Networking/AP_Networking_CAN.cpp index 250e78b0228cb6..5b278686bc0916 100644 --- a/libraries/AP_Networking/AP_Networking_CAN.cpp +++ b/libraries/AP_Networking/AP_Networking_CAN.cpp @@ -108,7 +108,6 @@ void AP_Networking_CAN::mcast_server(void) } char address[] = MCAST_ADDRESS_BASE; const uint32_t buffer_size = 20; // good for fw upload - uint8_t callback_id = 0; address[strlen(address)-1] = '0' + bus; if (!mcast_sockets[bus]->connect(address, MCAST_PORT)) { @@ -122,6 +121,14 @@ void AP_Networking_CAN::mcast_server(void) GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "CAN_MCAST[%u]: failed to register", unsigned(bus)); goto de_allocate; } +#ifndef HAL_BOOTLOADER_BUILD + // check if bridged mode is enabled + cbus->set_rx_cb_disabled(callback_id, !AP::network().is_can_mcast_ep_bridged(bus)); +#else + // never bridge in bootloader, as we can cause loops if multiple + // bootloaders with mcast are running on the same network + cbus->set_rx_cb_disabled(callback_id, true); +#endif #if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS // tell the ethernet interface that we want to receive all @@ -151,7 +158,8 @@ void AP_Networking_CAN::mcast_server(void) thread_sleep_us(delay_us); #endif for (uint8_t bus=0; busset_rx_cb_disabled(callback_id, !bridged); +#else + // never bridge in bootloader, as we can cause loops if multiple + // bootloaders with mcast are running on the same network and CAN Bus + bool bridged = false; +#endif while (frame_buffers[bus]->peek(frame)) { - auto *cbus = get_caniface(bus); - if (cbus == nullptr) { - break; + bool retcode; + if (!bridged) { + AP_HAL::CANIface::CanRxItem rx_item; + rx_item.timestamp_us = AP_HAL::micros64(); + rx_item.flags = AP_HAL::CANIface::IsMAVCAN; + rx_item.frame = frame; + retcode = cbus->add_to_rx_queue(rx_item); + } else { + retcode = cbus->send(frame, + AP_HAL::micros64() + timeout_us, + AP_HAL::CANIface::IsMAVCAN); } - auto retcode = cbus->send(frame, - AP_HAL::micros64() + timeout_us, - AP_HAL::CANIface::IsMAVCAN); if (retcode == 0) { break; } diff --git a/libraries/AP_Networking/AP_Networking_CAN.h b/libraries/AP_Networking/AP_Networking_CAN.h index 3e968e4fdb4422..bf8bd2145309dc 100644 --- a/libraries/AP_Networking/AP_Networking_CAN.h +++ b/libraries/AP_Networking/AP_Networking_CAN.h @@ -15,7 +15,7 @@ class AP_Networking_CAN { uint8_t bus_mask; AP_HAL::CANIface *get_caniface(uint8_t) const; - + uint8_t callback_id; #ifdef HAL_BOOTLOADER_BUILD static void mcast_trampoline(void *ctx); #endif