From 27b1ce572e6bc2b5cb3a867dc25b17ebb5e958fa Mon Sep 17 00:00:00 2001
From: bugobliterator <siddharthbharatpurohit@gmail.com>
Date: Wed, 2 Sep 2020 19:45:38 +0530
Subject: [PATCH] AP_CANManager: fix comms between SLCAN and second CAN Iface

---
 libraries/AP_CANManager/AP_CANManager.cpp |  6 +++++-
 libraries/AP_CANManager/AP_SLCANIface.cpp | 16 +++++++++-------
 libraries/AP_CANManager/AP_SLCANIface.h   |  6 +++---
 3 files changed, 17 insertions(+), 11 deletions(-)

diff --git a/libraries/AP_CANManager/AP_CANManager.cpp b/libraries/AP_CANManager/AP_CANManager.cpp
index 4c1385c26e..9f99c5a414 100644
--- a/libraries/AP_CANManager/AP_CANManager.cpp
+++ b/libraries/AP_CANManager/AP_CANManager.cpp
@@ -245,7 +245,11 @@ void AP_CANManager::init()
         if (_drivers[drv_num] == nullptr) {
             continue;
         }
-        if (_slcan_interface.get_drv_num() != drv_num) {
+        if (_slcan_interface.get_iface_num() >= HAL_NUM_CAN_IFACES ||
+            _slcan_interface.get_iface_num() < 0) {
+            continue;
+        }
+        if (_interfaces[_slcan_interface.get_iface_num()]._driver_number != drv_num + 1) {
             _drivers[drv_num]->init(drv_num, true);
         } else {
             _drivers[drv_num]->init(drv_num, false);
diff --git a/libraries/AP_CANManager/AP_SLCANIface.cpp b/libraries/AP_CANManager/AP_SLCANIface.cpp
index 2605278369..08801fd7ce 100644
--- a/libraries/AP_CANManager/AP_SLCANIface.cpp
+++ b/libraries/AP_CANManager/AP_SLCANIface.cpp
@@ -253,7 +253,7 @@ bool SLCAN::CANIface::init_passthrough(uint8_t i)
     }
 
     _can_iface = hal.can[i];
-    _drv_num = _slcan_can_port - 1;
+    _iface_num = _slcan_can_port - 1;
     _prev_ser_port = -1;
     AP::can().log_text(AP_CANManager::LOG_INFO, LOG_TAG, "Setting SLCAN Passthrough for CAN%d\n", _slcan_can_port - 1);
     return true;
@@ -470,11 +470,13 @@ void SLCAN::CANIface::update_slcan_port()
         // to that until reboot
         return;
     }
-    _port = AP::serialmanager().find_serial(AP_SerialManager::SerialProtocol_SLCAN, 0);
-    if (_port != nullptr) {
-        _port->lock_port(_serial_lock_key, _serial_lock_key);
-        _set_by_sermgr = true;
-        return;
+    if (_port == nullptr) {
+         _port = AP::serialmanager().find_serial(AP_SerialManager::SerialProtocol_SLCAN, 0);
+        if (_port != nullptr) {
+            _port->lock_port(_serial_lock_key, _serial_lock_key);
+            _set_by_sermgr = true;
+            return;
+        }
     }
     if (_prev_ser_port != _slcan_ser_port) {
         if (!_slcan_start_req) {
@@ -491,7 +493,7 @@ void SLCAN::CANIface::update_slcan_port()
         }
         _port->lock_port(_serial_lock_key, _serial_lock_key);
         _prev_ser_port = _slcan_ser_port;
-        gcs().send_text(MAV_SEVERITY_INFO, "CANManager: Starting SLCAN Passthrough on Serial %d with CAN%d", _slcan_ser_port.get(), _drv_num);
+        gcs().send_text(MAV_SEVERITY_INFO, "CANManager: Starting SLCAN Passthrough on Serial %d with CAN%d", _slcan_ser_port.get(), _iface_num);
         _last_had_activity = AP_HAL::native_millis();
     }
     if (_port == nullptr) {
diff --git a/libraries/AP_CANManager/AP_SLCANIface.h b/libraries/AP_CANManager/AP_SLCANIface.h
index 4e63264631..4f12bb6569 100644
--- a/libraries/AP_CANManager/AP_SLCANIface.h
+++ b/libraries/AP_CANManager/AP_SLCANIface.h
@@ -77,7 +77,7 @@ class CANIface: public AP_HAL::CANIface
     bool _slcan_start_req;
     uint32_t _slcan_start_req_time;
     int8_t _prev_ser_port;
-    int8_t _drv_num = -1;
+    int8_t _iface_num = -1;
     uint32_t _last_had_activity;
     AP_HAL::CANIface* _can_iface; // Can interface to be used for interaction by SLCAN interface
     HAL_Semaphore port_sem;
@@ -103,9 +103,9 @@ public:
     int set_port(AP_HAL::UARTDriver* port);
 
     void reset_params();
-    int8_t get_drv_num() const
+    int8_t get_iface_num() const
     {
-        return _drv_num;
+        return _iface_num;
     }
 
     // Overriden methods