From 606eb05c959c4a6ba26d62d2235779f80b19a61b Mon Sep 17 00:00:00 2001
From: Alexey Bulatov <alexey.bulatov@emlid.com>
Date: Thu, 1 Jun 2017 18:19:51 +0300
Subject: [PATCH] AP_BoardConfig: Linux CAN initialisation

---
 libraries/AP_BoardConfig/AP_BoardConfig.cpp   |  7 ++++
 .../AP_BoardConfig/AP_BoardConfig_CAN.cpp     | 37 ++++++++++++++++++-
 libraries/AP_BoardConfig/AP_BoardConfig_CAN.h |  5 ++-
 3 files changed, 47 insertions(+), 2 deletions(-)

diff --git a/libraries/AP_BoardConfig/AP_BoardConfig.cpp b/libraries/AP_BoardConfig/AP_BoardConfig.cpp
index 57be00eb76..00adf0306f 100644
--- a/libraries/AP_BoardConfig/AP_BoardConfig.cpp
+++ b/libraries/AP_BoardConfig/AP_BoardConfig.cpp
@@ -22,6 +22,13 @@
 #include "AP_BoardConfig.h"
 #include <stdio.h>
 
+#if HAL_WITH_UAVCAN
+#include <AP_UAVCAN/AP_UAVCAN.h>
+#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX
+#include <AP_HAL_Linux/CAN.h>
+#endif
+#endif
+
 #if CONFIG_HAL_BOARD == HAL_BOARD_PX4
 # define BOARD_SAFETY_ENABLE_DEFAULT 1
 #if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
diff --git a/libraries/AP_BoardConfig/AP_BoardConfig_CAN.cpp b/libraries/AP_BoardConfig/AP_BoardConfig_CAN.cpp
index 2437e00079..9ae089df71 100644
--- a/libraries/AP_BoardConfig/AP_BoardConfig_CAN.cpp
+++ b/libraries/AP_BoardConfig/AP_BoardConfig_CAN.cpp
@@ -29,6 +29,8 @@
 #include <unistd.h>
 
 #include <AP_HAL_PX4/CAN.h>
+#elif CONFIG_HAL_BOARD == HAL_BOARD_LINUX
+#include <AP_HAL_Linux/CAN.h>
 #endif
 
 #include <AP_UAVCAN/AP_UAVCAN.h>
@@ -90,8 +92,9 @@ void AP_BoardConfig_CAN::init()
 
 #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
     px4_setup_canbus();
+#elif CONFIG_HAL_BOARD == HAL_BOARD_LINUX
+    linux_setup_canbus();
 #endif
-
 }
 
 #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
@@ -157,6 +160,38 @@ void AP_BoardConfig_CAN::px4_setup_canbus(void)
         }
     }
 }
+#elif CONFIG_HAL_BOARD == HAL_BOARD_LINUX
+void AP_BoardConfig_CAN::linux_setup_canbus(void) {
+    for (uint8_t i = 0; i < MAX_NUMBER_OF_CAN_INTERFACES; i++) {
+    
+        uint8_t drv_num = _var_info_can[i]._driver_number;
+
+        if (drv_num != 0 && drv_num <= MAX_NUMBER_OF_CAN_DRIVERS) {
+            if (hal.can_mgr[drv_num - 1] == nullptr) {
+                const_cast <AP_HAL::HAL&> (hal).can_mgr[drv_num - 1] = new Linux::LinuxCANDriver;
+            }
+            
+            if (hal.can_mgr[drv_num - 1] != nullptr) {
+                _var_info_can_protocol[i]._uavcan = new AP_UAVCAN;
+                if (_var_info_can_protocol[i]._uavcan != nullptr) {
+                    AP_Param::load_object_from_eeprom(_var_info_can_protocol[i]._uavcan, AP_UAVCAN::var_info);
+                
+                    hal.can_mgr[i]->set_UAVCAN(_var_info_can_protocol[i]._uavcan);
+                    _var_info_can_protocol[i]._uavcan->set_parent_can_mgr(hal.can_mgr[i]); 
+                
+                    if (!hal.can_mgr[i]->begin(_var_info_can[i]._can_bitrate, i)) {
+                        hal.console->printf("Failed to initialize can_mgr\n");
+                    } else {
+                        hal.console->printf("can_mgr initialized well\n");
+                    }
+                } else {
+                    hal.console->printf("AP_UAVCAN failed to allocate\n");
+                }
+            }
+        }
+    
+    }
+}
 #endif
 #endif
 
diff --git a/libraries/AP_BoardConfig/AP_BoardConfig_CAN.h b/libraries/AP_BoardConfig/AP_BoardConfig_CAN.h
index 669ac271bf..c909f2208c 100644
--- a/libraries/AP_BoardConfig/AP_BoardConfig_CAN.h
+++ b/libraries/AP_BoardConfig/AP_BoardConfig_CAN.h
@@ -98,6 +98,9 @@ public:
     static int8_t _st_can_debug[MAX_NUMBER_OF_CAN_INTERFACES];
 #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
     void px4_setup_canbus(void);
-#endif // HAL_BOARD_PX4 || HAL_BOARD_VRBRAIN
+#elif CONFIG_HAL_BOARD == HAL_BOARD_LINUX
+    void linux_setup_canbus(void);
+#endif // HAL_BOARD_LINUX
+
 };
 #endif