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.c b/src/co_main.c index c3c0ae4..8d100c2 100644 --- a/src/co_main.c +++ b/src/co_main.c @@ -277,6 +277,7 @@ int co_sdo_read ( job->sdo.subindex = subindex; job->sdo.data = data; job->sdo.remain = size; + job->sdo.cached = false; job->callback = co_job_callback; job->timestamp = os_tick_current(); job->type = CO_JOB_SDO_READ; @@ -306,6 +307,7 @@ int co_sdo_write ( job->sdo.subindex = subindex; job->sdo.data = (uint8_t *)data; job->sdo.remain = size; + job->sdo.cached = false; job->callback = co_job_callback; job->timestamp = os_tick_current(); job->type = CO_JOB_SDO_WRITE; 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_od.c b/src/co_od.c index 49f481c..fa91c22 100644 --- a/src/co_od.c +++ b/src/co_od.c @@ -36,7 +36,7 @@ static int co_subindex_equals ( return entry->subindex == subindex; } -static void co_od_notify ( +void co_od_notify ( co_net_t * net, const co_obj_t * obj, const co_entry_t * entry, diff --git a/src/co_od.h b/src/co_od.h index f5e0803..53c3b55 100644 --- a/src/co_od.h +++ b/src/co_od.h @@ -271,6 +271,23 @@ uint32_t co_od_set_value ( uint8_t subindex, uint64_t value); +/** + * Trigger notification callback + * + * This functions triggers the notification callback of the subindex, + * if any. + * + * @param net network handle + * @param obj object descriptor + * @param entry entry descriptor + * @param subindex subindex + */ +void co_od_notify ( + co_net_t * net, + const co_obj_t * obj, + const co_entry_t * entry, + uint8_t subindex); + #ifdef __cplusplus } #endif diff --git a/src/co_pdo.c b/src/co_pdo.c index 6eb18a2..cae0ae8 100644 --- a/src/co_pdo.c +++ b/src/co_pdo.c @@ -84,11 +84,12 @@ void co_pdo_pack (co_net_t * net, co_pdo_t * pdo) const co_entry_t * entry = pdo->entries[ix]; const co_obj_t * obj = pdo->objs[ix]; size_t bitlength = pdo->mappings[ix] & 0xFF; + uint8_t subindex = (pdo->mappings[ix] >> 8) & 0xFF; uint64_t value = 0; if (entry != NULL) { - co_od_get_value (net, obj, entry, entry->subindex, &value); + co_od_get_value (net, obj, entry, subindex, &value); } bitslice_set (&pdo->frame, offset, bitlength, value); @@ -106,12 +107,13 @@ void co_pdo_unpack (co_net_t * net, co_pdo_t * pdo) const co_entry_t * entry = pdo->entries[ix]; const co_obj_t * obj = pdo->objs[ix]; size_t bitlength = pdo->mappings[ix] & 0xFF; + uint8_t subindex = (pdo->mappings[ix] >> 8) & 0xFF; uint64_t value; if (entry != NULL) { value = bitslice_get (&pdo->frame, offset, bitlength); - co_od_set_value (net, obj, entry, entry->subindex, value); + co_od_set_value (net, obj, entry, subindex, value); } offset += bitlength; @@ -140,6 +142,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 +557,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 +580,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_RX_PDO; ix++) + { + co_pdo_t * pdo = &net->pdo_rx[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 +833,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 */ diff --git a/src/co_sdo_server.c b/src/co_sdo_server.c index 3d5762f..c1b1afc 100644 --- a/src/co_sdo_server.c +++ b/src/co_sdo_server.c @@ -129,6 +129,7 @@ static int co_sdo_rx_upload_init_req ( job->type = CO_JOB_SDO_UPLOAD; job->sdo.index = co_fetch_uint16 (&data[1]); job->sdo.subindex = data[3]; + job->sdo.cached = false; job->timestamp = os_tick_current(); /* Find requested object */ @@ -306,6 +307,7 @@ static int co_sdo_rx_download_init_req ( job->type = CO_JOB_SDO_DOWNLOAD; job->sdo.index = co_fetch_uint16 (&data[1]); job->sdo.subindex = data[3]; + job->sdo.cached = false; job->timestamp = os_tick_current(); /* Find requested object */ @@ -466,34 +468,34 @@ static int co_sdo_rx_download_seg_req ( /* Write complete */ job->type = CO_JOB_NONE; - if (job->sdo.cached) + /* Find requested object */ + obj = co_obj_find (net, job->sdo.index); + if (obj == NULL) { - /* Find requested object */ - obj = co_obj_find (net, job->sdo.index); - if (obj == NULL) - { - co_sdo_abort ( - net, - 0x580 + net->node, - job->sdo.index, - job->sdo.subindex, - CO_SDO_ABORT_BAD_INDEX); - return -1; - } + co_sdo_abort ( + net, + 0x580 + net->node, + job->sdo.index, + job->sdo.subindex, + CO_SDO_ABORT_BAD_INDEX); + return -1; + } - /* Find requested subindex */ - entry = co_entry_find (net, obj, job->sdo.subindex); - if (entry == NULL) - { - co_sdo_abort ( - net, - 0x580 + net->node, - job->sdo.index, - job->sdo.subindex, - CO_SDO_ABORT_BAD_SUBINDEX); - return -1; - } + /* Find requested subindex */ + entry = co_entry_find (net, obj, job->sdo.subindex); + if (entry == NULL) + { + co_sdo_abort ( + net, + 0x580 + net->node, + job->sdo.index, + job->sdo.subindex, + CO_SDO_ABORT_BAD_SUBINDEX); + return -1; + } + if (job->sdo.cached) + { /* Atomically set value */ abort = co_od_set_value (net, obj, entry, job->sdo.subindex, job->sdo.value); @@ -508,6 +510,10 @@ static int co_sdo_rx_download_seg_req ( return -1; } } + else + { + co_od_notify (net, obj, entry, job->sdo.subindex); + } } /* Segmented response */ diff --git a/test/test_pdo.cpp b/test/test_pdo.cpp index 5ac50a7..552365f 100644 --- a/test/test_pdo.cpp +++ b/test/test_pdo.cpp @@ -152,6 +152,27 @@ TEST_F (PdoTest, PackWithPadding) EXPECT_EQ (1u, frame[1]); } +TEST_F (PdoTest, PackArray) +{ + co_pdo_t pdo; + uint8_t * frame = (uint8_t *)&pdo.frame; + const co_obj_t * obj2000 = find_obj (0x2000); + + memset (&pdo, 0, sizeof (pdo)); + + pdo.number_of_mappings = 2; + pdo.mappings[0] = 0x20000308; + pdo.mappings[1] = 0x20000708; + pdo.entries[0] = find_entry (obj2000, 3); + pdo.entries[1] = find_entry (obj2000, 7); + pdo.objs[0] = obj2000; + pdo.objs[1] = obj2000; + + co_pdo_pack (&net, &pdo); + EXPECT_EQ (3u, frame[0]); + EXPECT_EQ (7u, frame[1]); +} + TEST_F (PdoTest, Unpack) { co_pdo_t pdo; @@ -236,6 +257,31 @@ TEST_F (PdoTest, UnpackWithPadding) EXPECT_EQ (0x3322u, value6003_07); } +TEST_F (PdoTest, UnpackArray) +{ + co_pdo_t pdo; + uint8_t * frame = (uint8_t *)&pdo.frame; + const co_obj_t * obj2000 = find_obj (0x2000); + + memset (&pdo, 0, sizeof (pdo)); + + pdo.number_of_mappings = 2; + pdo.mappings[0] = 0x20000308; + pdo.mappings[1] = 0x20000708; + pdo.entries[0] = find_entry (obj2000, 3); + pdo.entries[1] = find_entry (obj2000, 7); + pdo.objs[0] = obj2000; + pdo.objs[1] = obj2000; + + frame[0] = 0x00; + frame[1] = 0x11; + + co_pdo_unpack (&net, &pdo); + + EXPECT_EQ (0x00u, arr2000[2]); + EXPECT_EQ (0x11u, arr2000[6]); +} + TEST_F (PdoTest, CommParamsSet) { const co_obj_t * obj1400 = find_obj (0x1400); @@ -811,3 +857,53 @@ TEST_F (PdoTest, SparsePdo) EXPECT_EQ (0u, result); EXPECT_EQ (0x1234u, value); } + +TEST_F (PdoTest, RPDOMonitoring) +{ + uint8_t pdo[][4] = { + {0x11, 0x22, 0x33, 0x44}, + }; + + net.state = STATE_OP; + + net.pdo_rx[0].cobid = 0x201; + net.pdo_rx[0].event_timer = 100; + + // Arm RPDO deadline monitoring + co_pdo_rx (&net, 0x201, pdo[0], sizeof (pdo[0])); + EXPECT_TRUE (net.pdo_rx[0].rpdo_monitoring); + + // Receive PDO, timer has not expired. Rearm timer. + mock_os_tick_current_result = 50 * 1000; + co_pdo_rx (&net, 0x201, pdo[0], sizeof (pdo[0])); + EXPECT_EQ (0u, mock_co_emcy_tx_calls); + + // Timer has not expired + mock_os_tick_current_result = 149 * 1000; + co_pdo_timer (&net, mock_os_tick_current_result); + EXPECT_EQ (0u, mock_co_emcy_tx_calls); + + // Timer has expired, should generate EMCY + mock_os_tick_current_result = 150 * 1000; + co_pdo_timer (&net, mock_os_tick_current_result); + EXPECT_EQ (1u, mock_co_emcy_tx_calls); + EXPECT_EQ (0x8250, mock_co_emcy_tx_code); + EXPECT_FALSE (net.pdo_rx[0].rpdo_monitoring); + + // Timer still expired, should not generate EMCY + mock_os_tick_current_result = 151 * 1000; + co_pdo_timer (&net, mock_os_tick_current_result); + EXPECT_EQ (1u, mock_co_emcy_tx_calls); + EXPECT_EQ (0x8250, mock_co_emcy_tx_code); + EXPECT_FALSE (net.pdo_rx[0].rpdo_monitoring); + + // Receive PDO. Rearm timer. + mock_os_tick_current_result = 160 * 1000; + co_pdo_rx (&net, 0x201, pdo[0], sizeof (pdo[0])); + EXPECT_EQ (1u, mock_co_emcy_tx_calls); + + // Receive PDO, timer has not expired + mock_os_tick_current_result = 259 * 1000; + co_pdo_rx (&net, 0x201, pdo[0], sizeof (pdo[0])); + EXPECT_EQ (1u, mock_co_emcy_tx_calls); +} diff --git a/test/test_sdo_client.cpp b/test/test_sdo_client.cpp index 80c1f45..e611867 100644 --- a/test/test_sdo_client.cpp +++ b/test/test_sdo_client.cpp @@ -58,7 +58,7 @@ TEST_F (SdoClientTest, ExpeditedUpload) TEST_F (SdoClientTest, ExpeditedDownload) { - co_job_t job; + co_job_t job{}; uint16_t value = 0; uint8_t expected[][8] = { @@ -136,7 +136,7 @@ TEST_F (SdoClientTest, SegmentedUpload) TEST_F (SdoClientTest, SegmentedDownload) { - co_job_t job; + co_job_t job{}; const char * s = "hello world"; uint8_t expected[][8] = { diff --git a/test/test_sdo_server.cpp b/test/test_sdo_server.cpp index e80633b..5132e0b 100644 --- a/test/test_sdo_server.cpp +++ b/test/test_sdo_server.cpp @@ -132,6 +132,7 @@ TEST_F (SdoServerTest, SegmentedDownload) } EXPECT_STREQ ("new slave name", name1009); + EXPECT_EQ (1u, cb_notify_calls); } TEST_F (SdoServerTest, SegmentedDownloadCached) @@ -196,7 +197,7 @@ TEST_F (SdoServerTest, SegmentedTimeout) TEST_F (SdoServerTest, BadSubIndex) { - const co_obj_t obj = {0, OTYPE_NULL, 0, NULL, NULL}; + static const co_obj_t obj = {0, OTYPE_NULL, 0, NULL, NULL}; uint8_t expected[][8] = { {0x80, 0x00, 0x10, 0x01, 0x11, 0x00, 0x09, 0x06}, }; diff --git a/test/test_util.h b/test/test_util.h index da845b7..8e6f34e 100644 --- a/test/test_util.h +++ b/test/test_util.h @@ -97,7 +97,7 @@ class TestBase : public ::testing::Test char name1009[20] = {0}; co_entry_t OD1009[1] = { - {0, OD_RW, DTYPE_VISIBLE_STRING, 8 * sizeof (name1009), 0, name1009}, + {0, OD_NOTIFY | OD_RW, DTYPE_VISIBLE_STRING, 8 * sizeof (name1009), 0, name1009}, }; char name100A[20];