Merge pull request #33 from nattgris/pdo-trigger

Make it possible to trigger only PDOs for a specific object

Author: hefloryd

include/co_api.h

@@ -394,6 +394,32 @@ CO_EXPORT void co_nmt (co_client_t * client, co_nmt_cmd_t cmd, uint8_t node);
  */
 CO_EXPORT void co_sync (co_client_t * client);
 
+/**
+ * Trigger event-based PDOs
+ *
+ * This function triggers transmission of all event-based PDOs.
+ *
+ * @param client  client handle
+ *
+ * @return 0 on success
+ */
+CO_EXPORT int co_pdo_event (co_client_t * client);
+
+/**
+ * Triggers event-based PDOs containing a specific object
+ *
+ * This function triggers transmission of all event-based PDOs
+ * that map the specified object.
+ *
+ * @param client    client handle
+ * @param index     index
+ * @param subindex  subindex
+ *
+ * @return 0 on success
+ */
+CO_EXPORT int co_pdo_obj_event (co_client_t * client, uint16_t index,
+                                uint8_t subindex);
+
 /**
  * Read dictionary object entry
  *

src/co_main.c

@@ -130,6 +130,7 @@ void co_main (void * arg)
          co_handle_rx (net);
          break;
       case CO_JOB_PDO_EVENT:
+      case CO_JOB_PDO_OBJ_EVENT:
          co_pdo_job (net, job);
          break;
       case CO_JOB_SDO_READ:
@@ -225,6 +226,21 @@ int co_pdo_event (co_client_t * client)
    return 0;
 }
 
+int co_pdo_obj_event (co_client_t * client, uint16_t index, uint8_t subindex)
+{
+   co_net_t * net = client->net;
+   co_job_t * job = &client->job;
+
+   job->client       = client;
+   job->callback     = NULL;
+   job->type         = CO_JOB_PDO_OBJ_EVENT;
+   job->pdo.index    = index;
+   job->pdo.subindex = subindex;
+
+   os_mbox_post (net->mbox, job, OS_WAIT_FOREVER);
+   return 0;
+}
+
 int co_sdo_read (
    co_client_t * client,
    uint8_t node,

src/co_main.h

@@ -94,6 +94,7 @@ typedef enum co_job_type
    CO_JOB_PERIODIC,
    CO_JOB_RX,
    CO_JOB_PDO_EVENT,
+   CO_JOB_PDO_OBJ_EVENT,
    CO_JOB_SDO_READ,
    CO_JOB_SDO_WRITE,
    CO_JOB_SDO_UPLOAD,
@@ -131,6 +132,13 @@ typedef struct co_emcy_job
    uint8_t value;
 } co_emcy_job_t;
 
+/** Parameters for PDO job */
+typedef struct co_pdo_job
+{
+   uint16_t index;
+   uint8_t subindex;
+} co_pdo_job_t;
+
 /** Generic job */
 typedef struct co_job
 {
@@ -139,6 +147,7 @@ typedef struct co_job
    {
       co_sdo_job_t sdo;
       co_emcy_job_t emcy;
+      co_pdo_job_t pdo;
    };
    uint32_t timestamp;
    struct co_client * client;

src/co_pdo.c

@@ -594,6 +594,52 @@ void co_pdo_trigger (co_net_t * net)
    }
 }
 
+void co_pdo_trigger_with_obj (co_net_t * net, uint16_t index, uint8_t subindex)
+{
+   unsigned int ix;
+   uint8_t n;
+   const co_obj_t * obj;
+   const co_entry_t * entry;
+
+   /* Find mapped object */
+   obj = co_obj_find (net, index);
+   if (obj == NULL)
+      return;
+
+   /* Find mapped entry */
+   entry = co_entry_find (net, obj, subindex);
+   if (entry == NULL)
+      return;
+
+   /* Check that object is mappable */
+   if ((entry->flags & OD_TPDO) == 0)
+      return;
+
+   /* Transmit event-driven TPDOs, queue acyclic TPDOs */
+   for (ix = 0; ix < MAX_TX_PDO; ix++)
+   {
+      co_pdo_t * pdo = &net->pdo_tx[ix];
+      if (pdo->cobid & CO_COBID_INVALID)
+         continue;
+         
+      for (n = 0; n < pdo->number_of_mappings; n++)
+      {
+         if (pdo->entries[n] == entry)
+         {
+            if (IS_EVENT (pdo->transmission_type))
+            {
+               co_pdo_transmit (net, pdo);
+            }
+            else if (IS_ACYCLIC (pdo->transmission_type))
+            {
+               pdo->queued = true;
+            }
+            break;
+         }
+      }
+   }
+}
+
 int co_pdo_sync (co_net_t * net, uint8_t * msg, size_t dlc)
 {
    unsigned int ix;
@@ -761,6 +807,9 @@ void co_pdo_job (co_net_t * net, co_job_t * job)
    case CO_JOB_PDO_EVENT:
       co_pdo_trigger (net);
       break;
+   case CO_JOB_PDO_OBJ_EVENT:
+      co_pdo_trigger_with_obj (net, job->pdo.index, job->pdo.subindex);
+      break;
    default:
       CC_ASSERT (0);
    }

