Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

ChibiOS: add support for timeout and closing SLCAN

This commit is contained in:
Siddharth Purohit 2019-01-04 17:48:35 +08:00 committed by Andrew Tridgell
parent 979a8dea6d
commit 32a71cef92
2 changed files with 83 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

84
libraries/AP_HAL_ChibiOS/CANSerialRouter.cpp
View File

@ -18,6 +18,8 @@
#include "CANSerialRouter.h" #include "CANSerialRouter.h"
#if HAL_WITH_UAVCAN #if HAL_WITH_UAVCAN
#include <AP_SerialManager/AP_SerialManager.h>
#include <AP_BoardConfig/AP_BoardConfig_CAN.h>
SLCANRouter* SLCANRouter::_singleton = nullptr; SLCANRouter* SLCANRouter::_singleton = nullptr;
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
@ -29,29 +31,68 @@ SLCANRouter &slcan_router()
void SLCANRouter::init(ChibiOS_CAN::CanIface* can_if, ChibiOS_CAN::BusEvent* update_event) void SLCANRouter::init(ChibiOS_CAN::CanIface* can_if, ChibiOS_CAN::BusEvent* update_event)
{ {
can_if_ = can_if; _can_if = can_if;
if (!slcan_if_.is_initialized()) { _update_event = update_event;
if (slcan_if_.init(921600, SLCAN::CAN::OperatingMode::NormalMode) < 0) { hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&SLCANRouter::timer, void));
}
void SLCANRouter::run()
{
if (!_slcan_if.is_initialized()) {
_port = AP_SerialManager::get_instance()->get_serial_by_id(AP::can().get_slcan_serial());
if (_slcan_if.init(921600, SLCAN::CAN::OperatingMode::NormalMode, _port) < 0) {
return; return;
} }
_slcan_rt_timeout = AP::can().get_slcan_timeout();
}
if (!_thread_started) {
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);
_thread_started = true;
_thread_suspended = false;
} else if (_thread_suspended) { //wake up threads
chSysLock();
if (_c2s_thd_ref && _s2c_thd_ref) {
chThdResumeS(&_c2s_thd_ref, MSG_OK);
chThdResumeS(&_s2c_thd_ref, MSG_OK);
_thread_suspended = false;
}
chSysUnlock();
}
}
void SLCANRouter::timer()
{
if ((!_thread_started || _thread_suspended) && (AP::can().get_slcan_serial() != -1)) {
run();
AP::can().reset_slcan_serial();
_last_active_time = AP_HAL::millis();
}
if (!_slcan_if.closed()) {
_last_active_time = AP_HAL::millis();
}
if (_thread_suspended) {
return;
}
if (AP_HAL::millis() - _last_active_time > (_slcan_rt_timeout * 1000) && _slcan_rt_timeout != 0) {
chSysLock();
_thread_suspended = true;
chSysUnlock();
} }
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) void SLCANRouter::route_frame_to_slcan(ChibiOS_CAN::CanIface* can_if, const uavcan::CanFrame& frame, uint64_t timestamp_usec)
{ {
if (can_if_ != can_if) { if (_can_if != can_if) {
return; return;
} }
CanRouteItem it; CanRouteItem it;
it.frame = frame; it.frame = frame;
it.utc_usec = timestamp_usec; it.utc_usec = timestamp_usec;
if (slcan_tx_queue_.space() == 0) { if (_slcan_tx_queue.space() == 0) {
slcan_tx_queue_.pop(); _slcan_tx_queue.pop();
} }
slcan_tx_queue_.push(it); _slcan_tx_queue.push(it);
} }
void SLCANRouter::route_frame_to_can(const uavcan::CanFrame& frame, uint64_t timestamp_usec) void SLCANRouter::route_frame_to_can(const uavcan::CanFrame& frame, uint64_t timestamp_usec)
@ -59,10 +100,10 @@ void SLCANRouter::route_frame_to_can(const uavcan::CanFrame& frame, uint64_t tim
CanRouteItem it; CanRouteItem it;
it.frame = frame; it.frame = frame;
it.utc_usec = timestamp_usec; it.utc_usec = timestamp_usec;
if (can_tx_queue_.space() == 0) { if (_can_tx_queue.space() == 0) {
can_tx_queue_.pop(); _can_tx_queue.pop();
} }
can_tx_queue_.push(it); _can_tx_queue.push(it);
} }
void SLCANRouter::slcan2can_router_trampoline(void) void SLCANRouter::slcan2can_router_trampoline(void)
@ -90,11 +131,18 @@ void SLCANRouter::can2slcan_router_trampoline(void)
{ {
CanRouteItem it; CanRouteItem it;
while(true) { while(true) {
update_event_->wait(uavcan::MonotonicDuration::fromUSec(1000)); chSysLock();
if (slcan_tx_queue_.available()) { _c2s_thd_ref = nullptr;
slcan_tx_queue_.peek(it); if (_thread_suspended) {
if (slcan_if_.send(it.frame, uavcan::MonotonicTime::fromUSec(AP_HAL::micros64() + 1000), 0)) { _port->lock_port(0, 0);
slcan_tx_queue_.pop(); chThdSuspendS(&_c2s_thd_ref);
}
chSysUnlock();
_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();
} }
} }
} }

23
libraries/AP_HAL_ChibiOS/CANSerialRouter.h
View File

@ -39,14 +39,24 @@ struct CanRouteItem {
class SLCANRouter class SLCANRouter
{ {
ChibiOS_CAN::CanIface* can_if_; ChibiOS_CAN::CanIface* _can_if;
SLCAN::CAN slcan_if_; SLCAN::CAN _slcan_if;
ObjectBuffer<CanRouteItem> can_tx_queue_; ObjectBuffer<CanRouteItem> _can_tx_queue;
ObjectBuffer<CanRouteItem> slcan_tx_queue_; ObjectBuffer<CanRouteItem> _slcan_tx_queue;
static SLCANRouter* _singleton; static SLCANRouter* _singleton;
ChibiOS_CAN::BusEvent* update_event_; ChibiOS_CAN::BusEvent* _update_event;
uint32_t _last_active_time = 0;
uint32_t _slcan_rt_timeout = 0;
bool _thread_started = false;
bool _thread_suspended = false;
HAL_Semaphore router_sem;
thread_reference_t _s2c_thd_ref;
thread_reference_t _c2s_thd_ref;
void timer(void);
AP_HAL::UARTDriver* _port;
public: 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) 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) { if (_singleton == nullptr) {
_singleton = this; _singleton = this;
@ -57,6 +67,7 @@ public:
void route_frame_to_can(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 slcan2can_router_trampoline(void);
void can2slcan_router_trampoline(void); void can2slcan_router_trampoline(void);
void run(void);
static SLCANRouter* instance() { static SLCANRouter* instance() {
if (_singleton == nullptr) { if (_singleton == nullptr) {
_singleton = new SLCANRouter; _singleton = new SLCANRouter;