Skip to content

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

pubsub: Send position array 

Send a position array instead of multiple variables
parent ff6b1813
Show whitespace changes
Inline Side-by-side
Showing with 183 additions and 116 deletions
include/dronedge.h
View file @ 5d30d69c
......@@ -14,10 +14,8 @@ 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_String message = {
.length = 0,
.data = NULL,
......@@ -25,36 +23,16 @@ UA_String message = {
VariableData droneVariableArray[] = {
{
.name = "latitude",
.description = "Latitude",
.value = &latitude,
.name = "positionArray",
.typeName = "Position Array Type",
.description = "Position Array",
.value = &positionArray,
.type = UA_TYPES_DOUBLE,
.builtInType = UA_NS0ID_DOUBLE,
.getter.getDouble = mavsdk_getLatitude,
},
{
.name = "longitude",
.description = "Longitude",
.value = &longitude,
.type = UA_TYPES_DOUBLE,
.builtInType = UA_NS0ID_DOUBLE,
.getter.getDouble = mavsdk_getLongitude,
},
{
.name = "altitude_abs",
.description = "Absolute Altitude (AMSL)",
.value = &altitude_abs,
.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 = positionArrayDims,
.getter.getArray = mavsdk_getPositionArray,
},
{
.name = "message",
......@@ -62,9 +40,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 @ 5d30d69c
#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
#ifdef __cplusplus
extern "C" {
#endif
......@@ -32,6 +40,7 @@ double mavsdk_getInitialLatitude(void);
double mavsdk_getInitialLongitude(void);
double mavsdk_getLatitude(void);
double mavsdk_getLongitude(void);
double *mavsdk_getPositionArray(void);
double mavsdk_getTakeOffAltitude(void);
float mavsdk_getYaw(void);
int mavsdk_healthAllOk(void);
......
include/pubsub.h
View file @ 5d30d69c
......@@ -17,30 +17,30 @@
typedef struct {
UA_UInt16 id;
UA_UInt32 positionArrayId;
UA_Double latitude;
UA_UInt32 latitudeId;
UA_Double longitude;
UA_UInt32 longitudeId;
UA_Float altitudeAbs;
UA_UInt32 altitudeAbsId;
UA_Float altitudeRel;
UA_UInt32 altitudeRelId;
UA_Double altitudeAbs;
UA_Double altitudeRel;
char message[MAX_MESSAGE_SIZE];
UA_UInt32 messageId;
} JSDroneData;
typedef struct {
char *name;
char *typeName;
UA_NodeId typeNodeId;
char *description;
void * UA_RESTRICT value;
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);
void* (*getArray)(void);
UA_String (*getString)(void);
} getter;
} VariableData;
int subscribeOnly(UA_String *transportProfile,
......
mavsdk_wrapper.cpp
View file @ 5d30d69c
......@@ -415,6 +415,16 @@ 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;
}
double mavsdk_getTakeOffAltitude(void) {
const std::pair<Action::Result, float> response = action->get_takeoff_altitude();
......
pubsub.c
View file @ 5d30d69c
......@@ -72,8 +72,24 @@ readVariable(UA_Server *server,
UA_DataValue *dataValue) {
UA_StatusCode retval;
VariableData varDetails = pubsubGetValue(nodeId->identifier.string);
retval = UA_Variant_setScalarCopy(&dataValue->value, varDetails.value,
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 +112,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);
}
......@@ -246,7 +261,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 +303,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 +315,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,10 +374,49 @@ 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*
setServer(UA_String *transportProfile,
UA_NetworkAddressUrlDataType *networkAddressUrl, UA_UInt32 id) {
......@@ -407,7 +464,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;
}
......@@ -427,6 +484,7 @@ int subscribeOnly(UA_String *transportProfile,
UA_StatusCode retval;
server = setServer(transportProfile, networkAddressUrl, id);
setVariableType(server, variableArray, nbVariable);
subscribe(server, variableArray, nbVariable, id, nbReader, interval,
init_node_id, get_reader_id, update);
......@@ -449,6 +507,7 @@ int runPubsub(UA_String *transportProfile,
UA_StatusCode retval;
server = setServer(transportProfile, networkAddressUrl, id);
setVariableType(server, variableArray, nbVariable);
/* Publishing */
......
qjs_wrapper.c
View file @ 5d30d69c
......@@ -180,54 +180,70 @@ 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();
break;
varDetails = droneVariableArray[i];
switch(varDetails.valueRank) {
case UA_VALUERANK_SCALAR:
switch(varDetails.type) {
case UA_TYPES_STRING:
*(UA_String*)var_details.value = droneVariableArray[i].getter.getString();
*(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_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");
}
return var_details;
free(array);
break;
default:
UA_LOG_ERROR(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "UA_VALUERANK not handled");
break;
}
}
}
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;
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;
}
}
......@@ -235,20 +251,16 @@ 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;
UA_String uaStr;;
UA_String uaStr;
UA_Double *positionArray;
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;
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];
return;
} else if (s->messageId == id) {
uaStr = *(UA_String*) var->value.data;
......@@ -268,23 +280,20 @@ void pubsub_print_coordinates(UA_UInt32 id, const UA_DataValue *var)
{
JSDroneData *s;
UA_String uaStr;
UA_Double *positionArray;
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;
if (s->positionArrayId == id) {
positionArray = *(UA_Double**) var->value.data;
s->latitude = positionArray[0];
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;
s->longitude = positionArray[1];
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;
s->altitudeAbs = positionArray[2];
UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_CLIENT, "Received absolute altitude of drone %d: %fm", s->id, s->altitudeAbs);
return;
} else if (s->altitudeRelId == id) {
s->altitudeRel = *(UA_Float*) var->value.data;
s->altitudeRel = positionArray[3];
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) {
uaStr = *(UA_String*) var->value.data;
......
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