Add checkpoint to pubsub variables

include/dronedge.h

@@ -3,8 +3,10 @@
 
 #include <open62541/server.h>
 #include "pubsub.h"
-UA_Float defaultFloat = 0;
+
 UA_Double defaultDouble = 0;
+UA_Float defaultFloat = 0;
+UA_UInt32 defaultUInt = 0;
 
 const VariableData droneVariableArray[] = {
     {
@@ -35,6 +37,13 @@ const VariableData droneVariableArray[] = {
         .type = UA_TYPES_FLOAT,
         .builtInType = UA_NS0ID_FLOAT,
     },
+    {
+        .name = "last_checkpoint",
+        .description = "Last checkpoint flown over",
+        .pdefaultValue = &defaultUInt,
+        .type = UA_TYPES_UINT32,
+        .builtInType = UA_NS0ID_UINT32,
+    },
 };
 
 

include/mavsdk_wrapper.h

@@ -14,14 +14,13 @@ int mavsdk_reboot(void);
 // Flight state management functions
 int mavsdk_arm(void);
 int mavsdk_doParachute(int param);
-int mavsdk_loiter(double radius);
+int mavsdk_loiter();
 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);
@@ -30,7 +29,8 @@ int mavsdk_setTargetCoordinatesXYZ(double x, double y, double z);
 int mavsdk_setTargetLatLong(double la, double lo);
 
 // Information functions
-double mavsdk_getAltitude(void);
+float mavsdk_getAltitude(void);
+float mavsdk_getAltitudeRel(void);
 double mavsdk_getInitialAltitude(void);
 double mavsdk_getInitialLatitude(void);
 double mavsdk_getInitialLongitude(void);

include/pubsub.h

