diff --git a/Tools/AP_Periph/AP_Periph.h b/Tools/AP_Periph/AP_Periph.h index 5a268cd3dc..9db9b8cceb 100644 --- a/Tools/AP_Periph/AP_Periph.h +++ b/Tools/AP_Periph/AP_Periph.h @@ -72,6 +72,10 @@ #define AP_PERIPH_SAFETY_SWITCH_ENABLED defined(HAL_PERIPH_ENABLE_RC_OUT) #endif +#ifndef HAL_PERIPH_CAN_MIRROR +#define HAL_PERIPH_CAN_MIRROR 0 +#endif + #include "Parameters.h" #if CONFIG_HAL_BOARD == HAL_BOARD_SITL @@ -104,7 +108,6 @@ struct CanardRxTransfer; #endif #endif - class AP_Periph_FW { public: AP_Periph_FW(); @@ -439,6 +442,9 @@ public: void process1HzTasks(uint64_t timestamp_usec); void processTx(void); void processRx(void); +#if HAL_PERIPH_CAN_MIRROR + void processMirror(void); +#endif // HAL_PERIPH_CAN_MIRROR void cleanup_stale_transactions(uint64_t ×tamp_usec); void update_rx_protocol_stats(int16_t res); void node_status_send(void); diff --git a/Tools/AP_Periph/Parameters.cpp b/Tools/AP_Periph/Parameters.cpp index 0ab3e29046..97e5c6ce18 100644 --- a/Tools/AP_Periph/Parameters.cpp +++ b/Tools/AP_Periph/Parameters.cpp @@ -599,6 +599,15 @@ const AP_Param::Info AP_Periph_FW::var_info[] = { #endif #endif // AP_SIM_ENABLED +#if HAL_PERIPH_CAN_MIRROR + // @Param: CAN_MIRROR_PORTS + // @DisplayName: CAN ports to mirror traffic between + // @Description: Any set ports will participate in blindly mirroring traffic from one port to the other. It is the users responsibility to ensure that no loops exist that cause traffic to be infinitly repeated, and both ports must be running the same baud rates. + // @Bitmask: 0:CAN1, 1:CAN2 + // @User: Advanced + GSCALAR(can_mirror_ports, "CAN_MIRROR_PORTS", 0), +#endif // HAL_PERIPH_CAN_MIRROR + AP_VAREND }; diff --git a/Tools/AP_Periph/Parameters.h b/Tools/AP_Periph/Parameters.h index d341f76633..030444030a 100644 --- a/Tools/AP_Periph/Parameters.h +++ b/Tools/AP_Periph/Parameters.h @@ -83,6 +83,7 @@ public: k_param_ahrs, k_param_battery_balance, k_param_battery_hide_mask, + k_param_can_mirror_ports, }; AP_Int16 format_version; @@ -192,6 +193,10 @@ public: AP_Int32 efi_baudrate; AP_Int8 efi_port; #endif + +#if HAL_PERIPH_CAN_MIRROR + AP_Int8 can_mirror_ports; +#endif // HAL_PERIPH_CAN_MIRROR #if HAL_CANFD_SUPPORTED AP_Int8 can_fdmode; diff --git a/Tools/AP_Periph/can.cpp b/Tools/AP_Periph/can.cpp index 01f53ee8a9..deadcd03f3 100644 --- a/Tools/AP_Periph/can.cpp +++ b/Tools/AP_Periph/can.cpp @@ -64,6 +64,12 @@ extern AP_Periph_FW periph; #define DEBUG_PKTS 0 +#if HAL_PERIPH_CAN_MIRROR + #ifndef HAL_PERIPH_CAN_MIRROR_QUEUE_SIZE + #define HAL_PERIPH_CAN_MIRROR_QUEUE_SIZE 64 + #endif +#endif //HAL_PERIPH_CAN_MIRROR + #ifndef HAL_PERIPH_SUPPORT_LONG_CAN_PRINTF // When enabled, can_printf() strings longer than the droneCAN max text length (90 chars) // are split into multiple packets instead of truncating the string. This is @@ -81,6 +87,14 @@ static struct instance_t { #elif CONFIG_HAL_BOARD == HAL_BOARD_SITL HALSITL::CANIface* iface; #endif + +#if HAL_PERIPH_CAN_MIRROR + #if HAL_NUM_CAN_IFACES < 2 + #error "Can't use HAL_PERIPH_CAN_MIRROR if there are not at least 2 HAL_NUM_CAN_IFACES" + #endif + ObjectBuffer *mirror_queue; + uint8_t mirror_fail_count; +#endif // HAL_PERIPH_CAN_MIRROR } instances[HAL_NUM_CAN_IFACES]; @@ -1223,6 +1237,17 @@ void AP_Periph_FW::processRx(void) if (ins.iface->receive(rxmsg, timestamp, flags) <= 0) { break; } +#if HAL_PERIPH_CAN_MIRROR + for (auto &other_instance : instances) { + if (other_instance.mirror_queue == nullptr) { // we aren't mirroring here, or failed on memory + continue; + } + if (other_instance.index == ins.index) { // don't self add + continue; + } + other_instance.mirror_queue->push(rxmsg); + } +#endif // HAL_PERIPH_CAN_MIRROR rx_frame.data_len = AP_HAL::CANFrame::dlcToDataLength(rxmsg.dlc); memcpy(rx_frame.data, rxmsg.data, rx_frame.data_len); #if HAL_CANFD_SUPPORTED @@ -1257,6 +1282,40 @@ void AP_Periph_FW::processRx(void) } } +#if HAL_PERIPH_CAN_MIRROR +void AP_Periph_FW::processMirror(void) +{ + const uint64_t deadline = AP_HAL::micros64() + 1000000; + + for (auto &ins : instances) { + if (ins.iface == nullptr || ins.mirror_queue == nullptr) { // can't send on a null interface + continue; + } + + const uint32_t pending = ins.mirror_queue->available(); + for (uint32_t i = 0; i < pending; i++) { // limit how long we can loop + AP_HAL::CANFrame txmsg {}; + + if (!ins.mirror_queue->peek(txmsg)) { + break; + } + + if (ins.iface->send(txmsg, deadline, 0) <= 0) { + if (ins.mirror_fail_count < 8) { + ins.mirror_fail_count++; + } else { + ins.mirror_queue->pop(); + } + break; + } else { + ins.mirror_fail_count = 0; + ins.mirror_queue->pop(); + } + } + } +} +#endif // HAL_PERIPH_CAN_MIRROR + uint16_t AP_Periph_FW::pool_peak_percent() { const CanardPoolAllocatorStatistics stats = canardGetPoolAllocatorStatistics(&dronecan.canard); @@ -1524,6 +1583,11 @@ void AP_Periph_FW::can_start() #endif instances[i].iface = can_iface_periph[i]; instances[i].index = i; +#if HAL_PERIPH_CAN_MIRROR + if ((g.can_mirror_ports & (1U << i)) != 0) { + instances[i].mirror_queue = new ObjectBuffer (HAL_PERIPH_CAN_MIRROR_QUEUE_SIZE); + } +#endif //HAL_PERIPH_CAN_MIRROR #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]); @@ -1749,6 +1813,9 @@ void AP_Periph_FW::can_update() processTx(); processRx(); +#if HAL_PERIPH_CAN_MIRROR + processMirror(); +#endif // HAL_PERIPH_CAN_MIRROR } }