HAL_ChibiOS: CANSerialRouter unlock the port in timer

This commit is contained in:
Siddharth Purohit 2019-01-18 15:09:36 +08:00 committed by Andrew Tridgell
parent 7912952afe
commit f6d165d8c1
1 changed files with 1 additions and 2 deletions

View File

@ -76,6 +76,7 @@ void SLCANRouter::timer()
} }
if (AP_HAL::millis() - _last_active_time > (_slcan_rt_timeout * 1000) && _slcan_rt_timeout != 0) { if (AP_HAL::millis() - _last_active_time > (_slcan_rt_timeout * 1000) && _slcan_rt_timeout != 0) {
chSysLock(); chSysLock();
_port->lock_port(0, 0);
_thread_suspended = true; _thread_suspended = true;
chSysUnlock(); chSysUnlock();
} }
@ -113,7 +114,6 @@ void SLCANRouter::slcan2can_router_trampoline(void)
chSysLock(); chSysLock();
_s2c_thd_ref = nullptr; _s2c_thd_ref = nullptr;
if (_thread_suspended) { if (_thread_suspended) {
_port->lock_port(0, 0);
chThdSuspendS(&_s2c_thd_ref); chThdSuspendS(&_s2c_thd_ref);
} }
chSysUnlock(); chSysUnlock();
@ -134,7 +134,6 @@ void SLCANRouter::can2slcan_router_trampoline(void)
chSysLock(); chSysLock();
_c2s_thd_ref = nullptr; _c2s_thd_ref = nullptr;
if (_thread_suspended) { if (_thread_suspended) {
_port->lock_port(0, 0);
chThdSuspendS(&_c2s_thd_ref); chThdSuspendS(&_c2s_thd_ref);
} }
chSysUnlock(); chSysUnlock();