diff --git a/src/co_emcy.c b/src/co_emcy.c index 238a762..ebce3ec 100644 --- a/src/co_emcy.c +++ b/src/co_emcy.c @@ -384,7 +384,7 @@ void co_emcy_handle_can_state (co_net_t * net) if ( !net->emcy.state.overrun && !net->emcy.state.error_passive && !net->emcy.state.bus_off && !net->emcy.node_guard_error && - !net->emcy.heartbeat_error) + !net->emcy.heartbeat_error && !net->emcy.rpdo_timeout) { co_emcy_error_register_clear (net, CO_ERR_COMMUNICATION); } diff --git a/src/co_main.h b/src/co_main.h index c1d29d4..e5b0ed7 100644 --- a/src/co_main.h +++ b/src/co_main.h @@ -82,6 +82,8 @@ typedef struct co_pdo { bool queued : 1; bool sync_wait : 1; + bool rpdo_monitoring : 1; + bool rpdo_timeout : 1; }; uint32_t mappings[MAX_PDO_ENTRIES]; const co_obj_t * objs[MAX_PDO_ENTRIES]; @@ -220,6 +222,7 @@ typedef struct co_emcy os_channel_state_t state; /**< CAN state */ bool node_guard_error; /**< Node guard error */ bool heartbeat_error; /**< Heartbeat error */ + bool rpdo_timeout; /**< RPDO timeout */ uint32_t cobids[MAX_EMCY_COBIDS]; /**< EMCY consumer object */ } co_emcy_t; diff --git a/src/co_pdo.c b/src/co_pdo.c index 6eb18a2..3c82ab6 100644 --- a/src/co_pdo.c +++ b/src/co_pdo.c @@ -140,6 +140,8 @@ static uint32_t co_pdo_mapping_validate (co_pdo_t * pdo, uint8_t number_of_mappi if (IS_CYCLIC (pdo->sync_start)) pdo->sync_wait = true; + pdo->rpdo_monitoring = false; + return 0; } @@ -553,6 +555,7 @@ static void co_pdo_transmit (co_net_t * net, co_pdo_t * pdo) int co_pdo_timer (co_net_t * net, os_tick_t now) { unsigned int ix; + bool rpdo_timeout = false; if (net->state != STATE_OP) return -1; @@ -575,6 +578,37 @@ int co_pdo_timer (co_net_t * net, os_tick_t now) } } + /* Check for RPDOs with event timer (deadline monitoring) */ + for (ix = 0; ix < MAX_TX_PDO; ix++) + { + co_pdo_t * pdo = &net->pdo_tx[ix]; + + if (pdo->cobid & CO_COBID_INVALID) + continue; + + if (pdo->rpdo_timeout) + { + /* Already signaled, just track the combined state. */ + rpdo_timeout = true; + continue; + } + + if (!pdo->rpdo_monitoring || pdo->event_timer == 0) + continue; + + if (co_is_expired (now, pdo->timestamp, 1000 * pdo->event_timer)) + { + /* Deadline timeout elapsed, transmit EMCY */ + pdo->rpdo_monitoring = false; + pdo->rpdo_timeout = rpdo_timeout = true; + co_emcy_error_register_set (net, CO_ERR_COMMUNICATION); + co_emcy_tx (net, 0x8250, 0, NULL); + } + } + + /* Update RPDO timeout state */ + net->emcy.rpdo_timeout = rpdo_timeout; + return 0; } @@ -797,6 +831,13 @@ void co_pdo_rx (co_net_t * net, uint32_t id, void * msg, size_t dlc) memcpy (&pdo->frame, msg, dlc); pdo->timestamp = os_tick_current(); + if (pdo->event_timer > 0) + { + /* Arm RPDO deadline monitoring */ + pdo->rpdo_monitoring = true; + pdo->rpdo_timeout = false; + } + if (IS_EVENT (pdo->transmission_type)) { /* Deliver event-driven RPDOs asynchronously */