Skip to content

Commit

Permalink
chore: catchup to a5be805
Browse files Browse the repository at this point in the history 
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
  • Loading branch information
rt-labs bot committed Jul 2, 2024
1 parent 92e646e commit a05a8a7
Show file tree
Hide file tree
Showing 11 changed files with 201 additions and 33 deletions.
2 changes: 1 addition & 1 deletion src/co_emcy.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
}
Expand Down
2 changes: 2 additions & 0 deletions src/co_main.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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;
Expand Down
3 changes: 3 additions & 0 deletions src/co_main.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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];
Expand Down Expand Up @@ -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;

Expand Down
2 changes: 1 addition & 1 deletion src/co_od.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand Down
17 changes: 17 additions & 0 deletions src/co_od.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
47 changes: 45 additions & 2 deletions src/co_pdo.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand All @@ -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;
Expand Down Expand Up @@ -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;
}

Expand Down Expand Up @@ -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;
Expand All @@ -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;
}

Expand Down Expand Up @@ -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 */
Expand Down
56 changes: 31 additions & 25 deletions src/co_sdo_server.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 */
Expand Down Expand Up @@ -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 */
Expand Down Expand Up @@ -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);
Expand All @@ -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 */
Expand Down
96 changes: 96 additions & 0 deletions test/test_pdo.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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);
}
4 changes: 2 additions & 2 deletions test/test_sdo_client.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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] = {
Expand Down Expand Up @@ -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] = {
Expand Down
Loading

0 comments on commit a05a8a7

Please sign in to comment.