Add source code to use MavSDK and open62541

Add a wrapper for fixedwings drone control functions of MavSDK as well as code to use publish and subscribe functionnalities of OPC-UA # Please enter the commit message for your changes. Lines starting

Makefile

+CPPFLAGS=-g -Wall -Wno-array-bounds -Wno-format-truncation -std=c99 -Iinclude -O2 -fPIC
+LDFLAGS+= -g
+LIBS=-lstdc++ -lmavsdk -lmavsdk_action -lmavsdk_mavlink_passthrough -lmavsdk_telemetry -lopen62541
+
+LIB_NAME := libqjswrapper.so
+
+SRCS := mavsdk_wrapper.cpp pubsub_publish.c pubsub_subscribe.c qjs_wrapper.c
+OBJS := mavsdk_wrapper.o pubsub_publish.o pubsub_subscribe.o qjs_wrapper.o
+
+all: $(LIB_NAME)
+
+%.o: 
+
+$(LIB_NAME): $(OBJS)
+	$(CC) $(LDFLAGS) -shared -o $@ $^ $(LIBS)
+
+install: all
+	mkdir -p "$(DESTDIR)$(prefix)/lib"
+	install -m644 $(LIB_NAME) "$(DESTDIR)$(prefix)/lib"
+
+clean:
+	$(RM) -f $(OBJS) $(LIB_NAME)

include/mavsdk_wrapper.h

+#ifndef __MAVSDK_H__
+#define __MAVSDK_H__
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+#include <stdint.h>
+// Connexion management functions
+int mavsdk_start(const char * url, const char * log_file, int timeout,
+                 void (*publishCoordinates)(double, double, float));
+int mavsdk_stop(void);
+int mavsdk_reboot(void);
+
+// Flight state management functions
+int mavsdk_arm(void);
+int mavsdk_doParachute(int param);
+int mavsdk_loiter(double radius);
+int mavsdk_land(void);
+int mavsdk_takeOff(void);
+int mavsdk_takeOffAndWait(void);
+
+// Flight management functions
+int mavsdk_doReposition(double la, double lo, double a, double y);
+int mavsdk_loiterUnlimited(double radius, double la, double lo, double a);
+int mavsdk_setAirspeed(double airspeed);
+int mavsdk_setAltitude(double altitude);
+int mavsdk_setTargetAltitude(double a);
+int mavsdk_setTargetCoordinates(double la, double lo, double a, double y);
+int mavsdk_setTargetCoordinatesXYZ(double x, double y, double z);
+int mavsdk_setTargetLatLong(double la, double lo);
+
+// Information functions
+double mavsdk_getAltitude(void);
+double mavsdk_getInitialAltitude(void);
+double mavsdk_getInitialLatitude(void);
+double mavsdk_getInitialLongitude(void);
+double mavsdk_getLatitude(void);
+double mavsdk_getLongitude(void);
+double mavsdk_getPitch(void);
+double mavsdk_getRoll(void);
+double mavsdk_getTakeOffAltitude(void);
+double mavsdk_getYaw(void);
+int mavsdk_healthAllOk(void);
+int mavsdk_landed(void);
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __MAVSDK_H__ */

include/pubsub_common.h

+#ifndef __PUBSUB_COMMON_H__
+#define __PUBSUB_COMMON_H__
+
+#include <open62541/server.h>
+
+#define countof(x) (sizeof(x) / sizeof((x)[0]))
+
+#endif /* __PUBSUB_COMMON_H__ */
\ No newline at end of file

include/pubsub_publish.h

+#ifndef __PUBSUB_PUBLISH_H__
+#define __PUBSUB_PUBLISH_H__
+
+#include "pubsub_common.h"
+
+void writeFloat(char *name, float value);
+void writeDouble(char *name, double value);
+int publish(UA_String *transportProfile,
+            UA_NetworkAddressUrlDataType *networkAddressUrl,
+            UA_Boolean *running);
+
+#endif /* __PUBSUB_PUBLISH_H__ */

