Multicopter api update

See merge request !10

include/autopilot_API.h

@@ -17,13 +17,13 @@ DLL_PUBLIC int reboot(void);
 // Flight state management functions
 DLL_PUBLIC int arm(void);
 DLL_PUBLIC int takeOff(void);
-DLL_PUBLIC int takeOffAndWait(void);
-DLL_PUBLIC int triggerParachute(void);
+DLL_PUBLIC int land(void);
 
 // Flight management functions
-DLL_PUBLIC void loiter(double la, double lo, float a, float radius);
-DLL_PUBLIC void setAirSpeed_async(float airspeed);
-DLL_PUBLIC void setTargetCoordinates(double la, double lo, float a);
+DLL_PUBLIC int loiter(double latitude, double longitude, float altitude,
+                      float radius, float speed);
+DLL_PUBLIC int setTargetCoordinates(double latitude, double longitude,
+                                    float altitude, float speed);
 
 // Information functions
 DLL_PUBLIC float getAltitude(void);
@@ -37,7 +37,7 @@ DLL_PUBLIC float getYaw(void);
 DLL_PUBLIC float getSpeed(void);
 DLL_PUBLIC float getClimbRate(void);
 DLL_PUBLIC int gpsIsOk(void);
-DLL_PUBLIC int healthAllOk(void);
+DLL_PUBLIC int isReadyToFly(void);
 DLL_PUBLIC int isLanding(void);
 DLL_PUBLIC void updateLogAndProjection(void);
 #ifdef __cplusplus

include/pubsub.h

@@ -17,10 +17,17 @@
 
 #define MAX_STR_SIZE 1024
 
