Skip to content

Commit a05a8a7

Browse files
author
rt-labs bot
committed
chore: catchup to a5be805
a5be805 Add test for RPDO deadline monitoring 465f138 Add RPDO deadline monitoring a0e054e Fix gcc-12 build failure e203bea Fix segmented download of objects larger than 64 bits d152e8d Add test for PDO mapping with OD_ARRAY entries ad68c17 Fix PDO mapping with OD_ARRAY entries Based-On-Commit: a5be805 Change-Id: Ia6542ddf1a5cbd6190b674fc79e93a25e0932c85
1 parent 92e646e commit a05a8a7

File tree

11 files changed

+201
-33
lines changed

11 files changed

+201
-33
lines changed

‎src/co_emcy.c‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -384,7 +384,7 @@ void co_emcy_handle_can_state (co_net_t * net)
384384
if (
385385
!net->emcy.state.overrun && !net->emcy.state.error_passive &&
386386
!net->emcy.state.bus_off && !net->emcy.node_guard_error &&
387-
!net->emcy.heartbeat_error)
387+
!net->emcy.heartbeat_error && !net->emcy.rpdo_timeout)
388388
{
389389
co_emcy_error_register_clear (net, CO_ERR_COMMUNICATION);
390390
}

‎src/co_main.c‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -277,6 +277,7 @@ int co_sdo_read (
277277
job->sdo.subindex = subindex;
278278
job->sdo.data = data;
279279
job->sdo.remain = size;
280+
job->sdo.cached = false;
280281
job->callback = co_job_callback;
281282
job->timestamp = os_tick_current();
282283
job->type = CO_JOB_SDO_READ;
@@ -306,6 +307,7 @@ int co_sdo_write (
306307
job->sdo.subindex = subindex;
307308
job->sdo.data = (uint8_t *)data;
308309
job->sdo.remain = size;
310+
job->sdo.cached = false;
309311
job->callback = co_job_callback;
310312
job->timestamp = os_tick_current();
311313
job->type = CO_JOB_SDO_WRITE;

‎src/co_main.h‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -82,6 +82,8 @@ typedef struct co_pdo
8282
{
8383
bool queued : 1;
8484
bool sync_wait : 1;
85+
bool rpdo_monitoring : 1;
86+
bool rpdo_timeout : 1;
8587
};
8688
uint32_t mappings[MAX_PDO_ENTRIES];
8789
const co_obj_t * objs[MAX_PDO_ENTRIES];
@@ -220,6 +222,7 @@ typedef struct co_emcy
220222
os_channel_state_t state; /**< CAN state */
221223
bool node_guard_error; /**< Node guard error */
222224
bool heartbeat_error; /**< Heartbeat error */
225+
bool rpdo_timeout; /**< RPDO timeout */
223226
uint32_t cobids[MAX_EMCY_COBIDS]; /**< EMCY consumer object */
224227
} co_emcy_t;
225228

‎src/co_od.c‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -36,7 +36,7 @@ static int co_subindex_equals (
3636
return entry->subindex == subindex;
3737
}
3838

39-
static void co_od_notify (
39+
void co_od_notify (
4040
co_net_t * net,
4141
const co_obj_t * obj,
4242
const co_entry_t * entry,

‎src/co_od.h‎

Lines changed: 17 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -271,6 +271,23 @@ uint32_t co_od_set_value (
271271
uint8_t subindex,
272272
uint64_t value);
273273

274+
/**
275+
* Trigger notification callback
276+
*
277+
* This functions triggers the notification callback of the subindex,
278+
* if any.
279+
*
280+
* @param net network handle
281+
* @param obj object descriptor
282+
* @param entry entry descriptor
283+
* @param subindex subindex
284+
*/
285+
void co_od_notify (
286+
co_net_t * net,
287+
const co_obj_t * obj,
288+
const co_entry_t * entry,
289+
uint8_t subindex);
290+
274291
#ifdef __cplusplus
275292
}
276293
#endif

‎src/co_pdo.c‎

Lines changed: 45 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -84,11 +84,12 @@ void co_pdo_pack (co_net_t * net, co_pdo_t * pdo)
8484
const co_entry_t * entry = pdo->entries[ix];
8585
const co_obj_t * obj = pdo->objs[ix];
8686
size_t bitlength = pdo->mappings[ix] & 0xFF;
87+
uint8_t subindex = (pdo->mappings[ix] >> 8) & 0xFF;
8788
uint64_t value = 0;
8889

8990
if (entry != NULL)
9091
{
91-
co_od_get_value (net, obj, entry, entry->subindex, &value);
92+
co_od_get_value (net, obj, entry, subindex, &value);
9293
}
9394

9495
bitslice_set (&pdo->frame, offset, bitlength, value);
@@ -106,12 +107,13 @@ void co_pdo_unpack (co_net_t * net, co_pdo_t * pdo)
106107
const co_entry_t * entry = pdo->entries[ix];
107108
const co_obj_t * obj = pdo->objs[ix];
108109
size_t bitlength = pdo->mappings[ix] & 0xFF;
110+
uint8_t subindex = (pdo->mappings[ix] >> 8) & 0xFF;
109111
uint64_t value;
110112

111113
if (entry != NULL)
112114
{
113115
value = bitslice_get (&pdo->frame, offset, bitlength);
114-
co_od_set_value (net, obj, entry, entry->subindex, value);
116+
co_od_set_value (net, obj, entry, subindex, value);
115117
}
116118

117119
offset += bitlength;
@@ -140,6 +142,8 @@ static uint32_t co_pdo_mapping_validate (co_pdo_t * pdo, uint8_t number_of_mappi
140142
if (IS_CYCLIC (pdo->sync_start))
141143
pdo->sync_wait = true;
142144

145+
pdo->rpdo_monitoring = false;
146+
143147
return 0;
144148
}
145149

@@ -553,6 +557,7 @@ static void co_pdo_transmit (co_net_t * net, co_pdo_t * pdo)
553557
int co_pdo_timer (co_net_t * net, os_tick_t now)
554558
{
555559
unsigned int ix;
560+
bool rpdo_timeout = false;
556561

557562
if (net->state != STATE_OP)
558563
return -1;
@@ -575,6 +580,37 @@ int co_pdo_timer (co_net_t * net, os_tick_t now)
575580
}
576581
}
577582

583+
/* Check for RPDOs with event timer (deadline monitoring) */
584+
for (ix = 0; ix < MAX_RX_PDO; ix++)
585+
{
586+
co_pdo_t * pdo = &net->pdo_rx[ix];
587+
588+
if (pdo->cobid & CO_COBID_INVALID)
589+
continue;
590+
591+
if (pdo->rpdo_timeout)
592+
{
593+
/* Already signaled, just track the combined state. */
594+
rpdo_timeout = true;
595+
continue;
596+
}
597+
598+
if (!pdo->rpdo_monitoring || pdo->event_timer == 0)
599+
continue;
600+
601+
if (co_is_expired (now, pdo->timestamp, 1000 * pdo->event_timer))
602+
{
603+
/* Deadline timeout elapsed, transmit EMCY */
604+
pdo->rpdo_monitoring = false;
605+
pdo->rpdo_timeout = rpdo_timeout = true;
606+
co_emcy_error_register_set (net, CO_ERR_COMMUNICATION);
607+
co_emcy_tx (net, 0x8250, 0, NULL);
608+
}
609+
}
610+
611+
/* Update RPDO timeout state */
612+
net->emcy.rpdo_timeout = rpdo_timeout;
613+
578614
return 0;
579615
}
580616

