Publish relative altitude

include/mavsdk_wrapper.h

@@ -7,7 +7,7 @@ extern "C" {
 #include <stdint.h>
 // Connexion management functions
 int mavsdk_start(const char * url, const char * log_file, int timeout,
-                 void (*publishCoordinates)(double, double, float));
+                 void (*publishCoordinates)(double, double, float, float));
 int mavsdk_stop(void);
 int mavsdk_reboot(void);
 

include/pubsub.h

@@ -15,8 +15,10 @@ typedef struct {
     UA_UInt32 latitudeId;
     UA_Double longitude;
     UA_UInt32 longitudeId;
-    UA_Float altitude;
-    UA_UInt32 altitudeId;
+    UA_Float altitudeAbs;
+    UA_UInt32 altitudeAbsId;
+    UA_Float altitudeRel;
+    UA_UInt32 altitudeRelId;
 } JSDroneData;
 
 typedef struct {

mavsdk_wrapper.cpp

@@ -53,7 +53,7 @@ static bool initial_coords_set = false;
 static const double EARTH_RADIUS = 6371000.F;
 
 static int mavsdk_started = 0;
-static void (*publish_fn)(double, double, float);
+static void (*publish_fn)(double, double, float, float);
 
 // Logs functions
 
@@ -105,7 +105,7 @@ static int doMavlinkCommand(MavlinkPassthrough::CommandLong command, std::string
 // Connexion management functions
 
 int mavsdk_start(const char * url, const char * log_file, int timeout,
-                 void (*publishCoordinates)(double, double, float)) {
+                 void (*publishCoordinates)(double, double, float, float)) {
     std::string connection_url(url);
     ConnectionResult connection_result;
     log_file_fd.open(log_file);
@@ -169,7 +169,7 @@ int mavsdk_start(const char * url, const char * log_file, int timeout,
         drone_lo = position.longitude_deg;
         drone_a = position.absolute_altitude_m;
         drone_at = position.relative_altitude_m;
-        publish_fn(drone_la, drone_lo, drone_a);
+        publish_fn(drone_la, drone_lo, drone_a, drone_at);
 
         if(!initial_coords_set && drone_la != 0) {
             initial_drone_la = drone_la;

qjs_wrapper.c

@@ -24,8 +24,15 @@ const VariableData droneVariableArray[] = {
         .builtInType = UA_NS0ID_DOUBLE,
     },
     {
-        .name = "altitude",
-        .description = "Altitude",
+        .name = "altitude_abs",
+        .description = "Absolute Altitude (AMSL)",
+        .pdefaultValue = &defaultFloat,
+        .type = UA_TYPES_FLOAT,
+        .builtInType = UA_NS0ID_FLOAT,
+    },
+    {
+        .name = "altitude_rel",
+        .description = "Relative Altitude",
         .pdefaultValue = &defaultFloat,
         .type = UA_TYPES_FLOAT,
         .builtInType = UA_NS0ID_FLOAT,
@@ -97,7 +104,9 @@ static JSValue js_drone_get(JSContext *ctx, JSValueConst this_val, int magic)
         case 2:
             return JS_NewFloat64(ctx, s->longitude);
         case 3:
-            return JS_NewFloat64(ctx, s->altitude);
+            return JS_NewFloat64(ctx, s->altitudeAbs);
+        case 4:
+            return JS_NewFloat64(ctx, s->altitudeRel);
         default:
             return JS_EXCEPTION;
     }
@@ -112,11 +121,12 @@ static const JSCFunctionListEntry js_drone_proto_funcs[] = {
     JS_CGETSET_MAGIC_DEF("id", js_drone_get, NULL, 0),
     JS_CGETSET_MAGIC_DEF("latitude", js_drone_get, NULL, 1),
     JS_CGETSET_MAGIC_DEF("longitude", js_drone_get, NULL, 2),
-    JS_CGETSET_MAGIC_DEF("altitude", js_drone_get, NULL, 3),
+    JS_CGETSET_MAGIC_DEF("altitudeAbs", js_drone_get, NULL, 3),
+    JS_CGETSET_MAGIC_DEF("altitudeRel", js_drone_get, NULL, 4),
     JS_CFUNC_DEF("init", 1, js_drone_init),
 };
 
-void pubsub_publish_coordinates(double latitude, double longitude, float altitude)
+void pubsub_publish_coordinates(double latitude, double longitude, float altitudeAbs, float altitudeRel)
 {
     UA_StatusCode res = UA_STATUSCODE_GOOD;
 
@@ -124,8 +134,10 @@ void pubsub_publish_coordinates(double latitude, double longitude, float altitud
                         UA_TYPES[droneVariableArray[0].type]);
     res |= writeVariable(droneVariableArray[1].name, &longitude,
                         UA_TYPES[droneVariableArray[1].type]);
-    res |= writeVariable(droneVariableArray[2].name, &altitude,
+    res |= writeVariable(droneVariableArray[2].name, &altitudeAbs,
                         UA_TYPES[droneVariableArray[2].type]);
+    res |= writeVariable(droneVariableArray[3].name, &altitudeRel,
+                         UA_TYPES[droneVariableArray[3].type]);
     if (res != UA_STATUSCODE_GOOD)
         UA_LOG_ERROR(UA_Log_Stdout, UA_LOGCATEGORY_SERVER,
                      "Writing variable returned value %x", res);
@@ -146,7 +158,10 @@ void init_node_id(UA_UInt32 id, int nb, int magic) {
             s->longitudeId = id;
             break;
         case 2:
-            s->altitudeId = id;
+            s->altitudeAbsId = id;
+            break;
+        case 3:
+            s->altitudeRelId = id;
             break;
         default:
             break;
@@ -164,8 +179,11 @@ void pubsub_update_coordinates(UA_UInt32 id, const UA_DataValue *var)
         } else if (s->longitudeId == id) {
             s->longitude = *(UA_Double*) var->value.data;
             return;
-        } else if (s->altitudeId == id) {
-            s->altitude = *(UA_Float*) var->value.data;
+        } else if (s->altitudeAbsId == id) {
+            s->altitudeAbs = *(UA_Float*) var->value.data;
+            return;
+        } else if (s->altitudeRelId == id) {
+            s->altitudeRel = *(UA_Float*) var->value.data;
             return;
         }
     }
@@ -212,24 +230,6 @@ static JSValue js_init_pubsub(JSContext *ctx, JSValueConst this_val,
     return JS_NewInt32(ctx, 0);
 }
 
-static JSValue js_pubsub_write(JSContext *ctx, JSValueConst this_val,
-                               int argc, JSValueConst *argv)
-{
-      double latitude;
-      double longitude;
-      double altitude;
-
-      if (JS_ToFloat64(ctx, &latitude, argv[0]))
-          return JS_EXCEPTION;
-      if (JS_ToFloat64(ctx, &longitude, argv[1]))
-          return JS_EXCEPTION;
-      if (JS_ToFloat64(ctx, &altitude, argv[2]))
-          return JS_EXCEPTION;
-
-      pubsub_publish_coordinates(latitude, longitude, altitude);
-      return JS_NewInt32(ctx, 0);
-}
-
 static JSValue js_stop_pubsub(JSContext *ctx, JSValueConst this_val,
                               int argc, JSValueConst *argv)
 {
@@ -522,7 +522,6 @@ static const JSCFunctionListEntry js_mavsdk_funcs[] = {
     JS_CFUNC_DEF("initPubsub", 1, js_init_pubsub ),
     JS_CFUNC_DEF("runPubsub", 2, js_run_pubsub ),
     JS_CFUNC_DEF("stopPubsub", 0, js_stop_pubsub ),
-    JS_CFUNC_DEF("pubsubWrite", 3, js_pubsub_write ),
 };
 
 static int js_mavsdk_init(JSContext *ctx, JSModuleDef *m)