Pub/Sub: add speedArray to variables

Add to published variables an array composed of the yaw angle of the the drone, its airspeed and its climb rate.

include/dronedge.h

@@ -16,6 +16,10 @@ typedef struct {
 
 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,
@@ -34,6 +38,18 @@ VariableData droneVariableArray[] = {
         .arrayDimensions = positionArrayDims,
         .getter.getArray = mavsdk_getPositionArray,
     },
+    {
+        .name = "speedArray",
+        .typeName = "Speed Array Type",
+        .description = "Speed Array",
+        .value = &speedArray,
+        .type = UA_TYPES_FLOAT,
+        .builtInType = UA_NS0ID_FLOAT,
+        .valueRank = UA_VALUERANK_ONE_DIMENSION,
+        .arrayDimensionsSize = 1,
+        .arrayDimensions = speedArrayDims,
+        .getter.getArray = mavsdk_getSpeedArray,
+    },
     {
         .name = "message",
         .description = "Message to send to the other drones",

include/mavsdk_wrapper.h

@@ -8,6 +8,12 @@
  * 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" {
@@ -41,8 +47,11 @@ 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

@@ -18,25 +18,29 @@
 typedef struct {
     UA_UInt16 id;
     UA_UInt32 positionArrayId;
+    UA_UInt32 speedArrayId;
     UA_Double latitude;
     UA_Double longitude;
     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 *typeName;
+    char*       name;
+    char*       typeName;
     UA_NodeId   typeNodeId;
-    char *description;
-    void *value;
+    char*       description;
+    void*       value;
     int         type;
     UA_Byte     builtInType;
     UA_Int32    valueRank;
     size_t      arrayDimensionsSize;
-    UA_UInt32 *arrayDimensions;
+    UA_UInt32*  arrayDimensions;
     union {
         void*     (*getArray)(void);
         UA_String (*getString)(void);

mavsdk_wrapper.cpp

@@ -425,6 +425,15 @@ double *mavsdk_getPositionArray(void) {
     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();
 
@@ -439,6 +448,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();
 }

qjs_wrapper.c

@@ -86,6 +86,12 @@ static JSValue js_drone_get(JSContext *ctx, JSValueConst thisVal, int magic)
         case 4:
             return JS_NewFloat64(ctx, s->altitudeRel);
         case 5:
+            return JS_NewFloat64(ctx, s->yaw);
+        case 6:
+            return JS_NewFloat64(ctx, s->speed);
+        case 7:
+            return JS_NewFloat64(ctx, s->climbRate);
+        case 8:
             pthread_mutex_lock(&mutex);
             res = JS_NewString(ctx, s->message);
             strcpy(s->message, "");
@@ -168,7 +174,10 @@ static const JSCFunctionListEntry js_drone_proto_funcs[] = {
     JS_CGETSET_MAGIC_DEF("longitude", js_drone_get, NULL, 2),
     JS_CGETSET_MAGIC_DEF("altitudeAbs", js_drone_get, NULL, 3),
     JS_CGETSET_MAGIC_DEF("altitudeRel", js_drone_get, NULL, 4),
-    JS_CGETSET_MAGIC_DEF("message", js_drone_get, NULL, 5),
+    JS_CGETSET_MAGIC_DEF("yaw", js_drone_get, NULL, 5),
+    JS_CGETSET_MAGIC_DEF("speed", js_drone_get, NULL, 6),
+    JS_CGETSET_MAGIC_DEF("climbRate", js_drone_get, NULL, 7),
+    JS_CGETSET_MAGIC_DEF("message", js_drone_get, NULL, 8),
     JS_CFUNC_DEF("init", 1, js_drone_init),
 };
 
@@ -240,6 +249,9 @@ void init_node_id(UA_UInt32 id, UA_UInt32 nb, UA_UInt32 magic) {
             s->positionArrayId = id;
             break;
         case 1:
+            s->speedArrayId = id;
+            break;
+        case 2:
             s->messageId = id;
             break;
         default:
@@ -250,9 +262,11 @@ void init_node_id(UA_UInt32 id, UA_UInt32 nb, UA_UInt32 magic) {
 
 void pubsub_update_coordinates(UA_UInt32 id, const UA_DataValue *var)
 {
-    JSDroneData *s;
+    JSDroneData*  s;
     UA_String     uaStr;
-    UA_Double *positionArray;
+    UA_Double*    positionArray;
+    UA_Float*     speedArray;
+
     for(UA_UInt32 i = 0; i < nbDrone; i++) {
         s = (JSDroneData *) JS_GetOpaque(droneObjectIdList[i], jsDroneClassId);
         if (s->positionArrayId == id) {
@@ -262,6 +276,12 @@ void pubsub_update_coordinates(UA_UInt32 id, const UA_DataValue *var)
             s->altitudeAbs = positionArray[2];
             s->altitudeRel = positionArray[3];
             return;
+        } else if (s->speedArrayId == id) {
+            speedArray = (UA_Float*) var->value.data;
+            s->yaw = speedArray[0];
+            s->speed = speedArray[1];
+            s->climbRate = speedArray[2];
+            return;
         } else if (s->messageId == id) {
             uaStr = *(UA_String*) var->value.data;
             pthread_mutex_lock(&mutex);
@@ -278,9 +298,10 @@ void pubsub_update_coordinates(UA_UInt32 id, const UA_DataValue *var)
 
 void pubsub_print_coordinates(UA_UInt32 id, const UA_DataValue *var)
 {
-    JSDroneData *s;
+    JSDroneData*  s;
     UA_String     uaStr;
-    UA_Double *positionArray;
+    UA_Double*    positionArray;
+    UA_Float*     directionArray;
     for(UA_UInt32 i = 0; i < nbDrone; i++) {
         s = (JSDroneData *) JS_GetOpaque(droneObjectIdList[i], jsDroneClassId);
         if (s->positionArrayId == id) {
@@ -295,7 +316,14 @@ void pubsub_print_coordinates(UA_UInt32 id, const UA_DataValue *var)
             UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_CLIENT, "Received relative altitude of drone %d: %fm", s->id, s->altitudeRel);
             s->altitudeAbs = *(UA_Float*) var->value.data;
             return;
-        } else if (s->messageId == id) {
+        } else if (s->directionArrayId == id) {
+            directionArray = (UA_Float*) var->value.data;
+            s->yaw = directionArray[0];
+            UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_CLIENT, "Received yaw angle of drone %d: %f°", s->id, s->yaw);
+            s->speed = directionArray[1];
+            UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_CLIENT, "Received airspeed of drone %d: %fm/s", s->id, s->speed);
+            return;
+        }else if (s->messageId == id) {
             uaStr = *(UA_String*) var->value.data;
             memcpy(s->message, uaStr.data, uaStr.length);
             s->message[uaStr.length] = '\0';
@@ -588,6 +616,12 @@ static JSValue js_mavsdk_getSpeed(JSContext *ctx, JSValueConst thisVal,
     return JS_NewFloat64(ctx, mavsdk_getSpeed());
 }
 
+static JSValue js_mavsdk_getClimbRate(JSContext *ctx, JSValueConst thisVal,
+                                   int argc, JSValueConst *argv)
+{
+    return JS_NewFloat64(ctx, mavsdk_getClimbRate());
+}
+
 static JSValue js_mavsdk_healthAllOk(JSContext *ctx, JSValueConst this_val,
                                      int argc, JSValueConst *argv)
 {
@@ -633,6 +667,7 @@ static const JSCFunctionListEntry js_mavsdk_funcs[] = {
     JS_CFUNC_DEF("getTakeOffAltitude", 0, js_mavsdk_getTakeOffAltitude ),
     JS_CFUNC_DEF("getYaw", 0, js_mavsdk_getYaw ),
     JS_CFUNC_DEF("getAirspeed", 0, js_mavsdk_getSpeed ),
+    JS_CFUNC_DEF("getClimbRate", 0, js_mavsdk_getClimbRate ),
     JS_CFUNC_DEF("isInManualMode", 0, js_mavsdk_isInManualMode ),
     JS_CFUNC_DEF("healthAllOk", 0, js_mavsdk_healthAllOk ),
     JS_CFUNC_DEF("landed", 0, js_mavsdk_landed ),