From 112f53536530a7dfe399eee12e298e4c81b70736 Mon Sep 17 00:00:00 2001 From: Siddharth Purohit 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 -#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 #include +#include + 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 +#if HAL_WITH_UAVCAN && !HAL_MINIMIZE_FEATURES && CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS -#include #include #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 #endif //#if HAL_WITH_UAVCAN && !HAL_MINIMIZE_FEATURES - -