From 8718261f27db546c701ea467daf9b5daecd594de Mon Sep 17 00:00:00 2001
From: Andy Piper <github@andypiper.com>
Date: Wed, 22 Nov 2023 13:45:41 +0000
Subject: [PATCH] AP_Periph: add CAN_TERMINATE for software driven termination

---
 Tools/AP_Periph/Parameters.cpp | 22 ++++++++++++++++++++++
 Tools/AP_Periph/Parameters.h   |  6 ++++++
 Tools/AP_Periph/can.cpp        | 10 ++++++++++
 3 files changed, 38 insertions(+)

diff --git a/Tools/AP_Periph/Parameters.cpp b/Tools/AP_Periph/Parameters.cpp
index 02da8a657a..7e17cb8dae 100644
--- a/Tools/AP_Periph/Parameters.cpp
+++ b/Tools/AP_Periph/Parameters.cpp
@@ -107,6 +107,16 @@ const AP_Param::Info AP_Periph_FW::var_info[] = {
     GSCALAR(can_slcan_cport, "CAN_SLCAN_CPORT", 1),
 #endif
 
+#ifdef HAL_GPIO_PIN_GPIO_CAN1_TERM
+    // @Param: CAN_TERMINATE
+    // @DisplayName: Enable CAN software temination in this node
+    // @Description: Enable CAN software temination in this node
+    // @Values: 0:Disabled,1:Enabled
+    // @User: Advanced
+    // @RebootRequired: True
+    GARRAY(can_terminate,   0, "CAN_TERMINATE", 0),
+#endif
+
 #if HAL_NUM_CAN_IFACES >= 2
     // @Param: CAN_PROTOCOL
     // @DisplayName: Enable use of specific protocol to be used on this port
@@ -124,6 +134,12 @@ const AP_Param::Info AP_Periph_FW::var_info[] = {
     // @Param: CAN2_PROTOCOL
     // @CopyFieldsFrom: CAN_PROTOCOL
     GARRAY(can_protocol,     1, "CAN2_PROTOCOL", float(AP_CAN::Protocol::DroneCAN)),
+
+#ifdef HAL_GPIO_PIN_GPIO_CAN2_TERM
+    // @Param: CAN2_TERMINATE
+    // @CopyFieldsFrom: CAN_TERMINATE
+    GARRAY(can_terminate,    1, "CAN2_TERMINATE", 0),
+#endif
 #endif
 
 #if HAL_NUM_CAN_IFACES >= 3
@@ -135,6 +151,12 @@ const AP_Param::Info AP_Periph_FW::var_info[] = {
     // @Param: CAN3_PROTOCOL
     // @CopyFieldsFrom: CAN_PROTOCOL
     GARRAY(can_protocol,    2, "CAN3_PROTOCOL", float(AP_CAN::Protocol::DroneCAN)),
+
+#ifdef HAL_GPIO_PIN_GPIO_CAN3_TERM
+    // @Param: CAN3_TERMINATE
+    // @CopyFieldsFrom: CAN_TERMINATE
+    GARRAY(can_terminate,    2, "CAN3_TERMINATE", 0),
+#endif
 #endif
 
 #if HAL_CANFD_SUPPORTED
diff --git a/Tools/AP_Periph/Parameters.h b/Tools/AP_Periph/Parameters.h
index fb4a628b02..a2a439b31f 100644
--- a/Tools/AP_Periph/Parameters.h
+++ b/Tools/AP_Periph/Parameters.h
@@ -85,6 +85,9 @@ public:
         k_param_battery_hide_mask,
         k_param_can_mirror_ports,
         k_param_rtc,
+        k_param_can_terminate0,
+        k_param_can_terminate1,
+        k_param_can_terminate2,
     };
 
     AP_Int16 format_version;
@@ -205,6 +208,9 @@ public:
 #else
     static constexpr uint8_t can_fdmode = 0;
 #endif
+
+    AP_Int8 can_terminate[HAL_NUM_CAN_IFACES];
+
     AP_Int8 node_stats;
     Parameters() {}
 };
diff --git a/Tools/AP_Periph/can.cpp b/Tools/AP_Periph/can.cpp
index 01adff69a5..89611c5cac 100644
--- a/Tools/AP_Periph/can.cpp
+++ b/Tools/AP_Periph/can.cpp
@@ -1576,6 +1576,16 @@ void AP_Periph_FW::can_start()
     }
 #endif // HAL_PERIPH_ENFORCE_AT_LEAST_ONE_PORT_IS_UAVCAN_1MHz
 
+#ifdef HAL_GPIO_PIN_GPIO_CAN1_TERM
+    palWriteLine(HAL_GPIO_PIN_GPIO_CAN1_TERM, g.can_terminate[0]);
+#endif
+#ifdef HAL_GPIO_PIN_GPIO_CAN2_TERM
+    palWriteLine(HAL_GPIO_PIN_GPIO_CAN2_TERM, g.can_terminate[1]);
+#endif
+#ifdef HAL_GPIO_PIN_GPIO_CAN3_TERM
+    palWriteLine(HAL_GPIO_PIN_GPIO_CAN3_TERM, g.can_terminate[2]);
+#endif
+
     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();