include/pubsub_subscribe.h

+#ifndef __PUBSUB_SUBSCRIBE_H__
+#define __PUBSUB_SUBSCRIBE_H__
+
+#include "pubsub_common.h"
+
+int subscribe(UA_String *transportProfile,
+              UA_NetworkAddressUrlDataType *networkAddressUrl,
+              UA_Boolean *running);
+
+#endif /* __PUBSUB_SUBSCRIBE_H__ */

pubsub_publish.c

+#include <open62541/plugin/log_stdout.h>
+#include <open62541/plugin/pubsub_udp.h>
+#include <open62541/server_config_default.h>
+
+#include "pubsub_publish.h"
+
+typedef struct PublishedVariable {
+    char *name;
+    char *description;
+    void * UA_RESTRICT pdefaultValue;
+    UA_DataType type;
+} PublishedVariable;
+
+static UA_Server *server;
+UA_NodeId connectionIdent, publishedDataSetIdent, writerGroupIdent;
+
+static void
+addPubSubConnection(UA_Server *server, UA_String *transportProfile,
+                    UA_NetworkAddressUrlDataType *networkAddressUrl){
+    /* Details about the connection configuration and handling are located
+     * in the pubsub connection tutorial */
+    UA_PubSubConnectionConfig connectionConfig;
+    memset(&connectionConfig, 0, sizeof(connectionConfig));
+    connectionConfig.name = UA_STRING("UADP Connection 1");
+    connectionConfig.transportProfileUri = *transportProfile;
+    connectionConfig.enabled = UA_TRUE;
+    UA_Variant_setScalar(&connectionConfig.address, networkAddressUrl,
+                         &UA_TYPES[UA_TYPES_NETWORKADDRESSURLDATATYPE]);
+    /* Changed to static publisherId from random generation to identify
+     * the publisher on Subscriber side */
+    connectionConfig.publisherId.numeric = 2234;
+    UA_Server_addPubSubConnection(server, &connectionConfig, &connectionIdent);
+}
+
+/**
+ * **PublishedDataSet handling**
+ *
+ * The PublishedDataSet (PDS) and PubSubConnection are the toplevel entities and
+ * can exist alone. The PDS contains the collection of the published fields. All
+ * other PubSub elements are directly or indirectly linked with the PDS or
+ * connection. */
+static void
+addPublishedDataSet(UA_Server *server) {
+    /* The PublishedDataSetConfig contains all necessary public
+    * information for the creation of a new PublishedDataSet */
+    UA_PublishedDataSetConfig publishedDataSetConfig;
+    memset(&publishedDataSetConfig, 0, sizeof(UA_PublishedDataSetConfig));
+    publishedDataSetConfig.publishedDataSetType = UA_PUBSUB_DATASET_PUBLISHEDITEMS;
+    publishedDataSetConfig.name = UA_STRING("Demo PDS");
+    /* Create new PublishedDataSet based on the PublishedDataSetConfig. */
+    UA_Server_addPublishedDataSet(server, &publishedDataSetConfig, &publishedDataSetIdent);
+}
+
+static void
+addVariable(UA_Server *server, PublishedVariable varDetails) {
+    UA_VariableAttributes attr = UA_VariableAttributes_default;
+    UA_Variant_setScalar(&attr.value, varDetails.pdefaultValue, &varDetails.type);
+    attr.description = UA_LOCALIZEDTEXT("en-US", varDetails.description);
+    attr.displayName = UA_LOCALIZEDTEXT("en-US", varDetails.description);
+    attr.dataType = varDetails.type.typeId;
+    attr.accessLevel = UA_ACCESSLEVELMASK_READ | UA_ACCESSLEVELMASK_WRITE;
+
+    UA_Server_addVariableNode(server, UA_NODEID_STRING(1, varDetails.name),
+                                      UA_NODEID_NUMERIC(0, UA_NS0ID_OBJECTSFOLDER),
+                                      UA_NODEID_NUMERIC(0, UA_NS0ID_ORGANIZES),
+                                      UA_QUALIFIEDNAME(1, varDetails.description),
+                                      UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE),
+                                      attr, NULL, NULL);
+}
+
+static void
+writeVariable(char *name, void * UA_RESTRICT pvalue,
+              UA_DataType type)
+{
+    UA_NodeId integerNodeId = UA_NODEID_STRING(1, name);
+    UA_Variant var;
+    UA_Variant_init(&var);
+    UA_Variant_setScalar(&var, pvalue, &type);
+    UA_Server_writeValue(server, integerNodeId, var);
+}
+
+void writeFloat(char *name, float externValue) {
+    float localValue = externValue;
+    writeVariable(name, &localValue, UA_TYPES[UA_TYPES_FLOAT]);
+}
+
+void writeDouble(char *name, double externValue) {
+    double localValue = externValue;
+    writeVariable(name, &localValue, UA_TYPES[UA_TYPES_DOUBLE]);
+}
+
+static void
+addDataSetField(UA_Server *server, PublishedVariable varDetails) {
+    UA_NodeId dataSetFieldIdent;
+    UA_DataSetFieldConfig dataSetFieldConfig;
+    memset(&dataSetFieldConfig, 0, sizeof(UA_DataSetFieldConfig));
+    dataSetFieldConfig.dataSetFieldType = UA_PUBSUB_DATASETFIELD_VARIABLE;
+    dataSetFieldConfig.field.variable.fieldNameAlias = UA_STRING(varDetails.description);
+    dataSetFieldConfig.field.variable.promotedField = UA_FALSE;
+    dataSetFieldConfig.field.variable.publishParameters.publishedVariable =
+    UA_NODEID_STRING(1, varDetails.name);
+    dataSetFieldConfig.field.variable.publishParameters.attributeId = UA_ATTRIBUTEID_VALUE;
+    UA_Server_addDataSetField(server, publishedDataSetIdent,
+                              &dataSetFieldConfig, &dataSetFieldIdent);
+}
+
+/**
+ * **WriterGroup handling**
+ *
+ * The WriterGroup (WG) is part of the connection and contains the primary
+ * configuration parameters for the message creation. */
+static void
+addWriterGroup(UA_Server *server) {
+    /* Now we create a new WriterGroupConfig and add the group to the existing
+     * PubSubConnection. */
+    UA_WriterGroupConfig writerGroupConfig;
+    memset(&writerGroupConfig, 0, sizeof(UA_WriterGroupConfig));
+    writerGroupConfig.name = UA_STRING("Demo WriterGroup");
+    writerGroupConfig.publishingInterval = 100;
+    writerGroupConfig.enabled = UA_FALSE;
+    writerGroupConfig.writerGroupId = 100;
+    writerGroupConfig.encodingMimeType = UA_PUBSUB_ENCODING_UADP;
+    writerGroupConfig.messageSettings.encoding             = UA_EXTENSIONOBJECT_DECODED;
+    writerGroupConfig.messageSettings.content.decoded.type = &UA_TYPES[UA_TYPES_UADPWRITERGROUPMESSAGEDATATYPE];
+    /* The configuration flags for the messages are encapsulated inside the
+     * message- and transport settings extension objects. These extension
+     * objects are defined by the standard. e.g.
+     * UadpWriterGroupMessageDataType */
+    UA_UadpWriterGroupMessageDataType *writerGroupMessage  = UA_UadpWriterGroupMessageDataType_new();
+    /* Change message settings of writerGroup to send PublisherId,
+     * WriterGroupId in GroupHeader and DataSetWriterId in PayloadHeader
+     * of NetworkMessage */
+    writerGroupMessage->networkMessageContentMask          = (UA_UadpNetworkMessageContentMask)(UA_UADPNETWORKMESSAGECONTENTMASK_PUBLISHERID |
+                                                              (UA_UadpNetworkMessageContentMask)UA_UADPNETWORKMESSAGECONTENTMASK_GROUPHEADER |
+                                                              (UA_UadpNetworkMessageContentMask)UA_UADPNETWORKMESSAGECONTENTMASK_WRITERGROUPID |
+                                                              (UA_UadpNetworkMessageContentMask)UA_UADPNETWORKMESSAGECONTENTMASK_PAYLOADHEADER);
+    writerGroupConfig.messageSettings.content.decoded.data = writerGroupMessage;
+    UA_Server_addWriterGroup(server, connectionIdent, &writerGroupConfig, &writerGroupIdent);
+    UA_Server_setWriterGroupOperational(server, writerGroupIdent);
+    UA_UadpWriterGroupMessageDataType_delete(writerGroupMessage);
+}
+
+/**
+ * **DataSetWriter handling**
+ *
+ * A DataSetWriter (DSW) is the glue between the WG and the PDS. The DSW is
+ * linked to exactly one PDS and contains additional information for the
+ * message generation. */
+static void
+addDataSetWriter(UA_Server *server) {
+    /* We need now a DataSetWriter within the WriterGroup. This means we must
+     * create a new DataSetWriterConfig and add call the addWriterGroup function. */
+    UA_NodeId dataSetWriterIdent;
+    UA_DataSetWriterConfig dataSetWriterConfig;
+    memset(&dataSetWriterConfig, 0, sizeof(UA_DataSetWriterConfig));
+    dataSetWriterConfig.name = UA_STRING("Demo DataSetWriter");
+    dataSetWriterConfig.dataSetWriterId = 62541;
+    dataSetWriterConfig.keyFrameCount = 10;
+    UA_Server_addDataSetWriter(server, writerGroupIdent, publishedDataSetIdent,
+                               &dataSetWriterConfig, &dataSetWriterIdent);
+}
+
+int publish(UA_String *transportProfile,
+            UA_NetworkAddressUrlDataType *networkAddressUrl,
+            UA_Boolean *running) {
+    int i;
+    UA_Float defaultFloat = 0;
+    UA_Double defaultDouble = 0;
+
+    const PublishedVariable publishedVariableArray[] = {
+        {
+            .name = "lattitude",
+            .description = "Lattitude",
+            .pdefaultValue = &defaultDouble,
+            .type = UA_TYPES[UA_TYPES_DOUBLE],
+        },
+        {
+            .name = "longitude",
+            .description = "Longitude",
+            .pdefaultValue = &defaultDouble,
+            .type = UA_TYPES[UA_TYPES_DOUBLE],
+        },
+        {
+            .name = "altitude",
+            .description = "Altitude",
+            .pdefaultValue = &defaultFloat,
+            .type = UA_TYPES[UA_TYPES_FLOAT],
+        },
+    };
+
+    server = UA_Server_new();
+    UA_ServerConfig *config = UA_Server_getConfig(server);
+    UA_ServerConfig_setDefault(config);
+    UA_ServerConfig_addPubSubTransportLayer(config, UA_PubSubTransportLayerUDPMP());
+
+    addPubSubConnection(server, transportProfile, networkAddressUrl);
+    addPublishedDataSet(server);
+    for(i = 0; i < countof(publishedVariableArray); i++) {
+        addVariable(server, publishedVariableArray[i]);
+        addDataSetField(server, publishedVariableArray[i]);
+    }
+    addWriterGroup(server);
+    addDataSetWriter(server);
+
+    UA_StatusCode retval = UA_Server_run(server, running);
+
+    UA_Server_delete(server);
+    return retval == UA_STATUSCODE_GOOD ? EXIT_SUCCESS : EXIT_FAILURE;
+}