@@ -797,6 +833,13 @@ void co_pdo_rx (co_net_t * net, uint32_t id, void * msg, size_t dlc)
797833
memcpy (&pdo->frame, msg, dlc);
798834
pdo->timestamp = os_tick_current();
799835

836+
if (pdo->event_timer > 0)
837+
{
838+
/* Arm RPDO deadline monitoring */
839+
pdo->rpdo_monitoring = true;
840+
pdo->rpdo_timeout = false;
841+
}
842+
800843
if (IS_EVENT (pdo->transmission_type))
801844
{
802845
/* Deliver event-driven RPDOs asynchronously */

‎src/co_sdo_server.c‎

Lines changed: 31 additions & 25 deletions
Original file line numberDiff line numberDiff line change
@@ -129,6 +129,7 @@ static int co_sdo_rx_upload_init_req (
129129
job->type = CO_JOB_SDO_UPLOAD;
130130
job->sdo.index = co_fetch_uint16 (&data[1]);
131131
job->sdo.subindex = data[3];
132+
job->sdo.cached = false;
132133
job->timestamp = os_tick_current();
133134

134135
/* Find requested object */
@@ -306,6 +307,7 @@ static int co_sdo_rx_download_init_req (
306307
job->type = CO_JOB_SDO_DOWNLOAD;
307308
job->sdo.index = co_fetch_uint16 (&data[1]);
308309
job->sdo.subindex = data[3];
310+
job->sdo.cached = false;
309311
job->timestamp = os_tick_current();
310312

311313
/* Find requested object */
@@ -466,34 +468,34 @@ static int co_sdo_rx_download_seg_req (
466468
/* Write complete */
467469
job->type = CO_JOB_NONE;
468470

469-
if (job->sdo.cached)
471+
/* Find requested object */
472+
obj = co_obj_find (net, job->sdo.index);
473+
if (obj == NULL)
470474
{
471-
/* Find requested object */
472-
obj = co_obj_find (net, job->sdo.index);
473-
if (obj == NULL)
474-
{
475-
co_sdo_abort (
476-
net,
477-
0x580 + net->node,
478-
job->sdo.index,
479-
job->sdo.subindex,
480-
CO_SDO_ABORT_BAD_INDEX);
481-
return -1;
482-
}
475+
co_sdo_abort (
476+
net,
477+
0x580 + net->node,
478+
job->sdo.index,
479+
job->sdo.subindex,
480+
CO_SDO_ABORT_BAD_INDEX);
481+
return -1;
482+
}
483483

484-
/* Find requested subindex */
485-
entry = co_entry_find (net, obj, job->sdo.subindex);
486-
if (entry == NULL)
487-
{
488-
co_sdo_abort (
489-
net,
490-
0x580 + net->node,
491-
job->sdo.index,
492-
job->sdo.subindex,
493-
CO_SDO_ABORT_BAD_SUBINDEX);
494-
return -1;
495-
}
484+
/* Find requested subindex */
485+
entry = co_entry_find (net, obj, job->sdo.subindex);
486+
if (entry == NULL)
487+
{
488+
co_sdo_abort (
489+
net,
490+
0x580 + net->node,
491+
job->sdo.index,
492+
job->sdo.subindex,
493+
CO_SDO_ABORT_BAD_SUBINDEX);
494+
return -1;
495+
}
496496

497+
if (job->sdo.cached)
498+
{
497499
/* Atomically set value */
498500
abort =
499501
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 (
508510
return -1;
509511
}
510512
}
513+
else
514+
{
515+
co_od_notify (net, obj, entry, job->sdo.subindex);
516+
}
511517
}
512518

513519
/* Segmented response */

‎test/test_pdo.cpp‎

Lines changed: 96 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -152,6 +152,27 @@ TEST_F (PdoTest, PackWithPadding)
152152
EXPECT_EQ (1u, frame[1]);
153153
}
154154

155+
TEST_F (PdoTest, PackArray)
156+
{
157+
co_pdo_t pdo;
158+
uint8_t * frame = (uint8_t *)&pdo.frame;
159+
const co_obj_t * obj2000 = find_obj (0x2000);
160+
161+
memset (&pdo, 0, sizeof (pdo));
162+
163+
pdo.number_of_mappings = 2;
164+
pdo.mappings[0] = 0x20000308;
165+
pdo.mappings[1] = 0x20000708;
166+
pdo.entries[0] = find_entry (obj2000, 3);
167+
pdo.entries[1] = find_entry (obj2000, 7);
168+
pdo.objs[0] = obj2000;
169+
pdo.objs[1] = obj2000;
170+
171+
co_pdo_pack (&net, &pdo);
172+
EXPECT_EQ (3u, frame[0]);
173+
EXPECT_EQ (7u, frame[1]);
174+
}
175+
155176
TEST_F (PdoTest, Unpack)
156177
{
157178
co_pdo_t pdo;
@@ -236,6 +257,31 @@ TEST_F (PdoTest, UnpackWithPadding)
236257
EXPECT_EQ (0x3322u, value6003_07);
237258
}
238259

260+
TEST_F (PdoTest, UnpackArray)
261+
{
262+
co_pdo_t pdo;
263+
uint8_t * frame = (uint8_t *)&pdo.frame;
264+
const co_obj_t * obj2000 = find_obj (0x2000);
265+
266+
memset (&pdo, 0, sizeof (pdo));
267+
268+
pdo.number_of_mappings = 2;
269+
pdo.mappings[0] = 0x20000308;
270+
pdo.mappings[1] = 0x20000708;
271+
pdo.entries[0] = find_entry (obj2000, 3);
272+
pdo.entries[1] = find_entry (obj2000, 7);
273+
pdo.objs[0] = obj2000;
274+
pdo.objs[1] = obj2000;
275+
276+
frame[0] = 0x00;
277+
frame[1] = 0x11;
278+
279+
co_pdo_unpack (&net, &pdo);
280+
281+
EXPECT_EQ (0x00u, arr2000[2]);
282+
EXPECT_EQ (0x11u, arr2000[6]);
283+
}
284+
239285
TEST_F (PdoTest, CommParamsSet)
240286
{
241287
const co_obj_t * obj1400 = find_obj (0x1400);
@@ -811,3 +857,53 @@ TEST_F (PdoTest, SparsePdo)
811857
EXPECT_EQ (0u, result);
812858
EXPECT_EQ (0x1234u, value);
813859
}
860+
861+
TEST_F (PdoTest, RPDOMonitoring)
862+
{
863+
uint8_t pdo[][4] = {
864+
{0x11, 0x22, 0x33, 0x44},
865+
};
866+
867+
net.state = STATE_OP;
868+
869+
net.pdo_rx[0].cobid = 0x201;
870+
net.pdo_rx[0].event_timer = 100;
871+
872+
// Arm RPDO deadline monitoring
873+
co_pdo_rx (&net, 0x201, pdo[0], sizeof (pdo[0]));
874+
EXPECT_TRUE (net.pdo_rx[0].rpdo_monitoring);
875+
876+
// Receive PDO, timer has not expired. Rearm timer.
877+
mock_os_tick_current_result = 50 * 1000;
878+
co_pdo_rx (&net, 0x201, pdo[0], sizeof (pdo[0]));
879+
EXPECT_EQ (0u, mock_co_emcy_tx_calls);
880+
881+
// Timer has not expired
882+
mock_os_tick_current_result = 149 * 1000;
883+
co_pdo_timer (&net, mock_os_tick_current_result);
884+
EXPECT_EQ (0u, mock_co_emcy_tx_calls);
885+
886+
// Timer has expired, should generate EMCY
887+
mock_os_tick_current_result = 150 * 1000;
888+
co_pdo_timer (&net, mock_os_tick_current_result);
889+
EXPECT_EQ (1u, mock_co_emcy_tx_calls);
890+
EXPECT_EQ (0x8250, mock_co_emcy_tx_code);
891+
EXPECT_FALSE (net.pdo_rx[0].rpdo_monitoring);
892+
893+
// Timer still expired, should not generate EMCY
894+
mock_os_tick_current_result = 151 * 1000;
895+
co_pdo_timer (&net, mock_os_tick_current_result);
896+
EXPECT_EQ (1u, mock_co_emcy_tx_calls);
897+
EXPECT_EQ (0x8250, mock_co_emcy_tx_code);
898+
EXPECT_FALSE (net.pdo_rx[0].rpdo_monitoring);
899+
900+
// Receive PDO. Rearm timer.
901+
mock_os_tick_current_result = 160 * 1000;
902+
co_pdo_rx (&net, 0x201, pdo[0], sizeof (pdo[0]));
903+
EXPECT_EQ (1u, mock_co_emcy_tx_calls);
904+
905+
// Receive PDO, timer has not expired
906+
mock_os_tick_current_result = 259 * 1000;
907+
co_pdo_rx (&net, 0x201, pdo[0], sizeof (pdo[0]));
908+
EXPECT_EQ (1u, mock_co_emcy_tx_calls);
909+
}

‎test/test_sdo_client.cpp‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -58,7 +58,7 @@ TEST_F (SdoClientTest, ExpeditedUpload)
5858

5959
TEST_F (SdoClientTest, ExpeditedDownload)
6060
{
61-
co_job_t job;
61+
co_job_t job{};
6262
uint16_t value = 0;
6363

6464
uint8_t expected[][8] = {
@@ -136,7 +136,7 @@ TEST_F (SdoClientTest, SegmentedUpload)
136136

137137
TEST_F (SdoClientTest, SegmentedDownload)
138138
{
139-
co_job_t job;
139+
co_job_t job{};
140140
const char * s = "hello world";
141141

142142
uint8_t expected[][8] = {

0 commit comments

Comments
 (0)