diff --git a/libraries/AP_HAL_ChibiOS/CANSerialRouter.cpp b/libraries/AP_HAL_ChibiOS/CANSerialRouter.cpp new file mode 100644 index 0000000000..5b4ff1c478 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/CANSerialRouter.cpp @@ -0,0 +1,103 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + * + * Siddharth Bharat Purohit + */ + +#include "CANSerialRouter.h" + +#if HAL_WITH_UAVCAN +SLCANRouter* SLCANRouter::_singleton = nullptr; + +extern const AP_HAL::HAL& hal; + +SLCANRouter &slcan_router() +{ + return *SLCANRouter::instance(); +} + +void SLCANRouter::init(ChibiOS_CAN::CanIface* can_if, ChibiOS_CAN::BusEvent* update_event) +{ + can_if_ = can_if; + if (!slcan_if_.is_initialized()) { + if (slcan_if_.init(921600, SLCAN::CAN::OperatingMode::NormalMode) < 0) { + return; + } + } + update_event_ = update_event; + hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&SLCANRouter::can2slcan_router_trampoline, void), "C2SRouter", 512, AP_HAL::Scheduler::PRIORITY_CAN, 0); + hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&SLCANRouter::slcan2can_router_trampoline, void), "S2CRouter", 512, AP_HAL::Scheduler::PRIORITY_CAN, 0); +} + +void SLCANRouter::route_frame_to_slcan(ChibiOS_CAN::CanIface* can_if, const uavcan::CanFrame& frame, uint64_t timestamp_usec) +{ + if (can_if_ != can_if) { + return; + } + CanRouteItem it; + it.frame = frame; + it.utc_usec = timestamp_usec; + if (slcan_tx_queue_.space() == 0) { + slcan_tx_queue_.pop(); + } + slcan_tx_queue_.push(it); +} + +void SLCANRouter::route_frame_to_can(const uavcan::CanFrame& frame, uint64_t timestamp_usec) +{ + CanRouteItem it; + it.frame = frame; + it.utc_usec = timestamp_usec; + if (can_tx_queue_.space() == 0) { + can_tx_queue_.pop(); + } + can_tx_queue_.push(it); +} + +void SLCANRouter::slcan2can_router_trampoline(void) +{ + CanRouteItem it; + while(true) { + chSysLock(); + _s2c_thd_ref = nullptr; + if (_thread_suspended) { + _port->lock_port(0, 0); + chThdSuspendS(&_s2c_thd_ref); + } + chSysUnlock(); + _slcan_if.reader(); + if (_can_tx_queue.available() && _can_if) { + _can_tx_queue.peek(it); + if (_can_if->send(it.frame, uavcan::MonotonicTime::fromUSec(AP_HAL::micros64() + 1000), 0)) { + _can_tx_queue.pop(); + } + } + } +} + +void SLCANRouter::can2slcan_router_trampoline(void) +{ + CanRouteItem it; + while(true) { + update_event_->wait(uavcan::MonotonicDuration::fromUSec(1000)); + if (slcan_tx_queue_.available()) { + slcan_tx_queue_.peek(it); + if (slcan_if_.send(it.frame, uavcan::MonotonicTime::fromUSec(AP_HAL::micros64() + 1000), 0)) { + slcan_tx_queue_.pop(); + } + } + } +} + +#endif diff --git a/libraries/AP_HAL_ChibiOS/CANSerialRouter.h b/libraries/AP_HAL_ChibiOS/CANSerialRouter.h new file mode 100644 index 0000000000..58492b0bec --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/CANSerialRouter.h @@ -0,0 +1,70 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + * + * Siddharth Bharat Purohit + */ + +#pragma once + +#include "AP_HAL_ChibiOS.h" + +#if HAL_WITH_UAVCAN +#include "CAN.h" +#include + +#include "CANThread.h" +#include "CANClock.h" +#include "CANIface.h" +#define SLCAN_ROUTER_QUEUE_SIZE 64 + +struct CanRouteItem { + uint64_t utc_usec; + uavcan::CanFrame frame; + CanRouteItem() : + utc_usec(0) + { + } +}; + +class SLCANRouter +{ + ChibiOS_CAN::CanIface* can_if_; + SLCAN::CAN slcan_if_; + ObjectBuffer can_tx_queue_; + ObjectBuffer slcan_tx_queue_; + static SLCANRouter* _singleton; + ChibiOS_CAN::BusEvent* update_event_; +public: + SLCANRouter() : slcan_if_(SLCAN_DRIVER_INDEX, SLCAN_RX_QUEUE_SIZE), can_tx_queue_(SLCAN_ROUTER_QUEUE_SIZE), slcan_tx_queue_(SLCAN_ROUTER_QUEUE_SIZE) + { + if (_singleton == nullptr) { + _singleton = this; + } + } + void init(ChibiOS_CAN::CanIface* can_if, ChibiOS_CAN::BusEvent* update_event); + void route_frame_to_slcan(ChibiOS_CAN::CanIface* can_if, const uavcan::CanFrame& frame, uint64_t timestamp_usec); + void route_frame_to_can(const uavcan::CanFrame& frame, uint64_t timestamp_usec); + void slcan2can_router_trampoline(void); + void can2slcan_router_trampoline(void); + static SLCANRouter* instance() { + if (_singleton == nullptr) { + _singleton = new SLCANRouter; + } + return _singleton; + } +}; + +SLCANRouter &slcan_router(); + +#endif