Skip to content

Q
qjs-wrapper
Commit 3ea4a41c authored by Léo-Paul Géneau's avatar Léo-Paul Géneau 👾
Browse files
Options

Add subscribe only (pubsub) function 

See merge request !2
parent 2a933857
Hide whitespace changes
Inline Side-by-side
Showing with 149 additions and 55 deletions
include/pubsub.h
View file @ 3ea4a41c
......@@ -42,6 +42,15 @@ typedef struct {
} VariableData;
int subscribeOnly(UA_String *transportProfile,
UA_NetworkAddressUrlDataType *networkAddressUrl,
VariableData *variableArray, size_t nbVariable,
UA_UInt32 id, UA_UInt32 nbReader,
void (*init_node_id)(UA_UInt32 id, UA_UInt32 nb, UA_UInt32 magic),
UA_UInt16 (*get_reader_id)(UA_UInt32 nb),
void (*update)(UA_UInt32 id, const UA_DataValue*),
UA_Boolean *running);
int runPubsub(UA_String *transportProfile,
UA_NetworkAddressUrlDataType *networkAddressUrl,
VariableData *variableArray, size_t nbVariable,
......@@ -60,6 +69,8 @@ void init_node_id(UA_UInt32 id, UA_UInt32 nb, UA_UInt32 magic);
void pubsub_update_coordinates(UA_UInt32 id, const UA_DataValue *var);
void pubsub_print_coordinates(UA_UInt32 id, const UA_DataValue *var);
VariableData pubsub_get_value(UA_String identifier);
void stop_pubsub(void);
......
pubsub.c
View file @ 3ea4a41c
......@@ -198,11 +198,11 @@ addReaderGroup(UA_Server *server) {
return UA_STATUSCODE_BADINTERNALERROR;
}
UA_StatusCode retval = UA_STATUSCODE_GOOD;
UA_StatusCode retval;
UA_ReaderGroupConfig readerGroupConfig;
memset (&readerGroupConfig, 0, sizeof(UA_ReaderGroupConfig));
readerGroupConfig.name = UA_STRING("Demo ReaderGroup");
retval |= UA_Server_addReaderGroup(server, connectionIdent, &readerGroupConfig,
retval = UA_Server_addReaderGroup(server, connectionIdent, &readerGroupConfig,
&readerGroupIdent);
UA_Server_setReaderGroupOperational(server, readerGroupIdent);
return retval;
......@@ -220,19 +220,15 @@ addReaderGroup(UA_Server *server) {
static UA_StatusCode
addDataSetReader(UA_Server *server, VariableData *variableArray,
size_t nbVariable, int id) {
if(server == NULL) {
if(server == NULL)
return UA_STATUSCODE_BADINTERNALERROR;
}
UA_StatusCode retval = UA_STATUSCODE_GOOD;
/* Setting up Meta data configuration in DataSetReader */
fillDataSetMetaData(&readerConfig.dataSetMetaData, variableArray,
nbVariable, id);
retval |= UA_Server_addDataSetReader(server, readerGroupIdent,
&readerConfig, &readerIdent);
return retval;
return UA_Server_addDataSetReader(server, readerGroupIdent,
&readerConfig, &readerIdent);
}
static void
......@@ -315,8 +311,8 @@ addSubscribedVariables(UA_Server *server, UA_NodeId dataSetReaderId, UA_UInt32 n
targetVars[i].targetVariable.targetNodeId = newNode;
}
retval = UA_Server_DataSetReader_createTargetVariables(server, dataSetReaderId,
readerConfig.dataSetMetaData.fieldsSize, targetVars);
retval |= UA_Server_DataSetReader_createTargetVariables(server, dataSetReaderId,
readerConfig.dataSetMetaData.fieldsSize, targetVars);
for(size_t i = 0; i < readerConfig.dataSetMetaData.fieldsSize; i++)
UA_FieldTargetDataType_clear(&targetVars[i].targetVariable);
......@@ -339,9 +335,8 @@ static void fillDataSetMetaData(UA_DataSetMetaDataType *pMetaData,
char name[12];
UA_snprintf(name, sizeof(name) - 1, "DataSet %d", id);
if(pMetaData == NULL) {
if(pMetaData == NULL)
return;
}
UA_DataSetMetaDataType_init (pMetaData);
pMetaData->name = UA_STRING (name);
......@@ -363,50 +358,34 @@ static void fillDataSetMetaData(UA_DataSetMetaDataType *pMetaData,
}
}
int runPubsub(UA_String *transportProfile,
UA_NetworkAddressUrlDataType *networkAddressUrl,
VariableData *variableArray, size_t nbVariable,
UA_UInt32 id, UA_UInt32 nbReader,
void (*init_node_id)(UA_UInt32 id, UA_UInt32 nb, UA_UInt32 magic),
UA_UInt16 (*get_reader_id)(UA_UInt32 nb),
VariableData (*get_value)(UA_String identifier),
void (*update)(UA_UInt32 id, const UA_DataValue*),
UA_Boolean *running) {
static UA_Server*
setServer(UA_String *transportProfile,
UA_NetworkAddressUrlDataType *networkAddressUrl, UA_UInt32 id) {
UA_Server *server;
UA_UInt16 publisherIdent;
char readerName[19];
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());
pubsubGetValue = get_value;
callbackUpdate = update;
retval |= addPubSubConnection(server, transportProfile, networkAddressUrl, id);
if (retval != UA_STATUSCODE_GOOD)
return EXIT_FAILURE;
/* Publishing */
addPublishedDataSet(server, id);
for(UA_UInt32 i = 0; i < nbVariable; i++) {
retval |= addDataSourceVariable(server, variableArray[i]);
if (retval != UA_STATUSCODE_GOOD)
return EXIT_FAILURE;
addDataSetField(server, variableArray[i]);
}
addPubSubConnection(server, transportProfile, networkAddressUrl, id);
return server;
}
addWriterGroup(server);
retval |= addDataSetWriter(server);
if (retval != UA_STATUSCODE_GOOD)
return EXIT_FAILURE;
static UA_StatusCode
subscribe(UA_Server *server,
VariableData *variableArray, size_t nbVariable,
UA_UInt32 id, UA_UInt32 nbReader,
void (*init_node_id)(UA_UInt32 id, UA_UInt32 nb, UA_UInt32 magic),
UA_UInt16 (*get_reader_id)(UA_UInt32 nb),
void (*update)(UA_UInt32 id, const UA_DataValue*)) {
UA_UInt16 publisherIdent;
UA_StatusCode retval;
char readerName[19];
/* Subscribing */
callbackUpdate = update;
retval |= addReaderGroup(server);
retval = addReaderGroup(server);
if (retval != UA_STATUSCODE_GOOD)
return EXIT_FAILURE;
......@@ -421,17 +400,77 @@ int runPubsub(UA_String *transportProfile,
readerConfig.name = UA_STRING(readerName);
readerConfig.publisherId.data = &publisherIdent;
retval |= addDataSetReader(server, variableArray, nbVariable, publisherIdent);
retval = addDataSetReader(server, variableArray, nbVariable, publisherIdent);
if (retval != UA_STATUSCODE_GOOD)
return EXIT_FAILURE;
/* Add SubscribedVariables to the created DataSetReader */
retval |= addSubscribedVariables(server, readerIdent, i, init_node_id);
retval = addSubscribedVariables(server, readerIdent, i, init_node_id);
if (retval != UA_STATUSCODE_GOOD)
return EXIT_FAILURE;
}
retval |= UA_Server_run(server, running);
return retval;
}
int subscribeOnly(UA_String *transportProfile,
UA_NetworkAddressUrlDataType *networkAddressUrl,
VariableData *variableArray, size_t nbVariable,
UA_UInt32 id, UA_UInt32 nbReader,
void (*init_node_id)(UA_UInt32 id, UA_UInt32 nb, UA_UInt32 magic),
UA_UInt16 (*get_reader_id)(UA_UInt32 nb),
void (*update)(UA_UInt32 id, const UA_DataValue*),
UA_Boolean *running) {
UA_Server *server;
UA_StatusCode retval;
server = setServer(transportProfile, networkAddressUrl, id);
subscribe(server, variableArray, nbVariable, id, nbReader, init_node_id,
get_reader_id, update);
retval = UA_Server_run(server, running);
UA_Server_delete(server);
return retval == UA_STATUSCODE_GOOD ? EXIT_SUCCESS : EXIT_FAILURE;
}
int runPubsub(UA_String *transportProfile,
UA_NetworkAddressUrlDataType *networkAddressUrl,
VariableData *variableArray, size_t nbVariable,
UA_UInt32 id, UA_UInt32 nbReader,
void (*init_node_id)(UA_UInt32 id, UA_UInt32 nb, UA_UInt32 magic),
UA_UInt16 (*get_reader_id)(UA_UInt32 nb),
VariableData (*get_value)(UA_String identifier),
void (*update)(UA_UInt32 id, const UA_DataValue*),
UA_Boolean *running) {
UA_Server *server;
UA_StatusCode retval;
server = setServer(transportProfile, networkAddressUrl, id);
/* Publishing */
pubsubGetValue = get_value;
addPublishedDataSet(server, id);
for(UA_UInt32 i = 0; i < nbVariable; i++) {
retval = addDataSourceVariable(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;
/* Subscribing */
subscribe(server, variableArray, nbVariable, id, nbReader, init_node_id,
get_reader_id, update);
retval = UA_Server_run(server, running);
UA_Server_delete(server);
return retval == UA_STATUSCODE_GOOD ? EXIT_SUCCESS : EXIT_FAILURE;
}
qjs_wrapper.c
View file @ 3ea4a41c
......@@ -202,6 +202,43 @@ void pubsub_update_coordinates(UA_UInt32 id, const UA_DataValue *var)
UA_LOG_ERROR(UA_Log_Stdout, UA_LOGCATEGORY_CLIENT, "NodeId not found");
}
void pubsub_print_coordinates(UA_UInt32 id, const UA_DataValue *var)
{
JSDroneData *s;
for(UA_UInt32 i = 0; i < nbDrone; i++) {
s = (JSDroneData *) JS_GetOpaque(drone_object_id_list[i], js_drone_class_id);
if (s->latitudeId == id) {
s->latitude = *(UA_Double*) var->value.data;
UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_CLIENT, "Received latitude of drone %d: %f°", s->id, s->latitude);
return;
} else if (s->longitudeId == id) {
s->longitude = *(UA_Double*) var->value.data;
UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_CLIENT, "Received longitude of drone %d: %f°", s->id, s->longitude);
return;
} else if (s->altitudeAbsId == id) {
s->altitudeAbs = *(UA_Float*) var->value.data;
UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_CLIENT, "Received absolute altitude of drone %d: %fm", s->id, s->altitudeAbs);
return;
} else if (s->altitudeRelId == id) {
s->altitudeRel = *(UA_Float*) var->value.data;
UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_CLIENT, "Received relative altitude of drone %d: %fm", s->id, s->altitudeRel);
return;
} else if (s->lastCheckpointId == id) {
s->lastCheckpoint = *(UA_UInt32*) var->value.data;
UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_CLIENT, "Received checkpoint of drone %d: %dm", s->id, s->lastCheckpoint);
return;
}
}
UA_LOG_ERROR(UA_Log_Stdout, UA_LOGCATEGORY_CLIENT, "NodeId not found");
}
/*
* arg 0 (string): IPv6 of the multicast group
* arg 1 (string): port used for multicast communication
* arg 2 (string): network interface used for multicast communication
* arg 3 (int): ID of the drone
* arg 4 (bool): true if there will be data to publish
*/
static JSValue js_run_pubsub(JSContext *ctx, JSValueConst this_val,
int argc, JSValueConst *argv)
{
......@@ -229,10 +266,17 @@ static JSValue js_run_pubsub(JSContext *ctx, JSValueConst this_val,
if (JS_ToUint32(ctx, &id, argv[3]))
return JS_EXCEPTION;
res = runPubsub(&transportProfile, &networkAddressUrl, droneVariableArray,
countof(droneVariableArray), id, nbDrone, init_node_id,
get_drone_id, pubsub_get_value, pubsub_update_coordinates,
&pubsub_running);
if (JS_ToBool(ctx, argv[4])) {
res = runPubsub(&transportProfile, &networkAddressUrl,
droneVariableArray, countof(droneVariableArray), id,
nbDrone, init_node_id, get_drone_id, pubsub_get_value,
pubsub_update_coordinates, &pubsub_running);
} else {
res = subscribeOnly(&transportProfile, &networkAddressUrl,
droneVariableArray, countof(droneVariableArray), id,
nbDrone, init_node_id, get_drone_id,
pubsub_print_coordinates, &pubsub_running);
}
JS_FreeCString(ctx, ipv6);
JS_FreeCString(ctx, port);
......@@ -522,7 +566,7 @@ static const JSCFunctionListEntry js_mavsdk_funcs[] = {
JS_CFUNC_DEF("takeOffAndWait", 0, js_mavsdk_takeOffAndWait ),
JS_CFUNC_DEF("land", 0, js_mavsdk_land ),
JS_CFUNC_DEF("initPubsub", 1, js_init_pubsub ),
JS_CFUNC_DEF("runPubsub", 4, js_run_pubsub ),
JS_CFUNC_DEF("runPubsub", 5, js_run_pubsub ),
JS_CFUNC_DEF("setCheckpoint", 1, js_drone_set_checkpoint ),
JS_CFUNC_DEF("stopPubsub", 0, js_stop_pubsub ),
};
......
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
GitLab Nexedi Edition | About GitLab | About Nexedi | 沪ICP备2021021310号-2 | 沪ICP备2021021310号-7