Add RPDO deadline monitoring

As described in CiA 301, Object 1400h to 15FFh: RPDO communication
parameter, Subindex 05h.

The indication to the application is done via the EMCY callback. The
EMCY transmission is not described, but since there is an EMCY code
defined for RPDO timeout, it's assumed to be meant for this purpose.

Change-Id: Ie98f67fb20b615d56fdbdfc207b38bfeef6d4fb6

Author: nattgris

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);
    }

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;
 

src/co_pdo.c

@@ -142,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;
 }
 
@@ -555,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;
@@ -577,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;
 }
 
@@ -799,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 */