src/co_pdo.h

@@ -116,6 +116,20 @@ int co_pdo_timer (co_net_t * net, uint32_t now);
  */
 void co_pdo_trigger (co_net_t * net);
 
+/**
+ * PDO trigger with object
+ *
+ * This function triggers an event on event-driven and acyclic
+ * TPDOs that map the specified object. Event-driven TPDOs will be
+ * transmitted immediately while acyclic TPDOs will be queued for
+ * transmission at next SYNC.
+ *
+ * @param net           network handle
+ * @param index         index
+ * @param subindex      subindex
+ */
+void co_pdo_trigger_with_obj (co_net_t * net, uint16_t index, uint8_t subindex);
+
 /**
  * Start PDO job
  *

test/test_pdo.cpp

@@ -728,6 +728,39 @@ TEST_F (PdoTest, TxAcyclic)
    EXPECT_EQ (0x181u, mock_os_channel_send_id);
 }
 
+TEST_F (PdoTest, TriggerWithObj)
+{
+   net.state = STATE_OP;
+
+   net.pdo_tx[0].transmission_type = 0xFF;
+   net.pdo_tx[0].inhibit_time      = 0;
+
+   // Should trigger PDO
+   mock_co_obj_find_result   = find_obj (0x6000);
+   mock_co_entry_find_result = find_entry (mock_co_obj_find_result, 0);
+   co_pdo_trigger_with_obj (&net, 0x6000, 0);
+   EXPECT_EQ (0x1u, mock_os_channel_send_calls);
+
+   // Should not trigger PDO, not mapped
+   mock_co_obj_find_result   = find_obj (0x6001);
+   mock_co_entry_find_result = find_entry (mock_co_obj_find_result, 0);
+   co_pdo_trigger_with_obj (&net, 0x6001, 0);
+   EXPECT_EQ (0x1u, mock_os_channel_send_calls);
+
+   // Should not trigger PDO, does not exist
+   mock_co_obj_find_result   = NULL;
+   mock_co_entry_find_result = NULL;
+   co_pdo_trigger_with_obj (&net, 0x6002, 0);
+   EXPECT_EQ (0x1u, mock_os_channel_send_calls);
+
+   // Should not trigger PDO, invalid cob-id
+   net.pdo_tx[0].cobid = CO_COBID_INVALID | 0x181;
+   mock_co_obj_find_result   = find_obj (0x6000);
+   mock_co_entry_find_result = find_entry (mock_co_obj_find_result, 0);
+   co_pdo_trigger_with_obj (&net, 0x6000, 0);
+   EXPECT_EQ (0x1u, mock_os_channel_send_calls);
+}
+
 TEST_F (PdoTest, SparsePdo)
 {
    const co_obj_t * obj1533 = find_obj (0x1533);

test/test_util.h

@@ -128,6 +128,10 @@ class TestBase : public ::testing::Test
       {0, OD_NOTIFY | OD_RO | OD_TPDO, DTYPE_UNSIGNED32, 32, 0, &value6000},
    };
 
+   const co_entry_t OD6001[1] = {
+      {0, OD_NOTIFY | OD_RO | OD_TPDO, DTYPE_UNSIGNED32, 32, 1, NULL},
+   };
+
    uint16_t value6003_07;
    uint8_t value6003_08;
    uint32_t value6003_09;
@@ -159,7 +163,7 @@ class TestBase : public ::testing::Test
       {0, OD_RW | OD_RPDO, DTYPE_UNSIGNED32, 32, 0, &value7000},
    };
 
-   const co_obj_t test_od[36] = {
+   const co_obj_t test_od[37] = {
       // clang-format off
       {0x1000, OTYPE_VAR,    0,               OD1000, NULL},
       {0x1001, OTYPE_VAR,    0,               OD1001, co_od1001_fn},
@@ -194,6 +198,7 @@ class TestBase : public ::testing::Test
       {0x2000, OTYPE_ARRAY,  8,               OD2000, NULL},
       {0x2001, OTYPE_RECORD, 2,               OD2001, cb2001},
       {0x6000, OTYPE_VAR,    0,               OD6000, NULL},
+      {0x6001, OTYPE_VAR,    0,               OD6001, NULL},
       {0x6003, OTYPE_RECORD, 12,              OD6003, NULL},
       {0x7000, OTYPE_VAR,    0,               OD7000, NULL},
       {0, OTYPE_NULL, 0, NULL, NULL},