pubsub_interrupt_publish.c 13 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323
  1. /* This work is licensed under a Creative Commons CCZero 1.0 Universal License.
  2. * See http://creativecommons.org/publicdomain/zero/1.0/ for more information.
  3. *
  4. * Copyright 2018-2019 (c) Kalycito Infotech
  5. * Copyright 2019 (c) Fraunhofer IOSB (Author: Andreas Ebner)
  6. * Copyright 2019 (c) Fraunhofer IOSB (Author: Julius Pfrommer)
  7. */
  8. #include <signal.h>
  9. #include <stdio.h>
  10. #include <time.h>
  11. #include <open62541/server.h>
  12. #include <open62541/server_config_default.h>
  13. #include <open62541/plugin/log_stdout.h>
  14. #include <open62541/plugin/pubsub_ethernet.h>
  15. #include "bufmalloc.h"
  16. #define ETH_PUBLISH_ADDRESS "opc.eth://0a-00-27-00-00-08"
  17. #define ETH_INTERFACE "enp0s8"
  18. #define MAX_MEASUREMENTS 10000
  19. #define MILLI_AS_NANO_SECONDS (1000 * 1000)
  20. #define SECONDS_AS_NANO_SECONDS (1000 * 1000 * 1000)
  21. #define CLOCKID CLOCK_REALTIME
  22. #define SIG SIGUSR1
  23. #define PUB_INTERVAL 0.25 /* Publish interval in milliseconds */
  24. #define DATA_SET_WRITER_ID 62541
  25. #define MEASUREMENT_OUTPUT "publisher_measurement.csv"
  26. UA_NodeId counterNodePublisher = {1, UA_NODEIDTYPE_NUMERIC, {1234}};
  27. UA_Int64 pubIntervalNs;
  28. UA_ServerCallback pubCallback = NULL;
  29. UA_Server *pubServer;
  30. UA_Boolean running = true;
  31. void *pubData;
  32. timer_t pubEventTimer;
  33. struct sigevent pubEvent;
  34. struct sigaction signalAction;
  35. /* Arrays to store measurement data */
  36. UA_Int32 currentPublishCycleTime[MAX_MEASUREMENTS+1];
  37. struct timespec calculatedCycleStartTime[MAX_MEASUREMENTS+1];
  38. struct timespec cycleStartDelay[MAX_MEASUREMENTS+1];
  39. struct timespec cycleDuration[MAX_MEASUREMENTS+1];
  40. size_t publisherMeasurementsCounter = 0;
  41. static void
  42. timespec_diff(struct timespec *start, struct timespec *stop,
  43. struct timespec *result) {
  44. if((stop->tv_nsec - start->tv_nsec) < 0) {
  45. result->tv_sec = stop->tv_sec - start->tv_sec - 1;
  46. result->tv_nsec = stop->tv_nsec - start->tv_nsec + 1000000000;
  47. } else {
  48. result->tv_sec = stop->tv_sec - start->tv_sec;
  49. result->tv_nsec = stop->tv_nsec - start->tv_nsec;
  50. }
  51. }
  52. /* Used to adjust the nanosecond > 1s field value */
  53. static void
  54. nanoSecondFieldConversion(struct timespec *timeSpecValue) {
  55. while(timeSpecValue->tv_nsec > (SECONDS_AS_NANO_SECONDS - 1)) {
  56. timeSpecValue->tv_sec += 1;
  57. timeSpecValue->tv_nsec -= SECONDS_AS_NANO_SECONDS;
  58. }
  59. }
  60. /* Signal handler */
  61. static void
  62. publishInterrupt(int sig, siginfo_t* si, void* uc) {
  63. if(si->si_value.sival_ptr != &pubEventTimer) {
  64. UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_USERLAND, "stray signal");
  65. return;
  66. }
  67. /* Execute the publish callback in the interrupt */
  68. struct timespec begin, end;
  69. clock_gettime(CLOCKID, &begin);
  70. useMembufAlloc();
  71. pubCallback(pubServer, pubData);
  72. useNormalAlloc();
  73. clock_gettime(CLOCKID, &end);
  74. if(publisherMeasurementsCounter >= MAX_MEASUREMENTS)
  75. return;
  76. /* Save current configured publish interval */
  77. currentPublishCycleTime[publisherMeasurementsCounter] = pubIntervalNs;
  78. /* Save the difference to the calculated time */
  79. timespec_diff(&calculatedCycleStartTime[publisherMeasurementsCounter],
  80. &begin, &cycleStartDelay[publisherMeasurementsCounter]);
  81. /* Save the duration of the publish callback */
  82. timespec_diff(&begin, &end, &cycleDuration[publisherMeasurementsCounter]);
  83. /* Save the calculated starting time for the next cycle */
  84. calculatedCycleStartTime[publisherMeasurementsCounter + 1].tv_nsec +=
  85. calculatedCycleStartTime[publisherMeasurementsCounter].tv_nsec + pubIntervalNs;
  86. calculatedCycleStartTime[publisherMeasurementsCounter + 1].tv_sec =
  87. calculatedCycleStartTime[publisherMeasurementsCounter].tv_sec;
  88. nanoSecondFieldConversion(&calculatedCycleStartTime[publisherMeasurementsCounter + 1]);
  89. publisherMeasurementsCounter++;
  90. if(publisherMeasurementsCounter == MAX_MEASUREMENTS) {
  91. /* Write the pubsub measurement data */
  92. FILE *fpPublisher = fopen(MEASUREMENT_OUTPUT, "w");
  93. for(UA_UInt32 i = 0; i < publisherMeasurementsCounter; i++) {
  94. fprintf(fpPublisher, "%u, %u, %ld.%09ld, %ld.%09ld, %ld.%09ld\n",
  95. i,
  96. currentPublishCycleTime[i],
  97. calculatedCycleStartTime[i].tv_sec,
  98. calculatedCycleStartTime[i].tv_nsec,
  99. cycleStartDelay[i].tv_sec,
  100. cycleStartDelay[i].tv_nsec,
  101. cycleDuration[i].tv_sec,
  102. cycleDuration[i].tv_nsec);
  103. }
  104. fclose(fpPublisher);
  105. }
  106. }
  107. /* The following three methods are originally defined in
  108. * /src/pubsub/ua_pubsub_manager.c. We provide a custom implementation here to
  109. * use system interrupts instead if time-triggered callbacks in the OPC UA
  110. * server control flow. */
  111. UA_StatusCode
  112. UA_PubSubManager_addRepeatedCallback(UA_Server *server,
  113. UA_ServerCallback callback,
  114. void *data, UA_Double interval_ms,
  115. UA_UInt64 *callbackId) {
  116. if(pubCallback) {
  117. UA_LOG_WARNING(UA_Log_Stdout, UA_LOGCATEGORY_USERLAND,
  118. "At most one publisher can be registered for interrupt callbacks");
  119. return UA_STATUSCODE_BADINTERNALERROR;
  120. }
  121. /* Set global values for the publish callback */
  122. int resultTimerCreate = 0;
  123. pubServer = server;
  124. pubCallback = callback;
  125. pubData = data;
  126. pubIntervalNs = (UA_Int64) (interval_ms * MILLI_AS_NANO_SECONDS);
  127. /* Handle the signal */
  128. memset(&signalAction, 0, sizeof(signalAction));
  129. signalAction.sa_flags = SA_SIGINFO;
  130. signalAction.sa_sigaction = publishInterrupt;
  131. sigemptyset(&signalAction.sa_mask);
  132. sigaction(SIG, &signalAction, NULL);
  133. /* Create the timer */
  134. memset(&pubEventTimer, 0, sizeof(pubEventTimer));
  135. pubEvent.sigev_notify = SIGEV_SIGNAL;
  136. pubEvent.sigev_signo = SIG;
  137. pubEvent.sigev_value.sival_ptr = &pubEventTimer;
  138. resultTimerCreate = timer_create(CLOCKID, &pubEvent, &pubEventTimer);
  139. if(resultTimerCreate != 0) {
  140. UA_LOG_WARNING(UA_Log_Stdout, UA_LOGCATEGORY_USERLAND,
  141. "Failed to create a system event");
  142. return UA_STATUSCODE_BADINTERNALERROR;
  143. }
  144. /* Arm the timer */
  145. struct itimerspec timerspec;
  146. timerspec.it_interval.tv_sec = (long int) (pubIntervalNs / (SECONDS_AS_NANO_SECONDS));
  147. timerspec.it_interval.tv_nsec = (long int) (pubIntervalNs % SECONDS_AS_NANO_SECONDS);
  148. timerspec.it_value.tv_sec = (long int) (pubIntervalNs / (SECONDS_AS_NANO_SECONDS));
  149. timerspec.it_value.tv_nsec = (long int) (pubIntervalNs % SECONDS_AS_NANO_SECONDS);
  150. resultTimerCreate = timer_settime(pubEventTimer, 0, &timerspec, NULL);
  151. if(resultTimerCreate != 0) {
  152. UA_LOG_WARNING(UA_Log_Stdout, UA_LOGCATEGORY_USERLAND,
  153. "Failed to arm the system timer");
  154. timer_delete(pubEventTimer);
  155. return UA_STATUSCODE_BADINTERNALERROR;
  156. }
  157. /* Start taking measurements */
  158. publisherMeasurementsCounter = 0;
  159. clock_gettime(CLOCKID, &calculatedCycleStartTime[0]);
  160. calculatedCycleStartTime[0].tv_nsec += pubIntervalNs;
  161. nanoSecondFieldConversion(&calculatedCycleStartTime[0]);
  162. return UA_STATUSCODE_GOOD;
  163. }
  164. UA_StatusCode
  165. UA_PubSubManager_changeRepeatedCallbackInterval(UA_Server *server,
  166. UA_UInt64 callbackId,
  167. UA_Double interval_ms) {
  168. struct itimerspec timerspec;
  169. int resultTimerCreate = 0;
  170. pubIntervalNs = (UA_Int64) (interval_ms * MILLI_AS_NANO_SECONDS);
  171. timerspec.it_interval.tv_sec = (long int) (pubIntervalNs % SECONDS_AS_NANO_SECONDS);
  172. timerspec.it_interval.tv_nsec = (long int) (pubIntervalNs % SECONDS_AS_NANO_SECONDS);
  173. timerspec.it_value.tv_sec = (long int) (pubIntervalNs / (SECONDS_AS_NANO_SECONDS));
  174. timerspec.it_value.tv_nsec = (long int) (pubIntervalNs % SECONDS_AS_NANO_SECONDS);
  175. resultTimerCreate = timer_settime(pubEventTimer, 0, &timerspec, NULL);
  176. if(resultTimerCreate != 0) {
  177. UA_LOG_WARNING(UA_Log_Stdout, UA_LOGCATEGORY_USERLAND,
  178. "Failed to arm the system timer");
  179. timer_delete(pubEventTimer);
  180. return UA_STATUSCODE_BADINTERNALERROR;
  181. }
  182. clock_gettime(CLOCKID, &calculatedCycleStartTime[publisherMeasurementsCounter]);
  183. calculatedCycleStartTime[publisherMeasurementsCounter].tv_nsec += pubIntervalNs;
  184. nanoSecondFieldConversion(&calculatedCycleStartTime[publisherMeasurementsCounter]);
  185. return UA_STATUSCODE_GOOD;
  186. }
  187. void
  188. UA_PubSubManager_removeRepeatedPubSubCallback(UA_Server *server, UA_UInt64 callbackId) {
  189. timer_delete(pubEventTimer);
  190. pubCallback = NULL; /* So that a new callback can be registered */
  191. }
  192. static void
  193. addPubSubConfiguration(UA_Server* server) {
  194. UA_NodeId connectionIdent;
  195. UA_NodeId publishedDataSetIdent;
  196. UA_NodeId writerGroupIdent;
  197. UA_PubSubConnectionConfig connectionConfig;
  198. memset(&connectionConfig, 0, sizeof(connectionConfig));
  199. connectionConfig.name = UA_STRING("UDP-UADP Connection 1");
  200. connectionConfig.transportProfileUri =
  201. UA_STRING("http://opcfoundation.org/UA-Profile/Transport/pubsub-eth-uadp");
  202. connectionConfig.enabled = true;
  203. UA_NetworkAddressUrlDataType networkAddressUrl =
  204. {UA_STRING(ETH_INTERFACE), UA_STRING(ETH_PUBLISH_ADDRESS)};
  205. UA_Variant_setScalar(&connectionConfig.address, &networkAddressUrl,
  206. &UA_TYPES[UA_TYPES_NETWORKADDRESSURLDATATYPE]);
  207. connectionConfig.publisherId.numeric = UA_UInt32_random();
  208. UA_Server_addPubSubConnection(server, &connectionConfig, &connectionIdent);
  209. UA_PublishedDataSetConfig publishedDataSetConfig;
  210. memset(&publishedDataSetConfig, 0, sizeof(UA_PublishedDataSetConfig));
  211. publishedDataSetConfig.publishedDataSetType = UA_PUBSUB_DATASET_PUBLISHEDITEMS;
  212. publishedDataSetConfig.name = UA_STRING("Demo PDS");
  213. /* Create new PublishedDataSet based on the PublishedDataSetConfig. */
  214. UA_Server_addPublishedDataSet(server, &publishedDataSetConfig,
  215. &publishedDataSetIdent);
  216. UA_NodeId dataSetFieldIdentCounter;
  217. UA_DataSetFieldConfig counterValue;
  218. memset(&counterValue, 0, sizeof(UA_DataSetFieldConfig));
  219. counterValue.dataSetFieldType = UA_PUBSUB_DATASETFIELD_VARIABLE;
  220. counterValue.field.variable.fieldNameAlias = UA_STRING ("Counter Variable 1");
  221. counterValue.field.variable.promotedField = UA_FALSE;
  222. counterValue.field.variable.publishParameters.publishedVariable = counterNodePublisher;
  223. counterValue.field.variable.publishParameters.attributeId = UA_ATTRIBUTEID_VALUE;
  224. UA_Server_addDataSetField(server, publishedDataSetIdent, &counterValue,
  225. &dataSetFieldIdentCounter);
  226. UA_WriterGroupConfig writerGroupConfig;
  227. memset(&writerGroupConfig, 0, sizeof(UA_WriterGroupConfig));
  228. writerGroupConfig.name = UA_STRING("Demo WriterGroup");
  229. writerGroupConfig.publishingInterval = PUB_INTERVAL;
  230. writerGroupConfig.enabled = UA_FALSE;
  231. writerGroupConfig.encodingMimeType = UA_PUBSUB_ENCODING_UADP;
  232. UA_Server_addWriterGroup(server, connectionIdent,
  233. &writerGroupConfig, &writerGroupIdent);
  234. UA_NodeId dataSetWriterIdent;
  235. UA_DataSetWriterConfig dataSetWriterConfig;
  236. memset(&dataSetWriterConfig, 0, sizeof(UA_DataSetWriterConfig));
  237. dataSetWriterConfig.name = UA_STRING("Demo DataSetWriter");
  238. dataSetWriterConfig.dataSetWriterId = DATA_SET_WRITER_ID;
  239. dataSetWriterConfig.keyFrameCount = 10;
  240. UA_Server_addDataSetWriter(server, writerGroupIdent, publishedDataSetIdent,
  241. &dataSetWriterConfig, &dataSetWriterIdent);
  242. }
  243. static void
  244. addServerNodes(UA_Server* server) {
  245. UA_UInt64 publishValue = 0;
  246. UA_VariableAttributes publisherAttr = UA_VariableAttributes_default;
  247. UA_Variant_setScalar(&publisherAttr.value, &publishValue, &UA_TYPES[UA_TYPES_UINT64]);
  248. publisherAttr.displayName = UA_LOCALIZEDTEXT("en-US", "Publisher Counter");
  249. publisherAttr.accessLevel = UA_ACCESSLEVELMASK_READ | UA_ACCESSLEVELMASK_WRITE;
  250. UA_Server_addVariableNode(server, counterNodePublisher,
  251. UA_NODEID_NUMERIC(0, UA_NS0ID_OBJECTSFOLDER),
  252. UA_NODEID_NUMERIC(0, UA_NS0ID_ORGANIZES),
  253. UA_QUALIFIEDNAME(1, "Publisher Counter"),
  254. UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE),
  255. publisherAttr, NULL, NULL);
  256. }
  257. /* Stop signal */
  258. static void stopHandler(int sign) {
  259. UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "received ctrl-c");
  260. running = UA_FALSE;
  261. }
  262. int main(void) {
  263. signal(SIGINT, stopHandler);
  264. signal(SIGTERM, stopHandler);
  265. UA_ServerConfig* config = UA_ServerConfig_new_minimal(4840, NULL);
  266. config->pubsubTransportLayers = (UA_PubSubTransportLayer *)
  267. UA_malloc(sizeof(UA_PubSubTransportLayer));
  268. if(!config->pubsubTransportLayers) {
  269. UA_ServerConfig_delete(config);
  270. return -1;
  271. }
  272. config->pubsubTransportLayers[0] = UA_PubSubTransportLayerEthernet();
  273. config->pubsubTransportLayersSize++;
  274. UA_Server *server = UA_Server_new(config);
  275. addServerNodes(server);
  276. addPubSubConfiguration(server);
  277. /* Run the server */
  278. UA_StatusCode retval = UA_Server_run(server, &running);
  279. UA_Server_delete(server);
  280. UA_ServerConfig_delete(config);
  281. return (int)retval;
  282. }