diff --git a/modules/canopennode/Kconfig b/modules/canopennode/Kconfig index 9be571229b35a..ace9ec2e59dc2 100644 --- a/modules/canopennode/Kconfig +++ b/modules/canopennode/Kconfig @@ -84,6 +84,14 @@ config CANOPENNODE_SYNC_THREAD TPDOs. Application layer must take care of SYNC RPDO and TPDO processing on its own if this is disabled. +config CANOPENNODE_SYNC_THREAD_PERIOD_US + int "Loop period of the periodic CANopen SYNC thread" + depends on CANOPENNODE_SYNC_THREAD + default 1000 + help + Loop period in microseconds of the periodic thread processing + CANopen SYNC RPDOs and TPDOs. + config CANOPENNODE_SYNC_THREAD_STACK_SIZE int "Stack size for the CANopen SYNC thread" depends on CANOPENNODE_SYNC_THREAD diff --git a/modules/canopennode/canopen_sync.c b/modules/canopennode/canopen_sync.c index 3a978d7405551..260868a27aac5 100644 --- a/modules/canopennode/canopen_sync.c +++ b/modules/canopennode/canopen_sync.c @@ -6,11 +6,25 @@ #include +static struct k_timer sync_timer; +static struct k_sem sync_sem; +static uint32_t last_sync_time; /* cycles */ + +/** + * @brief SYNC Timer expiry callback. + */ +static void sync_timer_expiry(struct k_timer *timer) +{ + ARG_UNUSED(timer); + k_sem_give(&sync_sem); +} + /** * @brief CANopen sync thread. * * The CANopen real-time sync thread processes SYNC RPDOs and TPDOs * through the CANopenNode stack with an interval of 1 millisecond. + * Uses a k_timer for deterministic timing. * * @param p1 Unused * @param p2 Unused @@ -18,18 +32,33 @@ */ static void canopen_sync_thread(void *p1, void *p2, void *p3) { - uint32_t start; /* cycles */ - uint32_t stop; /* cycles */ - uint32_t delta; /* cycles */ - uint32_t elapsed = 0; /* microseconds */ + uint32_t current_time; /* cycles */ + uint32_t delta; /* cycles */ + uint32_t elapsed = 0; /* microseconds */ bool sync; ARG_UNUSED(p1); ARG_UNUSED(p2); ARG_UNUSED(p3); + /* Initialize binary semaphore and timer */ + k_sem_init(&sync_sem, 0, 1); + k_timer_init(&sync_timer, sync_timer_expiry, NULL); + + /* Start the periodic timer */ + k_timer_start(&sync_timer, K_USEC(CONFIG_CANOPENNODE_SYNC_THREAD_PERIOD_US), + K_USEC(CONFIG_CANOPENNODE_SYNC_THREAD_PERIOD_US)); + last_sync_time = k_cycle_get_32(); + while (true) { - start = k_cycle_get_32(); + /* Wait for timer to fire - deterministic wake-up */ + k_sem_take(&sync_sem, K_FOREVER); + + current_time = k_cycle_get_32(); + delta = current_time - last_sync_time; + elapsed = (uint32_t)k_cyc_to_ns_floor64(delta) / NSEC_PER_USEC; + last_sync_time = current_time; + if (CO && CO->CANmodule[0] && CO->CANmodule[0]->CANnormal) { CO_LOCK_OD(); sync = CO_process_SYNC(CO, elapsed); @@ -37,14 +66,8 @@ static void canopen_sync_thread(void *p1, void *p2, void *p3) CO_process_TPDO(CO, sync, elapsed); CO_UNLOCK_OD(); } - - k_sleep(K_MSEC(1)); - stop = k_cycle_get_32(); - delta = stop - start; - elapsed = (uint32_t)k_cyc_to_ns_floor64(delta) / NSEC_PER_USEC; } } -K_THREAD_DEFINE(canopen_sync, CONFIG_CANOPENNODE_SYNC_THREAD_STACK_SIZE, - canopen_sync_thread, NULL, NULL, NULL, - CONFIG_CANOPENNODE_SYNC_THREAD_PRIORITY, 0, 1); +K_THREAD_DEFINE(canopen_sync, CONFIG_CANOPENNODE_SYNC_THREAD_STACK_SIZE, canopen_sync_thread, NULL, + NULL, NULL, CONFIG_CANOPENNODE_SYNC_THREAD_PRIORITY, 0, 1);