2018-11-16 06:14:30 -04:00
|
|
|
/*
|
|
|
|
* 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 <http://www.gnu.org/licenses/>.
|
2019-01-23 00:09:44 -04:00
|
|
|
*
|
2018-11-16 06:14:30 -04:00
|
|
|
* Siddharth Bharat Purohit
|
|
|
|
*/
|
|
|
|
|
|
|
|
#include "CANSerialRouter.h"
|
|
|
|
|
2019-01-28 22:03:32 -04:00
|
|
|
#if HAL_WITH_UAVCAN && !HAL_MINIMIZE_FEATURES
|
2019-01-04 05:48:35 -04:00
|
|
|
#include <AP_SerialManager/AP_SerialManager.h>
|
|
|
|
#include <AP_BoardConfig/AP_BoardConfig_CAN.h>
|
2018-11-16 06:14:30 -04:00
|
|
|
SLCANRouter* SLCANRouter::_singleton = nullptr;
|
|
|
|
|
|
|
|
extern const AP_HAL::HAL& hal;
|
|
|
|
|
|
|
|
SLCANRouter &slcan_router()
|
|
|
|
{
|
2019-02-10 14:53:21 -04:00
|
|
|
return *SLCANRouter::get_singleton();
|
2018-11-16 06:14:30 -04:00
|
|
|
}
|
|
|
|
|
|
|
|
void SLCANRouter::init(ChibiOS_CAN::CanIface* can_if, ChibiOS_CAN::BusEvent* update_event)
|
|
|
|
{
|
2019-01-04 05:48:35 -04:00
|
|
|
_can_if = can_if;
|
|
|
|
_update_event = update_event;
|
|
|
|
hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&SLCANRouter::timer, void));
|
|
|
|
}
|
|
|
|
|
|
|
|
void SLCANRouter::run()
|
|
|
|
{
|
2019-02-10 14:53:21 -04:00
|
|
|
_port = AP_SerialManager::get_singleton()->get_serial_by_id(AP::can().get_slcan_serial());
|
2019-01-22 23:43:43 -04:00
|
|
|
if (_slcan_if.init(921600, SLCAN::CAN::OperatingMode::NormalMode, _port) < 0) {
|
|
|
|
return;
|
2019-01-04 05:48:35 -04:00
|
|
|
}
|
2019-01-22 23:43:43 -04:00
|
|
|
_slcan_rt_timeout = AP::can().get_slcan_timeout();
|
2019-01-04 05:48:35 -04:00
|
|
|
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;
|
2019-01-23 00:09:44 -04:00
|
|
|
}
|
|
|
|
else if (_thread_suspended) { //wake up threads
|
2019-01-04 05:48:35 -04:00
|
|
|
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();
|
2019-01-18 03:09:36 -04:00
|
|
|
_port->lock_port(0, 0);
|
2019-01-19 01:31:44 -04:00
|
|
|
_port->flush();
|
2019-01-04 05:48:35 -04:00
|
|
|
_thread_suspended = true;
|
|
|
|
chSysUnlock();
|
2018-11-16 06:14:30 -04:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
void SLCANRouter::route_frame_to_slcan(ChibiOS_CAN::CanIface* can_if, const uavcan::CanFrame& frame, uint64_t timestamp_usec)
|
|
|
|
{
|
2019-01-04 05:48:35 -04:00
|
|
|
if (_can_if != can_if) {
|
2018-11-16 06:14:30 -04:00
|
|
|
return;
|
|
|
|
}
|
|
|
|
CanRouteItem it;
|
|
|
|
it.frame = frame;
|
|
|
|
it.utc_usec = timestamp_usec;
|
2019-01-04 05:48:35 -04:00
|
|
|
if (_slcan_tx_queue.space() == 0) {
|
|
|
|
_slcan_tx_queue.pop();
|
2018-11-16 06:14:30 -04:00
|
|
|
}
|
2019-01-04 05:48:35 -04:00
|
|
|
_slcan_tx_queue.push(it);
|
2018-11-16 06:14:30 -04:00
|
|
|
}
|
|
|
|
|
|
|
|
void SLCANRouter::route_frame_to_can(const uavcan::CanFrame& frame, uint64_t timestamp_usec)
|
|
|
|
{
|
|
|
|
CanRouteItem it;
|
|
|
|
it.frame = frame;
|
|
|
|
it.utc_usec = timestamp_usec;
|
2019-01-04 05:48:35 -04:00
|
|
|
if (_can_tx_queue.space() == 0) {
|
|
|
|
_can_tx_queue.pop();
|
2018-11-16 06:14:30 -04:00
|
|
|
}
|
2019-01-04 05:48:35 -04:00
|
|
|
_can_tx_queue.push(it);
|
2018-11-16 06:14:30 -04:00
|
|
|
}
|
|
|
|
|
|
|
|
void SLCANRouter::slcan2can_router_trampoline(void)
|
|
|
|
{
|
|
|
|
CanRouteItem it;
|
2019-01-23 00:09:44 -04:00
|
|
|
while (true) {
|
2018-11-16 06:14:30 -04:00
|
|
|
chSysLock();
|
2019-01-23 00:09:44 -04:00
|
|
|
_s2c_thd_ref = nullptr;
|
2018-11-16 06:14:30 -04:00
|
|
|
if (_thread_suspended) {
|
|
|
|
chThdSuspendS(&_s2c_thd_ref);
|
2019-01-19 01:31:44 -04:00
|
|
|
_port->flush();
|
2018-11-16 06:14:30 -04:00
|
|
|
}
|
|
|
|
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;
|
2019-01-23 00:09:44 -04:00
|
|
|
while (true) {
|
2019-01-04 05:48:35 -04:00
|
|
|
chSysLock();
|
|
|
|
_c2s_thd_ref = nullptr;
|
|
|
|
if (_thread_suspended) {
|
|
|
|
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();
|
2018-11-16 06:14:30 -04:00
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
#endif
|