Commit ce1c4016 authored by Léo-Paul Géneau's avatar Léo-Paul Géneau 👾

Merge publishing and subscribing servers

parent 1b893278
...@@ -4,8 +4,8 @@ LIBS=-lstdc++ -lmavsdk -lmavsdk_action -lmavsdk_mavlink_passthrough -lmavsdk_tel ...@@ -4,8 +4,8 @@ LIBS=-lstdc++ -lmavsdk -lmavsdk_action -lmavsdk_mavlink_passthrough -lmavsdk_tel
LIB_NAME := libqjswrapper.so LIB_NAME := libqjswrapper.so
SRCS := mavsdk_wrapper.cpp pubsub_publish.c pubsub_subscribe.c qjs_wrapper.c SRCS := mavsdk_wrapper.cpp pubsub.c qjs_wrapper.c
OBJS := mavsdk_wrapper.o pubsub_publish.o pubsub_subscribe.o qjs_wrapper.o OBJS := mavsdk_wrapper.o pubsub.o qjs_wrapper.o
all: $(LIB_NAME) all: $(LIB_NAME)
......
#ifndef __PUBSUB_COMMON_H__ #ifndef __PUBSUB_H__
#define __PUBSUB_COMMON_H__ #define __PUBSUB_H__
#include <open62541/server.h> #include <open62541/server.h>
#include <open62541/plugin/log_stdout.h> #include <open62541/plugin/log_stdout.h>
#define countof(x) (sizeof(x) / sizeof((x)[0])) #define countof(x) (sizeof(x) / sizeof((x)[0]))
#define DATA_SET_WRITER_ID 1
#define WRITER_GROUP_ID 1
typedef struct { typedef struct {
int id; int id;
UA_Double latitude; UA_Double latitude;
...@@ -24,4 +27,16 @@ typedef struct { ...@@ -24,4 +27,16 @@ typedef struct {
UA_Byte builtInType; UA_Byte builtInType;
} VariableData; } VariableData;
#endif /* __PUBSUB_COMMON_H__ */ UA_StatusCode writeVariable(char *name, void * UA_RESTRICT pvalue,
\ No newline at end of file UA_DataType type);
int runPubsub(UA_String *transportProfile,
UA_NetworkAddressUrlDataType *networkAddressUrl,
VariableData const *variableArray, int nbVariable,
int id, int nbReader,
void (*init_node_id)(UA_UInt32 id, int nb, int magic),
int (*get_reader_id)(int nb),
void (*update)(UA_UInt32 id, const UA_DataValue*),
UA_Boolean *running);
#endif /* __PUBSUB_H__ */
\ No newline at end of file
#ifndef __PUBSUB_PUBLISH_H__
#define __PUBSUB_PUBLISH_H__
#include "pubsub_common.h"
UA_StatusCode writeVariable(char *name, void * UA_RESTRICT pvalue,
UA_DataType type);
int publish(UA_String *transportProfile,
UA_NetworkAddressUrlDataType *networkAddressUrl,
VariableData const *variableArray, int nbVariable,
int id, UA_Boolean *running);
#endif /* __PUBSUB_PUBLISH_H__ */
#ifndef __PUBSUB_SUBSCRIBE_H__
#define __PUBSUB_SUBSCRIBE_H__
#include "pubsub_common.h"
int subscribe(UA_String *transportProfile,
UA_NetworkAddressUrlDataType *networkAddressUrl,
VariableData const *variableArray, int nbVariable, int nbReader,
void (*init_node_id)(UA_UInt32 id, int nb, int magic),
int (*get_reader_id)(int nb),
void (*update)(UA_UInt32 id, const UA_DataValue*),
UA_Boolean *running);
#endif /* __PUBSUB_SUBSCRIBE_H__ */
#include <open62541/client_subscriptions.h> #include <open62541/client_subscriptions.h>
#include <open62541/plugin/pubsub_udp.h>
#include <open62541/server_config_default.h> #include <open62541/server_config_default.h>
#include <open62541/types_generated.h> #include <open62541/types_generated.h>
#include <open62541/plugin/pubsub_udp.h>
#include "ua_pubsub.h" #include "ua_pubsub.h"
#include "pubsub_subscribe.h"
UA_NodeId connectionIdentifier; #include "pubsub.h"
UA_NodeId readerGroupIdentifier;
UA_NodeId readerIdentifier; static UA_Server *server;
UA_NodeId connectionIdent, publishedDataSetIdent, writerGroupIdent,
readerGroupIdent, readerIdent;
UA_DataSetReaderConfig readerConfig; UA_DataSetReaderConfig readerConfig;
...@@ -18,31 +19,147 @@ static void fillDataSetMetaData(UA_DataSetMetaDataType *pMetaData, ...@@ -18,31 +19,147 @@ static void fillDataSetMetaData(UA_DataSetMetaDataType *pMetaData,
VariableData const *variableArray, VariableData const *variableArray,
int nbVariable, int id); int nbVariable, int id);
/* Add new connection to the server */
static UA_StatusCode static UA_StatusCode
addPubSubConnection(UA_Server *server, UA_String *transportProfile, addPubSubConnection(UA_Server *server, UA_String *transportProfile,
UA_NetworkAddressUrlDataType *networkAddressUrl) { UA_NetworkAddressUrlDataType *networkAddressUrl,
int id){
// 18 characters for "UDPMC Connection " + 10 characters for id <= UINT32_MAX
char name[28];
if((server == NULL) || (transportProfile == NULL) || if((server == NULL) || (transportProfile == NULL) ||
(networkAddressUrl == NULL)) { (networkAddressUrl == NULL)) {
return UA_STATUSCODE_BADINTERNALERROR; return UA_STATUSCODE_BADINTERNALERROR;
} }
UA_StatusCode retval = UA_STATUSCODE_GOOD;
/* Configuration creation for the connection */
UA_PubSubConnectionConfig connectionConfig; UA_PubSubConnectionConfig connectionConfig;
memset (&connectionConfig, 0, sizeof(UA_PubSubConnectionConfig)); memset(&connectionConfig, 0, sizeof(connectionConfig));
connectionConfig.name = UA_STRING("UDPMC Connection 1");
UA_snprintf(name, sizeof(name - 1), "UDPMC Connection %d", id);
connectionConfig.name = UA_STRING(name);
connectionConfig.transportProfileUri = *transportProfile; connectionConfig.transportProfileUri = *transportProfile;
connectionConfig.enabled = UA_TRUE; connectionConfig.enabled = UA_TRUE;
UA_Variant_setScalar(&connectionConfig.address, networkAddressUrl, UA_Variant_setScalar(&connectionConfig.address, networkAddressUrl,
&UA_TYPES[UA_TYPES_NETWORKADDRESSURLDATATYPE]); &UA_TYPES[UA_TYPES_NETWORKADDRESSURLDATATYPE]);
connectionConfig.publisherId.numeric = UA_UInt32_random (); connectionConfig.publisherId.numeric = id;
retval |= UA_Server_addPubSubConnection (server, &connectionConfig, &connectionIdentifier); return UA_Server_addPubSubConnection(server, &connectionConfig, &connectionIdent);
if (retval != UA_STATUSCODE_GOOD) { }
return retval;
}
return retval; /**
* **PublishedDataSet handling**
*
* The PublishedDataSet (PDS) and PubSubConnection are the toplevel entities and
* can exist alone. The PDS contains the collection of the published fields. All
* other PubSub elements are directly or indirectly linked with the PDS or
* connection. */
static void
addPublishedDataSet(UA_Server *server, int id) {
/* The PublishedDataSetConfig contains all necessary public
* information for the creation of a new PublishedDataSet */
char name[8];
UA_snprintf(name, sizeof(name - 1), "PDS %d", id);
UA_PublishedDataSetConfig publishedDataSetConfig;
memset(&publishedDataSetConfig, 0, sizeof(UA_PublishedDataSetConfig));
publishedDataSetConfig.publishedDataSetType = UA_PUBSUB_DATASET_PUBLISHEDITEMS;
publishedDataSetConfig.name = UA_STRING(name);
/* Create new PublishedDataSet based on the PublishedDataSetConfig. */
UA_Server_addPublishedDataSet(server, &publishedDataSetConfig, &publishedDataSetIdent);
}
static UA_StatusCode
addVariable(UA_Server *server, VariableData varDetails) {
UA_VariableAttributes attr = UA_VariableAttributes_default;
UA_Variant_setScalar(&attr.value, varDetails.pdefaultValue, &UA_TYPES[varDetails.type]);
attr.description = UA_LOCALIZEDTEXT("en-US", varDetails.description);
attr.displayName = UA_LOCALIZEDTEXT("en-US", varDetails.description);
attr.dataType = UA_TYPES[varDetails.type].typeId;
attr.accessLevel = UA_ACCESSLEVELMASK_READ | UA_ACCESSLEVELMASK_WRITE;
return UA_Server_addVariableNode(server, UA_NODEID_STRING(1, varDetails.name),
UA_NODEID_NUMERIC(0, UA_NS0ID_OBJECTSFOLDER),
UA_NODEID_NUMERIC(0, UA_NS0ID_ORGANIZES),
UA_QUALIFIEDNAME(1, varDetails.description),
UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE),
attr, NULL, NULL);
}
UA_StatusCode writeVariable(char *name, void * UA_RESTRICT pvalue, UA_DataType type)
{
UA_NodeId varNodeId = UA_NODEID_STRING(1, name);
UA_Variant var;
UA_Variant_init(&var);
UA_Variant_setScalar(&var, pvalue, &type);
return UA_Server_writeValue(server, varNodeId, var);
}
static void
addDataSetField(UA_Server *server, VariableData varDetails) {
UA_NodeId dataSetFieldIdent;
UA_DataSetFieldConfig dataSetFieldConfig;
memset(&dataSetFieldConfig, 0, sizeof(UA_DataSetFieldConfig));
dataSetFieldConfig.dataSetFieldType = UA_PUBSUB_DATASETFIELD_VARIABLE;
dataSetFieldConfig.field.variable.fieldNameAlias = UA_STRING(varDetails.description);
dataSetFieldConfig.field.variable.promotedField = UA_FALSE;
dataSetFieldConfig.field.variable.publishParameters.publishedVariable =
UA_NODEID_STRING(1, varDetails.name);
dataSetFieldConfig.field.variable.publishParameters.attributeId = UA_ATTRIBUTEID_VALUE;
UA_Server_addDataSetField(server, publishedDataSetIdent,
&dataSetFieldConfig, &dataSetFieldIdent);
}
/**
* **WriterGroup handling**
*
* The WriterGroup (WG) is part of the connection and contains the primary
* configuration parameters for the message creation. */
static void
addWriterGroup(UA_Server *server) {
/* Now we create a new WriterGroupConfig and add the group to the existing
* PubSubConnection. */
UA_WriterGroupConfig writerGroupConfig;
memset(&writerGroupConfig, 0, sizeof(UA_WriterGroupConfig));
writerGroupConfig.name = UA_STRING("Demo WriterGroup");
writerGroupConfig.publishingInterval = 100;
writerGroupConfig.enabled = UA_FALSE;
writerGroupConfig.writerGroupId = WRITER_GROUP_ID;
writerGroupConfig.encodingMimeType = UA_PUBSUB_ENCODING_UADP;
writerGroupConfig.messageSettings.encoding = UA_EXTENSIONOBJECT_DECODED;
writerGroupConfig.messageSettings.content.decoded.type = &UA_TYPES[UA_TYPES_UADPWRITERGROUPMESSAGEDATATYPE];
/* The configuration flags for the messages are encapsulated inside the
* message- and transport settings extension objects. These extension
* objects are defined by the standard. e.g.
* UadpWriterGroupMessageDataType */
UA_UadpWriterGroupMessageDataType *writerGroupMessage = UA_UadpWriterGroupMessageDataType_new();
/* Change message settings of writerGroup to send PublisherId,
* WriterGroupId in GroupHeader and DataSetWriterId in PayloadHeader
* of NetworkMessage */
writerGroupMessage->networkMessageContentMask = (UA_UadpNetworkMessageContentMask)(UA_UADPNETWORKMESSAGECONTENTMASK_PUBLISHERID |
(UA_UadpNetworkMessageContentMask)UA_UADPNETWORKMESSAGECONTENTMASK_GROUPHEADER |
(UA_UadpNetworkMessageContentMask)UA_UADPNETWORKMESSAGECONTENTMASK_WRITERGROUPID |
(UA_UadpNetworkMessageContentMask)UA_UADPNETWORKMESSAGECONTENTMASK_PAYLOADHEADER);
writerGroupConfig.messageSettings.content.decoded.data = writerGroupMessage;
UA_Server_addWriterGroup(server, connectionIdent, &writerGroupConfig, &writerGroupIdent);
UA_Server_setWriterGroupOperational(server, writerGroupIdent);
UA_UadpWriterGroupMessageDataType_delete(writerGroupMessage);
}
/**
* **DataSetWriter handling**
*
* A DataSetWriter (DSW) is the glue between the WG and the PDS. The DSW is
* linked to exactly one PDS and contains additional information for the
* message generation. */
static UA_StatusCode
addDataSetWriter(UA_Server *server) {
/* We need now a DataSetWriter within the WriterGroup. This means we must
* create a new DataSetWriterConfig and add call the addWriterGroup function. */
UA_NodeId dataSetWriterIdent;
UA_DataSetWriterConfig dataSetWriterConfig;
memset(&dataSetWriterConfig, 0, sizeof(UA_DataSetWriterConfig));
dataSetWriterConfig.name = UA_STRING("Demo DataSetWriter");
dataSetWriterConfig.dataSetWriterId = DATA_SET_WRITER_ID;
dataSetWriterConfig.keyFrameCount = 10;
return UA_Server_addDataSetWriter(server, writerGroupIdent, publishedDataSetIdent,
&dataSetWriterConfig, &dataSetWriterIdent);
} }
/** /**
...@@ -61,10 +178,10 @@ addReaderGroup(UA_Server *server) { ...@@ -61,10 +178,10 @@ addReaderGroup(UA_Server *server) {
UA_StatusCode retval = UA_STATUSCODE_GOOD; UA_StatusCode retval = UA_STATUSCODE_GOOD;
UA_ReaderGroupConfig readerGroupConfig; UA_ReaderGroupConfig readerGroupConfig;
memset (&readerGroupConfig, 0, sizeof(UA_ReaderGroupConfig)); memset (&readerGroupConfig, 0, sizeof(UA_ReaderGroupConfig));
readerGroupConfig.name = UA_STRING("ReaderGroup1"); readerGroupConfig.name = UA_STRING("Demo ReaderGroup");
retval |= UA_Server_addReaderGroup(server, connectionIdentifier, &readerGroupConfig, retval |= UA_Server_addReaderGroup(server, connectionIdent, &readerGroupConfig,
&readerGroupIdentifier); &readerGroupIdent);
UA_Server_setReaderGroupOperational(server, readerGroupIdentifier); UA_Server_setReaderGroupOperational(server, readerGroupIdent);
return retval; return retval;
} }
...@@ -90,8 +207,8 @@ addDataSetReader(UA_Server *server, VariableData const *variableArray, ...@@ -90,8 +207,8 @@ addDataSetReader(UA_Server *server, VariableData const *variableArray,
fillDataSetMetaData(&readerConfig.dataSetMetaData, variableArray, fillDataSetMetaData(&readerConfig.dataSetMetaData, variableArray,
nbVariable, id); nbVariable, id);
retval |= UA_Server_addDataSetReader(server, readerGroupIdentifier, retval |= UA_Server_addDataSetReader(server, readerGroupIdent,
&readerConfig, &readerIdentifier); &readerConfig, &readerIdent);
return retval; return retval;
} }
...@@ -155,12 +272,13 @@ addSubscribedVariables (UA_Server *server, UA_NodeId dataSetReaderId, int nb, ...@@ -155,12 +272,13 @@ addSubscribedVariables (UA_Server *server, UA_NodeId dataSetReaderId, int nb,
vAttr.dataType = readerConfig.dataSetMetaData.fields[i].dataType; vAttr.dataType = readerConfig.dataSetMetaData.fields[i].dataType;
UA_NodeId newNode; UA_NodeId newNode;
retval |= UA_Server_addVariableNode(server, UA_NODEID_NUMERIC(1, (UA_UInt32)i + 50000), retval |= UA_Server_addVariableNode(server,
folderId, UA_NODEID_NUMERIC(1, (UA_UInt32) readerConfig.dataSetMetaData.fieldsSize*nb + i + 50000),
UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), folderId,
UA_QUALIFIEDNAME(1, (char *)readerConfig.dataSetMetaData.fields[i].name.data), UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT),
UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE), UA_QUALIFIEDNAME(1, (char *)readerConfig.dataSetMetaData.fields[i].name.data),
vAttr, NULL, &newNode); UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE),
vAttr, NULL, &newNode);
/*monitor variable*/ /*monitor variable*/
UA_MonitoredItemCreateRequest monRequest = UA_MonitoredItemCreateRequest_default(newNode); UA_MonitoredItemCreateRequest monRequest = UA_MonitoredItemCreateRequest_default(newNode);
...@@ -222,66 +340,72 @@ static void fillDataSetMetaData(UA_DataSetMetaDataType *pMetaData, ...@@ -222,66 +340,72 @@ static void fillDataSetMetaData(UA_DataSetMetaDataType *pMetaData,
} }
} }
int subscribe(UA_String *transportProfile, int runPubsub(UA_String *transportProfile,
UA_NetworkAddressUrlDataType *networkAddressUrl, UA_NetworkAddressUrlDataType *networkAddressUrl,
VariableData const *variableArray, int nbVariable, int nbReader, VariableData const *variableArray, int nbVariable,
int id, int nbReader,
void (*init_node_id)(UA_UInt32 id, int nb, int magic), void (*init_node_id)(UA_UInt32 id, int nb, int magic),
int (*get_reader_id)(int nb), int (*get_reader_id)(int nb),
void (*update)(UA_UInt32 id, const UA_DataValue*), void (*update)(UA_UInt32 id, const UA_DataValue*),
UA_Boolean *running) { UA_Boolean *running) {
/* Return value initialized to Status Good */ UA_UInt16 publisherIdent;
char readerName[19];
UA_StatusCode retval = UA_STATUSCODE_GOOD; UA_StatusCode retval = UA_STATUSCODE_GOOD;
UA_Server *server = UA_Server_new(); server = UA_Server_new();
UA_ServerConfig *config = UA_Server_getConfig(server); UA_ServerConfig *config = UA_Server_getConfig(server);
UA_ServerConfig_setMinimal(config, 4801, NULL); UA_ServerConfig_setDefault(config);
UA_ServerConfig_addPubSubTransportLayer(config, UA_PubSubTransportLayerUDPMP());
UA_UInt16 publisherIdentifier;
char readerName[19];
callbackUpdate = update; callbackUpdate = update;
/* Add the PubSub network layer implementation to the server config. retval |= addPubSubConnection(server, transportProfile, networkAddressUrl, id);
* The TransportLayer is acting as factory to create new connections if (retval != UA_STATUSCODE_GOOD)
* on runtime. Details about the PubSubTransportLayer can be found inside the return EXIT_FAILURE;
* tutorial_pubsub_connection */
UA_ServerConfig_addPubSubTransportLayer(config, UA_PubSubTransportLayerUDPMP());
/* API calls */ /* Publishing */
/* Add PubSubConnection */
retval |= addPubSubConnection(server, transportProfile, networkAddressUrl); addPublishedDataSet(server, id);
for(size_t i = 0; i < nbVariable; i++) {
retval |= addVariable(server, variableArray[i]);
if (retval != UA_STATUSCODE_GOOD)
return EXIT_FAILURE;
addDataSetField(server, variableArray[i]);
}
addWriterGroup(server);
retval |= addDataSetWriter(server);
if (retval != UA_STATUSCODE_GOOD) if (retval != UA_STATUSCODE_GOOD)
return EXIT_FAILURE; return EXIT_FAILURE;
/*UA_PubSubConnection *currentConnectionContext =
UA_PubSubConnection_findConnectionbyId(server, connectionIdentifier);
printf("connected %d\n", currentConnectionContext->isRegistered);*/
/* Add ReaderGroup to the created PubSubConnection */ /* Subscribing */
retval |= addReaderGroup(server); retval |= addReaderGroup(server);
if (retval != UA_STATUSCODE_GOOD) if (retval != UA_STATUSCODE_GOOD)
return EXIT_FAILURE; return EXIT_FAILURE;
//printf("connected %d\n", currentConnectionContext->isRegistered);
memset (&readerConfig, 0, sizeof(UA_DataSetReaderConfig)); memset (&readerConfig, 0, sizeof(UA_DataSetReaderConfig));
readerConfig.publisherId.type = &UA_TYPES[UA_TYPES_UINT16]; readerConfig.publisherId.type = &UA_TYPES[UA_TYPES_UINT16];
readerConfig.writerGroupId = 100; readerConfig.writerGroupId = WRITER_GROUP_ID;
readerConfig.dataSetWriterId = 62541; readerConfig.dataSetWriterId = DATA_SET_WRITER_ID;
for (size_t i = 0; i < nbReader; i++) { for (size_t i = 0; i < nbReader; i++) {
publisherIdentifier = get_reader_id(i); publisherIdent = get_reader_id(i);
UA_snprintf(readerName, sizeof(readerName - 1), "DataSet Reader %d", publisherIdentifier); UA_snprintf(readerName, sizeof(readerName - 1), "DataSet Reader %d", publisherIdent);
readerConfig.name = UA_STRING(readerName); readerConfig.name = UA_STRING(readerName);
readerConfig.publisherId.data = &publisherIdentifier; readerConfig.publisherId.data = &publisherIdent;
retval |= addDataSetReader(server, variableArray, nbVariable, publisherIdentifier); retval |= addDataSetReader(server, variableArray, nbVariable, publisherIdent);
if (retval != UA_STATUSCODE_GOOD) if (retval != UA_STATUSCODE_GOOD)
return EXIT_FAILURE; return EXIT_FAILURE;
/* Add SubscribedVariables to the created DataSetReader */ /* Add SubscribedVariables to the created DataSetReader */
retval |= addSubscribedVariables(server, readerIdentifier, i, init_node_id); retval |= addSubscribedVariables(server, readerIdent, i, init_node_id);
if (retval != UA_STATUSCODE_GOOD) if (retval != UA_STATUSCODE_GOOD)
return EXIT_FAILURE; return EXIT_FAILURE;
} }
retval = UA_Server_run(server, running); retval |= UA_Server_run(server, running);
UA_Server_delete(server); UA_Server_delete(server);
return retval == UA_STATUSCODE_GOOD ? EXIT_SUCCESS : EXIT_FAILURE; return retval == UA_STATUSCODE_GOOD ? EXIT_SUCCESS : EXIT_FAILURE;
} }
\ No newline at end of file
#include <open62541/plugin/log_stdout.h>
#include <open62541/plugin/pubsub_udp.h>
#include <open62541/server_config_default.h>
#include "pubsub_publish.h"
static UA_Server *server;
UA_NodeId connectionIdent, publishedDataSetIdent, writerGroupIdent;
static UA_StatusCode
addPubSubConnection(UA_Server *server, UA_String *transportProfile,
UA_NetworkAddressUrlDataType *networkAddressUrl,
int id){
char name[20];
UA_snprintf(name, sizeof(name - 1), "UADP Connection %d", id);
UA_PubSubConnectionConfig connectionConfig;
memset(&connectionConfig, 0, sizeof(connectionConfig));
connectionConfig.name = UA_STRING(name);
connectionConfig.transportProfileUri = *transportProfile;
connectionConfig.enabled = UA_TRUE;
UA_Variant_setScalar(&connectionConfig.address, networkAddressUrl,
&UA_TYPES[UA_TYPES_NETWORKADDRESSURLDATATYPE]);
connectionConfig.publisherId.numeric = id;
return UA_Server_addPubSubConnection(server, &connectionConfig, &connectionIdent);
}
/**
* **PublishedDataSet handling**
*
* The PublishedDataSet (PDS) and PubSubConnection are the toplevel entities and
* can exist alone. The PDS contains the collection of the published fields. All
* other PubSub elements are directly or indirectly linked with the PDS or
* connection. */
static void
addPublishedDataSet(UA_Server *server, int id) {
/* The PublishedDataSetConfig contains all necessary public
* information for the creation of a new PublishedDataSet */
char name[8];
UA_snprintf(name, sizeof(name - 1), "PDS %d", id);
UA_PublishedDataSetConfig publishedDataSetConfig;
memset(&publishedDataSetConfig, 0, sizeof(UA_PublishedDataSetConfig));
publishedDataSetConfig.publishedDataSetType = UA_PUBSUB_DATASET_PUBLISHEDITEMS;
publishedDataSetConfig.name = UA_STRING(name);
/* Create new PublishedDataSet based on the PublishedDataSetConfig. */
UA_Server_addPublishedDataSet(server, &publishedDataSetConfig, &publishedDataSetIdent);
}
static UA_StatusCode
addVariable(UA_Server *server, VariableData varDetails) {
UA_VariableAttributes attr = UA_VariableAttributes_default;
UA_Variant_setScalar(&attr.value, varDetails.pdefaultValue, &UA_TYPES[varDetails.type]);
attr.description = UA_LOCALIZEDTEXT("en-US", varDetails.description);
attr.displayName = UA_LOCALIZEDTEXT("en-US", varDetails.description);
attr.dataType = UA_TYPES[varDetails.type].typeId;
attr.accessLevel = UA_ACCESSLEVELMASK_READ | UA_ACCESSLEVELMASK_WRITE;
return UA_Server_addVariableNode(server, UA_NODEID_STRING(1, varDetails.name),
UA_NODEID_NUMERIC(0, UA_NS0ID_OBJECTSFOLDER),
UA_NODEID_NUMERIC(0, UA_NS0ID_ORGANIZES),
UA_QUALIFIEDNAME(1, varDetails.description),
UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE),
attr, NULL, NULL);
}
UA_StatusCode writeVariable(char *name, void * UA_RESTRICT pvalue, UA_DataType type)
{
UA_NodeId integerNodeId = UA_NODEID_STRING(1, name);
UA_Variant var;
UA_Variant_init(&var);
UA_Variant_setScalar(&var, pvalue, &type);
return UA_Server_writeValue(server, integerNodeId, var);
}
static void
addDataSetField(UA_Server *server, VariableData varDetails) {
UA_NodeId dataSetFieldIdent;
UA_DataSetFieldConfig dataSetFieldConfig;
memset(&dataSetFieldConfig, 0, sizeof(UA_DataSetFieldConfig));
dataSetFieldConfig.dataSetFieldType = UA_PUBSUB_DATASETFIELD_VARIABLE;
dataSetFieldConfig.field.variable.fieldNameAlias = UA_STRING(varDetails.description);
dataSetFieldConfig.field.variable.promotedField = UA_FALSE;
dataSetFieldConfig.field.variable.publishParameters.publishedVariable =
UA_NODEID_STRING(1, varDetails.name);
dataSetFieldConfig.field.variable.publishParameters.attributeId = UA_ATTRIBUTEID_VALUE;
UA_Server_addDataSetField(server, publishedDataSetIdent,
&dataSetFieldConfig, &dataSetFieldIdent);
}
/**
* **WriterGroup handling**
*
* The WriterGroup (WG) is part of the connection and contains the primary
* configuration parameters for the message creation. */
static void
addWriterGroup(UA_Server *server) {
/* Now we create a new WriterGroupConfig and add the group to the existing
* PubSubConnection. */
UA_WriterGroupConfig writerGroupConfig;
memset(&writerGroupConfig, 0, sizeof(UA_WriterGroupConfig));
writerGroupConfig.name = UA_STRING("Demo WriterGroup");
writerGroupConfig.publishingInterval = 100;
writerGroupConfig.enabled = UA_FALSE;
writerGroupConfig.writerGroupId = 100;
writerGroupConfig.encodingMimeType = UA_PUBSUB_ENCODING_UADP;
writerGroupConfig.messageSettings.encoding = UA_EXTENSIONOBJECT_DECODED;
writerGroupConfig.messageSettings.content.decoded.type = &UA_TYPES[UA_TYPES_UADPWRITERGROUPMESSAGEDATATYPE];
/* The configuration flags for the messages are encapsulated inside the
* message- and transport settings extension objects. These extension
* objects are defined by the standard. e.g.
* UadpWriterGroupMessageDataType */
UA_UadpWriterGroupMessageDataType *writerGroupMessage = UA_UadpWriterGroupMessageDataType_new();
/* Change message settings of writerGroup to send PublisherId,
* WriterGroupId in GroupHeader and DataSetWriterId in PayloadHeader
* of NetworkMessage */
writerGroupMessage->networkMessageContentMask = (UA_UadpNetworkMessageContentMask)(UA_UADPNETWORKMESSAGECONTENTMASK_PUBLISHERID |
(UA_UadpNetworkMessageContentMask)UA_UADPNETWORKMESSAGECONTENTMASK_GROUPHEADER |
(UA_UadpNetworkMessageContentMask)UA_UADPNETWORKMESSAGECONTENTMASK_WRITERGROUPID |
(UA_UadpNetworkMessageContentMask)UA_UADPNETWORKMESSAGECONTENTMASK_PAYLOADHEADER);
writerGroupConfig.messageSettings.content.decoded.data = writerGroupMessage;
UA_Server_addWriterGroup(server, connectionIdent, &writerGroupConfig, &writerGroupIdent);
UA_Server_setWriterGroupOperational(server, writerGroupIdent);
UA_UadpWriterGroupMessageDataType_delete(writerGroupMessage);
}
/**
* **DataSetWriter handling**
*
* A DataSetWriter (DSW) is the glue between the WG and the PDS. The DSW is
* linked to exactly one PDS and contains additional information for the
* message generation. */
static UA_StatusCode
addDataSetWriter(UA_Server *server) {
/* We need now a DataSetWriter within the WriterGroup. This means we must
* create a new DataSetWriterConfig and add call the addWriterGroup function. */
UA_NodeId dataSetWriterIdent;
UA_DataSetWriterConfig dataSetWriterConfig;
memset(&dataSetWriterConfig, 0, sizeof(UA_DataSetWriterConfig));
dataSetWriterConfig.name = UA_STRING("Demo DataSetWriter");
dataSetWriterConfig.dataSetWriterId = 62541;
dataSetWriterConfig.keyFrameCount = 10;
return UA_Server_addDataSetWriter(server, writerGroupIdent, publishedDataSetIdent,
&dataSetWriterConfig, &dataSetWriterIdent);
}
int publish(UA_String *transportProfile,
UA_NetworkAddressUrlDataType *networkAddressUrl,
VariableData const *variableArray, int nbVariable,
int id, UA_Boolean *running) {
int i;
UA_StatusCode retval = UA_STATUSCODE_GOOD;
server = UA_Server_new();
UA_ServerConfig *config = UA_Server_getConfig(server);
UA_ServerConfig_setDefault(config);
UA_ServerConfig_addPubSubTransportLayer(config, UA_PubSubTransportLayerUDPMP());
retval |= addPubSubConnection(server, transportProfile, networkAddressUrl, id);
if (retval != UA_STATUSCODE_GOOD)
return EXIT_FAILURE;
addPublishedDataSet(server, id);
for(i = 0; i < nbVariable; i++) {
retval |= addVariable(server, variableArray[i]);
if (retval != UA_STATUSCODE_GOOD)
return EXIT_FAILURE;
addDataSetField(server, variableArray[i]);
}
addWriterGroup(server);
retval |= addDataSetWriter(server);
if (retval != UA_STATUSCODE_GOOD)
return EXIT_FAILURE;
retval |= UA_Server_run(server, running);
UA_Server_delete(server);
return retval == UA_STATUSCODE_GOOD ? EXIT_SUCCESS : EXIT_FAILURE;
}
#include <quickjs/quickjs.h> #include <quickjs/quickjs.h>
#include "mavsdk_wrapper.h" #include "mavsdk_wrapper.h"
#include "pubsub_publish.h" #include "pubsub.h"
#include "pubsub_subscribe.h"
static JSClassID js_drone_class_id; static JSClassID js_drone_class_id;
...@@ -33,8 +32,7 @@ const VariableData droneVariableArray[] = { ...@@ -33,8 +32,7 @@ const VariableData droneVariableArray[] = {
}, },
}; };
static UA_Boolean publishing = true; static UA_Boolean pubsub_running = true;
static UA_Boolean subscribing = true;
int nbDrone; int nbDrone;
static JSValueConst *drone_object_id_list; static JSValueConst *drone_object_id_list;
...@@ -133,44 +131,6 @@ void pubsub_publish_coordinates(double latitude, double longitude, float altitud ...@@ -133,44 +131,6 @@ void pubsub_publish_coordinates(double latitude, double longitude, float altitud
"Writing variable returned value %x", res); "Writing variable returned value %x", res);
} }
static JSValue js_pubsub_publish(JSContext *ctx, JSValueConst this_val,
int argc, JSValueConst *argv)
{
const char *ipv6;
const char *port;
char urlBuffer[44];
int id;
int res;
ipv6 = JS_ToCString(ctx, argv[0]);
port = JS_ToCString(ctx, argv[1]);
UA_snprintf(urlBuffer, sizeof(urlBuffer), "opc.udp://[%s]:%s/", ipv6, port);
UA_String transportProfile =
UA_STRING("http://opcfoundation.org/UA-Profile/Transport/pubsub-udp-uadp");
UA_NetworkAddressUrlDataType networkAddressUrl =
{UA_STRING_NULL , UA_STRING(urlBuffer)};
if (JS_ToInt32(ctx, &id, argv[2]))
return JS_EXCEPTION;
res = publish(&transportProfile, &networkAddressUrl, droneVariableArray,
countof(droneVariableArray), id, &publishing);
JS_FreeCString(ctx, ipv6);
JS_FreeCString(ctx, port);
return JS_NewInt32(ctx, res);
}
static JSValue js_init_subscription(JSContext *ctx, JSValueConst this_val,
int argc, JSValueConst *argv)
{
if (JS_ToInt32(ctx, &nbDrone, argv[0]))
return JS_EXCEPTION;
drone_object_id_list = malloc(nbDrone * sizeof(JSValueConst));
return JS_NewInt32(ctx, 0);
}
int get_drone_id(int nb) { int get_drone_id(int nb) {
JSDroneData *s = JS_GetOpaque(drone_object_id_list[nb], js_drone_class_id); JSDroneData *s = JS_GetOpaque(drone_object_id_list[nb], js_drone_class_id);
return s->id; return s->id;
...@@ -212,30 +172,13 @@ void pubsub_update_coordinates(UA_UInt32 id, const UA_DataValue *var) ...@@ -212,30 +172,13 @@ void pubsub_update_coordinates(UA_UInt32 id, const UA_DataValue *var)
UA_LOG_ERROR(UA_Log_Stdout, UA_LOGCATEGORY_CLIENT, "NodeId not found"); UA_LOG_ERROR(UA_Log_Stdout, UA_LOGCATEGORY_CLIENT, "NodeId not found");
} }
static JSValue js_pubsub_write(JSContext *ctx, JSValueConst this_val, static JSValue js_run_pubsub(JSContext *ctx, JSValueConst this_val,
int argc, JSValueConst *argv) int argc, JSValueConst *argv)
{
double latitude;
double longitude;
double altitude;
if (JS_ToFloat64(ctx, &latitude, argv[0]))
return JS_EXCEPTION;
if (JS_ToFloat64(ctx, &longitude, argv[1]))
return JS_EXCEPTION;
if (JS_ToFloat64(ctx, &altitude, argv[2]))
return JS_EXCEPTION;
pubsub_publish_coordinates(latitude, longitude, altitude);
return JS_NewInt32(ctx, 0);
}
static JSValue js_pubsub_subscribe(JSContext *ctx, JSValueConst this_val,
int argc, JSValueConst *argv)
{ {
const char *ipv6; const char *ipv6;
const char *port; const char *port;
char urlBuffer[44]; char urlBuffer[44];
int id;
int res; int res;
ipv6 = JS_ToCString(ctx, argv[0]); ipv6 = JS_ToCString(ctx, argv[0]);
...@@ -247,25 +190,50 @@ static JSValue js_pubsub_subscribe(JSContext *ctx, JSValueConst this_val, ...@@ -247,25 +190,50 @@ static JSValue js_pubsub_subscribe(JSContext *ctx, JSValueConst this_val,
UA_NetworkAddressUrlDataType networkAddressUrl = UA_NetworkAddressUrlDataType networkAddressUrl =
{UA_STRING_NULL , UA_STRING(urlBuffer)}; {UA_STRING_NULL , UA_STRING(urlBuffer)};
res = subscribe(&transportProfile, &networkAddressUrl, droneVariableArray, if (JS_ToInt32(ctx, &id, argv[2]))
countof(droneVariableArray), nbDrone, init_node_id, return JS_EXCEPTION;
get_drone_id, pubsub_update_coordinates, &subscribing);
res = runPubsub(&transportProfile, &networkAddressUrl, droneVariableArray,
countof(droneVariableArray), id, nbDrone, init_node_id,
get_drone_id, pubsub_update_coordinates, &pubsub_running);
JS_FreeCString(ctx, ipv6); JS_FreeCString(ctx, ipv6);
JS_FreeCString(ctx, port); JS_FreeCString(ctx, port);
return JS_NewInt32(ctx, res); return JS_NewInt32(ctx, res);
} }
static JSValue js_stop_publishing(JSContext *ctx, JSValueConst this_val, static JSValue js_init_pubsub(JSContext *ctx, JSValueConst this_val,
int argc, JSValueConst *argv) int argc, JSValueConst *argv)
{ {
publishing = false; if (JS_ToInt32(ctx, &nbDrone, argv[0]))
return JS_EXCEPTION;
drone_object_id_list = malloc(nbDrone * sizeof(JSValueConst));
return JS_NewInt32(ctx, 0); return JS_NewInt32(ctx, 0);
} }
static JSValue js_stop_subscribing(JSContext *ctx, JSValueConst this_val, static JSValue js_pubsub_write(JSContext *ctx, JSValueConst this_val,
int argc, JSValueConst *argv) int argc, JSValueConst *argv)
{
double latitude;
double longitude;
double altitude;
if (JS_ToFloat64(ctx, &latitude, argv[0]))
return JS_EXCEPTION;
if (JS_ToFloat64(ctx, &longitude, argv[1]))
return JS_EXCEPTION;
if (JS_ToFloat64(ctx, &altitude, argv[2]))
return JS_EXCEPTION;
pubsub_publish_coordinates(latitude, longitude, altitude);
return JS_NewInt32(ctx, 0);
}
static JSValue js_stop_pubsub(JSContext *ctx, JSValueConst this_val,
int argc, JSValueConst *argv)
{ {
subscribing = false; pubsub_running = false;
free(drone_object_id_list); free(drone_object_id_list);
return JS_NewInt32(ctx, 0); return JS_NewInt32(ctx, 0);
} }
...@@ -551,11 +519,9 @@ static const JSCFunctionListEntry js_mavsdk_funcs[] = { ...@@ -551,11 +519,9 @@ static const JSCFunctionListEntry js_mavsdk_funcs[] = {
JS_CFUNC_DEF("takeOff", 0, js_mavsdk_takeOff ), JS_CFUNC_DEF("takeOff", 0, js_mavsdk_takeOff ),
JS_CFUNC_DEF("takeOffAndWait", 0, js_mavsdk_takeOffAndWait ), JS_CFUNC_DEF("takeOffAndWait", 0, js_mavsdk_takeOffAndWait ),
JS_CFUNC_DEF("land", 0, js_mavsdk_land ), JS_CFUNC_DEF("land", 0, js_mavsdk_land ),
JS_CFUNC_DEF("publish", 2, js_pubsub_publish ), JS_CFUNC_DEF("initPubsub", 1, js_init_pubsub ),
JS_CFUNC_DEF("subscribe", 2, js_pubsub_subscribe ), JS_CFUNC_DEF("runPubsub", 2, js_run_pubsub ),
JS_CFUNC_DEF("stopPublishing", 0, js_stop_publishing ), JS_CFUNC_DEF("stopPubsub", 0, js_stop_pubsub ),
JS_CFUNC_DEF("stopSubscribing", 0, js_stop_subscribing ),
JS_CFUNC_DEF("initSubscription", 1, js_init_subscription ),
JS_CFUNC_DEF("pubsubWrite", 3, js_pubsub_write ), JS_CFUNC_DEF("pubsubWrite", 3, js_pubsub_write ),
}; };
......
Markdown is supported
0%
or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment