Group variables

See merge request !6

include/dronedge.h

@@ -14,10 +14,12 @@ typedef struct {
   struct messageNode *tail;
 } MessageQueue;
 
-UA_Double latitude = 0;
-UA_Double longitude = 0;
-UA_Float altitude_abs = 0;
-UA_Float altitude_rel = 0;
+UA_Double positionArray[POSITION_ARRAY_SIZE] = { 0 };
+UA_UInt32 positionArrayDims[] = {POSITION_ARRAY_SIZE};
+
+UA_Double speedArray[SPEED_ARRAY_SIZE] = { 0 };
+UA_UInt32 speedArrayDims[] = {SPEED_ARRAY_SIZE};
+
 UA_String message = {
     .length = 0,
     .data = NULL,
@@ -25,36 +27,28 @@ UA_String message = {
 
 VariableData droneVariableArray[] = {
     {
-        .name = "latitude",
-        .description = "Latitude",
-        .value = &latitude,
-        .type = UA_TYPES_DOUBLE,
-        .builtInType = UA_NS0ID_DOUBLE,
-        .getter.getDouble = mavsdk_getLatitude,
-    },
-    {
-        .name = "longitude",
-        .description = "Longitude",
-        .value = &longitude,
+        .name = "positionArray",
+        .typeName = "Position Array Type",
+        .description = "Position Array",
+        .value = &positionArray,
         .type = UA_TYPES_DOUBLE,
         .builtInType = UA_NS0ID_DOUBLE,
-        .getter.getDouble = mavsdk_getLongitude,
+        .valueRank = UA_VALUERANK_ONE_DIMENSION,
+        .arrayDimensionsSize = 1,
+        .arrayDimensions = positionArrayDims,
+        .getter.getArray = mavsdk_getPositionArray,
     },
     {
-        .name = "altitude_abs",
-        .description = "Absolute Altitude (AMSL)",
-        .value = &altitude_abs,
+        .name = "speedArray",
+        .typeName = "Speed Array Type",
+        .description = "Speed Array",
+        .value = &speedArray,
         .type = UA_TYPES_FLOAT,
         .builtInType = UA_NS0ID_FLOAT,
-        .getter.getFloat = mavsdk_getAltitude,
-    },
-    {
-        .name = "altitude_rel",
-        .description = "Relative Altitude",
-        .value = &altitude_rel,
-        .type = UA_TYPES_FLOAT,
-        .builtInType = UA_NS0ID_FLOAT,
-        .getter.getFloat = mavsdk_getAltitudeRel,
+        .valueRank = UA_VALUERANK_ONE_DIMENSION,
+        .arrayDimensionsSize = 1,
+        .arrayDimensions = speedArrayDims,
+        .getter.getArray = mavsdk_getSpeedArray,
     },
     {
         .name = "message",
@@ -62,9 +56,11 @@ VariableData droneVariableArray[] = {
         .value = &message,
         .type = UA_TYPES_STRING,
         .builtInType = UA_NS0ID_STRING,
-        .getter.getString = get_message
+        .valueRank = UA_VALUERANK_SCALAR,
+        .arrayDimensionsSize = 0,
+        .arrayDimensions = NULL,
+        .getter.getString = get_message,
     },
 };
 
-
 #endif /* __DRONEDGE_H__ */

include/mavsdk_wrapper.h

 #ifndef __MAVSDK_H__
 #define __MAVSDK_H__
 
+/*
+ * 0. latitude (double, degrees)
+ * 1. longitude (double, degrees)
+ * 2. absolute altitude (double, meters)
+ * 3. relative altitude (double, meters)
+*/
+#define POSITION_ARRAY_SIZE 4
+/*
+ * 0. yaw angle (float, degrees)
+ * 1. air speed (float, m/s)
+ * 2. climb rate (float, m/s)
+*/
+#define SPEED_ARRAY_SIZE 3
+
 #ifdef __cplusplus
 extern "C" {
 #endif
@@ -32,8 +46,12 @@ double mavsdk_getInitialLatitude(void);
 double mavsdk_getInitialLongitude(void);
 double mavsdk_getLatitude(void);
 double mavsdk_getLongitude(void);
+double *mavsdk_getPositionArray(void);
+float *mavsdk_getSpeedArray(void);
 double mavsdk_getTakeOffAltitude(void);
 float mavsdk_getYaw(void);
+float mavsdk_getSpeed(void);
+float mavsdk_getClimbRate(void);
 int mavsdk_healthAllOk(void);
 bool mavsdk_isInManualMode(void);
 int mavsdk_landed(void);

include/pubsub.h

@@ -17,41 +17,36 @@
 
 typedef struct {
     UA_UInt16 id;
+    UA_UInt32 positionArrayId;
+    UA_UInt32 speedArrayId;
     UA_Double latitude;
-    UA_UInt32 latitudeId;
     UA_Double longitude;
-    UA_UInt32 longitudeId;
-    UA_Float altitudeAbs;
-    UA_UInt32 altitudeAbsId;
-    UA_Float altitudeRel;
-    UA_UInt32 altitudeRelId;
-    char message[MAX_MESSAGE_SIZE];
+    UA_Double altitudeAbs;
+    UA_Double altitudeRel;
+    UA_Float  yaw;
+    UA_Float  speed;
+    UA_Float  climbRate;
+    char      message[MAX_MESSAGE_SIZE];
     UA_UInt32 messageId;
 } JSDroneData;
 
 typedef struct {
-    char *name;
-    char *description;
-    void * UA_RESTRICT value;
-    int type;
-    UA_Byte builtInType;
+    char*       name;
+    char*       typeName;
+    UA_NodeId   typeNodeId;
+    char*       description;
+    void*       value;
+    int         type;
+    UA_Byte     builtInType;
+    UA_Int32    valueRank;
+    size_t      arrayDimensionsSize;
+    UA_UInt32*  arrayDimensions;
     union {
-        UA_Double   (*getDouble)(void);
-        UA_Float    (*getFloat)(void);
-        UA_String   (*getString)(void);
+        void*     (*getArray)(void);
+        UA_String (*getString)(void);
     } getter;
-
 } VariableData;
 
-int subscribeOnly(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),
-                  UA_UInt16 (*get_reader_id)(UA_UInt32 nb),
-                  void (*update)(UA_UInt32 id, const UA_DataValue*),
-                  UA_Boolean *running);
-
 int runPubsub(UA_String *transportProfile,
               UA_NetworkAddressUrlDataType *networkAddressUrl,
               VariableData *variableArray, size_t nbVariable, 
@@ -59,8 +54,8 @@ int runPubsub(UA_String *transportProfile,
               void (*init_node_id)(UA_UInt32 id, UA_UInt32 nb, UA_UInt32 magic),
               UA_UInt16 (*get_reader_id)(UA_UInt32 nb),
               VariableData (*get_value)(UA_String identifier),
-              void (*update)(UA_UInt32 id, const UA_DataValue*),
-              UA_Boolean *running);
+              void (*update)(UA_UInt32 id, const UA_DataValue*, bool print),
+              bool publish, UA_Boolean *running);
 
 UA_String get_message(void);
 
@@ -68,10 +63,6 @@ UA_UInt16 get_drone_id(UA_UInt32 nb);
 
 void init_node_id(UA_UInt32 id, UA_UInt32 nb, UA_UInt32 magic);
 
-void pubsub_update_coordinates(UA_UInt32 id, const UA_DataValue *var);
-
-void pubsub_print_coordinates(UA_UInt32 id, const UA_DataValue *var);
-
 VariableData pubsub_get_value(UA_String identifier);
 
 DLL_PUBLIC JSModuleDef *js_init_module(JSContext *ctx, const char *module_name);

mavsdk_wrapper.cpp

@@ -34,7 +34,11 @@ static std::shared_ptr<System> msystem;
 static auto prom = std::promise<std::shared_ptr<System>>{};
 static std::future<std::shared_ptr<System>> fut;
 
+static const double EARTH_GRAVITY = 9.81;
 static const double EARTH_RADIUS = 6371000.F;
+static const double MIN_YAW_DIFF = 1;
+static const double MAX_ROLL = 35;
+static const double YAW_VELOCITY_COEF = 0.5;
 
 static bool autocontinue = false;
 static double initialBearing;
@@ -93,6 +97,10 @@ static double toRad(double angle) {
     return M_PI * angle / 180;
 }
 
+static double toDeg(double angle) {
+    return 180 * angle / M_PI;
+}
+
 int mavsdk_start(const char * url, const char * log_file, int timeout) {
     std::string connection_url(url);
     ConnectionResult connection_result;
@@ -354,6 +362,7 @@ static int setManualControlInput(float x, float y, float z, float r) {
 
 int mavsdk_setManualControlInput(void) {
     double b;
+    float speed = mavsdk_getSpeed();
 
     if (autocontinue) {
         b = initialBearing;
@@ -370,9 +379,9 @@ int mavsdk_setManualControlInput(void) {
 */
         if (abs(b - previousBearing) > 160) {
             autocontinue = true;
-        } else {
-            previousBearing = b;
+            return 0;
         }
+        previousBearing = b;
     }
 
     float angle_diff = (float)b - mavsdk_getYaw();
@@ -382,7 +391,26 @@ int mavsdk_setManualControlInput(void) {
         angle_diff -= 360;
     }
 
-    return setManualControlInput(0, (angle_diff > 0 ? 1 : -1) * std::min(abs(angle_diff), 70.0f) / 70, 0, 0);
+    if (abs(angle_diff) < MIN_YAW_DIFF) {
+        initialBearing = b;
+        return 0;
+    }
+
+    /*
+    * radius is speed²/g*tan(B) where B is roll angle
+    * let's define yaw angular velocity Ys as speed*360/perimeter
+    * so tan(B) = 2PI*Ys*speed / 360*g
+    */
+    double wanted_yaw_velocity = angle_diff * YAW_VELOCITY_COEF;
+    double roll = toDeg(atan(
+      2 * M_PI * wanted_yaw_velocity * speed / (360 * EARTH_GRAVITY)
+    ));
+    return setManualControlInput(
+      0,
+      (float)((roll > 0 ? 1 : -1) * std::min(abs(roll), MAX_ROLL) / MAX_ROLL),
+      0,
+      0
+    );
 }
 
 // Information functions
@@ -415,6 +443,25 @@ double mavsdk_getLongitude(void) {
     return telemetry->position().longitude_deg;
 }
 
+double *mavsdk_getPositionArray(void) {
+    Telemetry::Position position = telemetry->position();
+    double *positionArray = (double*) malloc(POSITION_ARRAY_SIZE * sizeof(double));
+    positionArray[0] = position.latitude_deg;
+    positionArray[1] = position.longitude_deg;
+    positionArray[2] = (double) position.absolute_altitude_m;
+    positionArray[3] = (double) position.relative_altitude_m;
+    return positionArray;
+}
+
+float *mavsdk_getSpeedArray(void) {
+    Telemetry::FixedwingMetrics metrics = telemetry->fixedwing_metrics();
+    float *speedArray = (float*) malloc(SPEED_ARRAY_SIZE * sizeof(float));
+    speedArray[0] = mavsdk_getYaw();
+    speedArray[1] = metrics.airspeed_m_s;
+    speedArray[2] = metrics.climb_rate_m_s;
+    return speedArray;
+}
+
 double mavsdk_getTakeOffAltitude(void) {
     const std::pair<Action::Result, float> response = action->get_takeoff_altitude();
 
@@ -429,6 +476,14 @@ float mavsdk_getYaw(void) {
     return telemetry->attitude_euler().yaw_deg;
 }
 
+float mavsdk_getSpeed(void) {
+    return telemetry->fixedwing_metrics().airspeed_m_s;
+}
+
+float mavsdk_getClimbRate(void) {
+    return telemetry->fixedwing_metrics().climb_rate_m_s;
+}
+
 int mavsdk_healthAllOk(void) {
     return telemetry->health_all_ok();
 }