@@ -19,6 +19,8 @@ typedef struct {
     UA_UInt32 altitudeAbsId;
     UA_Float altitudeRel;
     UA_UInt32 altitudeRelId;
+    UA_UInt32 lastCheckpoint;
+    UA_UInt32 lastCheckpointId;
 } JSDroneData;
 
 typedef struct {

mavsdk_wrapper.cpp

@@ -260,15 +260,11 @@ int mavsdk_land(void) {
     return 0;
 }
 
-int mavsdk_loiter(double radius) {
+int mavsdk_loiter() {
     if(!mavsdk_started)
         return -1;
 
-    if(flight_mode == Telemetry::FlightMode::Hold) {
-	      std::cout << "Flight mode is " << flight_mode << std::endl;
-	      return 0;
-    }
-    return loiterUnlimited(radius, drone_la, drone_lo, drone_a);
+    return mavsdk_doReposition(drone_la, drone_lo, drone_a, 0);
 }
 
 int mavsdk_takeOff(void) {
@@ -315,22 +311,6 @@ int mavsdk_doReposition(double la, double lo, double a, double y) {
     return doMavlinkCommand(command, "Reposition failed");
 }
 
-int loiterUnlimited(double radius, double la, double lo, double a) {
-    if(!mavsdk_started)
-        return -1;
-
-    MavlinkPassthrough::CommandLong command;
-    command.command = MAV_CMD_NAV_LOITER_UNLIM;
-    command.param2 = radius; // Loiter radius around waypoint. If positive loiter clockwise, else counter-clockwise
-    command.param5 = la;
-    command.param6 = lo;
-    command.param7 = a;
-    command.target_sysid = mavlink_passthrough->get_target_sysid();
-    command.target_compid = mavlink_passthrough->get_target_compid();
-
-    return doMavlinkCommand(command, "Loiter failed");
-}
-
 int mavsdk_setAirspeed(double airspeed) {
     if(!mavsdk_started)
         return -1;
@@ -403,10 +383,14 @@ int mavsdk_setTargetLatLong(double la, double lo) {
 
 // Information functions
 
-double mavsdk_getAltitude(void) {
+float mavsdk_getAltitude(void) {
     return drone_a;
 }
 
+float mavsdk_getAltitudeRel(void) {
+    return drone_at;
+}
+
 double mavsdk_getInitialAltitude(void) {
     return initial_drone_a;
 }

qjs_wrapper.c

@@ -73,6 +73,8 @@ static JSValue js_drone_get(JSContext *ctx, JSValueConst this_val, int magic)
             return JS_NewFloat64(ctx, s->altitudeAbs);
         case 4:
             return JS_NewFloat64(ctx, s->altitudeRel);
+        case 5:
+            return JS_NewUint32(ctx, s->lastCheckpoint);
         default:
             return JS_EXCEPTION;
     }
@@ -89,6 +91,7 @@ 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("lastCheckpoint", js_drone_get, NULL, 5),
     JS_CFUNC_DEF("init", 1, js_drone_init),
 };
 
@@ -104,11 +107,26 @@ void pubsub_publish_coordinates(double latitude, double longitude, float altitud
                         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);
 }
 
+static JSValue js_publish_checkpoint(JSContext *ctx, JSValueConst this_val,
+                                     int argc, JSValueConst *argv)
+{
+    int checkpoint;
+    int res;
+
+    if (JS_ToInt32(ctx, &checkpoint, argv[0]))
+        return JS_EXCEPTION;
+
+    res = writeVariable(droneVariableArray[4].name, &checkpoint,
+                        UA_TYPES[droneVariableArray[4].type]);
+    return JS_NewInt32(ctx, res);
+}
+
 int get_drone_id(int nb) {
     JSDroneData *s = JS_GetOpaque(drone_object_id_list[nb], js_drone_class_id);
     return s->id;
@@ -129,6 +147,9 @@ void init_node_id(UA_UInt32 id, int nb, int magic) {
         case 3:
             s->altitudeRelId = id;
             break;
+        case 4:
+            s->lastCheckpointId = id;
+            break;
         default:
             break;
     }
@@ -151,6 +172,9 @@ void pubsub_update_coordinates(UA_UInt32 id, const UA_DataValue *var)
         } else if (s->altitudeRelId == id) {
             s->altitudeRel = *(UA_Float*) var->value.data;
             return;
+        } else if (s->lastCheckpointId == id) {
+            s->lastCheckpoint = *(UA_UInt32*) var->value.data;
+            return;
         }
     }
     UA_LOG_ERROR(UA_Log_Stdout, UA_LOGCATEGORY_CLIENT, "NodeId not found");
@@ -381,22 +405,16 @@ static JSValue js_mavsdk_getAltitude(JSContext *ctx, JSValueConst this_val,
     return JS_NewFloat64(ctx, mavsdk_getAltitude());
 }
 
+static JSValue js_mavsdk_getAltitudeRel(JSContext *ctx, JSValueConst this_val,
+                                     int argc, JSValueConst *argv)
+{
+    return JS_NewFloat64(ctx, mavsdk_getAltitudeRel());
+}
+
 static JSValue js_mavsdk_loiter(JSContext *ctx, JSValueConst this_val,
                                 int argc, JSValueConst *argv)
 {
-    JSValueConst options;
-    JSValue val;
-    double radius = 10;
-
-    if(argc >= 1) {
-        options = argv[1];
-	      val = JS_GetPropertyStr(ctx, options, "radius");
-	      if(JS_ToFloat64(ctx, &radius, val)) {
-	          return JS_EXCEPTION;
-	      }
-	      JS_FreeValue(ctx, val);
-    }
-    return JS_NewInt32(ctx, mavsdk_loiter(radius));
+    return JS_NewInt32(ctx, mavsdk_loiter());
 }
 
 static JSValue js_mavsdk_doParachute(JSContext *ctx, JSValueConst this_val,
@@ -484,11 +502,13 @@ static const JSCFunctionListEntry js_mavsdk_funcs[] = {
     JS_CFUNC_DEF("getTakeOffAltitude", 0, js_mavsdk_getTakeOffAltitude ),
     JS_CFUNC_DEF("getInitialAltitude", 0, js_mavsdk_getInitialAltitude ),
     JS_CFUNC_DEF("getAltitude", 0, js_mavsdk_getAltitude ),
+    JS_CFUNC_DEF("getAltitudeRel", 0, js_mavsdk_getAltitudeRel ),
     JS_CFUNC_DEF("takeOff", 0, js_mavsdk_takeOff ),
     JS_CFUNC_DEF("takeOffAndWait", 0, js_mavsdk_takeOffAndWait ),
     JS_CFUNC_DEF("land", 0, js_mavsdk_land ),
     JS_CFUNC_DEF("initPubsub", 1, js_init_pubsub ),
     JS_CFUNC_DEF("runPubsub", 2, js_run_pubsub ),
+    JS_CFUNC_DEF("publishCheckpoint", 1, js_publish_checkpoint ),
     JS_CFUNC_DEF("stopPubsub", 0, js_stop_pubsub ),
 };