Skip to content

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

Allow subscribers to send messages

parent 62eaed3d
Hide whitespace changes
Inline Side-by-side
Showing with 145 additions and 85 deletions
include/dronedge.h
View file @ 63cd9e69
......@@ -5,6 +5,8 @@
#include "autopilot_wrapper.h"
#include "pubsub.h"
#define MAX_VARIABLE_NB 3
struct messageNode {
char *message;
struct messageNode *next;
......@@ -63,4 +65,28 @@ VariableData droneVariableArray[] = {
},
};
VariableStruct droneVariables = {
.nbVariable = 3,
.variableArray = droneVariableArray,
};
VariableData subscriberVariableArray[] = {
{
.name = "message",
.description = "Message to send to the other drones",
.value = &message,
.type = UA_TYPES_STRING,
.builtInType = UA_NS0ID_STRING,
.valueRank = UA_VALUERANK_SCALAR,
.arrayDimensionsSize = 0,
.arrayDimensions = NULL,
.getter.getString = get_message,
},
};
VariableStruct subscriberVariables = {
.nbVariable = 1,
.variableArray = subscriberVariableArray,
};
#endif /* __DRONEDGE_H__ */
include/pubsub.h
View file @ 63cd9e69
......@@ -49,21 +49,33 @@ typedef struct {
} getter;
} VariableData;
typedef struct {
size_t nbVariable;
VariableData *variableArray;
} VariableStruct;
typedef struct {
VariableStruct variables;
void (*init_node_id)(UA_UInt32 id, UA_UInt32 nb, UA_UInt32 magic);
} InstanceData;
int runPubsub(UA_String *transportProfile,
UA_NetworkAddressUrlDataType *networkAddressUrl,
VariableData *variableArray, size_t nbVariable,
UA_UInt32 id, UA_UInt32 nbReader, UA_Duration interval,
void (*init_node_id)(UA_UInt32 id, UA_UInt32 nb, UA_UInt32 magic),
VariableStruct variables, UA_UInt32 id,
InstanceData *readerArray, UA_UInt32 nbReader,
UA_UInt32 maxVariableNb, UA_Duration interval,
UA_UInt16 (*get_reader_id)(UA_UInt32 nb),
VariableData (*get_value)(UA_String identifier),
void (*update)(UA_UInt32 id, const UA_DataValue*, bool print),
bool publish, UA_Boolean *running);
void (*update)(UA_UInt32 id, const UA_DataValue*),
UA_Boolean *running);
UA_String get_message(void);
UA_UInt16 get_drone_id(UA_UInt32 nb);
void init_node_id(UA_UInt32 id, UA_UInt32 nb, UA_UInt32 magic);
void init_drone_node_id(UA_UInt32 id, UA_UInt32 nb, UA_UInt32 magic);
void init_subscriber_node_id(UA_UInt32 id, UA_UInt32 nb, UA_UInt32 magic);
VariableData pubsub_get_value(UA_String identifier);
......
pubsub.c
View file @ 63cd9e69
......@@ -10,14 +10,12 @@ UA_NodeId connectionIdent, publishedDataSetIdent, writerGroupIdent,
readerGroupIdent, readerIdent;
UA_DataSetReaderConfig readerConfig;
bool isPublisher;
VariableData (*pubsubGetValue)(UA_String identifier);
static void (*callbackUpdate)(UA_UInt32, const UA_DataValue*, bool print);
static void (*callbackUpdate)(UA_UInt32, const UA_DataValue*);
static void fillDataSetMetaData(UA_DataSetMetaDataType *pMetaData,
VariableData *variableArray,
size_t nbVariable, int id);
VariableStruct variables, int id);
static UA_StatusCode
addPubSubConnection(UA_Server *server, UA_String *transportProfile,
......@@ -234,14 +232,12 @@ addReaderGroup(UA_Server *server) {
* SubscribedDataSet and be contained within a ReaderGroup. */
/* Add DataSetReader to the ReaderGroup */
static UA_StatusCode
addDataSetReader(UA_Server *server, VariableData *variableArray,
size_t nbVariable, int id) {
addDataSetReader(UA_Server *server, VariableStruct variables, int id) {
if(server == NULL)
return UA_STATUSCODE_BADINTERNALERROR;
/* Setting up Meta data configuration in DataSetReader */
fillDataSetMetaData(&readerConfig.dataSetMetaData, variableArray,
nbVariable, id);
fillDataSetMetaData(&readerConfig.dataSetMetaData, variables, id);
return UA_Server_addDataSetReader(server, readerGroupIdent,
&readerConfig, &readerIdent);
......@@ -252,7 +248,7 @@ dataChangeNotificationCallback(UA_Server *server, UA_UInt32 monitoredItemId,
void *monitoredItemContext, const UA_NodeId *nodeId,
void *nodeContext, UA_UInt32 attributeId,
const UA_DataValue *var) {
callbackUpdate(nodeId->identifier.numeric, var, !isPublisher);
callbackUpdate(nodeId->identifier.numeric, var);
}
/**
......@@ -263,7 +259,7 @@ dataChangeNotificationCallback(UA_Server *server, UA_UInt32 monitoredItemId,
static UA_StatusCode
addSubscribedVariables(UA_Server *server, UA_NodeId dataSetReaderId,
VariableData *variableArray, UA_UInt32 nb,
UA_Duration samplingInterval,
UA_UInt32 maxVariableNb, UA_Duration samplingInterval,
void (*init_node_id)(UA_UInt32 id, UA_UInt32 nb, UA_UInt32 magic)) {
if(server == NULL)
return UA_STATUSCODE_BADINTERNALERROR;
......@@ -312,7 +308,7 @@ addSubscribedVariables(UA_Server *server, UA_NodeId dataSetReaderId,
UA_NodeId newNode;
retval |= UA_Server_addVariableNode(server,
UA_NODEID_NUMERIC(1, (UA_UInt32) readerConfig.dataSetMetaData.fieldsSize*nb + i + 50000),
UA_NODEID_NUMERIC(1, (UA_UInt32)maxVariableNb*nb + i + 50000),
folderId,
UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT),
UA_QUALIFIEDNAME(1, (char *)readerConfig.dataSetMetaData.fields[i].name.data),
......@@ -351,8 +347,7 @@ addSubscribedVariables(UA_Server *server, UA_NodeId dataSetReaderId,
* and PublishedDataSetFields of Publisher */
/* Define MetaData for TargetVariables */
static void fillDataSetMetaData(UA_DataSetMetaDataType *pMetaData,
VariableData *variableArray,
size_t nbVariable, int id) {
VariableStruct variables, int id) {
char name[12];
UA_snprintf(name, sizeof(name) - 1, "DataSet %d", id);
......@@ -364,7 +359,7 @@ static void fillDataSetMetaData(UA_DataSetMetaDataType *pMetaData,
/* Definition of number of fields sizeto create different
* targetVariables of distinct datatype */
pMetaData->fieldsSize = nbVariable;
pMetaData->fieldsSize = variables.nbVariable;
UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_USERLAND, "fieldsSize %d", (int) pMetaData->fieldsSize);
pMetaData->fields = (UA_FieldMetaData*)UA_Array_new (pMetaData->fieldsSize,
&UA_TYPES[UA_TYPES_FIELDMETADATA]);
......@@ -372,26 +367,26 @@ static void fillDataSetMetaData(UA_DataSetMetaDataType *pMetaData,
for(size_t i = 0; i < pMetaData->fieldsSize; i++) {
UA_FieldMetaData_init (&pMetaData->fields[i]);
UA_NodeId_copy(&UA_TYPES[variableArray[i].type].typeId, &pMetaData->fields[i].dataType);
pMetaData->fields[i].builtInType = variableArray[i].builtInType;
pMetaData->fields[i].name = UA_STRING (variableArray[i].name);
pMetaData->fields[i].valueRank = variableArray[i].valueRank;
pMetaData->fields[i].arrayDimensions = variableArray[i].arrayDimensions;
pMetaData->fields[i].arrayDimensionsSize = variableArray[i].arrayDimensionsSize;
UA_NodeId_copy(&UA_TYPES[variables.variableArray[i].type].typeId, &pMetaData->fields[i].dataType);
pMetaData->fields[i].builtInType = variables.variableArray[i].builtInType;
pMetaData->fields[i].name = UA_STRING (variables.variableArray[i].name);
pMetaData->fields[i].valueRank = variables.variableArray[i].valueRank;
pMetaData->fields[i].arrayDimensions = variables.variableArray[i].arrayDimensions;
pMetaData->fields[i].arrayDimensionsSize = variables.variableArray[i].arrayDimensionsSize;
}
}
static void
setVariableType(UA_Server *server, VariableData *variableArray, size_t nbVariable) {
setVariableType(UA_Server *server, VariableStruct variables) {
VariableData vDetails;
UA_VariableTypeAttributes vtAttr;
for(UA_UInt32 i = 0; i < nbVariable; i++) {
vDetails = variableArray[i];
for(UA_UInt32 i = 0; i < variables.nbVariable; i++) {
vDetails = variables.variableArray[i];
switch(vDetails.valueRank) {
case UA_VALUERANK_SCALAR:
variableArray[i].typeNodeId = UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE);
vDetails.typeNodeId = UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE);
break;
case UA_VALUERANK_ONE_DIMENSION:
......@@ -433,12 +428,10 @@ setServer(UA_String *transportProfile,
}
static UA_StatusCode
subscribe(UA_Server *server,
VariableData *variableArray, size_t nbVariable,
UA_UInt32 id, UA_UInt32 nbReader, UA_Duration interval,
void (*init_node_id)(UA_UInt32 id, UA_UInt32 nb, UA_UInt32 magic),
subscribe(UA_Server *server, InstanceData *instanceArray, UA_UInt32 id,
UA_UInt32 nbReader, UA_UInt32 maxVariableNb, UA_Duration interval,
UA_UInt16 (*get_reader_id)(UA_UInt32 nb),
void (*update)(UA_UInt32 id, const UA_DataValue*, bool print)) {
void (*update)(UA_UInt32 id, const UA_DataValue*)) {
UA_UInt16 publisherIdent;
UA_StatusCode retval;
char readerName[19];
......@@ -460,12 +453,16 @@ subscribe(UA_Server *server,
readerConfig.name = UA_STRING(readerName);
readerConfig.publisherId.data = &publisherIdent;
retval = addDataSetReader(server, variableArray, nbVariable, publisherIdent);
retval = addDataSetReader(server, instanceArray[i].variables,
publisherIdent);
if (retval != UA_STATUSCODE_GOOD)
return EXIT_FAILURE;
/* Add SubscribedVariables to the created DataSetReader */
retval = addSubscribedVariables(server, readerIdent, variableArray, i, interval, init_node_id);
retval = addSubscribedVariables(server, readerIdent,
instanceArray[i].variables.variableArray,
i, maxVariableNb, interval,
instanceArray[i].init_node_id);
if (retval != UA_STATUSCODE_GOOD)
return EXIT_FAILURE;
}
......@@ -475,43 +472,40 @@ subscribe(UA_Server *server,
int runPubsub(UA_String *transportProfile,
UA_NetworkAddressUrlDataType *networkAddressUrl,
VariableData *variableArray, size_t nbVariable,
UA_UInt32 id, UA_UInt32 nbReader, UA_Duration interval,
void (*init_node_id)(UA_UInt32 id, UA_UInt32 nb, UA_UInt32 magic),
VariableStruct variables, UA_UInt32 id,
InstanceData *readerArray, UA_UInt32 nbReader,
UA_UInt32 maxVariableNb, UA_Duration interval,
UA_UInt16 (*get_reader_id)(UA_UInt32 nb),
VariableData (*get_value)(UA_String identifier),
void (*update)(UA_UInt32 id, const UA_DataValue*, bool print),
bool publish, UA_Boolean *running) {
void (*update)(UA_UInt32 id, const UA_DataValue*),
UA_Boolean *running) {
UA_Server *server;
UA_StatusCode retval;
server = setServer(transportProfile, networkAddressUrl, id);
setVariableType(server, variableArray, nbVariable);
setVariableType(server, variables);
/* Publishing */
isPublisher = publish;
if (isPublisher) {
pubsubGetValue = get_value;
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, interval);
retval = addDataSetWriter(server);
addPublishedDataSet(server, id);
for(UA_UInt32 i = 0; i < variables.nbVariable; i++) {
retval = addDataSourceVariable(server, variables.variableArray[i]);
if (retval != UA_STATUSCODE_GOOD)
return EXIT_FAILURE;
addDataSetField(server, variables.variableArray[i]);
}
addWriterGroup(server, interval);
retval = addDataSetWriter(server);
if (retval != UA_STATUSCODE_GOOD)
return EXIT_FAILURE;
/* Subscribing */
subscribe(server, variableArray, nbVariable, id, nbReader, interval,
init_node_id, get_reader_id, update);
subscribe(server, readerArray, id, nbReader, maxVariableNb, interval,
get_reader_id, update);
retval = UA_Server_run(server, running);
UA_Server_delete(server);
......
qjs_wrapper.c
View file @ 63cd9e69
......@@ -7,6 +7,7 @@ static UA_Boolean pubsubShouldRun = true;
static UA_Boolean pubsubExited = false;
static UA_UInt32 nbDrone;
static UA_UInt32 nbSubscriber;
static JSValueConst *droneObjectIdList;
static MessageQueue messageQueue = {
.head = NULL,
......@@ -17,6 +18,8 @@ UA_String currentMessage;
pthread_mutex_t mutex;
pthread_cond_t threadCond;
bool isADrone;
// Drone class functions
static void js_drone_finalizer(JSRuntime *rt, JSValue val)
......@@ -188,15 +191,15 @@ VariableData pubsub_get_value(UA_String identifier) {
UA_DataType type;
UA_Double *array;
for(UA_UInt32 i = 0; i < countof(droneVariableArray); i++) {
UA_String name = UA_STRING(droneVariableArray[i].name);
for(UA_UInt32 i = 0; i < droneVariables.nbVariable; i++) {
UA_String name = UA_STRING(droneVariables.variableArray[i].name);
if(UA_String_equal(&identifier, &name)) {
varDetails = droneVariableArray[i];
varDetails = droneVariables.variableArray[i];
switch(varDetails.valueRank) {
case UA_VALUERANK_SCALAR:
switch(varDetails.type) {
case UA_TYPES_STRING:
*(UA_String*)varDetails.value = droneVariableArray[i].getter.getString();
*(UA_String*)varDetails.value = varDetails.getter.getString();
break;
default:
UA_LOG_ERROR(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "UA_TYPE not handled");
......@@ -207,7 +210,7 @@ VariableData pubsub_get_value(UA_String identifier) {
case UA_VALUERANK_ONE_DIMENSION:
type = UA_TYPES[varDetails.type];
size_t size = varDetails.arrayDimensions[0];
array = (UA_Double *) droneVariableArray[i].getter.getArray();
array = (UA_Double *) varDetails.getter.getArray();
if(type.pointerFree) {
memcpy(varDetails.value, array, type.memSize * size);
......@@ -237,7 +240,7 @@ VariableData pubsub_get_value(UA_String identifier) {
return varDetails;
}
void init_node_id(UA_UInt32 id, UA_UInt32 nb, UA_UInt32 magic) {
void init_drone_node_id(UA_UInt32 id, UA_UInt32 nb, UA_UInt32 magic) {
JSDroneData *s = (JSDroneData *) JS_GetOpaque(droneObjectIdList[nb], jsDroneClassId);
switch(magic) {
case 0:
......@@ -255,14 +258,26 @@ void init_node_id(UA_UInt32 id, UA_UInt32 nb, UA_UInt32 magic) {
}
}
static void pubsub_update_variables(UA_UInt32 id, const UA_DataValue *var, bool print)
void init_subscriber_node_id(UA_UInt32 id, UA_UInt32 nb, UA_UInt32 magic) {
JSDroneData *s = (JSDroneData *) JS_GetOpaque(droneObjectIdList[nb], jsDroneClassId);
switch(magic) {
case 0:
s->messageId = id;
break;
default:
UA_LOG_ERROR(UA_Log_Stdout, UA_LOGCATEGORY_USERLAND, "Unknown variable id");
break;
}
}
static void pubsub_update_variables(UA_UInt32 id, const UA_DataValue *var)
{
JSDroneData* s;
UA_String uaStr;
UA_Double* positionArray;
UA_Float* speedArray;
for(UA_UInt32 i = 0; i < nbDrone; i++) {
for(UA_UInt32 i = 0; i < nbDrone + nbSubscriber; i++) {
s = (JSDroneData *) JS_GetOpaque(droneObjectIdList[i], jsDroneClassId);
if (s->positionArrayId == id) {
positionArray = (UA_Double*) var->value.data;
......@@ -271,7 +286,7 @@ static void pubsub_update_variables(UA_UInt32 id, const UA_DataValue *var, bool
s->altitudeAbs = positionArray[2];
s->altitudeRel = positionArray[3];
if (print) {
if (!isADrone) {
UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_CLIENT,
"Received position of drone %d: %f° %f° %fm %fm",
s->id, s->latitude, s->longitude, s->altitudeAbs, s->altitudeRel);
......@@ -283,7 +298,7 @@ static void pubsub_update_variables(UA_UInt32 id, const UA_DataValue *var, bool
s->speed = speedArray[1];
s->climbRate = speedArray[2];
if (print) {
if (!isADrone) {
UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_CLIENT,
"Received speed of drone %d: %f° %fm/s %fm/s",
s->id, s->yaw, s->speed, s->climbRate);
......@@ -292,21 +307,14 @@ static void pubsub_update_variables(UA_UInt32 id, const UA_DataValue *var, bool
} else if (s->messageId == id) {
uaStr = *(UA_String*) var->value.data;
if (!print) {
pthread_mutex_lock(&mutex);
while(strlen(s->message) != 0)
pthread_cond_wait(&threadCond, &mutex);
}
pthread_mutex_lock(&mutex);
while(strlen(s->message) != 0)
pthread_cond_wait(&threadCond, &mutex);
memcpy(s->message, uaStr.data, uaStr.length);
s->message[uaStr.length] = '\0';
if (print) {
UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_CLIENT,
"Received message for drone %d: %s", s->id, s->message);
} else {
pthread_mutex_unlock(&mutex);
}
pthread_mutex_unlock(&mutex);
return;
}
}
......@@ -331,6 +339,9 @@ static JSValue js_run_pubsub(JSContext *ctx, JSValueConst this_val,
char urlBuffer[44];
UA_UInt32 id;
UA_Duration interval;
VariableStruct variables;
InstanceData *instanceArray;
UA_UInt32 nbPeer = nbDrone + nbSubscriber;
int res;
ipv6 = JS_ToCString(ctx, argv[0]);
......@@ -352,13 +363,26 @@ static JSValue js_run_pubsub(JSContext *ctx, JSValueConst this_val,
if (JS_ToFloat64(ctx, &interval, argv[4]))
return JS_EXCEPTION;
res = runPubsub(&transportProfile, &networkAddressUrl, droneVariableArray,
countof(droneVariableArray), id, nbDrone, interval,
init_node_id, get_drone_id, pubsub_get_value,
pubsub_update_variables, JS_ToBool(ctx, argv[5]),
isADrone = JS_ToBool(ctx, argv[5]);
variables = isADrone ? droneVariables : subscriberVariables;
instanceArray = (InstanceData *) malloc((nbPeer) * sizeof(InstanceData));
for(UA_UInt32 i = 0; i < nbDrone; i++) {
instanceArray[i].variables = droneVariables;
instanceArray[i].init_node_id = init_drone_node_id;
}
for(UA_UInt32 i = nbDrone; i < nbPeer; i++) {
instanceArray[i].variables = subscriberVariables;
instanceArray[i].init_node_id = init_subscriber_node_id;
}
res = runPubsub(&transportProfile, &networkAddressUrl, variables, id,
instanceArray, nbPeer, MAX_VARIABLE_NB, interval,
get_drone_id, pubsub_get_value, pubsub_update_variables,
&pubsubShouldRun);
pubsubExited = true;
free(instanceArray);
JS_FreeCString(ctx, ipv6);
JS_FreeCString(ctx, port);
free(notConstNetIface);
......@@ -372,8 +396,11 @@ static JSValue js_init_pubsub(JSContext *ctx, JSValueConst thisVal,
if (JS_ToUint32(ctx, &nbDrone, argv[0]))
return JS_EXCEPTION;
if (JS_ToUint32(ctx, &nbSubscriber, argv[1]))
return JS_EXCEPTION;
currentMessage = UA_STRING("");
droneObjectIdList = (JSValue *) malloc(nbDrone * sizeof(JSValueConst));
droneObjectIdList = (JSValue *) malloc((nbDrone + nbSubscriber) * sizeof(JSValueConst));
return JS_NewInt32(ctx, 0);
}
......@@ -393,6 +420,7 @@ static JSValue js_stop_pubsub(JSContext *ctx, JSValueConst thisVal,
delete_message_node(current);
}
clear_message(currentMessage);
return JS_NewInt32(ctx, 0);
}
......@@ -617,7 +645,7 @@ static const JSCFunctionListEntry js_funcs[] = {
JS_CFUNC_DEF("getAirspeed", 0, js_getSpeed ),
JS_CFUNC_DEF("getClimbRate", 0, js_getClimbRate ),
JS_CFUNC_DEF("healthAllOk", 0, js_healthAllOk ),
JS_CFUNC_DEF("initPubsub", 1, js_init_pubsub ),
JS_CFUNC_DEF("initPubsub", 2, js_init_pubsub ),
};
static int js_init(JSContext *ctx, JSModuleDef *m)
......
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