Skip to content

Q
qjs-wrapper
Commit 6dd2502f authored by Léo-Paul Géneau's avatar Léo-Paul Géneau 👾
Browse files
Options

Group variables 

See merge request !6
parents 12bcca0c 5e1da262
Hide whitespace changes
Inline Side-by-side
Showing with 341 additions and 214 deletions
include/dronedge.h
View file @ 6dd2502f
......@@ -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
View file @ 6dd2502f
#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
View file @ 6dd2502f
......@@ -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
View file @ 6dd2502f
......@@ -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();
}
......
pubsub.c
View file @ 6dd2502f
......@@ -10,9 +10,10 @@ UA_NodeId connectionIdent, publishedDataSetIdent, writerGroupIdent,
readerGroupIdent, readerIdent;
UA_DataSetReaderConfig readerConfig;
bool isPublisher;
VariableData (*pubsubGetValue)(UA_String identifier);
static void (*callbackUpdate)(UA_UInt32, const UA_DataValue*);
static void (*callbackUpdate)(UA_UInt32, const UA_DataValue*, bool print);
static void fillDataSetMetaData(UA_DataSetMetaDataType *pMetaData,
VariableData *variableArray,
......@@ -66,14 +67,30 @@ addPublishedDataSet(UA_Server *server, UA_UInt32 id) {
static UA_StatusCode
readVariable(UA_Server *server,
const UA_NodeId *sessionId, void *sessionContext,
const UA_NodeId *nodeId, void *nodeContext,
UA_Boolean sourceTimeStamp, const UA_NumericRange *range,
UA_DataValue *dataValue) {
const UA_NodeId *sessionId, void *sessionContext,
const UA_NodeId *nodeId, void *nodeContext,
UA_Boolean sourceTimeStamp, const UA_NumericRange *range,
UA_DataValue *dataValue) {
UA_StatusCode retval;
VariableData varDetails = pubsubGetValue(nodeId->identifier.string);
retval = UA_Variant_setScalarCopy(&dataValue->value, varDetails.value,
&UA_TYPES[varDetails.type]);
switch(varDetails.valueRank) {
case UA_VALUERANK_SCALAR:
retval = UA_Variant_setScalarCopy(&dataValue->value,
varDetails.value,
&UA_TYPES[varDetails.type]);
break;
case UA_VALUERANK_ONE_DIMENSION:
retval = UA_Variant_setArrayCopy(&dataValue->value,
varDetails.value,
varDetails.arrayDimensions[0],
&UA_TYPES[varDetails.type]);
break;
default:
retval = UA_STATUSCODE_BAD;
break;
}
if(retval != UA_STATUSCODE_GOOD)
UA_LOG_ERROR(UA_Log_Stdout, UA_LOGCATEGORY_USERLAND, "read returned value %d", retval);
dataValue->hasValue = true;
......@@ -96,21 +113,20 @@ addDataSourceVariable(UA_Server *server, VariableData varDetails) {
attr.description = UA_LOCALIZEDTEXT("en-US", varDetails.description);
attr.displayName = UA_LOCALIZEDTEXT("en-US", varDetails.name);
attr.dataType = UA_TYPES[varDetails.type].typeId;
attr.valueRank = varDetails.valueRank;
attr.arrayDimensions = varDetails.arrayDimensions;
attr.arrayDimensionsSize = varDetails.arrayDimensionsSize;
attr.accessLevel = UA_ACCESSLEVELMASK_READ | UA_ACCESSLEVELMASK_WRITE;
UA_NodeId currentNodeId = UA_NODEID_STRING(1, varDetails.name);
UA_QualifiedName currentName = UA_QUALIFIEDNAME(1, varDetails.name);
UA_NodeId parentNodeId = UA_NODEID_NUMERIC(0, UA_NS0ID_OBJECTSFOLDER);
UA_NodeId parentReferenceNodeId = UA_NODEID_NUMERIC(0, UA_NS0ID_ORGANIZES);
UA_NodeId variableTypeNodeId = UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE);
UA_DataSource dataSource;
dataSource.read = readVariable;
dataSource.write = writeVariable;
return UA_Server_addDataSourceVariableNode(server, currentNodeId,
parentNodeId,
parentReferenceNodeId,
currentName, variableTypeNodeId,
return UA_Server_addDataSourceVariableNode(server,
UA_NODEID_STRING(1, varDetails.name),
UA_NODEID_NUMERIC(0, UA_NS0ID_OBJECTSFOLDER),
UA_NODEID_NUMERIC(0, UA_NS0ID_ORGANIZES),
UA_QUALIFIEDNAME(1, varDetails.name),
varDetails.typeNodeId,
attr, dataSource, NULL, NULL);
}
......@@ -236,7 +252,7 @@ dataChangeNotificationCallback(UA_Server *server, UA_UInt32 monitoredItemId,
void *monitoredItemContext, const UA_NodeId *nodeId,
void *nodeContext, UA_UInt32 attributeId,
const UA_DataValue *var) {
callbackUpdate(nodeId->identifier.numeric, var);
callbackUpdate(nodeId->identifier.numeric, var, !isPublisher);
}
/**
......@@ -246,7 +262,8 @@ dataChangeNotificationCallback(UA_Server *server, UA_UInt32 monitoredItemId,
* Add subscribedvariables to the DataSetReader */
static UA_StatusCode
addSubscribedVariables(UA_Server *server, UA_NodeId dataSetReaderId,
UA_UInt32 nb, UA_Duration samplingInterval,
VariableData *variableArray, UA_UInt32 nb,
UA_Duration samplingInterval,
void (*init_node_id)(UA_UInt32 id, UA_UInt32 nb, UA_UInt32 magic)) {
if(server == NULL)
return UA_STATUSCODE_BADINTERNALERROR;
......@@ -287,9 +304,11 @@ addSubscribedVariables(UA_Server *server, UA_NodeId dataSetReaderId,
UA_VariableAttributes vAttr = UA_VariableAttributes_default;
UA_LocalizedText_copy(&readerConfig.dataSetMetaData.fields[i].description,
&vAttr.description);
vAttr.displayName.locale = UA_STRING("en-US");
vAttr.displayName.text = readerConfig.dataSetMetaData.fields[i].name;
vAttr.displayName = UA_LOCALIZEDTEXT("en-US", variableArray[i].name);
vAttr.dataType = readerConfig.dataSetMetaData.fields[i].dataType;
vAttr.valueRank = readerConfig.dataSetMetaData.fields[i].valueRank;
vAttr.arrayDimensions = readerConfig.dataSetMetaData.fields[i].arrayDimensions;
vAttr.arrayDimensionsSize = readerConfig.dataSetMetaData.fields[i].arrayDimensionsSize;
UA_NodeId newNode;
retval |= UA_Server_addVariableNode(server,
......@@ -297,7 +316,7 @@ addSubscribedVariables(UA_Server *server, UA_NodeId dataSetReaderId,
folderId,
UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT),
UA_QUALIFIEDNAME(1, (char *)readerConfig.dataSetMetaData.fields[i].name.data),
UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE),
variableArray[i].typeNodeId,
vAttr, NULL, &newNode);
/*monitor variable*/
......@@ -356,8 +375,47 @@ static void fillDataSetMetaData(UA_DataSetMetaDataType *pMetaData,
UA_NodeId_copy(&UA_TYPES[variableArray[i].type].typeId, &pMetaData->fields[i].dataType);
pMetaData->fields[i].builtInType = variableArray[i].builtInType;
pMetaData->fields[i].name = UA_STRING (variableArray[i].name);
pMetaData->fields[i].valueRank = -1; /* scalar */
pMetaData->fields[i].valueRank = variableArray[i].valueRank;
pMetaData->fields[i].arrayDimensions = variableArray[i].arrayDimensions;
pMetaData->fields[i].arrayDimensionsSize = variableArray[i].arrayDimensionsSize;
}
}
static void
setVariableType(UA_Server *server, VariableData *variableArray, size_t nbVariable) {
VariableData vDetails;
UA_VariableTypeAttributes vtAttr;
for(UA_UInt32 i = 0; i < nbVariable; i++) {
vDetails = variableArray[i];
switch(vDetails.valueRank) {
case UA_VALUERANK_SCALAR:
variableArray[i].typeNodeId = UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE);
break;
case UA_VALUERANK_ONE_DIMENSION:
vtAttr = UA_VariableTypeAttributes_default;
UA_NodeId_copy(&UA_TYPES[vDetails.type].typeId, &vtAttr.dataType);
vtAttr.valueRank = vDetails.valueRank;
vtAttr.arrayDimensions = vDetails.arrayDimensions;
vtAttr.arrayDimensionsSize = vDetails.arrayDimensionsSize;
vtAttr.displayName = UA_LOCALIZEDTEXT("en-US", vDetails.typeName);
UA_Variant_setArray(&vtAttr.value, vDetails.value,
vDetails.arrayDimensions[0], &UA_TYPES[vDetails.type]);
UA_Server_addVariableTypeNode(server, UA_NODEID_NULL,
UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE),
UA_NODEID_NUMERIC(0, UA_NS0ID_HASSUBTYPE),
UA_QUALIFIEDNAME(1, vDetails.typeName),
UA_NODEID_NULL, vtAttr, NULL, &vDetails.typeNodeId);
break;
default:
UA_LOG_ERROR(UA_Log_Stdout, UA_LOGCATEGORY_USERLAND, "Valuerank not handled");
break;
}
}
return;
}
static UA_Server*
......@@ -380,7 +438,7 @@ subscribe(UA_Server *server,
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*)) {
void (*update)(UA_UInt32 id, const UA_DataValue*, bool print)) {
UA_UInt16 publisherIdent;
UA_StatusCode retval;
char readerName[19];
......@@ -407,7 +465,7 @@ subscribe(UA_Server *server,
return EXIT_FAILURE;
/* Add SubscribedVariables to the created DataSetReader */
retval = addSubscribedVariables(server, readerIdent, i, interval, init_node_id);
retval = addSubscribedVariables(server, readerIdent, variableArray, i, interval, init_node_id);
if (retval != UA_STATUSCODE_GOOD)
return EXIT_FAILURE;
}
......@@ -415,27 +473,6 @@ subscribe(UA_Server *server,
return retval;
}
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) {
UA_Server *server;
UA_StatusCode retval;
server = setServer(transportProfile, networkAddressUrl, id);
subscribe(server, variableArray, nbVariable, id, nbReader, interval,
init_node_id, get_reader_id, update);
retval = UA_Server_run(server, running);
UA_Server_delete(server);
return retval == UA_STATUSCODE_GOOD ? EXIT_SUCCESS : EXIT_FAILURE;
}
int runPubsub(UA_String *transportProfile,
UA_NetworkAddressUrlDataType *networkAddressUrl,
VariableData *variableArray, size_t nbVariable,
......@@ -443,30 +480,34 @@ 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_Server *server;
UA_StatusCode retval;
server = setServer(transportProfile, networkAddressUrl, id);
setVariableType(server, variableArray, nbVariable);
/* Publishing */
pubsubGetValue = get_value;
isPublisher = publish;
if (isPublisher) {
pubsubGetValue = get_value;
addPublishedDataSet(server, id);
for(UA_UInt32 i = 0; i < nbVariable; i++) {
retval = addDataSourceVariable(server, variableArray[i]);
addPublishedDataSet(server, id);
for(UA_UInt32 i = 0; i < nbVariable; i++) {
retval = addDataSourceVariable(server, variableArray[i]);
if (retval != UA_STATUSCODE_GOOD)
return EXIT_FAILURE;
addDataSetField(server, variableArray[i]);
}
addWriterGroup(server, interval);
retval = addDataSetWriter(server);
if (retval != UA_STATUSCODE_GOOD)
return EXIT_FAILURE;
addDataSetField(server, variableArray[i]);
}
addWriterGroup(server, interval);
retval = addDataSetWriter(server);
if (retval != UA_STATUSCODE_GOOD)
return EXIT_FAILURE;
/* Subscribing */
subscribe(server, variableArray, nbVariable, id, nbReader, interval,
......
qjs_wrapper.c
View file @ 6dd2502f
......@@ -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),
};
......@@ -180,117 +189,129 @@ UA_UInt16 get_drone_id(UA_UInt32 nb) {
}
VariableData pubsub_get_value(UA_String identifier) {
VariableData var_details;
UA_String name;
VariableData varDetails;
UA_DataType type;
UA_Double *array;
for(UA_UInt32 i = 0; i < countof(droneVariableArray); i++) {
name = UA_STRING(droneVariableArray[i].name);
UA_String name = UA_STRING(droneVariableArray[i].name);
if(UA_String_equal(&identifier, &name)) {
var_details = droneVariableArray[i];
switch(var_details.type) {
case UA_TYPES_DOUBLE:
*(UA_Double*)var_details.value = droneVariableArray[i].getter.getDouble();
break;
case UA_TYPES_FLOAT:
*(UA_Float*)var_details.value = droneVariableArray[i].getter.getFloat();
varDetails = droneVariableArray[i];
switch(varDetails.valueRank) {
case UA_VALUERANK_SCALAR:
switch(varDetails.type) {
case UA_TYPES_STRING:
*(UA_String*)varDetails.value = droneVariableArray[i].getter.getString();
break;
default:
UA_LOG_ERROR(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "UA_TYPE not handled");
break;
}
break;
case UA_TYPES_STRING:
*(UA_String*)var_details.value = droneVariableArray[i].getter.getString();
case UA_VALUERANK_ONE_DIMENSION:
type = UA_TYPES[varDetails.type];
size_t size = varDetails.arrayDimensions[0];
array = (UA_Double *) droneVariableArray[i].getter.getArray();
if(type.pointerFree) {
memcpy(varDetails.value, array, type.memSize * size);
} else {
uintptr_t ptrs = (uintptr_t)array;
uintptr_t ptrd = (uintptr_t)varDetails.value;
UA_StatusCode retval = UA_STATUSCODE_GOOD;
for(size_t j = 0; j < size; ++j) {
retval |= UA_copy((void*)ptrs, (void*)ptrd, &type);
ptrs += type.memSize;
ptrd += type.memSize;
}
if(retval != UA_STATUSCODE_GOOD)
UA_LOG_ERROR(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Failed to update array");
}
free(array);
break;
default:
UA_LOG_ERROR(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "UA_TYPE not handled");
UA_LOG_ERROR(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "UA_VALUERANK not handled");
break;
}
break;
}
}
return var_details;
return varDetails;
}
void init_node_id(UA_UInt32 id, UA_UInt32 nb, UA_UInt32 magic) {
JSDroneData *s = (JSDroneData *) JS_GetOpaque(droneObjectIdList[nb], jsDroneClassId);
switch(magic) {
case 0:
s->latitudeId = id;
s->positionArrayId = id;
break;
case 1:
s->longitudeId = id;
s->speedArrayId = id;
break;
case 2:
s->altitudeAbsId = id;
break;
case 3:
s->altitudeRelId = id;
break;
case 4:
s->messageId = id;
break;
default:
UA_LOG_ERROR(UA_Log_Stdout, UA_LOGCATEGORY_USERLAND, "Unknown variable id");
break;
}
}
void pubsub_update_coordinates(UA_UInt32 id, const UA_DataValue *var)
static void pubsub_update_variables(UA_UInt32 id, const UA_DataValue *var, bool print)
{
JSDroneData *s;
UA_String uaStr;;
for(UA_UInt32 i = 0; i < nbDrone; i++) {
s = (JSDroneData *) JS_GetOpaque(droneObjectIdList[i], jsDroneClassId);
if (s->latitudeId == id) {
s->latitude = *(UA_Double*) var->value.data;
return;
} else if (s->longitudeId == id) {
s->longitude = *(UA_Double*) var->value.data;
return;
} 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;
} else if (s->messageId == id) {
uaStr = *(UA_String*) var->value.data;
pthread_mutex_lock(&mutex);
while(strlen(s->message) != 0)
pthread_cond_wait(&threadCond, &mutex);
memcpy(s->message, uaStr.data, uaStr.length);
s->message[uaStr.length] = '\0';
pthread_mutex_unlock(&mutex);
return;
}
}
UA_LOG_ERROR(UA_Log_Stdout, UA_LOGCATEGORY_CLIENT, "NodeId not found");
}
JSDroneData* s;
UA_String uaStr;
UA_Double* positionArray;
UA_Float* speedArray;
void pubsub_print_coordinates(UA_UInt32 id, const UA_DataValue *var)
{
JSDroneData *s;
UA_String uaStr;
for(UA_UInt32 i = 0; i < nbDrone; i++) {
s = (JSDroneData *) JS_GetOpaque(droneObjectIdList[i], jsDroneClassId);
if (s->latitudeId == id) {
s->latitude = *(UA_Double*) var->value.data;
UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_CLIENT, "Received latitude of drone %d: %f°", s->id, s->latitude);
return;
} else if (s->longitudeId == id) {
s->longitude = *(UA_Double*) var->value.data;
UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_CLIENT, "Received longitude of drone %d: %f°", s->id, s->longitude);
return;
} else if (s->altitudeAbsId == id) {
s->altitudeAbs = *(UA_Float*) var->value.data;
UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_CLIENT, "Received absolute altitude of drone %d: %fm", s->id, s->altitudeAbs);
if (s->positionArrayId == id) {
positionArray = (UA_Double*) var->value.data;
s->latitude = positionArray[0];
s->longitude = positionArray[1];
s->altitudeAbs = positionArray[2];
s->altitudeRel = positionArray[3];
if (print) {
UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_CLIENT,
"Received position of drone %d: %f° %f° %fm %fm",
s->id, s->latitude, s->longitude, s->altitudeAbs, s->altitudeRel);
}
return;
} else if (s->altitudeRelId == id) {
s->altitudeRel = *(UA_Float*) var->value.data;
UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_CLIENT, "Received relative altitude of drone %d: %fm", s->id, s->altitudeRel);
} else if (s->speedArrayId == id) {
speedArray = (UA_Float*) var->value.data;
s->yaw = speedArray[0];
s->speed = speedArray[1];
s->climbRate = speedArray[2];
if (print) {
UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_CLIENT,
"Received speed of drone %d: %f° %fm/s %fm/s",
s->id, s->yaw, s->speed, s->climbRate);
}
return;
} else if (s->messageId == id) {
uaStr = *(UA_String*) var->value.data;
if (!print) {
pthread_mutex_lock(&mutex);
while(strlen(s->message) != 0)
pthread_cond_wait(&threadCond, &mutex);
}
memcpy(s->message, uaStr.data, uaStr.length);
s->message[uaStr.length] = '\0';
UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_CLIENT, "Received message of drone %d: %s", s->id, s->message);
if (print) {
UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_CLIENT,
"Received message for drone %d: %s", s->id, s->message);
} else {
pthread_mutex_unlock(&mutex);
}
return;
}
}
......@@ -336,18 +357,11 @@ static JSValue js_run_pubsub(JSContext *ctx, JSValueConst this_val,
if (JS_ToFloat64(ctx, &interval, argv[4]))
return JS_EXCEPTION;
if (JS_ToBool(ctx, argv[5])) {
res = runPubsub(&transportProfile, &networkAddressUrl,
droneVariableArray, countof(droneVariableArray), id,
nbDrone, interval, init_node_id, get_drone_id,
pubsub_get_value, pubsub_update_coordinates,
&pubsubShouldRun);
} else {
res = subscribeOnly(&transportProfile, &networkAddressUrl,
droneVariableArray, countof(droneVariableArray), id,
nbDrone, interval, init_node_id, get_drone_id,
pubsub_print_coordinates, &pubsubShouldRun);
}
res = runPubsub(&transportProfile, &networkAddressUrl, droneVariableArray,
countof(droneVariableArray), id, nbDrone, interval,
init_node_id, get_drone_id, pubsub_get_value,
pubsub_update_variables, JS_ToBool(ctx, argv[5]),
&pubsubShouldRun);
pubsubExited = true;
JS_FreeCString(ctx, ipv6);
......@@ -457,11 +471,9 @@ static JSValue js_mavsdk_triggerParachute(JSContext *ctx, JSValueConst thisVal,
static JSValue js_mavsdk_loiter(JSContext *ctx, JSValueConst thisVal,
int argc, JSValueConst *argv)
{
double radius = 100;
if (argc > 0) {
if (JS_ToFloat64(ctx, &radius, argv[0]))
return JS_EXCEPTION;
}
double radius;
if (JS_ToFloat64(ctx, &radius, argv[0]))
return JS_EXCEPTION;
return JS_NewInt32(ctx, mavsdk_loiter((float)radius));
}
......@@ -575,6 +587,18 @@ static JSValue js_mavsdk_getYaw(JSContext *ctx, JSValueConst thisVal,
return JS_NewFloat64(ctx, mavsdk_getYaw());
}
static JSValue js_mavsdk_getSpeed(JSContext *ctx, JSValueConst thisVal,
int argc, JSValueConst *argv)
{
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)
{
......@@ -605,7 +629,7 @@ static const JSCFunctionListEntry js_mavsdk_funcs[] = {
JS_CFUNC_DEF("takeOff", 0, js_mavsdk_takeOff ),
JS_CFUNC_DEF("takeOffAndWait", 0, js_mavsdk_takeOffAndWait ),
JS_CFUNC_DEF("triggerParachute", 0, js_mavsdk_triggerParachute ),
JS_CFUNC_DEF("loiter", 0, js_mavsdk_loiter ),
JS_CFUNC_DEF("loiter", 1, js_mavsdk_loiter ),
JS_CFUNC_DEF("setAirspeed", 1, js_mavsdk_setAirspeed ),
JS_CFUNC_DEF("setAltitude", 1, js_mavsdk_setAltitude ),
JS_CFUNC_DEF("setTargetCoordinates", 3, js_mavsdk_setTargetCoordinates ),
......@@ -619,6 +643,8 @@ static const JSCFunctionListEntry js_mavsdk_funcs[] = {
JS_CFUNC_DEF("getLongitude", 0, js_mavsdk_getLongitude ),
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 ),
......
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
GitLab Nexedi Edition | About GitLab | About Nexedi | 沪ICP备2021021310号-2 | 沪ICP备2021021310号-7