Skip to content

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

Multicopter api update 

See merge request !10
parents 7165e6d6 b13c2c85
Hide whitespace changes
Inline Side-by-side
Showing with 212 additions and 227 deletions
include/autopilot_API.h
View file @ a4742d62
......@@ -17,13 +17,13 @@ DLL_PUBLIC int reboot(void);
// Flight state management functions
DLL_PUBLIC int arm(void);
DLL_PUBLIC int takeOff(void);
DLL_PUBLIC int takeOffAndWait(void);
DLL_PUBLIC int triggerParachute(void);
DLL_PUBLIC int land(void);
// Flight management functions
DLL_PUBLIC void loiter(double la, double lo, float a, float radius);
DLL_PUBLIC void setAirSpeed_async(float airspeed);
DLL_PUBLIC void setTargetCoordinates(double la, double lo, float a);
DLL_PUBLIC int loiter(double latitude, double longitude, float altitude,
float radius, float speed);
DLL_PUBLIC int setTargetCoordinates(double latitude, double longitude,
float altitude, float speed);
// Information functions
DLL_PUBLIC float getAltitude(void);
......@@ -37,7 +37,7 @@ DLL_PUBLIC float getYaw(void);
DLL_PUBLIC float getSpeed(void);
DLL_PUBLIC float getClimbRate(void);
DLL_PUBLIC int gpsIsOk(void);
DLL_PUBLIC int healthAllOk(void);
DLL_PUBLIC int isReadyToFly(void);
DLL_PUBLIC int isLanding(void);
DLL_PUBLIC void updateLogAndProjection(void);
#ifdef __cplusplus
......
include/pubsub.h
View file @ a4742d62
......@@ -17,10 +17,17 @@
#define MAX_STR_SIZE 1024
struct strNode {
char *str;
struct strNode *next;
};
typedef struct {
struct strNode *head;
struct strNode *tail;
} StrQueue;
typedef struct {
UA_UInt16 id;
UA_UInt32 positionArrayId;
UA_UInt32 speedArrayId;
UA_Double latitude;
UA_Double longitude;
UA_Double altitudeAbs;
......@@ -29,19 +36,27 @@ typedef struct {
UA_Float yaw;
UA_Float speed;
UA_Float climbRate;
char message[MAX_STR_SIZE];
UA_UInt32 messageId;
char log[MAX_STR_SIZE];
UA_UInt32 logId;
StrQueue receiveMessageQueue;
StrQueue receiveLogQueue;
} JSDroneData;
typedef struct {
UA_Double x;
UA_Double y;
UA_Double z;
UA_Double latitude;
UA_Double longitude;
UA_Double altitude;
UA_Int64 timestamp;
} JSPositionData;
typedef union {
struct {
UA_UInt32 message;
UA_UInt32 positionArray;
UA_UInt32 speedArray;
UA_UInt32 log;
};
UA_UInt32 getter[4];
} PubsubVariableIDs;
typedef struct {
char* name;
char* typeName;
......@@ -64,16 +79,12 @@ typedef struct {
VariableData *variableArray;
} VariableStruct;
typedef struct {
VariableStruct variables;
void (*init_node_id)(UA_UInt32 id, UA_UInt32 nb, UA_UInt32 magic);
} InstanceData;
int runPubsub(const UA_Logger *logger, UA_String *transportProfile,
UA_NetworkAddressUrlDataType *networkAddressUrl,
VariableStruct variables, UA_UInt32 id,
InstanceData *readerArray, UA_UInt32 nbReader,
UA_UInt32 maxVariableNb, UA_Duration interval,
VariableStruct *pubsubVariableArray,
PubsubVariableIDs *pubsubIDArray,
UA_UInt32 nbReader, UA_Duration interval,
UA_UInt16 (*get_reader_id)(UA_UInt32 nb),
VariableData (*get_value)(UA_String identifier),
void (*update)(UA_UInt32 id, const UA_DataValue*, bool print),
......@@ -85,10 +96,6 @@ UA_String get_log(void);
UA_UInt16 get_drone_id(UA_UInt32 nb);
void init_drone_node_id(UA_UInt32 id, UA_UInt32 nb, UA_UInt32 magic);
void init_subscriber_node_id(UA_UInt32 id, UA_UInt32 nb, UA_UInt32 magic);
VariableData pubsub_get_value(UA_String identifier);
DLL_PUBLIC JSModuleDef *js_init_module(JSContext *ctx, const char *module_name);
......
pubsub.c
View file @ a4742d62
......@@ -260,9 +260,9 @@ dataChangeNotificationCallback(UA_Server *server, UA_UInt32 monitoredItemId,
* Add subscribedvariables to the DataSetReader */
static UA_StatusCode
addSubscribedVariables(UA_Server *server, UA_NodeId dataSetReaderId,
VariableData *variableArray, UA_UInt32 nb,
UA_UInt32 maxVariableNb, UA_Duration samplingInterval,
void (*init_node_id)(UA_UInt32 id, UA_UInt32 nb, UA_UInt32 magic)) {
VariableData *variableArray,
PubsubVariableIDs pubsubIDs, UA_UInt32 nb,
UA_Duration samplingInterval) {
if(server == NULL)
return UA_STATUSCODE_BADINTERNALERROR;
......@@ -310,7 +310,7 @@ addSubscribedVariables(UA_Server *server, UA_NodeId dataSetReaderId,
UA_NodeId newNode;
retval |= UA_Server_addVariableNode(server,
UA_NODEID_NUMERIC(1, (UA_UInt32)maxVariableNb*nb + i + 50000),
UA_NODEID_NUMERIC(1, pubsubIDs.getter[i]),
folderId,
UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT),
UA_QUALIFIEDNAME(1, (char *)readerConfig.dataSetMetaData.fields[i].name.data),
......@@ -319,7 +319,6 @@ addSubscribedVariables(UA_Server *server, UA_NodeId dataSetReaderId,
/*monitor variable*/
UA_MonitoredItemCreateRequest monRequest = UA_MonitoredItemCreateRequest_default(newNode);
init_node_id(newNode.identifier.numeric, nb, i);
monRequest.requestedParameters.samplingInterval = samplingInterval;
UA_Server_createDataChangeMonitoredItem(server, UA_TIMESTAMPSTORETURN_SOURCE,
monRequest, NULL, dataChangeNotificationCallback);
......@@ -430,8 +429,9 @@ setServer(UA_String *transportProfile,
}
static UA_StatusCode
subscribe(UA_Server *server, InstanceData *instanceArray, UA_UInt32 id,
UA_UInt32 nbReader, UA_UInt32 maxVariableNb, UA_Duration interval,
subscribe(UA_Server *server, VariableStruct *pubsubVariableArray,
PubsubVariableIDs *pubsubIDArray, UA_UInt32 id,
UA_UInt32 nbReader, UA_Duration interval,
UA_UInt16 (*get_reader_id)(UA_UInt32 nb),
void (*update)(UA_UInt32 id, const UA_DataValue*, bool print)) {
UA_UInt16 publisherIdent;
......@@ -455,16 +455,15 @@ subscribe(UA_Server *server, InstanceData *instanceArray, UA_UInt32 id,
readerConfig.name = UA_STRING(readerName);
readerConfig.publisherId.data = &publisherIdent;
retval = addDataSetReader(server, instanceArray[i].variables,
retval = addDataSetReader(server, pubsubVariableArray[i],
publisherIdent);
if (retval != UA_STATUSCODE_GOOD)
return EXIT_FAILURE;
/* Add SubscribedVariables to the created DataSetReader */
retval = addSubscribedVariables(server, readerIdent,
instanceArray[i].variables.variableArray,
i, maxVariableNb, interval,
instanceArray[i].init_node_id);
pubsubVariableArray[i].variableArray,
pubsubIDArray[i], i, interval);
if (retval != UA_STATUSCODE_GOOD)
return EXIT_FAILURE;
}
......@@ -475,8 +474,9 @@ subscribe(UA_Server *server, InstanceData *instanceArray, UA_UInt32 id,
int runPubsub(const UA_Logger *logger, UA_String *transportProfile,
UA_NetworkAddressUrlDataType *networkAddressUrl,
VariableStruct variables, UA_UInt32 id,
InstanceData *readerArray, UA_UInt32 nbReader,
UA_UInt32 maxVariableNb, UA_Duration interval,
VariableStruct *pubsubVariableArray,
PubsubVariableIDs *pubsubIDArray,
UA_UInt32 nbReader, UA_Duration interval,
UA_UInt16 (*get_reader_id)(UA_UInt32 nb),
VariableData (*get_value)(UA_String identifier),
void (*update)(UA_UInt32 id, const UA_DataValue*, bool print),
......@@ -507,8 +507,8 @@ int runPubsub(const UA_Logger *logger, UA_String *transportProfile,
/* Subscribing */
subscribe(server, readerArray, id, nbReader, maxVariableNb, interval,
get_reader_id, update);
subscribe(server, pubsubVariableArray, pubsubIDArray, id, nbReader,
interval, get_reader_id, update);
retval = UA_Server_run(server, running);
UA_Server_delete(server);
......
qjs_wrapper.c
View file @ a4742d62
......@@ -9,14 +9,6 @@
#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};
......@@ -30,6 +22,17 @@ UA_String initial_ua_string = {
};
VariableData droneVariableArray[] = {
{
.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 = "positionArray",
.typeName = "Position Array Type",
......@@ -58,17 +61,6 @@ VariableData droneVariableArray[] = {
.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",
......@@ -115,22 +107,22 @@ static UA_Boolean pubsubExited = true;
static UA_UInt32 nbDrone;
static UA_UInt32 nbSubscriber;
static JSValueConst *droneObjectIdList;
static StrQueue messageQueue = {
static StrQueue sendMessageQueue = {
.head = NULL,
.tail = NULL,
};
UA_String currentMessage;
static StrQueue logQueue = {
UA_String currentSendMessage;
static StrQueue sendLogQueue = {
.head = NULL,
.tail = NULL,
};
UA_String currentLog;
pthread_mutex_t mutex;
pthread_cond_t threadCond;
UA_String currentSendLog;
bool isADrone;
PubsubVariableIDs *pubsubIdsArray;
const UA_UInt32 VARIABLE_ID_BASE = 50000;
// Position class functions
static void js_position_finalizer(JSRuntime *rt, JSValue val)
......@@ -155,10 +147,10 @@ static JSValue js_new_position(JSContext *ctx, JSValueConst thisVal,
return JS_EXCEPTION;
}
newPositionArray = getPositionArray();
s->x = (double)newPositionArray[LATITUDE] / 1e7;
s->y = (double)newPositionArray[LONGITUDE] / 1e7;
s->z = (UA_Double)(newPositionArray[REL_ALTITUDE] / 1000);
s->timestamp = newPositionArray[TIMESTAMP];
s->latitude = (double)positionArray[LATITUDE] / 1e7;
s->longitude = (double)positionArray[LONGITUDE] / 1e7;
s->altitude = (UA_Double)(positionArray[REL_ALTITUDE] / 1000);
s->timestamp = positionArray[TIMESTAMP];
JS_SetOpaque(obj, s);
free(newPositionArray);
return obj;
......@@ -172,11 +164,11 @@ static JSValue js_position_get(JSContext *ctx, JSValueConst thisVal, int magic)
return JS_EXCEPTION;
switch(magic) {
case 0:
return JS_NewFloat64(ctx, s->x);
return JS_NewFloat64(ctx, s->latitude);
case 1:
return JS_NewFloat64(ctx, s->y);
return JS_NewFloat64(ctx, s->longitude);
case 2:
return JS_NewFloat64(ctx, s->z);
return JS_NewFloat64(ctx, s->altitude);
case 3:
return JS_NewInt64(ctx, s->timestamp);
default:
......@@ -190,17 +182,35 @@ static JSClassDef jsPositionClass = {
};
static const JSCFunctionListEntry js_position_proto_funcs[] = {
JS_CGETSET_MAGIC_DEF("x", js_position_get, NULL, 0),
JS_CGETSET_MAGIC_DEF("y", js_position_get, NULL, 1),
JS_CGETSET_MAGIC_DEF("z", js_position_get, NULL, 2),
JS_CGETSET_MAGIC_DEF("latitude", js_position_get, NULL, 0),
JS_CGETSET_MAGIC_DEF("longitude", js_position_get, NULL, 1),
JS_CGETSET_MAGIC_DEF("altitude", js_position_get, NULL, 2),
JS_CGETSET_MAGIC_DEF("timestamp", js_position_get, NULL, 3),
};
// Drone class functions
static void delete_str_node(struct strNode *node) {
free(node->str);
free(node);
}
static void cleanQueue(StrQueue *pQueue)
{
struct strNode *current;
while (pQueue->head != NULL) {
current = pQueue->head;
pQueue->head = current->next;
delete_str_node(current);
}
}
static void js_drone_finalizer(JSRuntime *rt, JSValue val)
{
JSDroneData *s = (JSDroneData *) JS_GetOpaque(val, jsDroneClassId);
cleanQueue(&(s->receiveMessageQueue));
cleanQueue(&(s->receiveLogQueue));
js_free_rt(rt, s);
}
......@@ -218,6 +228,14 @@ static JSValue js_drone_ctor(JSContext *ctx, JSValueConst newTarget,
if (JS_ToUint32(ctx, &uint32, argv[0]))
goto fail;
s->id = (uint16_t)uint32;
s->receiveMessageQueue = (StrQueue){
.head = NULL,
.tail = NULL,
};
s->receiveLogQueue = (StrQueue){
.head = NULL,
.tail = NULL,
};
proto = JS_GetPropertyStr(ctx, newTarget, "prototype");
if (JS_IsException(proto))
......@@ -247,15 +265,26 @@ static JSValue js_drone_init(JSContext *ctx, JSValueConst thisVal,
return JS_UNDEFINED;
}
static JSValue readDroneDataStr(JSContext *ctx, char *str)
static JSValue readDroneDataStr(JSContext *ctx, StrQueue *pQueue, bool keepAtLeastAnElement)
{
JSValue res;
struct strNode *current;
pthread_mutex_lock(&mutex);
res = JS_NewString(ctx, str);
strcpy(str, "");
pthread_cond_signal(&threadCond);
pthread_mutex_unlock(&mutex);
current = pQueue->head;
if (current != NULL) {
res = JS_NewString(ctx, current->str);
if (current->next != NULL) {
pQueue->head = current->next;
delete_str_node(current);
} else {
if (!keepAtLeastAnElement) {
pQueue->head = (pQueue->tail = NULL);
delete_str_node(current);
}
}
} else {
res = JS_NewString(ctx, "");
}
return res;
}
......@@ -282,9 +311,9 @@ static JSValue js_drone_get(JSContext *ctx, JSValueConst thisVal, int magic)
case 7:
return JS_NewFloat64(ctx, s->climbRate);
case 8:
return readDroneDataStr(ctx, s->message);
return readDroneDataStr(ctx, &(s->receiveMessageQueue), true);
case 9:
return readDroneDataStr(ctx, s->log);
return readDroneDataStr(ctx, &(s->receiveLogQueue), false);
case 10:
return JS_NewInt64(ctx, s->timestamp);
default:
......@@ -292,7 +321,7 @@ static JSValue js_drone_get(JSContext *ctx, JSValueConst thisVal, int magic)
}
}
static void addStrToQueue(const char* str, StrQueue* pQueue)
static void addStrToQueue(const char *str, StrQueue *pQueue)
{
struct strNode *newNode;
newNode = (struct strNode*)malloc(sizeof(struct strNode));
......@@ -307,7 +336,8 @@ static void addStrToQueue(const char* str, StrQueue* pQueue)
}
}
static JSValue add_jsstr_to_queue(JSContext *ctx, JSValueConst jsStr, StrQueue *queue)
static JSValue add_jsstr_to_queue(JSContext *ctx, JSValueConst jsStr,
StrQueue *pQueue)
{
const char *str;
str = JS_ToCString(ctx, jsStr);
......@@ -316,7 +346,7 @@ static JSValue add_jsstr_to_queue(JSContext *ctx, JSValueConst jsStr, StrQueue *
return JS_EXCEPTION;
}
addStrToQueue(str, queue);
addStrToQueue(str, pQueue);
JS_FreeCString(ctx, str);
return JS_UNDEFINED;
}
......@@ -324,18 +354,13 @@ static JSValue add_jsstr_to_queue(JSContext *ctx, JSValueConst jsStr, StrQueue *
static JSValue js_drone_set_message(JSContext *ctx, JSValueConst thisVal,
int argc, JSValueConst *argv)
{
return add_jsstr_to_queue(ctx, argv[0], &messageQueue);
return add_jsstr_to_queue(ctx, argv[0], &sendMessageQueue);
}
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);
return add_jsstr_to_queue(ctx, argv[0], &sendLogQueue);
}
static UA_Boolean UA_String_isEmpty(const UA_String *s) {
......@@ -347,15 +372,15 @@ static void clear_str(UA_String *pstr) {
UA_String_clear(pstr);
}
static UA_String get_StrQueue_content(StrQueue *queue, UA_String currentStr)
static UA_String get_StrQueue_content(StrQueue *pQueue, UA_String currentStr)
{
struct strNode *current;
current = queue->head;
current = pQueue->head;
if (current != NULL) {
clear_str(&currentStr);
currentStr = UA_STRING_ALLOC(current->str);
queue->head = current->next == NULL ? (queue->tail = NULL) : current->next;
pQueue->head = current->next == NULL ? (pQueue->tail = NULL) : current->next;
delete_str_node(current);
}
return currentStr;
......@@ -363,12 +388,12 @@ static UA_String get_StrQueue_content(StrQueue *queue, UA_String currentStr)
UA_String get_message(void)
{
return get_StrQueue_content(&messageQueue, currentMessage);
return get_StrQueue_content(&sendMessageQueue, currentSendMessage);
}
UA_String get_log(void)
{
return get_StrQueue_content(&logQueue, currentLog);
return get_StrQueue_content(&sendLogQueue, currentSendLog);
}
static JSClassDef jsDroneClass = {
......@@ -394,7 +419,8 @@ static const JSCFunctionListEntry js_drone_proto_funcs[] = {
// pubsub functions
UA_UInt16 get_drone_id(UA_UInt32 nb) {
JSDroneData *s = (JSDroneData *) JS_GetOpaque(droneObjectIdList[nb], jsDroneClassId);
JSDroneData *s =
(JSDroneData *) JS_GetOpaque(droneObjectIdList[nb], jsDroneClassId);
return s->id;
}
......@@ -411,10 +437,12 @@ VariableData pubsub_get_value(UA_String identifier) {
case UA_VALUERANK_SCALAR:
switch(varDetails.type) {
case UA_TYPES_STRING:
*(UA_String*)varDetails.value = varDetails.getter.getString();
*(UA_String*)varDetails.value =
varDetails.getter.getString();
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_TYPE not handled");
break;
}
break;
......@@ -436,14 +464,16 @@ VariableData pubsub_get_value(UA_String identifier) {
ptrd += type.memSize;
}
if(retval != UA_STATUSCODE_GOOD)
UA_LOG_ERROR(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Failed to update array");
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_VALUERANK not handled");
UA_LOG_ERROR(UA_Log_Stdout, UA_LOGCATEGORY_SERVER,
"UA_VALUERANK not handled");
break;
}
}
......@@ -452,50 +482,13 @@ VariableData pubsub_get_value(UA_String identifier) {
return varDetails;
}
void init_drone_node_id(UA_UInt32 id, UA_UInt32 nb, UA_UInt32 magic) {
JSDroneData *s = (JSDroneData *) JS_GetOpaque(droneObjectIdList[nb], jsDroneClassId);
switch(magic) {
case 0:
s->positionArrayId = id;
break;
case 1:
s->speedArrayId = id;
break;
case 2:
s->messageId = id;
break;
case 3:
s->logId = id;
break;
default:
UA_LOG_ERROR(UA_Log_Stdout, UA_LOGCATEGORY_USERLAND, "Unknown variable id");
break;
}
}
void init_subscriber_node_id(UA_UInt32 id, UA_UInt32 nb, UA_UInt32 magic) {
JSDroneData *s = (JSDroneData *) JS_GetOpaque(droneObjectIdList[nb], jsDroneClassId);
switch(magic) {
case 0:
s->messageId = id;
break;
default:
UA_LOG_ERROR(UA_Log_Stdout, UA_LOGCATEGORY_USERLAND, "Unknown variable id");
break;
}
}
static void setDroneDataStr(void *data, char *str)
static void setDroneDataStr(void *data, StrQueue* pQueue)
{
UA_String uaStr = *(UA_String*) data;
pthread_mutex_lock(&mutex);
while(strlen(str) != 0)
pthread_cond_wait(&threadCond, &mutex);
char str[MAX_STR_SIZE];
memcpy(str, uaStr.data, uaStr.length);
str[uaStr.length] = '\0';
pthread_mutex_unlock(&mutex);
addStrToQueue(str, pQueue);
}
static void pubsub_update_variables(UA_UInt32 id, const UA_DataValue *var, bool print)
......@@ -506,7 +499,7 @@ static void pubsub_update_variables(UA_UInt32 id, const UA_DataValue *var, bool
for(UA_UInt32 i = 0; i < nbDrone + nbSubscriber; i++) {
s = (JSDroneData *) JS_GetOpaque(droneObjectIdList[i], jsDroneClassId);
if (s->positionArrayId == id) {
if (pubsubIdsArray[i].positionArray == id) {
updatedPositionArray = (UA_Int64*) var->value.data;
s->latitude = (double)updatedPositionArray[LATITUDE] / 1e7;
s->longitude = (double)updatedPositionArray[LONGITUDE] / 1e7;
......@@ -520,7 +513,7 @@ static void pubsub_update_variables(UA_UInt32 id, const UA_DataValue *var, bool
s->id, s->latitude, s->longitude, s->altitudeAbs, s->altitudeRel);
}
return;
} else if (s->speedArrayId == id) {
} else if (pubsubIdsArray[i].speedArray == id) {
updatedSpeedArray = (UA_Float*) var->value.data;
s->yaw = updatedSpeedArray[YAW];
s->speed = updatedSpeedArray[SPEED];
......@@ -532,17 +525,18 @@ static void pubsub_update_variables(UA_UInt32 id, const UA_DataValue *var, bool
s->id, s->yaw, s->speed, s->climbRate);
}
return;
} else if (s->messageId == id) {
setDroneDataStr(var->value.data, s->message);
} else if (pubsubIdsArray[i].message == id) {
setDroneDataStr(var->value.data, &(s->receiveMessageQueue));
return;
} else if (s->logId == id) {
} else if (pubsubIdsArray[i].log == id) {
if (!isADrone) {
setDroneDataStr(var->value.data, s->log);
setDroneDataStr(var->value.data, &(s->receiveLogQueue));
}
return;
}
}
UA_LOG_ERROR(UA_Log_Stdout, UA_LOGCATEGORY_CLIENT, "NodeId not found");
UA_LOG_ERROR(UA_Log_Stdout, UA_LOGCATEGORY_CLIENT,
"NodeId %i not found", id);
}
/*
......@@ -551,7 +545,7 @@ static void pubsub_update_variables(UA_UInt32 id, const UA_DataValue *var, bool
* arg 2 (string): network interface used for multicast communication
* arg 3 (int): ID of the drone
* arg 4 (double): publication/subscription interval in ms
* arg 5 (bool): true if there will be data to publish
* arg 5 (bool): true if the peer is a drone
*/
static JSValue js_run_pubsub(JSContext *ctx, JSValueConst this_val,
int argc, JSValueConst *argv) {
......@@ -563,7 +557,7 @@ static JSValue js_run_pubsub(JSContext *ctx, JSValueConst this_val,
UA_UInt32 id;
UA_Duration interval;
VariableStruct variables;
InstanceData *instanceArray;
VariableStruct *pubsubVariableArray;
UA_UInt32 nbPeer = nbDrone + nbSubscriber;
int res;
......@@ -589,25 +583,35 @@ static JSValue js_run_pubsub(JSContext *ctx, JSValueConst this_val,
isADrone = JS_ToBool(ctx, argv[5]);
variables = isADrone ? droneVariables : subscriberVariables;
instanceArray = (InstanceData *) malloc((nbPeer) * sizeof(InstanceData));
pubsubVariableArray = (VariableStruct *) malloc((nbPeer) * sizeof(VariableStruct));
pubsubIdsArray = (PubsubVariableIDs *) malloc((nbPeer) * sizeof(PubsubVariableIDs));
for(UA_UInt32 i = 0; i < nbDrone; i++) {
instanceArray[i].variables = droneVariables;
instanceArray[i].init_node_id = init_drone_node_id;
pubsubVariableArray[i] = droneVariables;
PubsubVariableIDs ids = {
.message = (UA_UInt32) DRONE_VARIABLE_NB * i + VARIABLE_ID_BASE,
.positionArray = (UA_UInt32) DRONE_VARIABLE_NB * i + VARIABLE_ID_BASE + 1,
.speedArray = (UA_UInt32) DRONE_VARIABLE_NB * i + VARIABLE_ID_BASE + 2,
.log = (UA_UInt32) DRONE_VARIABLE_NB * i + VARIABLE_ID_BASE + 3,
};
pubsubIdsArray[i] = ids;
}
for(UA_UInt32 i = nbDrone; i < nbPeer; i++) {
instanceArray[i].variables = subscriberVariables;
instanceArray[i].init_node_id = init_subscriber_node_id;
pubsubVariableArray[i] = subscriberVariables;
PubsubVariableIDs ids = {
.message = (UA_UInt32) DRONE_VARIABLE_NB * i + VARIABLE_ID_BASE,
};
pubsubIdsArray[i] = ids;
}
pubsubExited = false;
res = runPubsub(UA_Log_Stdout, &transportProfile, &networkAddressUrl,
variables, id, instanceArray, nbPeer,
fmax(DRONE_VARIABLE_NB, SUBSCRIBER_VARIABLE_NB), interval,
get_drone_id, pubsub_get_value, pubsub_update_variables,
&pubsubShouldRun);
variables, id, pubsubVariableArray, pubsubIdsArray, nbPeer,
interval, get_drone_id, pubsub_get_value,
pubsub_update_variables, &pubsubShouldRun);
pubsubExited = true;
free(instanceArray);
free(pubsubVariableArray);
free(pubsubIdsArray);
JS_FreeCString(ctx, ipv6);
JS_FreeCString(ctx, port);
free(notConstNetIface);
......@@ -624,24 +628,12 @@ static JSValue js_init_pubsub(JSContext *ctx, JSValueConst thisVal,
if (JS_ToUint32(ctx, &nbSubscriber, argv[1]))
return JS_EXCEPTION;
currentMessage = UA_STRING("");
currentLog = UA_STRING("");
currentSendMessage = UA_STRING("");
currentSendLog = UA_STRING("");
droneObjectIdList = (JSValue *) malloc((nbDrone + nbSubscriber) * sizeof(JSValueConst));
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,
int argc, JSValueConst *argv)
{
......@@ -650,8 +642,10 @@ static JSValue js_stop_pubsub(JSContext *ctx, JSValueConst thisVal,
sleep(1);
free(droneObjectIdList);
cleanQueue(messageQueue, currentMessage);
cleanQueue(logQueue, currentLog);
cleanQueue(&sendMessageQueue);
clear_str(&currentSendMessage);
cleanQueue(&sendLogQueue);
clear_str(&currentSendLog);
return JS_NewInt32(ctx, 0);
}
......@@ -707,16 +701,10 @@ static JSValue js_takeOff(JSContext *ctx, JSValueConst thisVal,
return JS_NewInt32(ctx, takeOff());
}
static JSValue js_takeOffAndWait(JSContext *ctx, JSValueConst thisVal,
int argc, JSValueConst *argv)
{
return JS_NewInt32(ctx, takeOffAndWait());
}
static JSValue js_triggerParachute(JSContext *ctx, JSValueConst thisVal,
int argc, JSValueConst *argv)
static JSValue js_land(JSContext *ctx, JSValueConst thisVal,
int argc, JSValueConst *argv)
{
return JS_NewInt32(ctx, triggerParachute());
return JS_NewInt32(ctx, land());
}
// Flight management functions
......@@ -724,53 +712,45 @@ static JSValue js_triggerParachute(JSContext *ctx, JSValueConst thisVal,
static JSValue js_loiter(JSContext *ctx, JSValueConst thisVal,
int argc, JSValueConst *argv)
{
double la_arg_double;
double lo_arg_double;
double a_arg_double;
double latitude;
double longitude;
double altitude;
double radius;
double speed;
if (JS_ToFloat64(ctx, &la_arg_double, argv[0]))
if (JS_ToFloat64(ctx, &latitude, argv[0]))
return JS_EXCEPTION;
if (JS_ToFloat64(ctx, &lo_arg_double, argv[1]))
if (JS_ToFloat64(ctx, &longitude, argv[1]))
return JS_EXCEPTION;
if (JS_ToFloat64(ctx, &a_arg_double, argv[2]))
if (JS_ToFloat64(ctx, &altitude, argv[2]))
return JS_EXCEPTION;
if (JS_ToFloat64(ctx, &radius, argv[3]))
return JS_EXCEPTION;
loiter(la_arg_double, lo_arg_double, (float)a_arg_double, (float)radius);
return JS_NewInt32(ctx, 0);
}
static JSValue js_setAirSpeed(JSContext *ctx, JSValueConst thisVal,
int argc, JSValueConst *argv)
{
double altitude;
if (JS_ToFloat64(ctx, &altitude, argv[0]))
if (JS_ToFloat64(ctx, &speed, argv[4]))
return JS_EXCEPTION;
setAirSpeed_async((float)altitude);
return JS_NewInt32(ctx, 0);
return JS_NewInt32(ctx, loiter(latitude, longitude, (float)altitude, (float)radius, (float)speed));
}
static JSValue js_setTargetCoordinates(JSContext *ctx,
JSValueConst thisVal,
int argc, JSValueConst *argv)
{
double la_arg_double;
double lo_arg_double;
double a_arg_double;
double latitude;
double longitude;
double altitude;
double speed;
if (JS_ToFloat64(ctx, &la_arg_double, argv[0]))
if (JS_ToFloat64(ctx, &latitude, argv[0]))
return JS_EXCEPTION;
if (JS_ToFloat64(ctx, &longitude, argv[1]))
return JS_EXCEPTION;
if (JS_ToFloat64(ctx, &lo_arg_double, argv[1]))
if (JS_ToFloat64(ctx, &altitude, argv[2]))
return JS_EXCEPTION;
if (JS_ToFloat64(ctx, &a_arg_double, argv[2]))
if (JS_ToFloat64(ctx, &speed, argv[3]))
return JS_EXCEPTION;
setTargetCoordinates(la_arg_double, lo_arg_double, (float)a_arg_double);
return JS_NewInt32(ctx, 0);
return JS_NewInt32(ctx, setTargetCoordinates(latitude, longitude, (float)altitude, (float)speed));
}
// Information functions
......@@ -828,15 +808,15 @@ static JSValue js_getClimbRate(JSContext *ctx, JSValueConst thisVal,
}
static JSValue js_gpsIsOk(JSContext *ctx, JSValueConst this_val,
int argc, JSValueConst *argv)
int argc, JSValueConst *argv)
{
return JS_NewBool(ctx, gpsIsOk());
}
static JSValue js_healthAllOk(JSContext *ctx, JSValueConst this_val,
int argc, JSValueConst *argv)
static JSValue js_isReadyToFly(JSContext *ctx, JSValueConst this_val,
int argc, JSValueConst *argv)
{
return JS_NewBool(ctx, healthAllOk());
return JS_NewBool(ctx, isReadyToFly());
}
static JSValue js_isLanding(JSContext *ctx, JSValueConst this_val,
......@@ -865,11 +845,9 @@ static const JSCFunctionListEntry js_funcs[] = {
JS_CFUNC_DEF("reboot", 0, js_reboot ),
JS_CFUNC_DEF("arm", 0, js_arm ),
JS_CFUNC_DEF("takeOff", 0, js_takeOff ),
JS_CFUNC_DEF("takeOffAndWait", 0, js_takeOffAndWait ),
JS_CFUNC_DEF("triggerParachute", 0, js_triggerParachute ),
JS_CFUNC_DEF("loiter", 4, js_loiter ),
JS_CFUNC_DEF("setAirSpeed", 1, js_setAirSpeed ),
JS_CFUNC_DEF("setTargetCoordinates", 3, js_setTargetCoordinates ),
JS_CFUNC_DEF("land", 0, js_land ),
JS_CFUNC_DEF("loiter", 5, js_loiter ),
JS_CFUNC_DEF("setTargetCoordinates", 4, js_setTargetCoordinates ),
JS_CFUNC_DEF("getPosition", 0, js_new_position ),
JS_CFUNC_DEF("getAltitude", 0, js_getAltitude ),
JS_CFUNC_DEF("getInitialAltitude", 0, js_getInitialAltitude ),
......@@ -877,11 +855,11 @@ static const JSCFunctionListEntry js_funcs[] = {
JS_CFUNC_DEF("getInitialLongitude", 0, js_getInitialLongitude ),
JS_CFUNC_DEF("getTakeOffAltitude", 0, js_getTakeOffAltitude ),
JS_CFUNC_DEF("getYaw", 0, js_getYaw ),
JS_CFUNC_DEF("getAirspeed", 0, js_getSpeed ),
JS_CFUNC_DEF("getSpeed", 0, js_getSpeed ),
JS_CFUNC_DEF("getClimbRate", 0, js_getClimbRate ),
JS_CFUNC_DEF("gpsIsOk", 0, js_gpsIsOk ),
JS_CFUNC_DEF("isReadyToFly", 0, js_isReadyToFly ),
JS_CFUNC_DEF("isLanding", 0, js_isLanding ),
JS_CFUNC_DEF("healthAllOk", 0, js_healthAllOk ),
JS_CFUNC_DEF("updateLogAndProjection", 0, js_updateLogAndProjection ),
#endif
};
......
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