Commit f0906899 authored by Léo-Paul Géneau's avatar Léo-Paul Géneau 👾

Publish relative altitude

parent 8b442812
......@@ -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);
......
......@@ -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 {
......
......@@ -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;
......
......@@ -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)
......
Markdown is supported
0%
or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment