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

pubsub: Send position array

Send a position array instead of multiple variables
parent ff6b1813
...@@ -14,10 +14,8 @@ typedef struct { ...@@ -14,10 +14,8 @@ typedef struct {
struct messageNode *tail; struct messageNode *tail;
} MessageQueue; } MessageQueue;
UA_Double latitude = 0; UA_Double positionArray[POSITION_ARRAY_SIZE] = { 0 };
UA_Double longitude = 0; UA_UInt32 positionArrayDims[] = {POSITION_ARRAY_SIZE};
UA_Float altitude_abs = 0;
UA_Float altitude_rel = 0;
UA_String message = { UA_String message = {
.length = 0, .length = 0,
.data = NULL, .data = NULL,
...@@ -25,36 +23,16 @@ UA_String message = { ...@@ -25,36 +23,16 @@ UA_String message = {
VariableData droneVariableArray[] = { VariableData droneVariableArray[] = {
{ {
.name = "latitude", .name = "positionArray",
.description = "Latitude", .typeName = "Position Array Type",
.value = &latitude, .description = "Position Array",
.value = &positionArray,
.type = UA_TYPES_DOUBLE, .type = UA_TYPES_DOUBLE,
.builtInType = UA_NS0ID_DOUBLE, .builtInType = UA_NS0ID_DOUBLE,
.getter.getDouble = mavsdk_getLatitude, .valueRank = UA_VALUERANK_ONE_DIMENSION,
}, .arrayDimensionsSize = 1,
{ .arrayDimensions = positionArrayDims,
.name = "longitude", .getter.getArray = mavsdk_getPositionArray,
.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,
}, },
{ {
.name = "message", .name = "message",
...@@ -62,9 +40,11 @@ VariableData droneVariableArray[] = { ...@@ -62,9 +40,11 @@ VariableData droneVariableArray[] = {
.value = &message, .value = &message,
.type = UA_TYPES_STRING, .type = UA_TYPES_STRING,
.builtInType = UA_NS0ID_STRING, .builtInType = UA_NS0ID_STRING,
.getter.getString = get_message .valueRank = UA_VALUERANK_SCALAR,
.arrayDimensionsSize = 0,
.arrayDimensions = NULL,
.getter.getString = get_message,
}, },
}; };
#endif /* __DRONEDGE_H__ */ #endif /* __DRONEDGE_H__ */
#ifndef __MAVSDK_H__ #ifndef __MAVSDK_H__
#define __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 #ifdef __cplusplus
extern "C" { extern "C" {
#endif #endif
...@@ -32,6 +40,7 @@ double mavsdk_getInitialLatitude(void); ...@@ -32,6 +40,7 @@ double mavsdk_getInitialLatitude(void);
double mavsdk_getInitialLongitude(void); double mavsdk_getInitialLongitude(void);
double mavsdk_getLatitude(void); double mavsdk_getLatitude(void);
double mavsdk_getLongitude(void); double mavsdk_getLongitude(void);
double *mavsdk_getPositionArray(void);
double mavsdk_getTakeOffAltitude(void); double mavsdk_getTakeOffAltitude(void);
float mavsdk_getYaw(void); float mavsdk_getYaw(void);
int mavsdk_healthAllOk(void); int mavsdk_healthAllOk(void);
......
...@@ -17,30 +17,30 @@ ...@@ -17,30 +17,30 @@
typedef struct { typedef struct {
UA_UInt16 id; UA_UInt16 id;
UA_UInt32 positionArrayId;
UA_Double latitude; UA_Double latitude;
UA_UInt32 latitudeId;
UA_Double longitude; UA_Double longitude;
UA_UInt32 longitudeId; UA_Double altitudeAbs;
UA_Float altitudeAbs; UA_Double altitudeRel;
UA_UInt32 altitudeAbsId;
UA_Float altitudeRel;
UA_UInt32 altitudeRelId;
char message[MAX_MESSAGE_SIZE]; char message[MAX_MESSAGE_SIZE];
UA_UInt32 messageId; UA_UInt32 messageId;
} JSDroneData; } JSDroneData;
typedef struct { typedef struct {
char *name; char *name;
char *typeName;
UA_NodeId typeNodeId;
char *description; char *description;
void * UA_RESTRICT value; void *value;
int type; int type;
UA_Byte builtInType; UA_Byte builtInType;
UA_Int32 valueRank;
size_t arrayDimensionsSize;
UA_UInt32 *arrayDimensions;
union { union {
UA_Double (*getDouble)(void); void* (*getArray)(void);
UA_Float (*getFloat)(void); UA_String (*getString)(void);
UA_String (*getString)(void);
} getter; } getter;
} VariableData; } VariableData;
int subscribeOnly(UA_String *transportProfile, int subscribeOnly(UA_String *transportProfile,
......
...@@ -415,6 +415,16 @@ double mavsdk_getLongitude(void) { ...@@ -415,6 +415,16 @@ double mavsdk_getLongitude(void) {
return telemetry->position().longitude_deg; 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) { double mavsdk_getTakeOffAltitude(void) {
const std::pair<Action::Result, float> response = action->get_takeoff_altitude(); const std::pair<Action::Result, float> response = action->get_takeoff_altitude();
......
...@@ -66,14 +66,30 @@ addPublishedDataSet(UA_Server *server, UA_UInt32 id) { ...@@ -66,14 +66,30 @@ addPublishedDataSet(UA_Server *server, UA_UInt32 id) {
static UA_StatusCode static UA_StatusCode
readVariable(UA_Server *server, readVariable(UA_Server *server,
const UA_NodeId *sessionId, void *sessionContext, const UA_NodeId *sessionId, void *sessionContext,
const UA_NodeId *nodeId, void *nodeContext, const UA_NodeId *nodeId, void *nodeContext,
UA_Boolean sourceTimeStamp, const UA_NumericRange *range, UA_Boolean sourceTimeStamp, const UA_NumericRange *range,
UA_DataValue *dataValue) { UA_DataValue *dataValue) {
UA_StatusCode retval; UA_StatusCode retval;
VariableData varDetails = pubsubGetValue(nodeId->identifier.string); 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) if(retval != UA_STATUSCODE_GOOD)
UA_LOG_ERROR(UA_Log_Stdout, UA_LOGCATEGORY_USERLAND, "read returned value %d", retval); UA_LOG_ERROR(UA_Log_Stdout, UA_LOGCATEGORY_USERLAND, "read returned value %d", retval);
dataValue->hasValue = true; dataValue->hasValue = true;
...@@ -96,21 +112,20 @@ addDataSourceVariable(UA_Server *server, VariableData varDetails) { ...@@ -96,21 +112,20 @@ addDataSourceVariable(UA_Server *server, VariableData varDetails) {
attr.description = UA_LOCALIZEDTEXT("en-US", varDetails.description); attr.description = UA_LOCALIZEDTEXT("en-US", varDetails.description);
attr.displayName = UA_LOCALIZEDTEXT("en-US", varDetails.name); attr.displayName = UA_LOCALIZEDTEXT("en-US", varDetails.name);
attr.dataType = UA_TYPES[varDetails.type].typeId; 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; 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; UA_DataSource dataSource;
dataSource.read = readVariable; dataSource.read = readVariable;
dataSource.write = writeVariable; dataSource.write = writeVariable;
return UA_Server_addDataSourceVariableNode(server, currentNodeId, return UA_Server_addDataSourceVariableNode(server,
parentNodeId, UA_NODEID_STRING(1, varDetails.name),
parentReferenceNodeId, UA_NODEID_NUMERIC(0, UA_NS0ID_OBJECTSFOLDER),
currentName, variableTypeNodeId, UA_NODEID_NUMERIC(0, UA_NS0ID_ORGANIZES),
UA_QUALIFIEDNAME(1, varDetails.name),
varDetails.typeNodeId,
attr, dataSource, NULL, NULL); attr, dataSource, NULL, NULL);
} }
...@@ -246,7 +261,8 @@ dataChangeNotificationCallback(UA_Server *server, UA_UInt32 monitoredItemId, ...@@ -246,7 +261,8 @@ dataChangeNotificationCallback(UA_Server *server, UA_UInt32 monitoredItemId,
* Add subscribedvariables to the DataSetReader */ * Add subscribedvariables to the DataSetReader */
static UA_StatusCode static UA_StatusCode
addSubscribedVariables(UA_Server *server, UA_NodeId dataSetReaderId, 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)) { void (*init_node_id)(UA_UInt32 id, UA_UInt32 nb, UA_UInt32 magic)) {
if(server == NULL) if(server == NULL)
return UA_STATUSCODE_BADINTERNALERROR; return UA_STATUSCODE_BADINTERNALERROR;
...@@ -287,9 +303,11 @@ addSubscribedVariables(UA_Server *server, UA_NodeId dataSetReaderId, ...@@ -287,9 +303,11 @@ addSubscribedVariables(UA_Server *server, UA_NodeId dataSetReaderId,
UA_VariableAttributes vAttr = UA_VariableAttributes_default; UA_VariableAttributes vAttr = UA_VariableAttributes_default;
UA_LocalizedText_copy(&readerConfig.dataSetMetaData.fields[i].description, UA_LocalizedText_copy(&readerConfig.dataSetMetaData.fields[i].description,
&vAttr.description); &vAttr.description);
vAttr.displayName.locale = UA_STRING("en-US"); vAttr.displayName = UA_LOCALIZEDTEXT("en-US", variableArray[i].name);
vAttr.displayName.text = readerConfig.dataSetMetaData.fields[i].name;
vAttr.dataType = readerConfig.dataSetMetaData.fields[i].dataType; 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; UA_NodeId newNode;
retval |= UA_Server_addVariableNode(server, retval |= UA_Server_addVariableNode(server,
...@@ -297,7 +315,7 @@ addSubscribedVariables(UA_Server *server, UA_NodeId dataSetReaderId, ...@@ -297,7 +315,7 @@ addSubscribedVariables(UA_Server *server, UA_NodeId dataSetReaderId,
folderId, folderId,
UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT),
UA_QUALIFIEDNAME(1, (char *)readerConfig.dataSetMetaData.fields[i].name.data), UA_QUALIFIEDNAME(1, (char *)readerConfig.dataSetMetaData.fields[i].name.data),
UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE), variableArray[i].typeNodeId,
vAttr, NULL, &newNode); vAttr, NULL, &newNode);
/*monitor variable*/ /*monitor variable*/
...@@ -356,8 +374,47 @@ static void fillDataSetMetaData(UA_DataSetMetaDataType *pMetaData, ...@@ -356,8 +374,47 @@ static void fillDataSetMetaData(UA_DataSetMetaDataType *pMetaData,
UA_NodeId_copy(&UA_TYPES[variableArray[i].type].typeId, &pMetaData->fields[i].dataType); UA_NodeId_copy(&UA_TYPES[variableArray[i].type].typeId, &pMetaData->fields[i].dataType);
pMetaData->fields[i].builtInType = variableArray[i].builtInType; pMetaData->fields[i].builtInType = variableArray[i].builtInType;
pMetaData->fields[i].name = UA_STRING (variableArray[i].name); 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* static UA_Server*
...@@ -407,7 +464,7 @@ subscribe(UA_Server *server, ...@@ -407,7 +464,7 @@ subscribe(UA_Server *server,
return EXIT_FAILURE; return EXIT_FAILURE;
/* Add SubscribedVariables to the created DataSetReader */ /* 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) if (retval != UA_STATUSCODE_GOOD)
return EXIT_FAILURE; return EXIT_FAILURE;
} }
...@@ -427,6 +484,7 @@ int subscribeOnly(UA_String *transportProfile, ...@@ -427,6 +484,7 @@ int subscribeOnly(UA_String *transportProfile,
UA_StatusCode retval; UA_StatusCode retval;
server = setServer(transportProfile, networkAddressUrl, id); server = setServer(transportProfile, networkAddressUrl, id);
setVariableType(server, variableArray, nbVariable);
subscribe(server, variableArray, nbVariable, id, nbReader, interval, subscribe(server, variableArray, nbVariable, id, nbReader, interval,
init_node_id, get_reader_id, update); init_node_id, get_reader_id, update);
...@@ -449,6 +507,7 @@ int runPubsub(UA_String *transportProfile, ...@@ -449,6 +507,7 @@ int runPubsub(UA_String *transportProfile,
UA_StatusCode retval; UA_StatusCode retval;
server = setServer(transportProfile, networkAddressUrl, id); server = setServer(transportProfile, networkAddressUrl, id);
setVariableType(server, variableArray, nbVariable);
/* Publishing */ /* Publishing */
......
...@@ -180,54 +180,70 @@ UA_UInt16 get_drone_id(UA_UInt32 nb) { ...@@ -180,54 +180,70 @@ UA_UInt16 get_drone_id(UA_UInt32 nb) {
} }
VariableData pubsub_get_value(UA_String identifier) { VariableData pubsub_get_value(UA_String identifier) {
VariableData var_details; VariableData varDetails;
UA_String name; UA_DataType type;
UA_Double *array;
for(UA_UInt32 i = 0; i < countof(droneVariableArray); i++) { 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)) { if(UA_String_equal(&identifier, &name)) {
var_details = droneVariableArray[i]; varDetails = droneVariableArray[i];
switch(varDetails.valueRank) {
switch(var_details.type) { case UA_VALUERANK_SCALAR:
case UA_TYPES_DOUBLE: switch(varDetails.type) {
*(UA_Double*)var_details.value = droneVariableArray[i].getter.getDouble(); case UA_TYPES_STRING:
break; *(UA_String*)varDetails.value = droneVariableArray[i].getter.getString();
case UA_TYPES_FLOAT: break;
*(UA_Float*)var_details.value = droneVariableArray[i].getter.getFloat(); default:
UA_LOG_ERROR(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "UA_TYPE not handled");
break;
}
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; break;
default: 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;
} }
break;
} }
} }
return var_details; return varDetails;
} }
void init_node_id(UA_UInt32 id, UA_UInt32 nb, UA_UInt32 magic) { void init_node_id(UA_UInt32 id, UA_UInt32 nb, UA_UInt32 magic) {
JSDroneData *s = (JSDroneData *) JS_GetOpaque(droneObjectIdList[nb], jsDroneClassId); JSDroneData *s = (JSDroneData *) JS_GetOpaque(droneObjectIdList[nb], jsDroneClassId);
switch(magic) { switch(magic) {
case 0: case 0:
s->latitudeId = id; s->positionArrayId = id;
break; break;
case 1: case 1:
s->longitudeId = id;
break;
case 2:
s->altitudeAbsId = id;
break;
case 3:
s->altitudeRelId = id;
break;
case 4:
s->messageId = id; s->messageId = id;
break; break;
default: default:
UA_LOG_ERROR(UA_Log_Stdout, UA_LOGCATEGORY_USERLAND, "Unknown variable id");
break; break;
} }
} }
...@@ -235,20 +251,16 @@ void init_node_id(UA_UInt32 id, UA_UInt32 nb, UA_UInt32 magic) { ...@@ -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) void pubsub_update_coordinates(UA_UInt32 id, const UA_DataValue *var)
{ {
JSDroneData *s; JSDroneData *s;
UA_String uaStr;; UA_String uaStr;
UA_Double *positionArray;
for(UA_UInt32 i = 0; i < nbDrone; i++) { for(UA_UInt32 i = 0; i < nbDrone; i++) {
s = (JSDroneData *) JS_GetOpaque(droneObjectIdList[i], jsDroneClassId); s = (JSDroneData *) JS_GetOpaque(droneObjectIdList[i], jsDroneClassId);
if (s->latitudeId == id) { if (s->positionArrayId == id) {
s->latitude = *(UA_Double*) var->value.data; positionArray = (UA_Double*) var->value.data;
return; s->latitude = positionArray[0];
} else if (s->longitudeId == id) { s->longitude = positionArray[1];
s->longitude = *(UA_Double*) var->value.data; s->altitudeAbs = positionArray[2];
return; s->altitudeRel = positionArray[3];
} 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; return;
} else if (s->messageId == id) { } else if (s->messageId == id) {
uaStr = *(UA_String*) var->value.data; uaStr = *(UA_String*) var->value.data;
...@@ -268,23 +280,20 @@ void pubsub_print_coordinates(UA_UInt32 id, const UA_DataValue *var) ...@@ -268,23 +280,20 @@ void pubsub_print_coordinates(UA_UInt32 id, const UA_DataValue *var)
{ {
JSDroneData *s; JSDroneData *s;
UA_String uaStr; UA_String uaStr;
UA_Double *positionArray;
for(UA_UInt32 i = 0; i < nbDrone; i++) { for(UA_UInt32 i = 0; i < nbDrone; i++) {
s = (JSDroneData *) JS_GetOpaque(droneObjectIdList[i], jsDroneClassId); s = (JSDroneData *) JS_GetOpaque(droneObjectIdList[i], jsDroneClassId);
if (s->latitudeId == id) { if (s->positionArrayId == id) {
s->latitude = *(UA_Double*) var->value.data; 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); UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_CLIENT, "Received latitude of drone %d: %f°", s->id, s->latitude);
return; s->longitude = positionArray[1];
} 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); UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_CLIENT, "Received longitude of drone %d: %f°", s->id, s->longitude);
return; s->altitudeAbs = positionArray[2];
} 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); UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_CLIENT, "Received absolute altitude of drone %d: %fm", s->id, s->altitudeAbs);
return; s->altitudeRel = positionArray[3];
} 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); 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; return;
} else if (s->messageId == id) { } else if (s->messageId == id) {
uaStr = *(UA_String*) var->value.data; 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