From 53e0c4175e8fb2614f1b9a17555ca22e0964dac0 Mon Sep 17 00:00:00 2001
From: Tom Pittenger <tom.pittenger@krausaerospace.com>
Date: Thu, 6 Oct 2022 20:19:55 -0700
Subject: [PATCH] AP_Periph: add support for SLCAN

---
 Tools/AP_Periph/AP_Periph.h    |  4 ++++
 Tools/AP_Periph/Parameters.cpp | 10 ++++++++++
 Tools/AP_Periph/Parameters.h   |  5 +++++
 Tools/AP_Periph/can.cpp        | 19 ++++++++++++++++++-
 4 files changed, 37 insertions(+), 1 deletion(-)

diff --git a/Tools/AP_Periph/AP_Periph.h b/Tools/AP_Periph/AP_Periph.h
index 428180ab80..ccc3d0f171 100644
--- a/Tools/AP_Periph/AP_Periph.h
+++ b/Tools/AP_Periph/AP_Periph.h
@@ -108,6 +108,10 @@ public:
     static HALSITL::CANIface* can_iface_periph[HAL_NUM_CAN_IFACES];
 #endif
 
+#ifdef HAL_PERIPH_ENABLE_SLCAN
+    static SLCAN::CANIface slcan_interface;
+#endif
+
     AP_SerialManager serial_manager;
 
 #if AP_STATS_ENABLED
diff --git a/Tools/AP_Periph/Parameters.cpp b/Tools/AP_Periph/Parameters.cpp
index f075a755a6..61e585f6ea 100644
--- a/Tools/AP_Periph/Parameters.cpp
+++ b/Tools/AP_Periph/Parameters.cpp
@@ -92,6 +92,16 @@ const AP_Param::Info AP_Periph_FW::var_info[] = {
     // @RebootRequired: True
     GARRAY(can_baudrate,     0, "CAN_BAUDRATE", 1000000),
 
+#ifdef HAL_PERIPH_ENABLE_SLCAN
+    // @Param: CAN_SLCAN_CPORT
+    // @DisplayName: SLCAN Route
+    // @Description: CAN Interface ID to be routed to SLCAN, 0 means no routing
+    // @Values: 0:Disabled,1:First interface,2:Second interface
+    // @User: Standard
+    // @RebootRequired: True
+    GSCALAR(can_slcan_cport, "CAN_SLCAN_CPORT", 1),
+#endif
+
 #if HAL_NUM_CAN_IFACES >= 2
     // @Param: CAN_PROTOCOL
     // @DisplayName: Enable use of specific protocol to be used on this port
diff --git a/Tools/AP_Periph/Parameters.h b/Tools/AP_Periph/Parameters.h
index 60e55cb8c2..792c314dc2 100644
--- a/Tools/AP_Periph/Parameters.h
+++ b/Tools/AP_Periph/Parameters.h
@@ -60,6 +60,7 @@ public:
         k_param_efi_port,
         k_param_efi_baudrate,
         k_param_esc_telem_rate,
+        k_param_can_slcan_cport,
     };
 
     AP_Int16 format_version;
@@ -70,6 +71,10 @@ public:
     AP_Enum<AP_CANManager::Driver_Type> can_protocol[HAL_NUM_CAN_IFACES];
 #endif
 
+#ifdef HAL_PERIPH_ENABLE_SLCAN
+    AP_Int8 can_slcan_cport;
+#endif
+
 #ifdef HAL_PERIPH_ENABLE_BUZZER_WITHOUT_NOTIFY
     AP_Int8 buzz_volume;
 #endif
diff --git a/Tools/AP_Periph/can.cpp b/Tools/AP_Periph/can.cpp
index 857e24c098..73661fde35 100644
--- a/Tools/AP_Periph/can.cpp
+++ b/Tools/AP_Periph/can.cpp
@@ -85,7 +85,7 @@ static struct instance_t {
     uint8_t index;
 
 #if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
-    ChibiOS::CANIface* iface;
+    AP_HAL::CANIface* iface;
 #elif CONFIG_HAL_BOARD == HAL_BOARD_SITL
     HALSITL::CANIface* iface;
 #endif
@@ -159,6 +159,9 @@ ChibiOS::CANIface* AP_Periph_FW::can_iface_periph[HAL_NUM_CAN_IFACES];
 HALSITL::CANIface* AP_Periph_FW::can_iface_periph[HAL_NUM_CAN_IFACES];
 #endif
 
+#ifdef HAL_PERIPH_ENABLE_SLCAN
+SLCAN::CANIface AP_Periph_FW::slcan_interface;
+#endif
 
 /*
  * Node status variables
@@ -1516,6 +1519,20 @@ void AP_Periph_FW::can_start()
 #endif
         }
     }
+
+#ifdef HAL_PERIPH_ENABLE_SLCAN
+    const uint8_t slcan_selected_index = g.can_slcan_cport - 1;
+    if (slcan_selected_index < HAL_NUM_CAN_IFACES) {
+        slcan_interface.set_can_iface(can_iface_periph[slcan_selected_index]);
+        instances[slcan_selected_index].iface = (AP_HAL::CANIface*)&slcan_interface;
+
+        // ensure there's a serial port mapped to SLCAN
+        if (!periph.serial_manager.have_serial(AP_SerialManager::SerialProtocol_SLCAN, 0)) {
+            periph.serial_manager.set_protocol_and_baud(SERIALMANAGER_NUM_PORTS-1, AP_SerialManager::SerialProtocol_SLCAN, 1500000);
+        }
+    }
+#endif
+
     canardInit(&dronecan.canard, (uint8_t *)dronecan.canard_memory_pool, sizeof(dronecan.canard_memory_pool),
             onTransferReceived, shouldAcceptTransfer, NULL);