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