Allow subscribers to send messages

include/dronedge.h

@@ -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

@@ -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);
 

qjs_wrapper.c

@@ -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)