From 112f53536530a7dfe399eee12e298e4c81b70736 Mon Sep 17 00:00:00 2001
From: Siddharth Purohit <siddharthbharatpurohit@gmail.com>
Date: Fri, 16 Nov 2018 18:14:06 +0800
Subject: [PATCH] AP_UAVCAN: implement routing SLCAN to and from CANbus

---
 libraries/AP_UAVCAN/AP_UAVCAN_SLCAN.cpp | 11 +++++++----
 libraries/AP_UAVCAN/AP_UAVCAN_SLCAN.h   | 20 +++++++++-----------
 2 files changed, 16 insertions(+), 15 deletions(-)

diff --git a/libraries/AP_UAVCAN/AP_UAVCAN_SLCAN.cpp b/libraries/AP_UAVCAN/AP_UAVCAN_SLCAN.cpp
index 48df87900b..dd24bbea94 100644
--- a/libraries/AP_UAVCAN/AP_UAVCAN_SLCAN.cpp
+++ b/libraries/AP_UAVCAN/AP_UAVCAN_SLCAN.cpp
@@ -19,11 +19,14 @@
 
 #include <AP_HAL/AP_HAL.h>
 
-#if HAL_WITH_UAVCAN && !HAL_MINIMIZE_FEATURES
+#if HAL_WITH_UAVCAN && !HAL_MINIMIZE_FEATURES && CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
+
 #include "AP_UAVCAN_SLCAN.h"
 #include <AP_SerialManager/AP_SerialManager.h>
 #include <AP_Common/Semaphore.h>
 
+#include <AP_HAL_ChibiOS/CANSerialRouter.h>
+
 extern const AP_HAL::HAL& hal;
 
 static uint8_t nibble2hex(uint8_t x)
@@ -37,7 +40,6 @@ static uint8_t nibble2hex(uint8_t x)
 }
 
 static bool hex2nibble_error;
-SLCAN::SLCANRouter* SLCAN::SLCANRouter::_singleton = nullptr;
 
 static uint8_t hex2nibble(char ch)
 {
@@ -77,6 +79,7 @@ bool SLCAN::CAN::push_Frame(uavcan::CanFrame &frame)
     frm.frame = frame;
     frm.flags = 0;
     frm.utc_usec = AP_HAL::micros64();
+    slcan_router().route_frame_to_can(frm.frame, frm.utc_usec);
     return rx_queue_.push(frm);
 }
 
@@ -204,7 +207,7 @@ static inline const char* getASCIIStatusCode(bool status) { return status ? "\r"
 
 bool SLCAN::CANManager::begin(uint32_t bitrate, uint8_t can_number)
 {
-    if (driver_.init(1000000, SLCAN::CAN::NormalMode) < 0) {
+    if (driver_.init(bitrate, SLCAN::CAN::NormalMode) < 0) {
         return false;
     }
     if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&SLCAN::CANManager::reader_trampoline, void), "SLCAN", 4096, AP_HAL::Scheduler::PRIORITY_CAN, -1)) {
@@ -476,7 +479,7 @@ void SLCAN::CAN::reader() {
         return;
     }
     if (!_port_initialised) {
-        _port->begin(921600);
+        _port->begin(bitrate_);
         _port_initialised = true;
 
     }
diff --git a/libraries/AP_UAVCAN/AP_UAVCAN_SLCAN.h b/libraries/AP_UAVCAN/AP_UAVCAN_SLCAN.h
index e6a61e9413..930d837990 100644
--- a/libraries/AP_UAVCAN/AP_UAVCAN_SLCAN.h
+++ b/libraries/AP_UAVCAN/AP_UAVCAN_SLCAN.h
@@ -1,18 +1,19 @@
 
 #pragma once
 
-#if HAL_WITH_UAVCAN && !HAL_MINIMIZE_FEATURES
+#include <AP_HAL/AP_HAL.h>
+#if HAL_WITH_UAVCAN && !HAL_MINIMIZE_FEATURES && CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
 
-#include <AP_HAL/CAN.h>
 #include <AP_UAVCAN/AP_UAVCAN.h>
 #include "AP_HAL/utility/RingBuffer.h"
 
 
-
 #define SLCAN_BUFFER_SIZE 200
-#define SLCAN_STM32_RX_QUEUE_SIZE 64
+#define SLCAN_RX_QUEUE_SIZE 64
 #define SLCAN_DRIVER_INDEX 2
 
+class SLCANRouter;
+
 namespace SLCAN {
 /**
  * Driver error codes.
@@ -41,9 +42,9 @@ struct CanRxItem {
     {
     }
 };
-
 class CAN: public AP_HAL::CAN {
     friend class CANManager;
+    friend class ::SLCANRouter;
     struct TxItem {
         uavcan::MonotonicTime deadline;
         uavcan::CanFrame frame;
@@ -105,11 +106,10 @@ class CAN: public AP_HAL::CAN {
     bool handle_FrameRTRExt(const char* cmd);
     bool handle_FrameDataStd(const char* cmd);
     bool handle_FrameDataExt(const char* cmd);
+    void reader(void);
 
     inline void addByte(const uint8_t byte);
 
-    void reader(void);
-
     bool initialized_;
     bool _port_initialised;
     char buf_[SLCAN_BUFFER_SIZE + 1];
@@ -185,7 +185,7 @@ class CANManager: public AP_HAL::CANManager, public uavcan::ICanDriver {
     thread_t *_irq_handler_ctx = nullptr;
 public:
     CANManager()
-        :  AP_HAL::CANManager(this), initialized_(false), driver_(SLCAN_DRIVER_INDEX, SLCAN_STM32_RX_QUEUE_SIZE)
+        :  AP_HAL::CANManager(this), initialized_(false), driver_(SLCAN_DRIVER_INDEX, SLCAN_RX_QUEUE_SIZE)
         { }
 
     /**
@@ -216,12 +216,10 @@ public:
         return _ifaces_num;
     }
 
-    static void reader(void);
     void reader_trampoline(void);
 };
 
 }
+#include <AP_HAL_ChibiOS/CANSerialRouter.h>
 
 #endif //#if HAL_WITH_UAVCAN && !HAL_MINIMIZE_FEATURES
-
-