+struct strNode {
+  char *str;
+  struct strNode *next;
+};
+typedef struct {
+  struct strNode *head;
+  struct strNode *tail;
+} StrQueue;
+
 typedef struct {
     UA_UInt16 id;
-    UA_UInt32 positionArrayId;
-    UA_UInt32 speedArrayId;
     UA_Double latitude;
     UA_Double longitude;
     UA_Double altitudeAbs;
@@ -29,19 +36,27 @@ typedef struct {
     UA_Float  yaw;
     UA_Float  speed;
     UA_Float  climbRate;
-    char      message[MAX_STR_SIZE];
-    UA_UInt32 messageId;
-    char      log[MAX_STR_SIZE];
-    UA_UInt32 logId;
+    StrQueue receiveMessageQueue;
+    StrQueue receiveLogQueue;
 } JSDroneData;
 
 typedef struct {
-    UA_Double x;
-    UA_Double y;
-    UA_Double z;
+    UA_Double latitude;
+    UA_Double longitude;
+    UA_Double altitude;
     UA_Int64 timestamp;
 } JSPositionData;
 
+typedef union {
+    struct {
+        UA_UInt32 message;
+        UA_UInt32 positionArray;
+        UA_UInt32 speedArray;
+        UA_UInt32 log;
+    };
+    UA_UInt32 getter[4];
+ } PubsubVariableIDs;
+
 typedef struct {
     char*       name;
     char*       typeName;
@@ -64,16 +79,12 @@ typedef struct {
     VariableData *variableArray;
 } VariableStruct;
 
-typedef struct {
-    VariableStruct variables;
-    void          (*init_node_id)(UA_UInt32 id, UA_UInt32 nb, UA_UInt32 magic);
-} InstanceData;
-
 int runPubsub(const UA_Logger *logger, UA_String *transportProfile,
               UA_NetworkAddressUrlDataType *networkAddressUrl,
               VariableStruct variables, UA_UInt32 id,
-              InstanceData *readerArray, UA_UInt32 nbReader,
-              UA_UInt32 maxVariableNb, UA_Duration interval,
+              VariableStruct *pubsubVariableArray,
+              PubsubVariableIDs *pubsubIDArray,
+              UA_UInt32 nbReader, 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),
@@ -85,10 +96,6 @@ UA_String get_log(void);
 
 UA_UInt16 get_drone_id(UA_UInt32 nb);
 
-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);
 
 DLL_PUBLIC JSModuleDef *js_init_module(JSContext *ctx, const char *module_name);

pubsub.c

@@ -260,9 +260,9 @@ dataChangeNotificationCallback(UA_Server *server, UA_UInt32 monitoredItemId,
  * Add subscribedvariables to the DataSetReader */
 static UA_StatusCode
 addSubscribedVariables(UA_Server *server, UA_NodeId dataSetReaderId,
-                       VariableData *variableArray, UA_UInt32 nb,
-                       UA_UInt32 maxVariableNb, UA_Duration samplingInterval,
-                       void (*init_node_id)(UA_UInt32 id, UA_UInt32 nb, UA_UInt32 magic)) {
+                       VariableData *variableArray,
+                       PubsubVariableIDs pubsubIDs, UA_UInt32 nb,
+                       UA_Duration samplingInterval) {
     if(server == NULL)
         return UA_STATUSCODE_BADINTERNALERROR;
 
@@ -310,7 +310,7 @@ addSubscribedVariables(UA_Server *server, UA_NodeId dataSetReaderId,
 
         UA_NodeId newNode;
         retval |= UA_Server_addVariableNode(server,
-                                            UA_NODEID_NUMERIC(1, (UA_UInt32)maxVariableNb*nb + i + 50000),
+                                            UA_NODEID_NUMERIC(1, pubsubIDs.getter[i]),
                                             folderId,
                                             UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT),
                                             UA_QUALIFIEDNAME(1, (char *)readerConfig.dataSetMetaData.fields[i].name.data),
@@ -319,7 +319,6 @@ addSubscribedVariables(UA_Server *server, UA_NodeId dataSetReaderId,
 
         /*monitor variable*/
         UA_MonitoredItemCreateRequest monRequest = UA_MonitoredItemCreateRequest_default(newNode);
-        init_node_id(newNode.identifier.numeric, nb, i);
         monRequest.requestedParameters.samplingInterval = samplingInterval;
         UA_Server_createDataChangeMonitoredItem(server, UA_TIMESTAMPSTORETURN_SOURCE,
                                                 monRequest, NULL, dataChangeNotificationCallback);
@@ -430,8 +429,9 @@ setServer(UA_String *transportProfile,
 }
 
 static UA_StatusCode
-subscribe(UA_Server *server, InstanceData *instanceArray, UA_UInt32 id,
-          UA_UInt32 nbReader, UA_UInt32 maxVariableNb, UA_Duration interval,
+subscribe(UA_Server *server, VariableStruct *pubsubVariableArray,
+          PubsubVariableIDs *pubsubIDArray, UA_UInt32 id,
+          UA_UInt32 nbReader, UA_Duration interval,
           UA_UInt16 (*get_reader_id)(UA_UInt32 nb),
           void (*update)(UA_UInt32 id, const UA_DataValue*, bool print)) {
     UA_UInt16 publisherIdent;
@@ -455,16 +455,15 @@ subscribe(UA_Server *server, InstanceData *instanceArray, UA_UInt32 id,
         readerConfig.name = UA_STRING(readerName);
         readerConfig.publisherId.data = &publisherIdent;
 
-        retval = addDataSetReader(server, instanceArray[i].variables,
+        retval = addDataSetReader(server, pubsubVariableArray[i],
                                   publisherIdent);
         if (retval != UA_STATUSCODE_GOOD)
             return EXIT_FAILURE;
 
         /* Add SubscribedVariables to the created DataSetReader */
         retval = addSubscribedVariables(server, readerIdent,
-                                        instanceArray[i].variables.variableArray,
-                                        i, maxVariableNb, interval,
-                                        instanceArray[i].init_node_id);
+                                        pubsubVariableArray[i].variableArray,
+                                        pubsubIDArray[i], i, interval);
         if (retval != UA_STATUSCODE_GOOD)
             return EXIT_FAILURE;
     }
@@ -475,8 +474,9 @@ subscribe(UA_Server *server, InstanceData *instanceArray, UA_UInt32 id,
 int runPubsub(const UA_Logger *logger, UA_String *transportProfile,
               UA_NetworkAddressUrlDataType *networkAddressUrl,
               VariableStruct variables, UA_UInt32 id,
-              InstanceData *readerArray, UA_UInt32 nbReader,
-              UA_UInt32 maxVariableNb, UA_Duration interval,
+              VariableStruct *pubsubVariableArray,
+              PubsubVariableIDs *pubsubIDArray,
+              UA_UInt32 nbReader, 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),
@@ -507,8 +507,8 @@ int runPubsub(const UA_Logger *logger, UA_String *transportProfile,
 
     /* Subscribing */
 
-    subscribe(server, readerArray, id, nbReader, maxVariableNb, interval,
-              get_reader_id, update);
+    subscribe(server, pubsubVariableArray, pubsubIDArray, id, nbReader,
+              interval, get_reader_id, update);
 
     retval = UA_Server_run(server, running);
     UA_Server_delete(server);

qjs_wrapper.c

@@ -9,14 +9,6 @@
 
 #define DRONE_VARIABLE_NB 4
 #define SUBSCRIBER_VARIABLE_NB 1
-struct strNode {
-  char *str;
-  struct strNode *next;
-};
-typedef struct {
-  struct strNode *head;
-  struct strNode *tail;
-} StrQueue;
 
 UA_Int64 positionArray[POSITION_ARRAY_SIZE] = { 0 };
 UA_UInt32 positionArrayDims[] = {POSITION_ARRAY_SIZE};
@@ -30,6 +22,17 @@ UA_String initial_ua_string = {
 };
 
 VariableData droneVariableArray[] = {
+    {
+        .name = "message",
+        .description = "Message to send to the other drones",
+        .value = &initial_ua_string,
+        .type = UA_TYPES_STRING,
+        .builtInType = UA_NS0ID_STRING,
+        .valueRank = UA_VALUERANK_SCALAR,
+        .arrayDimensionsSize = 0,
+        .arrayDimensions = NULL,
+        .getter.getString = get_message,
+    },
     {
         .name = "positionArray",
         .typeName = "Position Array Type",
@@ -58,17 +61,6 @@ VariableData droneVariableArray[] = {
         .getter.getArray = (void * (*)(void))getSpeedArray,
 #endif
     },
-    {
-        .name = "message",
-        .description = "Message to send to the other drones",
-        .value = &initial_ua_string,
-        .type = UA_TYPES_STRING,
-        .builtInType = UA_NS0ID_STRING,
-        .valueRank = UA_VALUERANK_SCALAR,
-        .arrayDimensionsSize = 0,
-        .arrayDimensions = NULL,
-        .getter.getString = get_message,
-    },
     {
         .name = "log",
         .description = "Message to send to the other drones",
@@ -115,22 +107,22 @@ static UA_Boolean pubsubExited = true;
 static UA_UInt32 nbDrone;
 static UA_UInt32 nbSubscriber;
 static JSValueConst *droneObjectIdList;
-static StrQueue messageQueue = {
+static StrQueue sendMessageQueue = {
     .head = NULL,
     .tail = NULL,
 };
-UA_String currentMessage;
-static StrQueue logQueue = {
+UA_String currentSendMessage;
+static StrQueue sendLogQueue = {
     .head = NULL,
     .tail = NULL,
 };
-UA_String currentLog;
-
-pthread_mutex_t mutex;
-pthread_cond_t threadCond;
+UA_String currentSendLog;
 
 bool isADrone;
 
+PubsubVariableIDs *pubsubIdsArray;
+const UA_UInt32 VARIABLE_ID_BASE = 50000;
+
 // Position class functions
 
 static void js_position_finalizer(JSRuntime *rt, JSValue val)
@@ -155,10 +147,10 @@ static JSValue js_new_position(JSContext *ctx, JSValueConst thisVal,
         return JS_EXCEPTION;
     }
     newPositionArray = getPositionArray();
-    s->x = (double)newPositionArray[LATITUDE] / 1e7;
-    s->y = (double)newPositionArray[LONGITUDE] / 1e7;
-    s->z = (UA_Double)(newPositionArray[REL_ALTITUDE] / 1000);
-    s->timestamp = newPositionArray[TIMESTAMP];
+    s->latitude = (double)positionArray[LATITUDE] / 1e7;
+    s->longitude = (double)positionArray[LONGITUDE] / 1e7;
+    s->altitude = (UA_Double)(positionArray[REL_ALTITUDE] / 1000);
+    s->timestamp = positionArray[TIMESTAMP];
     JS_SetOpaque(obj, s);
     free(newPositionArray);
     return obj;
@@ -172,11 +164,11 @@ static JSValue js_position_get(JSContext *ctx, JSValueConst thisVal, int magic)
         return JS_EXCEPTION;
     switch(magic) {
         case 0:
-            return JS_NewFloat64(ctx, s->x);
+            return JS_NewFloat64(ctx, s->latitude);
         case 1:
-            return JS_NewFloat64(ctx, s->y);
+            return JS_NewFloat64(ctx, s->longitude);
         case 2:
-            return JS_NewFloat64(ctx, s->z);
+            return JS_NewFloat64(ctx, s->altitude);
         case 3:
             return JS_NewInt64(ctx, s->timestamp);
         default:
@@ -190,17 +182,35 @@ static JSClassDef jsPositionClass = {
 };
 
 static const JSCFunctionListEntry js_position_proto_funcs[] = {
-    JS_CGETSET_MAGIC_DEF("x", js_position_get, NULL, 0),
-    JS_CGETSET_MAGIC_DEF("y", js_position_get, NULL, 1),
-    JS_CGETSET_MAGIC_DEF("z", js_position_get, NULL, 2),
+    JS_CGETSET_MAGIC_DEF("latitude", js_position_get, NULL, 0),
+    JS_CGETSET_MAGIC_DEF("longitude", js_position_get, NULL, 1),
+    JS_CGETSET_MAGIC_DEF("altitude", js_position_get, NULL, 2),
     JS_CGETSET_MAGIC_DEF("timestamp", js_position_get, NULL, 3),
 };
 
 // Drone class functions
 
+static void delete_str_node(struct strNode *node) {
+    free(node->str);
+    free(node);
+}
+
+static void cleanQueue(StrQueue *pQueue)
+{
+    struct strNode *current;
+
+    while (pQueue->head != NULL) {
+        current = pQueue->head;
+        pQueue->head = current->next;
+        delete_str_node(current);
+    }
+}
+
 static void js_drone_finalizer(JSRuntime *rt, JSValue val)
 {
     JSDroneData *s = (JSDroneData *) JS_GetOpaque(val, jsDroneClassId);
+    cleanQueue(&(s->receiveMessageQueue));
+    cleanQueue(&(s->receiveLogQueue));
     js_free_rt(rt, s);
 }
 
@@ -218,6 +228,14 @@ static JSValue js_drone_ctor(JSContext *ctx, JSValueConst newTarget,
     if (JS_ToUint32(ctx, &uint32, argv[0]))
         goto fail;
     s->id = (uint16_t)uint32;
+    s->receiveMessageQueue = (StrQueue){
+        .head = NULL,
+        .tail = NULL,
+    };
+    s->receiveLogQueue = (StrQueue){
+        .head = NULL,
+        .tail = NULL,
+    };
 
     proto = JS_GetPropertyStr(ctx, newTarget, "prototype");
     if (JS_IsException(proto))
@@ -247,15 +265,26 @@ static JSValue js_drone_init(JSContext *ctx, JSValueConst thisVal,
     return JS_UNDEFINED;
 }
 
-static JSValue readDroneDataStr(JSContext *ctx, char *str)
+static JSValue readDroneDataStr(JSContext *ctx, StrQueue *pQueue, bool keepAtLeastAnElement)
 {
     JSValue res;
+    struct strNode *current;
 
-    pthread_mutex_lock(&mutex);
-    res = JS_NewString(ctx, str);
-    strcpy(str, "");
-    pthread_cond_signal(&threadCond);
-    pthread_mutex_unlock(&mutex);
+    current = pQueue->head;
+    if (current != NULL) {
+        res = JS_NewString(ctx, current->str);
+        if (current->next != NULL) {
+            pQueue->head = current->next;
+            delete_str_node(current);
+        } else {
+            if (!keepAtLeastAnElement) {
+                pQueue->head = (pQueue->tail = NULL);
+                delete_str_node(current);
+            }
+        }
+    } else {
+        res = JS_NewString(ctx, "");
+    }
     return res;
 }
 
@@ -282,9 +311,9 @@ static JSValue js_drone_get(JSContext *ctx, JSValueConst thisVal, int magic)
         case 7:
             return JS_NewFloat64(ctx, s->climbRate);
         case 8:
-            return readDroneDataStr(ctx, s->message);
+            return readDroneDataStr(ctx, &(s->receiveMessageQueue), true);
         case 9:
-            return readDroneDataStr(ctx, s->log);
+            return readDroneDataStr(ctx, &(s->receiveLogQueue), false);
         case 10:
             return JS_NewInt64(ctx, s->timestamp);
         default:
@@ -292,7 +321,7 @@ static JSValue js_drone_get(JSContext *ctx, JSValueConst thisVal, int magic)
     }
 }
 
-static void addStrToQueue(const char* str, StrQueue* pQueue)
+static void addStrToQueue(const char *str, StrQueue *pQueue)
 {
     struct strNode *newNode;
     newNode = (struct strNode*)malloc(sizeof(struct strNode));
@@ -307,7 +336,8 @@ static void addStrToQueue(const char* str, StrQueue* pQueue)
     }
 }
 
-static JSValue add_jsstr_to_queue(JSContext *ctx, JSValueConst jsStr, StrQueue *queue)
+static JSValue add_jsstr_to_queue(JSContext *ctx, JSValueConst jsStr,
+                                  StrQueue *pQueue)
 {
     const char *str;
     str = JS_ToCString(ctx, jsStr);
@@ -316,7 +346,7 @@ static JSValue add_jsstr_to_queue(JSContext *ctx, JSValueConst jsStr, StrQueue *
         return JS_EXCEPTION;
     }
 
-    addStrToQueue(str, queue);
+    addStrToQueue(str, pQueue);
     JS_FreeCString(ctx, str);
     return JS_UNDEFINED;
 }
@@ -324,18 +354,13 @@ static JSValue add_jsstr_to_queue(JSContext *ctx, JSValueConst jsStr, StrQueue *
 static JSValue js_drone_set_message(JSContext *ctx, JSValueConst thisVal,
                                     int argc, JSValueConst *argv)
 {
-    return add_jsstr_to_queue(ctx, argv[0], &messageQueue);
+    return add_jsstr_to_queue(ctx, argv[0], &sendMessageQueue);
 }
 
 static JSValue js_drone_set_log(JSContext *ctx, JSValueConst thisVal,
                                 int argc, JSValueConst *argv)
 {
-    return add_jsstr_to_queue(ctx, argv[0], &logQueue);
-}
-
-static void delete_str_node(struct strNode *node) {
-    free(node->str);
-    free(node);
+    return add_jsstr_to_queue(ctx, argv[0], &sendLogQueue);
 }
 
 static UA_Boolean UA_String_isEmpty(const UA_String *s) {
@@ -347,15 +372,15 @@ static void clear_str(UA_String *pstr) {
         UA_String_clear(pstr); 
 }
 
-static UA_String get_StrQueue_content(StrQueue *queue, UA_String currentStr)
+static UA_String get_StrQueue_content(StrQueue *pQueue, UA_String currentStr)
 {
     struct strNode *current;
 
-    current = queue->head;
+    current = pQueue->head;
     if (current != NULL) {
         clear_str(&currentStr);
         currentStr = UA_STRING_ALLOC(current->str);
-        queue->head = current->next == NULL ? (queue->tail = NULL) : current->next;
+        pQueue->head = current->next == NULL ? (pQueue->tail = NULL) : current->next;
         delete_str_node(current);
     }
     return currentStr;
@@ -363,12 +388,12 @@ static UA_String get_StrQueue_content(StrQueue *queue, UA_String currentStr)
 
 UA_String get_message(void)
 {
-    return get_StrQueue_content(&messageQueue, currentMessage);
+    return get_StrQueue_content(&sendMessageQueue, currentSendMessage);
 }
 
 UA_String get_log(void)
 {
-    return get_StrQueue_content(&logQueue, currentLog);
+    return get_StrQueue_content(&sendLogQueue, currentSendLog);
 }
 
 static JSClassDef jsDroneClass = {
@@ -394,7 +419,8 @@ static const JSCFunctionListEntry js_drone_proto_funcs[] = {
 // pubsub functions
 
 UA_UInt16 get_drone_id(UA_UInt32 nb) {
-    JSDroneData *s = (JSDroneData *) JS_GetOpaque(droneObjectIdList[nb], jsDroneClassId);
+    JSDroneData *s =
+        (JSDroneData *) JS_GetOpaque(droneObjectIdList[nb], jsDroneClassId);
     return s->id;
 }
 
@@ -411,10 +437,12 @@ VariableData pubsub_get_value(UA_String identifier) {
                 case UA_VALUERANK_SCALAR:
                     switch(varDetails.type) {
                         case UA_TYPES_STRING:
-                            *(UA_String*)varDetails.value = varDetails.getter.getString();
+                            *(UA_String*)varDetails.value =
+                                varDetails.getter.getString();
                             break;
                         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");
                             break;
                     }
                     break;
@@ -436,14 +464,16 @@ VariableData pubsub_get_value(UA_String identifier) {
                             ptrd += type.memSize;
                         }
                         if(retval != UA_STATUSCODE_GOOD)
-                            UA_LOG_ERROR(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Failed to update array");
+                            UA_LOG_ERROR(UA_Log_Stdout, UA_LOGCATEGORY_SERVER,
+                                         "Failed to update array");
                     }
 
                     free(array);
                     break;
 
                 default:
-                    UA_LOG_ERROR(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "UA_VALUERANK not handled");
+                    UA_LOG_ERROR(UA_Log_Stdout, UA_LOGCATEGORY_SERVER,
+                                 "UA_VALUERANK not handled");
                     break;
             }
         }
@@ -452,50 +482,13 @@ VariableData pubsub_get_value(UA_String identifier) {
     return varDetails;
 }
 
-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:
-            s->positionArrayId = id;
-            break;
-        case 1:
-            s->speedArrayId = id;
-            break;
-        case 2:
-            s->messageId = id;
-            break;
-        case 3:
-            s->logId = id;
-            break;
-        default:
-            UA_LOG_ERROR(UA_Log_Stdout, UA_LOGCATEGORY_USERLAND, "Unknown variable id");
-            break;
-    }
-}
-
-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 setDroneDataStr(void *data, char *str)
+static void setDroneDataStr(void *data, StrQueue* pQueue)
 {
     UA_String uaStr = *(UA_String*) data;
-    pthread_mutex_lock(&mutex);
-    while(strlen(str) != 0)
-        pthread_cond_wait(&threadCond, &mutex);
-
+    char str[MAX_STR_SIZE];
     memcpy(str, uaStr.data, uaStr.length);
     str[uaStr.length] = '\0';
-
-    pthread_mutex_unlock(&mutex);
+    addStrToQueue(str, pQueue);
 }
 
 static void pubsub_update_variables(UA_UInt32 id, const UA_DataValue *var, bool print)
@@ -506,7 +499,7 @@ static void pubsub_update_variables(UA_UInt32 id, const UA_DataValue *var, bool
 
     for(UA_UInt32 i = 0; i < nbDrone + nbSubscriber; i++) {
         s = (JSDroneData *) JS_GetOpaque(droneObjectIdList[i], jsDroneClassId);
-        if (s->positionArrayId == id) {
+        if (pubsubIdsArray[i].positionArray == id) {
             updatedPositionArray = (UA_Int64*) var->value.data;
             s->latitude = (double)updatedPositionArray[LATITUDE] / 1e7;
             s->longitude = (double)updatedPositionArray[LONGITUDE] / 1e7;
@@ -520,7 +513,7 @@ static void pubsub_update_variables(UA_UInt32 id, const UA_DataValue *var, bool
                   s->id, s->latitude, s->longitude, s->altitudeAbs, s->altitudeRel);
             }
             return;
-        } else if (s->speedArrayId == id) {
+        } else if (pubsubIdsArray[i].speedArray == id) {
             updatedSpeedArray = (UA_Float*) var->value.data;
             s->yaw = updatedSpeedArray[YAW];
             s->speed = updatedSpeedArray[SPEED];
@@ -532,17 +525,18 @@ static void pubsub_update_variables(UA_UInt32 id, const UA_DataValue *var, bool
                   s->id, s->yaw, s->speed, s->climbRate);
             }
             return;
-        } else if (s->messageId == id) {
-            setDroneDataStr(var->value.data, s->message);
+        } else if (pubsubIdsArray[i].message == id) {
+            setDroneDataStr(var->value.data, &(s->receiveMessageQueue));
             return;
-        } else if (s->logId == id) {
+        } else if (pubsubIdsArray[i].log == id) {
             if (!isADrone) {
-                setDroneDataStr(var->value.data, s->log);
+                setDroneDataStr(var->value.data, &(s->receiveLogQueue));
             }
             return;
         }
     }
-    UA_LOG_ERROR(UA_Log_Stdout, UA_LOGCATEGORY_CLIENT, "NodeId not found");
+    UA_LOG_ERROR(UA_Log_Stdout, UA_LOGCATEGORY_CLIENT,
+                 "NodeId %i not found", id);
 }
 
 /*
@@ -551,7 +545,7 @@ static void pubsub_update_variables(UA_UInt32 id, const UA_DataValue *var, bool
  * arg 2 (string): network interface used for multicast communication
  * arg 3 (int): ID of the drone
  * arg 4 (double): publication/subscription interval in ms
- * arg 5 (bool): true if there will be data to publish
+ * arg 5 (bool): true if the peer is a drone
 */
 static JSValue js_run_pubsub(JSContext *ctx, JSValueConst this_val,
                              int argc, JSValueConst *argv) {
@@ -563,7 +557,7 @@ static JSValue js_run_pubsub(JSContext *ctx, JSValueConst this_val,
     UA_UInt32 id;
     UA_Duration interval;
     VariableStruct variables;
-    InstanceData *instanceArray;
+    VariableStruct *pubsubVariableArray;
     UA_UInt32 nbPeer = nbDrone + nbSubscriber;
     int res;
 
@@ -589,25 +583,35 @@ static JSValue js_run_pubsub(JSContext *ctx, JSValueConst this_val,
     isADrone = JS_ToBool(ctx, argv[5]);
     variables = isADrone ? droneVariables : subscriberVariables;
 
-    instanceArray = (InstanceData *) malloc((nbPeer) * sizeof(InstanceData));
+    pubsubVariableArray = (VariableStruct *) malloc((nbPeer) * sizeof(VariableStruct));
+    pubsubIdsArray = (PubsubVariableIDs *) malloc((nbPeer) * sizeof(PubsubVariableIDs));
     for(UA_UInt32 i = 0; i < nbDrone; i++) {
-      instanceArray[i].variables = droneVariables;
-      instanceArray[i].init_node_id = init_drone_node_id;
+      pubsubVariableArray[i] = droneVariables;
+      PubsubVariableIDs ids = {
+          .message = (UA_UInt32) DRONE_VARIABLE_NB * i + VARIABLE_ID_BASE,
+          .positionArray = (UA_UInt32) DRONE_VARIABLE_NB * i + VARIABLE_ID_BASE + 1,
+          .speedArray = (UA_UInt32) DRONE_VARIABLE_NB * i + VARIABLE_ID_BASE + 2,
+          .log = (UA_UInt32) DRONE_VARIABLE_NB * i + VARIABLE_ID_BASE + 3,
+      };
+      pubsubIdsArray[i] = ids;
     }
     for(UA_UInt32 i = nbDrone; i < nbPeer; i++) {
-      instanceArray[i].variables = subscriberVariables;
-      instanceArray[i].init_node_id = init_subscriber_node_id;
+      pubsubVariableArray[i] = subscriberVariables;
+      PubsubVariableIDs ids = {
+          .message = (UA_UInt32) DRONE_VARIABLE_NB * i + VARIABLE_ID_BASE,
+      };
+      pubsubIdsArray[i] = ids;
     }
 
     pubsubExited = false;
     res = runPubsub(UA_Log_Stdout, &transportProfile, &networkAddressUrl,
-                    variables, id, instanceArray, nbPeer,
-                    fmax(DRONE_VARIABLE_NB, SUBSCRIBER_VARIABLE_NB), interval,
-                    get_drone_id, pubsub_get_value, pubsub_update_variables,
-                    &pubsubShouldRun);
+                    variables, id, pubsubVariableArray, pubsubIdsArray, nbPeer,
+                    interval, get_drone_id, pubsub_get_value,
+                    pubsub_update_variables, &pubsubShouldRun);
     pubsubExited = true;
 
-    free(instanceArray);
+    free(pubsubVariableArray);
+    free(pubsubIdsArray);
     JS_FreeCString(ctx, ipv6);
     JS_FreeCString(ctx, port);
     free(notConstNetIface);
@@ -624,24 +628,12 @@ static JSValue js_init_pubsub(JSContext *ctx, JSValueConst thisVal,
     if (JS_ToUint32(ctx, &nbSubscriber, argv[1]))
         return JS_EXCEPTION;
 
-    currentMessage = UA_STRING("");
-    currentLog = UA_STRING("");
+    currentSendMessage = UA_STRING("");
+    currentSendLog = UA_STRING("");
     droneObjectIdList = (JSValue *) malloc((nbDrone + nbSubscriber) * sizeof(JSValueConst));
     return JS_NewInt32(ctx, 0);
 }
 
-static void cleanQueue(StrQueue queue, UA_String current_str)
-{
-    struct strNode *current;
-
-    while (queue.head != NULL) {
-        current = queue.head;
-        queue.head = current->next;
-        delete_str_node(current);
-    }
-    clear_str(&current_str);
-}
-
 static JSValue js_stop_pubsub(JSContext *ctx, JSValueConst thisVal,
                               int argc, JSValueConst *argv)
 {
@@ -650,8 +642,10 @@ static JSValue js_stop_pubsub(JSContext *ctx, JSValueConst thisVal,
         sleep(1);
     free(droneObjectIdList);
 
-    cleanQueue(messageQueue, currentMessage);
-    cleanQueue(logQueue, currentLog);
+    cleanQueue(&sendMessageQueue);
+    clear_str(&currentSendMessage);
+    cleanQueue(&sendLogQueue);
+    clear_str(&currentSendLog);
 
     return JS_NewInt32(ctx, 0);
 }
@@ -707,16 +701,10 @@ static JSValue js_takeOff(JSContext *ctx, JSValueConst thisVal,
     return JS_NewInt32(ctx, takeOff());
 }
 
-static JSValue js_takeOffAndWait(JSContext *ctx, JSValueConst thisVal,
-                                 int argc, JSValueConst *argv)
-{
-    return JS_NewInt32(ctx, takeOffAndWait());
-}
-
-static JSValue js_triggerParachute(JSContext *ctx, JSValueConst thisVal,
-                                   int argc, JSValueConst *argv)
+static JSValue js_land(JSContext *ctx, JSValueConst thisVal,
+                       int argc, JSValueConst *argv)
 {
-    return JS_NewInt32(ctx, triggerParachute());
+    return JS_NewInt32(ctx, land());
 }
 
 // Flight management functions
@@ -724,53 +712,45 @@ static JSValue js_triggerParachute(JSContext *ctx, JSValueConst thisVal,
 static JSValue js_loiter(JSContext *ctx, JSValueConst thisVal,
                          int argc, JSValueConst *argv)
 {
-    double la_arg_double;
-    double lo_arg_double;
-    double a_arg_double;
+    double latitude;
+    double longitude;
+    double altitude;
     double radius;
+    double speed;
 
-    if (JS_ToFloat64(ctx, &la_arg_double, argv[0]))
+    if (JS_ToFloat64(ctx, &latitude, argv[0]))
         return JS_EXCEPTION;
-    if (JS_ToFloat64(ctx, &lo_arg_double, argv[1]))
+    if (JS_ToFloat64(ctx, &longitude, argv[1]))
         return JS_EXCEPTION;
-    if (JS_ToFloat64(ctx, &a_arg_double, argv[2]))
+    if (JS_ToFloat64(ctx, &altitude, argv[2]))
         return JS_EXCEPTION;
     if (JS_ToFloat64(ctx, &radius, argv[3]))
         return JS_EXCEPTION;
-
-    loiter(la_arg_double, lo_arg_double, (float)a_arg_double, (float)radius);
-    return JS_NewInt32(ctx, 0);
-}
-
-static JSValue js_setAirSpeed(JSContext *ctx, JSValueConst thisVal,
-                              int argc, JSValueConst *argv)
-{
-    double altitude;
-
-    if (JS_ToFloat64(ctx, &altitude, argv[0]))
+    if (JS_ToFloat64(ctx, &speed, argv[4]))
         return JS_EXCEPTION;
 
-    setAirSpeed_async((float)altitude);
-    return JS_NewInt32(ctx, 0);
+    return JS_NewInt32(ctx, loiter(latitude, longitude, (float)altitude, (float)radius, (float)speed));
 }
 
 static JSValue js_setTargetCoordinates(JSContext *ctx,
                                        JSValueConst thisVal,
                                        int argc, JSValueConst *argv)
 {
-    double la_arg_double;
-    double lo_arg_double;
-    double a_arg_double;
+    double latitude;
+    double longitude;
+    double altitude;
+    double speed;
 
-    if (JS_ToFloat64(ctx, &la_arg_double, argv[0]))
+    if (JS_ToFloat64(ctx, &latitude, argv[0]))
+        return JS_EXCEPTION;
+    if (JS_ToFloat64(ctx, &longitude, argv[1]))
         return JS_EXCEPTION;
-    if (JS_ToFloat64(ctx, &lo_arg_double, argv[1]))
+    if (JS_ToFloat64(ctx, &altitude, argv[2]))
         return JS_EXCEPTION;
-    if (JS_ToFloat64(ctx, &a_arg_double, argv[2]))
+    if (JS_ToFloat64(ctx, &speed, argv[3]))
         return JS_EXCEPTION;
 
-    setTargetCoordinates(la_arg_double, lo_arg_double, (float)a_arg_double);
-    return JS_NewInt32(ctx, 0);
+    return JS_NewInt32(ctx, setTargetCoordinates(latitude, longitude, (float)altitude, (float)speed));
 }
 
 // Information functions
@@ -828,15 +808,15 @@ static JSValue js_getClimbRate(JSContext *ctx, JSValueConst thisVal,
 }
 
 static JSValue js_gpsIsOk(JSContext *ctx, JSValueConst this_val,
-                              int argc, JSValueConst *argv)
+                          int argc, JSValueConst *argv)
 {
     return JS_NewBool(ctx, gpsIsOk());
 }
 
-static JSValue js_healthAllOk(JSContext *ctx, JSValueConst this_val,
-                              int argc, JSValueConst *argv)
+static JSValue js_isReadyToFly(JSContext *ctx, JSValueConst this_val,
+                               int argc, JSValueConst *argv)
 {
-    return JS_NewBool(ctx, healthAllOk());
+    return JS_NewBool(ctx, isReadyToFly());
 }
 
 static JSValue js_isLanding(JSContext *ctx, JSValueConst this_val,
@@ -865,11 +845,9 @@ static const JSCFunctionListEntry js_funcs[] = {
     JS_CFUNC_DEF("reboot", 0, js_reboot ),
     JS_CFUNC_DEF("arm", 0, js_arm ),
     JS_CFUNC_DEF("takeOff", 0, js_takeOff ),
-    JS_CFUNC_DEF("takeOffAndWait", 0, js_takeOffAndWait ),
-    JS_CFUNC_DEF("triggerParachute", 0, js_triggerParachute ),
-    JS_CFUNC_DEF("loiter", 4, js_loiter ),
-    JS_CFUNC_DEF("setAirSpeed", 1, js_setAirSpeed ),
-    JS_CFUNC_DEF("setTargetCoordinates", 3, js_setTargetCoordinates ),
+    JS_CFUNC_DEF("land", 0, js_land ),
+    JS_CFUNC_DEF("loiter", 5, js_loiter ),
+    JS_CFUNC_DEF("setTargetCoordinates", 4, js_setTargetCoordinates ),
     JS_CFUNC_DEF("getPosition", 0, js_new_position ),
     JS_CFUNC_DEF("getAltitude", 0, js_getAltitude ),
     JS_CFUNC_DEF("getInitialAltitude", 0, js_getInitialAltitude ),
@@ -877,11 +855,11 @@ static const JSCFunctionListEntry js_funcs[] = {
     JS_CFUNC_DEF("getInitialLongitude", 0, js_getInitialLongitude ),
     JS_CFUNC_DEF("getTakeOffAltitude", 0, js_getTakeOffAltitude ),
     JS_CFUNC_DEF("getYaw", 0, js_getYaw ),
-    JS_CFUNC_DEF("getAirspeed", 0, js_getSpeed ),
+    JS_CFUNC_DEF("getSpeed", 0, js_getSpeed ),
     JS_CFUNC_DEF("getClimbRate", 0, js_getClimbRate ),
     JS_CFUNC_DEF("gpsIsOk", 0, js_gpsIsOk ),
+    JS_CFUNC_DEF("isReadyToFly", 0, js_isReadyToFly ),
     JS_CFUNC_DEF("isLanding", 0, js_isLanding ),
-    JS_CFUNC_DEF("healthAllOk", 0, js_healthAllOk ),
     JS_CFUNC_DEF("updateLogAndProjection", 0, js_updateLogAndProjection ),
 #endif
 };