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

support subscriber instance

See merge request !9
parents 19d6f938 415bd433
CFLAGS=-std=c99 -D_POSIX_C_SOURCE=200809L -pipe -Wall -Wextra -Wpedantic -Werror -Wno-overlength-strings -Wno-unused-parameter -Wc++-compat -Wformat -Wformat-security -Wformat-nonliteral -Wmissing-prototypes -Wstrict-prototypes -Wredundant-decls -Wuninitialized -Winit-self -Wcast-qual -Wstrict-overflow -Wnested-externs -Wmultichar -Wundef -fno-strict-aliasing -fexceptions -fstack-protector-strong -fstack-clash-protection -ffunction-sections -fdata-sections -fno-unwind-tables -fno-asynchronous-unwind-tables -fno-math-errno -O3 -flto -fno-fat-lto-objects -Wshadow -Wconversion -fvisibility=hidden CFLAGS=-std=c99 -D_POSIX_C_SOURCE=200809L -pipe -Wall -Wextra -Wpedantic -Werror -Wno-overlength-strings -Wno-unused-parameter -Wc++-compat -Wformat -Wformat-security -Wformat-nonliteral -Wmissing-prototypes -Wstrict-prototypes -Wredundant-decls -Wuninitialized -Winit-self -Wcast-qual -Wstrict-overflow -Wnested-externs -Wmultichar -Wundef -fno-strict-aliasing -fexceptions -fstack-protector-strong -fstack-clash-protection -ffunction-sections -fdata-sections -fno-unwind-tables -fno-asynchronous-unwind-tables -fno-math-errno -O3 -flto -fno-fat-lto-objects -Wshadow -Wconversion -fvisibility=hidden
LIBS=-lautopilotwrapper -lopen62541 LIBS=-lopen62541
ifeq ($(WITH_AUTOPILOT),y)
LIBS+= -lautopilotwrapper
CFLAGS+= -DWITH_AUTOPILOT
endif
LIB_NAME := libqjswrapper.so LIB_NAME := libqjswrapper.so
......
#ifndef __AUTOPILOT_H__ #ifndef __AUTOPILOT_API_H__
#define __AUTOPILOT_H__ #define __AUTOPILOT_API_H__
#ifndef DLL_PUBLIC #ifndef DLL_PUBLIC
#define DLL_PUBLIC __attribute__ ((visibility ("default"))) #define DLL_PUBLIC __attribute__ ((visibility ("default")))
#endif #endif
/*
* 0. latitude (double, degrees)
* 1. longitude (double, degrees)
* 2. absolute altitude (double, meters)
* 3. relative altitude (double, meters)
* 4. timestamp (double, milliseconds)
*/
#define POSITION_ARRAY_SIZE 5
/*
* 0. yaw angle (float, degrees)
* 1. air speed (float, m/s)
* 2. climb rate (float, m/s)
*/
#define SPEED_ARRAY_SIZE 3
#ifdef __cplusplus #ifdef __cplusplus
extern "C" { extern "C" {
#endif #endif
...@@ -59,4 +44,4 @@ DLL_PUBLIC void updateLogAndProjection(void); ...@@ -59,4 +44,4 @@ DLL_PUBLIC void updateLogAndProjection(void);
} }
#endif #endif
#endif /* __AUTOPILOT_H__ */ #endif /* __AUTOPILOT__API_H__ */
#ifndef __DRONEDGE_H__
#define __DRONEDGE_H__
#include <open62541/server.h>
#include "autopilot_wrapper.h"
#include "pubsub.h"
#define DRONE_VARIABLE_NB 3
#define SUBSCRIBER_VARIABLE_NB 1
struct messageNode {
char *message;
struct messageNode *next;
};
typedef struct {
struct messageNode *head;
struct messageNode *tail;
} MessageQueue;
UA_Int64 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,
};
VariableData droneVariableArray[] = {
{
.name = "positionArray",
.typeName = "Position Array Type",
.description = "Position Array",
.value = &positionArray,
.type = UA_TYPES_INT64,
.builtInType = UA_NS0ID_INT64,
.valueRank = UA_VALUERANK_ONE_DIMENSION,
.arrayDimensionsSize = 1,
.arrayDimensions = positionArrayDims,
.getter.getArray = getPositionArray,
},
{
.name = "speedArray",
.typeName = "Speed Array Type",
.description = "Speed Array",
.value = &speedArray,
.type = UA_TYPES_FLOAT,
.builtInType = UA_NS0ID_FLOAT,
.valueRank = UA_VALUERANK_ONE_DIMENSION,
.arrayDimensionsSize = 1,
.arrayDimensions = speedArrayDims,
.getter.getArray = getSpeedArray,
},
{
.name = "message",
.description = "Message to send to the other drones",
.value = &message,
.type = UA_TYPES_STRING,
.builtInType = UA_NS0ID_STRING,
.valueRank = UA_VALUERANK_SCALAR,
.arrayDimensionsSize = 0,
.arrayDimensions = NULL,
.getter.getString = get_message,
},
};
VariableStruct droneVariables = {
.nbVariable = DRONE_VARIABLE_NB,
.variableArray = droneVariableArray,
};
VariableData subscriberVariableArray[] = {
{
.name = "message",
.description = "Message to send to the other drones",
.value = &message,
.type = UA_TYPES_STRING,
.builtInType = UA_NS0ID_STRING,
.valueRank = UA_VALUERANK_SCALAR,
.arrayDimensionsSize = 0,
.arrayDimensions = NULL,
.getter.getString = get_message,
},
};
VariableStruct subscriberVariables = {
.nbVariable = SUBSCRIBER_VARIABLE_NB,
.variableArray = subscriberVariableArray,
};
void Subscriber_log(void *context, UA_LogLevel level, UA_LogCategory category,
const char *msg, va_list args);
#endif /* __DRONEDGE_H__ */
#ifndef __DRONEDGE_PROTOCOL_H__
#define __DRONEDGE_PROTOCOL_H__
#define POSITION_ARRAY_SIZE 5
#define SPEED_ARRAY_SIZE 3
/*
* 0. latitude (double, degrees)
* 1. longitude (double, degrees)
* 2. absolute altitude (double, meters)
* 3. relative altitude (double, meters)
* 4. timestamp (double, milliseconds)
*/
typedef enum
{
LATITUDE = 0x0,
LONGITUDE,
ABS_ALTITUDE,
REL_ALTITUDE,
TIMESTAMP,
} POSITION_ENUM;
/*
* 0. yaw angle (float, degrees)
* 1. air speed (float, m/s)
* 2. climb rate (float, m/s)
*/
typedef enum
{
YAW = 0x0,
SPEED,
CLIMB_RATE,
} SPEED_ENUM;
#endif /* __DRONEDGE_PROTOCOL_H__ */
...@@ -15,7 +15,7 @@ ...@@ -15,7 +15,7 @@
#define DATA_SET_WRITER_ID 1 #define DATA_SET_WRITER_ID 1
#define WRITER_GROUP_ID 1 #define WRITER_GROUP_ID 1
#define MAX_MESSAGE_SIZE 1024 #define MAX_STR_SIZE 1024
typedef struct { typedef struct {
UA_UInt16 id; UA_UInt16 id;
...@@ -29,8 +29,10 @@ typedef struct { ...@@ -29,8 +29,10 @@ typedef struct {
UA_Float yaw; UA_Float yaw;
UA_Float speed; UA_Float speed;
UA_Float climbRate; UA_Float climbRate;
char message[MAX_MESSAGE_SIZE]; char message[MAX_STR_SIZE];
UA_UInt32 messageId; UA_UInt32 messageId;
char log[MAX_STR_SIZE];
UA_UInt32 logId;
} JSDroneData; } JSDroneData;
typedef struct { typedef struct {
...@@ -79,6 +81,8 @@ int runPubsub(const UA_Logger *logger, UA_String *transportProfile, ...@@ -79,6 +81,8 @@ int runPubsub(const UA_Logger *logger, UA_String *transportProfile,
UA_String get_message(void); UA_String get_message(void);
UA_String get_log(void);
UA_UInt16 get_drone_id(UA_UInt32 nb); UA_UInt16 get_drone_id(UA_UInt32 nb);
void init_drone_node_id(UA_UInt32 id, UA_UInt32 nb, UA_UInt32 magic); void init_drone_node_id(UA_UInt32 id, UA_UInt32 nb, UA_UInt32 magic);
......
#include <math.h> #include <math.h>
#include <pthread.h> #include <pthread.h>
#include "dronedge.h" #include <open62541/server.h>
#include "dronedge_protocol.h"
#include "pubsub.h"
#ifdef WITH_AUTOPILOT
#include "autopilot_API.h"
#endif
#define DRONE_VARIABLE_NB 4
#define SUBSCRIBER_VARIABLE_NB 1
struct strNode {
char *str;
struct strNode *next;
};
typedef struct {
struct strNode *head;
struct strNode *tail;
} StrQueue;
UA_Int64 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 initial_ua_string = {
.length = 0,
.data = NULL,
};
VariableData droneVariableArray[] = {
{
.name = "positionArray",
.typeName = "Position Array Type",
.description = "Position Array",
.value = &positionArray,
.type = UA_TYPES_INT64,
.builtInType = UA_NS0ID_INT64,
.valueRank = UA_VALUERANK_ONE_DIMENSION,
.arrayDimensionsSize = 1,
.arrayDimensions = positionArrayDims,
#ifdef WITH_AUTOPILOT
.getter.getArray = (void * (*)(void))getPositionArray,
#endif
},
{
.name = "speedArray",
.typeName = "Speed Array Type",
.description = "Speed Array",
.value = &speedArray,
.type = UA_TYPES_FLOAT,
.builtInType = UA_NS0ID_FLOAT,
.valueRank = UA_VALUERANK_ONE_DIMENSION,
.arrayDimensionsSize = 1,
.arrayDimensions = speedArrayDims,
#ifdef WITH_AUTOPILOT
.getter.getArray = (void * (*)(void))getSpeedArray,
#endif
},
{
.name = "message",
.description = "Message to send to the other drones",
.value = &initial_ua_string,
.type = UA_TYPES_STRING,
.builtInType = UA_NS0ID_STRING,
.valueRank = UA_VALUERANK_SCALAR,
.arrayDimensionsSize = 0,
.arrayDimensions = NULL,
.getter.getString = get_message,
},
{
.name = "log",
.description = "Message to send to the other drones",
.value = &initial_ua_string,
.type = UA_TYPES_STRING,
.builtInType = UA_NS0ID_STRING,
.valueRank = UA_VALUERANK_SCALAR,
.arrayDimensionsSize = 0,
.arrayDimensions = NULL,
.getter.getString = get_log,
},
};
VariableStruct droneVariables = {
.nbVariable = DRONE_VARIABLE_NB,
.variableArray = droneVariableArray,
};
# define ANSI_COLOR_RED "\x1b[31m" VariableData subscriberVariableArray[] = {
# define ANSI_COLOR_GREEN "\x1b[32m" {
# define ANSI_COLOR_YELLOW "\x1b[33m" .name = "message",
# define ANSI_COLOR_BLUE "\x1b[34m" .description = "Message to send to the other drones",
# define ANSI_COLOR_MAGENTA "\x1b[35m" .value = &initial_ua_string,
# define ANSI_COLOR_CYAN "\x1b[36m" .type = UA_TYPES_STRING,
# define ANSI_COLOR_RESET "\x1b[0m" .builtInType = UA_NS0ID_STRING,
.valueRank = UA_VALUERANK_SCALAR,
.arrayDimensionsSize = 0,
.arrayDimensions = NULL,
.getter.getString = get_message,
},
};
VariableStruct subscriberVariables = {
.nbVariable = SUBSCRIBER_VARIABLE_NB,
.variableArray = subscriberVariableArray,
};
static JSClassID jsDroneClassId; static JSClassID jsDroneClassId;
static JSClassID jsPositionClassId; static JSClassID jsPositionClassId;
...@@ -19,30 +115,21 @@ static UA_Boolean pubsubExited = true; ...@@ -19,30 +115,21 @@ static UA_Boolean pubsubExited = true;
static UA_UInt32 nbDrone; static UA_UInt32 nbDrone;
static UA_UInt32 nbSubscriber; static UA_UInt32 nbSubscriber;
static JSValueConst *droneObjectIdList; static JSValueConst *droneObjectIdList;
static MessageQueue logQueue = { static StrQueue messageQueue = {
.head = NULL, .head = NULL,
.tail = NULL, .tail = NULL,
}; };
static MessageQueue messageQueue = { UA_String currentMessage;
static StrQueue logQueue = {
.head = NULL, .head = NULL,
.tail = NULL, .tail = NULL,
}; };
UA_String currentMessage; UA_String currentLog;
const UA_Logger Subscriber_Log = {Subscriber_log, NULL, UA_Log_Stdout_clear};
const UA_Logger *logger;
pthread_mutex_t mutex; pthread_mutex_t mutex;
pthread_cond_t threadCond; pthread_cond_t threadCond;
const char *logLevelNames[6] = {"trace", "debug", bool isADrone;
ANSI_COLOR_GREEN "info",
ANSI_COLOR_YELLOW "warn",
ANSI_COLOR_RED "error",
ANSI_COLOR_MAGENTA "fatal"};
const char *logCategoryNames[10] =
{"network", "channel", "session", "server", "client",
"userland", "securitypolicy", "eventloop", "pubsub", "discovery"};
// Position class functions // Position class functions
...@@ -52,12 +139,13 @@ static void js_position_finalizer(JSRuntime *rt, JSValue val) ...@@ -52,12 +139,13 @@ static void js_position_finalizer(JSRuntime *rt, JSValue val)
js_free_rt(rt, s); js_free_rt(rt, s);
} }
#ifdef WITH_AUTOPILOT
static JSValue js_new_position(JSContext *ctx, JSValueConst thisVal, static JSValue js_new_position(JSContext *ctx, JSValueConst thisVal,
int argc, JSValueConst *argv) int argc, JSValueConst *argv)
{ {
JSPositionData *s; JSPositionData *s;
JSValue obj; JSValue obj;
UA_Int64 *positionArray; UA_Int64 *newPositionArray;
obj = JS_NewObjectClass(ctx, (int) jsPositionClassId); obj = JS_NewObjectClass(ctx, (int) jsPositionClassId);
if (JS_IsException(obj)) if (JS_IsException(obj))
return obj; return obj;
...@@ -66,15 +154,16 @@ static JSValue js_new_position(JSContext *ctx, JSValueConst thisVal, ...@@ -66,15 +154,16 @@ static JSValue js_new_position(JSContext *ctx, JSValueConst thisVal,
JS_FreeValue(ctx, obj); JS_FreeValue(ctx, obj);
return JS_EXCEPTION; return JS_EXCEPTION;
} }
positionArray = getPositionArray(); newPositionArray = getPositionArray();
s->x = (double)positionArray[0] / 1e7; s->x = (double)newPositionArray[LATITUDE] / 1e7;
s->y = (double)positionArray[1] / 1e7; s->y = (double)newPositionArray[LONGITUDE] / 1e7;
s->z = (UA_Double)(positionArray[3] / 1000); //relative altitude s->z = (UA_Double)(newPositionArray[REL_ALTITUDE] / 1000);
s->timestamp = positionArray[4]; s->timestamp = newPositionArray[TIMESTAMP];
JS_SetOpaque(obj, s); JS_SetOpaque(obj, s);
free(positionArray); free(newPositionArray);
return obj; return obj;
} }
#endif
static JSValue js_position_get(JSContext *ctx, JSValueConst thisVal, int magic) static JSValue js_position_get(JSContext *ctx, JSValueConst thisVal, int magic)
{ {
...@@ -158,10 +247,21 @@ static JSValue js_drone_init(JSContext *ctx, JSValueConst thisVal, ...@@ -158,10 +247,21 @@ static JSValue js_drone_init(JSContext *ctx, JSValueConst thisVal,
return JS_UNDEFINED; return JS_UNDEFINED;
} }
static JSValue readDroneDataStr(JSContext *ctx, char *str)
{
JSValue res;
pthread_mutex_lock(&mutex);
res = JS_NewString(ctx, str);
strcpy(str, "");
pthread_cond_signal(&threadCond);
pthread_mutex_unlock(&mutex);
return res;
}
static JSValue js_drone_get(JSContext *ctx, JSValueConst thisVal, int magic) static JSValue js_drone_get(JSContext *ctx, JSValueConst thisVal, int magic)
{ {
JSDroneData *s = (JSDroneData *) JS_GetOpaque2(ctx, thisVal, jsDroneClassId); JSDroneData *s = (JSDroneData *) JS_GetOpaque2(ctx, thisVal, jsDroneClassId);
JSValue res;
if (!s) if (!s)
return JS_EXCEPTION; return JS_EXCEPTION;
switch(magic) { switch(magic) {
...@@ -182,24 +282,21 @@ static JSValue js_drone_get(JSContext *ctx, JSValueConst thisVal, int magic) ...@@ -182,24 +282,21 @@ static JSValue js_drone_get(JSContext *ctx, JSValueConst thisVal, int magic)
case 7: case 7:
return JS_NewFloat64(ctx, s->climbRate); return JS_NewFloat64(ctx, s->climbRate);
case 8: case 8:
pthread_mutex_lock(&mutex); return readDroneDataStr(ctx, s->message);
res = JS_NewString(ctx, s->message);
strcpy(s->message, "");
pthread_cond_signal(&threadCond);
pthread_mutex_unlock(&mutex);
return res;
case 9: case 9:
return readDroneDataStr(ctx, s->log);
case 10:
return JS_NewInt64(ctx, s->timestamp); return JS_NewInt64(ctx, s->timestamp);
default: default:
return JS_EXCEPTION; return JS_EXCEPTION;
} }
} }
static void addMessageToQueue(const char* msg, MessageQueue* pQueue) static void addStrToQueue(const char* str, StrQueue* pQueue)
{ {
struct messageNode *newNode; struct strNode *newNode;
newNode = (struct messageNode*)malloc(sizeof(struct messageNode)); newNode = (struct strNode*)malloc(sizeof(struct strNode));
newNode->message = strdup(msg); newNode->str = strdup(str);
newNode->next = NULL; newNode->next = NULL;
if (pQueue->tail == NULL) { if (pQueue->tail == NULL) {
...@@ -210,23 +307,34 @@ static void addMessageToQueue(const char* msg, MessageQueue* pQueue) ...@@ -210,23 +307,34 @@ static void addMessageToQueue(const char* msg, MessageQueue* pQueue)
} }
} }
static JSValue js_drone_set_message(JSContext *ctx, JSValueConst thisVal, static JSValue add_jsstr_to_queue(JSContext *ctx, JSValueConst jsStr, StrQueue *queue)
int argc, JSValueConst *argv)
{ {
const char *message; const char *str;
message = JS_ToCString(ctx, argv[0]); str = JS_ToCString(ctx, jsStr);
if (strlen(message) > MAX_MESSAGE_SIZE) { if (strlen(str) > MAX_STR_SIZE) {
UA_LOG_ERROR(logger, UA_LOGCATEGORY_SERVER, "Message too long"); UA_LOG_ERROR(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "String too long");
return JS_EXCEPTION; return JS_EXCEPTION;
} }
addMessageToQueue(message, &messageQueue); addStrToQueue(str, queue);
JS_FreeCString(ctx, message); JS_FreeCString(ctx, str);
return JS_UNDEFINED; return JS_UNDEFINED;
} }
static void delete_message_node(struct messageNode *node) { static JSValue js_drone_set_message(JSContext *ctx, JSValueConst thisVal,
free(node->message); int argc, JSValueConst *argv)
{
return add_jsstr_to_queue(ctx, argv[0], &messageQueue);
}
static JSValue js_drone_set_log(JSContext *ctx, JSValueConst thisVal,
int argc, JSValueConst *argv)
{
return add_jsstr_to_queue(ctx, argv[0], &logQueue);
}
static void delete_str_node(struct strNode *node) {
free(node->str);
free(node); free(node);
} }
...@@ -234,23 +342,33 @@ static UA_Boolean UA_String_isEmpty(const UA_String *s) { ...@@ -234,23 +342,33 @@ static UA_Boolean UA_String_isEmpty(const UA_String *s) {
return (s->length == 0 || s->data == NULL); return (s->length == 0 || s->data == NULL);
} }
static void clear_message(UA_String message) { static void clear_str(UA_String *pstr) {
if (!UA_String_isEmpty(&message)) if (!UA_String_isEmpty(pstr))
UA_String_clear(&message); UA_String_clear(pstr);
} }
UA_String get_message(void) static UA_String get_StrQueue_content(StrQueue *queue, UA_String currentStr)
{ {
struct messageNode *current; struct strNode *current;
current = messageQueue.head; current = queue->head;
if (current != NULL) { if (current != NULL) {
clear_message(currentMessage); clear_str(&currentStr);
currentMessage = UA_STRING_ALLOC(current->message); currentStr = UA_STRING_ALLOC(current->str);
messageQueue.head = current->next == NULL ? (messageQueue.tail = NULL) : current->next; queue->head = current->next == NULL ? (queue->tail = NULL) : current->next;
delete_message_node(current); delete_str_node(current);
} }
return currentMessage; return currentStr;
}
UA_String get_message(void)
{
return get_StrQueue_content(&messageQueue, currentMessage);
}
UA_String get_log(void)
{
return get_StrQueue_content(&logQueue, currentLog);
} }
static JSClassDef jsDroneClass = { static JSClassDef jsDroneClass = {
...@@ -268,7 +386,8 @@ static const JSCFunctionListEntry js_drone_proto_funcs[] = { ...@@ -268,7 +386,8 @@ static const JSCFunctionListEntry js_drone_proto_funcs[] = {
JS_CGETSET_MAGIC_DEF("speed", js_drone_get, NULL, 6), JS_CGETSET_MAGIC_DEF("speed", js_drone_get, NULL, 6),
JS_CGETSET_MAGIC_DEF("climbRate", js_drone_get, NULL, 7), JS_CGETSET_MAGIC_DEF("climbRate", js_drone_get, NULL, 7),
JS_CGETSET_MAGIC_DEF("message", js_drone_get, NULL, 8), JS_CGETSET_MAGIC_DEF("message", js_drone_get, NULL, 8),
JS_CGETSET_MAGIC_DEF("timestamp", js_drone_get, NULL, 9), JS_CGETSET_MAGIC_DEF("log", js_drone_get, NULL, 9),
JS_CGETSET_MAGIC_DEF("timestamp", js_drone_get, NULL, 10),
JS_CFUNC_DEF("init", 1, js_drone_init), JS_CFUNC_DEF("init", 1, js_drone_init),
}; };
...@@ -295,7 +414,7 @@ VariableData pubsub_get_value(UA_String identifier) { ...@@ -295,7 +414,7 @@ VariableData pubsub_get_value(UA_String identifier) {
*(UA_String*)varDetails.value = varDetails.getter.getString(); *(UA_String*)varDetails.value = varDetails.getter.getString();
break; break;
default: default:
UA_LOG_ERROR(logger, UA_LOGCATEGORY_SERVER, "UA_TYPE not handled"); UA_LOG_ERROR(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "UA_TYPE not handled");
break; break;
} }
break; break;
...@@ -317,14 +436,14 @@ VariableData pubsub_get_value(UA_String identifier) { ...@@ -317,14 +436,14 @@ VariableData pubsub_get_value(UA_String identifier) {
ptrd += type.memSize; ptrd += type.memSize;
} }
if(retval != UA_STATUSCODE_GOOD) if(retval != UA_STATUSCODE_GOOD)
UA_LOG_ERROR(logger, UA_LOGCATEGORY_SERVER, "Failed to update array"); UA_LOG_ERROR(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Failed to update array");
} }
free(array); free(array);
break; break;
default: default:
UA_LOG_ERROR(logger, UA_LOGCATEGORY_SERVER, "UA_VALUERANK not handled"); UA_LOG_ERROR(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "UA_VALUERANK not handled");
break; break;
} }
} }
...@@ -345,8 +464,11 @@ void init_drone_node_id(UA_UInt32 id, UA_UInt32 nb, UA_UInt32 magic) { ...@@ -345,8 +464,11 @@ void init_drone_node_id(UA_UInt32 id, UA_UInt32 nb, UA_UInt32 magic) {
case 2: case 2:
s->messageId = id; s->messageId = id;
break; break;
case 3:
s->logId = id;
break;
default: default:
UA_LOG_ERROR(logger, UA_LOGCATEGORY_USERLAND, "Unknown variable id"); UA_LOG_ERROR(UA_Log_Stdout, UA_LOGCATEGORY_USERLAND, "Unknown variable id");
break; break;
} }
} }
...@@ -358,105 +480,69 @@ void init_subscriber_node_id(UA_UInt32 id, UA_UInt32 nb, UA_UInt32 magic) { ...@@ -358,105 +480,69 @@ void init_subscriber_node_id(UA_UInt32 id, UA_UInt32 nb, UA_UInt32 magic) {
s->messageId = id; s->messageId = id;
break; break;
default: default:
UA_LOG_ERROR(logger, UA_LOGCATEGORY_USERLAND, "Unknown variable id"); UA_LOG_ERROR(UA_Log_Stdout, UA_LOGCATEGORY_USERLAND, "Unknown variable id");
break; break;
} }
} }
static void setDroneDataStr(void *data, char *str)
{
UA_String uaStr = *(UA_String*) data;
pthread_mutex_lock(&mutex);
while(strlen(str) != 0)
pthread_cond_wait(&threadCond, &mutex);
memcpy(str, uaStr.data, uaStr.length);
str[uaStr.length] = '\0';
pthread_mutex_unlock(&mutex);
}
static void pubsub_update_variables(UA_UInt32 id, const UA_DataValue *var, bool print) static void pubsub_update_variables(UA_UInt32 id, const UA_DataValue *var, bool print)
{ {
JSDroneData* s; JSDroneData* s;
UA_String uaStr; UA_Int64* updatedPositionArray;
UA_Int64* positionArray; UA_Float* updatedSpeedArray;
UA_Float* speedArray;
for(UA_UInt32 i = 0; i < nbDrone + nbSubscriber; i++) { for(UA_UInt32 i = 0; i < nbDrone + nbSubscriber; i++) {
s = (JSDroneData *) JS_GetOpaque(droneObjectIdList[i], jsDroneClassId); s = (JSDroneData *) JS_GetOpaque(droneObjectIdList[i], jsDroneClassId);
if (s->positionArrayId == id) { if (s->positionArrayId == id) {
positionArray = (UA_Int64*) var->value.data; updatedPositionArray = (UA_Int64*) var->value.data;
s->latitude = (double)positionArray[0] / 1e7; s->latitude = (double)updatedPositionArray[LATITUDE] / 1e7;
s->longitude = (double)positionArray[1] / 1e7; s->longitude = (double)updatedPositionArray[LONGITUDE] / 1e7;
s->altitudeAbs = (UA_Double)(positionArray[2] / 1000); s->altitudeAbs = (UA_Double)(updatedPositionArray[ABS_ALTITUDE] / 1000);
s->altitudeRel = (UA_Double)(positionArray[3] / 1000); s->altitudeRel = (UA_Double)(updatedPositionArray[REL_ALTITUDE] / 1000);
s->timestamp = positionArray[4]; s->timestamp = updatedPositionArray[TIMESTAMP];
if (print) { if (print) {
UA_LOG_INFO(logger, UA_LOGCATEGORY_CLIENT, UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_CLIENT,
"Received position of drone %d: %.6f ° %.6f ° %.2f m %.2f m", "Received position of drone %d: %.6f ° %.6f ° %.2f m %.2f m",
s->id, s->latitude, s->longitude, s->altitudeAbs, s->altitudeRel); s->id, s->latitude, s->longitude, s->altitudeAbs, s->altitudeRel);
} }
return; return;
} else if (s->speedArrayId == id) { } else if (s->speedArrayId == id) {
speedArray = (UA_Float*) var->value.data; updatedSpeedArray = (UA_Float*) var->value.data;
s->yaw = speedArray[0]; s->yaw = updatedSpeedArray[YAW];
s->speed = speedArray[1]; s->speed = updatedSpeedArray[SPEED];
s->climbRate = speedArray[2]; s->climbRate = updatedSpeedArray[CLIMB_RATE];
if (print) { if (print) {
UA_LOG_INFO(logger, UA_LOGCATEGORY_CLIENT, UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_CLIENT,
"Received speed of drone %d: %.2f ° %.2f m/s %.2f m/s", "Received speed of drone %d: %.2f ° %.2f m/s %.2f m/s",
s->id, s->yaw, s->speed, s->climbRate); s->id, s->yaw, s->speed, s->climbRate);
} }
return; return;
} else if (s->messageId == id) { } else if (s->messageId == id) {
uaStr = *(UA_String*) var->value.data; setDroneDataStr(var->value.data, s->message);
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; return;
} else if (s->logId == id) {
if (!isADrone) {
setDroneDataStr(var->value.data, s->log);
} }
}
UA_LOG_ERROR(logger, UA_LOGCATEGORY_CLIENT, "NodeId not found");
}
void Subscriber_log(void *context, UA_LogLevel level, UA_LogCategory category,
const char *msg, va_list args) {
char log_str[MAX_MESSAGE_SIZE];
char formatted_msg[MAX_MESSAGE_SIZE];
/* MinLevel encoded in the context pointer */
UA_LogLevel minLevel = (UA_LogLevel)(uintptr_t)context;
if(minLevel > level)
return; return;
UA_Int64 tOffset = UA_DateTime_localTimeUtcOffset();
UA_DateTimeStruct dts = UA_DateTime_toStruct(UA_DateTime_now() + tOffset);
int logLevelSlot = (int)level;
if(logLevelSlot < 0 || logLevelSlot > 5)
logLevelSlot = 5; /* Set to fatal if the level is outside the range */
/* Log */
snprintf(log_str, MAX_MESSAGE_SIZE,
"[%04u-%02u-%02u %02u:%02u:%02u.%03u (UTC%+05d)] %s/%s" ANSI_COLOR_RESET "\t",
dts.year, dts.month, dts.day, dts.hour, dts.min, dts.sec, dts.milliSec,
(int)(tOffset / UA_DATETIME_SEC / 36), logLevelNames[logLevelSlot],
logCategoryNames[category]);
vsnprintf(formatted_msg, MAX_MESSAGE_SIZE, msg, args);
strncat(log_str, formatted_msg, MAX_MESSAGE_SIZE - strlen(log_str) - 1);
strcat(log_str, "\n");
addMessageToQueue(log_str, &logQueue);
}
static JSValue js_get_log(JSContext *ctx, JSValueConst this_val,
int argc, JSValueConst *argv) {
JSValue res;
struct messageNode *current;
current = logQueue.head;
if (current != NULL) {
res = JS_NewString(ctx, current->message);
logQueue.head = current->next == NULL ? (logQueue.tail = NULL) : current->next;
delete_message_node(current);
} else {
res = JS_NewString(ctx, "");
} }
return res; }
UA_LOG_ERROR(UA_Log_Stdout, UA_LOGCATEGORY_CLIENT, "NodeId not found");
} }
/* /*
...@@ -476,7 +562,6 @@ static JSValue js_run_pubsub(JSContext *ctx, JSValueConst this_val, ...@@ -476,7 +562,6 @@ static JSValue js_run_pubsub(JSContext *ctx, JSValueConst this_val,
char urlBuffer[44]; char urlBuffer[44];
UA_UInt32 id; UA_UInt32 id;
UA_Duration interval; UA_Duration interval;
bool isADrone;
VariableStruct variables; VariableStruct variables;
InstanceData *instanceArray; InstanceData *instanceArray;
UA_UInt32 nbPeer = nbDrone + nbSubscriber; UA_UInt32 nbPeer = nbDrone + nbSubscriber;
...@@ -503,7 +588,6 @@ static JSValue js_run_pubsub(JSContext *ctx, JSValueConst this_val, ...@@ -503,7 +588,6 @@ static JSValue js_run_pubsub(JSContext *ctx, JSValueConst this_val,
isADrone = JS_ToBool(ctx, argv[5]); isADrone = JS_ToBool(ctx, argv[5]);
variables = isADrone ? droneVariables : subscriberVariables; variables = isADrone ? droneVariables : subscriberVariables;
logger = isADrone ? UA_Log_Stdout : &Subscriber_Log;
instanceArray = (InstanceData *) malloc((nbPeer) * sizeof(InstanceData)); instanceArray = (InstanceData *) malloc((nbPeer) * sizeof(InstanceData));
for(UA_UInt32 i = 0; i < nbDrone; i++) { for(UA_UInt32 i = 0; i < nbDrone; i++) {
...@@ -516,8 +600,8 @@ static JSValue js_run_pubsub(JSContext *ctx, JSValueConst this_val, ...@@ -516,8 +600,8 @@ static JSValue js_run_pubsub(JSContext *ctx, JSValueConst this_val,
} }
pubsubExited = false; pubsubExited = false;
res = runPubsub(logger, &transportProfile, &networkAddressUrl, variables, res = runPubsub(UA_Log_Stdout, &transportProfile, &networkAddressUrl,
id, instanceArray, nbPeer, variables, id, instanceArray, nbPeer,
fmax(DRONE_VARIABLE_NB, SUBSCRIBER_VARIABLE_NB), interval, fmax(DRONE_VARIABLE_NB, SUBSCRIBER_VARIABLE_NB), interval,
get_drone_id, pubsub_get_value, pubsub_update_variables, get_drone_id, pubsub_get_value, pubsub_update_variables,
&pubsubShouldRun); &pubsubShouldRun);
...@@ -541,43 +625,44 @@ static JSValue js_init_pubsub(JSContext *ctx, JSValueConst thisVal, ...@@ -541,43 +625,44 @@ static JSValue js_init_pubsub(JSContext *ctx, JSValueConst thisVal,
return JS_EXCEPTION; return JS_EXCEPTION;
currentMessage = UA_STRING(""); currentMessage = UA_STRING("");
currentLog = UA_STRING("");
droneObjectIdList = (JSValue *) malloc((nbDrone + nbSubscriber) * sizeof(JSValueConst)); droneObjectIdList = (JSValue *) malloc((nbDrone + nbSubscriber) * sizeof(JSValueConst));
return JS_NewInt32(ctx, 0); return JS_NewInt32(ctx, 0);
} }
static void cleanQueue(StrQueue queue, UA_String current_str)
{
struct strNode *current;
while (queue.head != NULL) {
current = queue.head;
queue.head = current->next;
delete_str_node(current);
}
clear_str(&current_str);
}
static JSValue js_stop_pubsub(JSContext *ctx, JSValueConst thisVal, static JSValue js_stop_pubsub(JSContext *ctx, JSValueConst thisVal,
int argc, JSValueConst *argv) int argc, JSValueConst *argv)
{ {
struct messageNode *current;
pubsubShouldRun = false; pubsubShouldRun = false;
while(!pubsubExited) while(!pubsubExited)
sleep(1); sleep(1);
free(droneObjectIdList); free(droneObjectIdList);
while (messageQueue.head != NULL) { cleanQueue(messageQueue, currentMessage);
current = messageQueue.head; cleanQueue(logQueue, currentLog);
messageQueue.head = current->next;
delete_message_node(current);
}
clear_message(currentMessage);
while(logQueue.head != NULL) {
current = logQueue.head;
logQueue.head = current->next;
delete_message_node(current);
}
return JS_NewInt32(ctx, 0); return JS_NewInt32(ctx, 0);
} }
// Connexion management functions // Connexion management functions
#ifdef WITH_AUTOPILOT
static JSValue js_start(JSContext *ctx, JSValueConst thisVal, static JSValue js_start(JSContext *ctx, JSValueConst thisVal,
int argc, JSValueConst *argv) int argc, JSValueConst *argv)
{ {
const char *ip; const char *ip;
const char *log_file; const char *mavsdk_log_file;
int port; int port;
int timeout; int timeout;
int res; int res;
...@@ -585,13 +670,13 @@ static JSValue js_start(JSContext *ctx, JSValueConst thisVal, ...@@ -585,13 +670,13 @@ static JSValue js_start(JSContext *ctx, JSValueConst thisVal,
ip = JS_ToCString(ctx, argv[0]); ip = JS_ToCString(ctx, argv[0]);
if (JS_ToInt32(ctx, &port, argv[1])) if (JS_ToInt32(ctx, &port, argv[1]))
return JS_EXCEPTION; return JS_EXCEPTION;
log_file = JS_ToCString(ctx, argv[2]); mavsdk_log_file = JS_ToCString(ctx, argv[2]);
if (JS_ToInt32(ctx, &timeout, argv[3])) if (JS_ToInt32(ctx, &timeout, argv[3]))
return JS_EXCEPTION; return JS_EXCEPTION;
res = start(ip, port, log_file, timeout); res = start(ip, port, mavsdk_log_file, timeout);
JS_FreeCString(ctx, ip); JS_FreeCString(ctx, ip);
JS_FreeCString(ctx, log_file); JS_FreeCString(ctx, mavsdk_log_file);
return JS_NewInt32(ctx, res); return JS_NewInt32(ctx, res);
} }
...@@ -766,11 +851,15 @@ static JSValue js_updateLogAndProjection(JSContext *ctx, JSValueConst this_val, ...@@ -766,11 +851,15 @@ static JSValue js_updateLogAndProjection(JSContext *ctx, JSValueConst this_val,
updateLogAndProjection(); updateLogAndProjection();
return JS_NewInt32(ctx, 0); return JS_NewInt32(ctx, 0);
} }
#endif
static const JSCFunctionListEntry js_funcs[] = { static const JSCFunctionListEntry js_funcs[] = {
JS_CFUNC_DEF("setMessage", 1, js_drone_set_message ), JS_CFUNC_DEF("setMessage", 1, js_drone_set_message ),
JS_CFUNC_DEF("setLog", 1, js_drone_set_log ),
JS_CFUNC_DEF("runPubsub", 6, js_run_pubsub ), JS_CFUNC_DEF("runPubsub", 6, js_run_pubsub ),
JS_CFUNC_DEF("stopPubsub", 0, js_stop_pubsub ), JS_CFUNC_DEF("stopPubsub", 0, js_stop_pubsub ),
JS_CFUNC_DEF("initPubsub", 2, js_init_pubsub ),
#ifdef WITH_AUTOPILOT
JS_CFUNC_DEF("start", 4, js_start ), JS_CFUNC_DEF("start", 4, js_start ),
JS_CFUNC_DEF("stop", 0, js_stop ), JS_CFUNC_DEF("stop", 0, js_stop ),
JS_CFUNC_DEF("reboot", 0, js_reboot ), JS_CFUNC_DEF("reboot", 0, js_reboot ),
...@@ -793,9 +882,8 @@ static const JSCFunctionListEntry js_funcs[] = { ...@@ -793,9 +882,8 @@ static const JSCFunctionListEntry js_funcs[] = {
JS_CFUNC_DEF("gpsIsOk", 0, js_gpsIsOk ), JS_CFUNC_DEF("gpsIsOk", 0, js_gpsIsOk ),
JS_CFUNC_DEF("isLanding", 0, js_isLanding ), JS_CFUNC_DEF("isLanding", 0, js_isLanding ),
JS_CFUNC_DEF("healthAllOk", 0, js_healthAllOk ), JS_CFUNC_DEF("healthAllOk", 0, js_healthAllOk ),
JS_CFUNC_DEF("initPubsub", 2, js_init_pubsub ),
JS_CFUNC_DEF("getLog", 0, js_get_log ),
JS_CFUNC_DEF("updateLogAndProjection", 0, js_updateLogAndProjection ), JS_CFUNC_DEF("updateLogAndProjection", 0, js_updateLogAndProjection ),
#endif
}; };
static int js_init(JSContext *ctx, JSModuleDef *m) static int js_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