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

Allow subscribers to send messages

parent 62ab1198
...@@ -5,6 +5,8 @@ ...@@ -5,6 +5,8 @@
#include "autopilot_wrapper.h" #include "autopilot_wrapper.h"
#include "pubsub.h" #include "pubsub.h"
#define MAX_VARIABLE_NB 3
struct messageNode { struct messageNode {
char *message; char *message;
struct messageNode *next; struct messageNode *next;
...@@ -63,4 +65,28 @@ VariableData droneVariableArray[] = { ...@@ -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__ */ #endif /* __DRONEDGE_H__ */
...@@ -49,21 +49,33 @@ typedef struct { ...@@ -49,21 +49,33 @@ typedef struct {
} getter; } getter;
} VariableData; } 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, int runPubsub(UA_String *transportProfile,
UA_NetworkAddressUrlDataType *networkAddressUrl, UA_NetworkAddressUrlDataType *networkAddressUrl,
VariableData *variableArray, size_t nbVariable, VariableStruct variables, UA_UInt32 id,
UA_UInt32 id, UA_UInt32 nbReader, UA_Duration interval, InstanceData *readerArray, UA_UInt32 nbReader,
void (*init_node_id)(UA_UInt32 id, UA_UInt32 nb, UA_UInt32 magic), UA_UInt32 maxVariableNb, UA_Duration interval,
UA_UInt16 (*get_reader_id)(UA_UInt32 nb), UA_UInt16 (*get_reader_id)(UA_UInt32 nb),
VariableData (*get_value)(UA_String identifier), VariableData (*get_value)(UA_String identifier),
void (*update)(UA_UInt32 id, const UA_DataValue*, bool print), void (*update)(UA_UInt32 id, const UA_DataValue*, bool print),
bool publish, UA_Boolean *running); UA_Boolean *running);
UA_String get_message(void); UA_String get_message(void);
UA_UInt16 get_drone_id(UA_UInt32 nb); 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); VariableData pubsub_get_value(UA_String identifier);
......
This diff is collapsed.
...@@ -7,6 +7,7 @@ static UA_Boolean pubsubShouldRun = true; ...@@ -7,6 +7,7 @@ static UA_Boolean pubsubShouldRun = true;
static UA_Boolean pubsubExited = false; static UA_Boolean pubsubExited = false;
static UA_UInt32 nbDrone; static UA_UInt32 nbDrone;
static UA_UInt32 nbSubscriber;
static JSValueConst *droneObjectIdList; static JSValueConst *droneObjectIdList;
static MessageQueue messageQueue = { static MessageQueue messageQueue = {
.head = NULL, .head = NULL,
...@@ -188,15 +189,15 @@ VariableData pubsub_get_value(UA_String identifier) { ...@@ -188,15 +189,15 @@ VariableData pubsub_get_value(UA_String identifier) {
UA_DataType type; UA_DataType type;
UA_Double *array; UA_Double *array;
for(UA_UInt32 i = 0; i < countof(droneVariableArray); i++) { for(UA_UInt32 i = 0; i < droneVariables.nbVariable; i++) {
UA_String name = UA_STRING(droneVariableArray[i].name); UA_String name = UA_STRING(droneVariables.variableArray[i].name);
if(UA_String_equal(&identifier, &name)) { if(UA_String_equal(&identifier, &name)) {
varDetails = droneVariableArray[i]; varDetails = droneVariables.variableArray[i];
switch(varDetails.valueRank) { switch(varDetails.valueRank) {
case UA_VALUERANK_SCALAR: case UA_VALUERANK_SCALAR:
switch(varDetails.type) { switch(varDetails.type) {
case UA_TYPES_STRING: case UA_TYPES_STRING:
*(UA_String*)varDetails.value = droneVariableArray[i].getter.getString(); *(UA_String*)varDetails.value = varDetails.getter.getString();
break; break;
default: default:
UA_LOG_ERROR(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "UA_TYPE not handled"); UA_LOG_ERROR(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "UA_TYPE not handled");
...@@ -207,7 +208,7 @@ VariableData pubsub_get_value(UA_String identifier) { ...@@ -207,7 +208,7 @@ VariableData pubsub_get_value(UA_String identifier) {
case UA_VALUERANK_ONE_DIMENSION: case UA_VALUERANK_ONE_DIMENSION:
type = UA_TYPES[varDetails.type]; type = UA_TYPES[varDetails.type];
size_t size = varDetails.arrayDimensions[0]; size_t size = varDetails.arrayDimensions[0];
array = (UA_Double *) droneVariableArray[i].getter.getArray(); array = (UA_Double *) varDetails.getter.getArray();
if(type.pointerFree) { if(type.pointerFree) {
memcpy(varDetails.value, array, type.memSize * size); memcpy(varDetails.value, array, type.memSize * size);
...@@ -237,7 +238,7 @@ VariableData pubsub_get_value(UA_String identifier) { ...@@ -237,7 +238,7 @@ VariableData pubsub_get_value(UA_String identifier) {
return varDetails; 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); JSDroneData *s = (JSDroneData *) JS_GetOpaque(droneObjectIdList[nb], jsDroneClassId);
switch(magic) { switch(magic) {
case 0: case 0:
...@@ -255,6 +256,18 @@ void init_node_id(UA_UInt32 id, UA_UInt32 nb, UA_UInt32 magic) { ...@@ -255,6 +256,18 @@ void init_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) {
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, bool print) static void pubsub_update_variables(UA_UInt32 id, const UA_DataValue *var, bool print)
{ {
JSDroneData* s; JSDroneData* s;
...@@ -262,7 +275,7 @@ static void pubsub_update_variables(UA_UInt32 id, const UA_DataValue *var, bool ...@@ -262,7 +275,7 @@ static void pubsub_update_variables(UA_UInt32 id, const UA_DataValue *var, bool
UA_Double* positionArray; UA_Double* positionArray;
UA_Float* speedArray; 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); s = (JSDroneData *) JS_GetOpaque(droneObjectIdList[i], jsDroneClassId);
if (s->positionArrayId == id) { if (s->positionArrayId == id) {
positionArray = (UA_Double*) var->value.data; positionArray = (UA_Double*) var->value.data;
...@@ -292,21 +305,14 @@ static void pubsub_update_variables(UA_UInt32 id, const UA_DataValue *var, bool ...@@ -292,21 +305,14 @@ static void pubsub_update_variables(UA_UInt32 id, const UA_DataValue *var, bool
} else if (s->messageId == id) { } else if (s->messageId == id) {
uaStr = *(UA_String*) var->value.data; uaStr = *(UA_String*) var->value.data;
if (!print) { pthread_mutex_lock(&mutex);
pthread_mutex_lock(&mutex); while(strlen(s->message) != 0)
while(strlen(s->message) != 0) pthread_cond_wait(&threadCond, &mutex);
pthread_cond_wait(&threadCond, &mutex);
}
memcpy(s->message, uaStr.data, uaStr.length); memcpy(s->message, uaStr.data, uaStr.length);
s->message[uaStr.length] = '\0'; s->message[uaStr.length] = '\0';
if (print) { pthread_mutex_unlock(&mutex);
UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_CLIENT,
"Received message for drone %d: %s", s->id, s->message);
} else {
pthread_mutex_unlock(&mutex);
}
return; return;
} }
} }
...@@ -331,6 +337,10 @@ static JSValue js_run_pubsub(JSContext *ctx, JSValueConst this_val, ...@@ -331,6 +337,10 @@ static JSValue js_run_pubsub(JSContext *ctx, JSValueConst this_val,
char urlBuffer[44]; char urlBuffer[44];
UA_UInt32 id; UA_UInt32 id;
UA_Duration interval; UA_Duration interval;
bool isADrone;
VariableStruct variables;
InstanceData *instanceArray;
UA_UInt32 nbPeer = nbDrone + nbSubscriber;
int res; int res;
ipv6 = JS_ToCString(ctx, argv[0]); ipv6 = JS_ToCString(ctx, argv[0]);
...@@ -352,13 +362,26 @@ static JSValue js_run_pubsub(JSContext *ctx, JSValueConst this_val, ...@@ -352,13 +362,26 @@ static JSValue js_run_pubsub(JSContext *ctx, JSValueConst this_val,
if (JS_ToFloat64(ctx, &interval, argv[4])) if (JS_ToFloat64(ctx, &interval, argv[4]))
return JS_EXCEPTION; return JS_EXCEPTION;
res = runPubsub(&transportProfile, &networkAddressUrl, droneVariableArray, isADrone = JS_ToBool(ctx, argv[5]);
countof(droneVariableArray), id, nbDrone, interval, variables = isADrone ? droneVariables : subscriberVariables;
init_node_id, get_drone_id, pubsub_get_value,
pubsub_update_variables, JS_ToBool(ctx, argv[5]), 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); &pubsubShouldRun);
pubsubExited = true; pubsubExited = true;
free(instanceArray);
JS_FreeCString(ctx, ipv6); JS_FreeCString(ctx, ipv6);
JS_FreeCString(ctx, port); JS_FreeCString(ctx, port);
free(notConstNetIface); free(notConstNetIface);
...@@ -372,8 +395,11 @@ static JSValue js_init_pubsub(JSContext *ctx, JSValueConst thisVal, ...@@ -372,8 +395,11 @@ static JSValue js_init_pubsub(JSContext *ctx, JSValueConst thisVal,
if (JS_ToUint32(ctx, &nbDrone, argv[0])) if (JS_ToUint32(ctx, &nbDrone, argv[0]))
return JS_EXCEPTION; return JS_EXCEPTION;
if (JS_ToUint32(ctx, &nbSubscriber, argv[1]))
return JS_EXCEPTION;
currentMessage = UA_STRING(""); currentMessage = UA_STRING("");
droneObjectIdList = (JSValue *) malloc(nbDrone * sizeof(JSValueConst)); droneObjectIdList = (JSValue *) malloc((nbDrone + nbSubscriber) * sizeof(JSValueConst));
return JS_NewInt32(ctx, 0); return JS_NewInt32(ctx, 0);
} }
...@@ -393,6 +419,7 @@ static JSValue js_stop_pubsub(JSContext *ctx, JSValueConst thisVal, ...@@ -393,6 +419,7 @@ static JSValue js_stop_pubsub(JSContext *ctx, JSValueConst thisVal,
delete_message_node(current); delete_message_node(current);
} }
clear_message(currentMessage); clear_message(currentMessage);
return JS_NewInt32(ctx, 0); return JS_NewInt32(ctx, 0);
} }
...@@ -617,7 +644,7 @@ static const JSCFunctionListEntry js_funcs[] = { ...@@ -617,7 +644,7 @@ static const JSCFunctionListEntry js_funcs[] = {
JS_CFUNC_DEF("getAirspeed", 0, js_getSpeed ), JS_CFUNC_DEF("getAirspeed", 0, js_getSpeed ),
JS_CFUNC_DEF("getClimbRate", 0, js_getClimbRate ), JS_CFUNC_DEF("getClimbRate", 0, js_getClimbRate ),
JS_CFUNC_DEF("healthAllOk", 0, js_healthAllOk ), 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) 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