Integration of update, before test on the drones
This commit is contained in:
parent
b9fe7a6e2c
commit
c0b930927c
|
@ -38,7 +38,8 @@ add_executable(rosbuzz_node src/rosbuzz.cpp
|
||||||
src/roscontroller.cpp
|
src/roscontroller.cpp
|
||||||
src/buzz_utility.cpp
|
src/buzz_utility.cpp
|
||||||
src/buzzuav_closures.cpp
|
src/buzzuav_closures.cpp
|
||||||
src/uav_utility.cpp)
|
src/uav_utility.cpp
|
||||||
|
src/buzz_update.cpp)
|
||||||
target_link_libraries(rosbuzz_node ${catkin_LIBRARIES} /usr/local/lib/libbuzz.so /usr/local/lib/libbuzzdbg.so -lpthread)
|
target_link_libraries(rosbuzz_node ${catkin_LIBRARIES} /usr/local/lib/libbuzz.so /usr/local/lib/libbuzzdbg.so -lpthread)
|
||||||
|
|
||||||
# Executables and libraries for installation to do
|
# Executables and libraries for installation to do
|
||||||
|
|
|
@ -0,0 +1,122 @@
|
||||||
|
#ifndef BUZZ_UPDATE_H
|
||||||
|
#define BUZZ_UPDATE_H
|
||||||
|
#include <stdlib.h>
|
||||||
|
#include <stdio.h>
|
||||||
|
#include <buzz/buzztype.h>
|
||||||
|
#include <buzz/buzzdict.h>
|
||||||
|
#include <buzz/buzzdarray.h>
|
||||||
|
#include <buzz/buzzvstig.h>
|
||||||
|
|
||||||
|
#define delete_p(p) do { free(p); p = NULL; } while(0)
|
||||||
|
|
||||||
|
|
||||||
|
/*********************/
|
||||||
|
/* Updater states */
|
||||||
|
/********************/
|
||||||
|
|
||||||
|
typedef enum {
|
||||||
|
CODE_RUNNING = 0, // Code executing
|
||||||
|
CODE_UPDATE, // Updating code
|
||||||
|
CODE_NEIGHBOUR, // Neighbour triggered an update
|
||||||
|
CODE_STANDBY, // Standing by for others to update
|
||||||
|
} code_states_e;
|
||||||
|
|
||||||
|
/*********************/
|
||||||
|
/*Message types */
|
||||||
|
/********************/
|
||||||
|
|
||||||
|
typedef enum {
|
||||||
|
SEND_CODE = 0, // Broadcast code with state
|
||||||
|
STATE_MSG, // Broadcast state
|
||||||
|
} code_message_e;
|
||||||
|
|
||||||
|
/*************************/
|
||||||
|
/*Updater message queue */
|
||||||
|
/*************************/
|
||||||
|
|
||||||
|
struct updater_msgqueue_s {
|
||||||
|
uint8_t* queue;
|
||||||
|
uint8_t* size;
|
||||||
|
} ;
|
||||||
|
typedef struct updater_msgqueue_s* updater_msgqueue_t;
|
||||||
|
|
||||||
|
/**************************/
|
||||||
|
/*Updater data*/
|
||||||
|
/**************************/
|
||||||
|
|
||||||
|
struct buzz_updater_elem_s {
|
||||||
|
/* robot id */
|
||||||
|
uint16_t robotid;
|
||||||
|
/*current Bytecode content */
|
||||||
|
uint8_t* bcode;
|
||||||
|
/*current bcode size*/
|
||||||
|
size_t bcode_size;
|
||||||
|
/*updater out msg queue */
|
||||||
|
updater_msgqueue_t outmsg_queue;
|
||||||
|
/*updater in msg queue*/
|
||||||
|
updater_msgqueue_t inmsg_queue;
|
||||||
|
/*Current state of the updater one in code_states_e ENUM*/
|
||||||
|
int mode;
|
||||||
|
/*Table with state of other neighbours*/
|
||||||
|
buzzdict_t state_dict;
|
||||||
|
/*Update number to ensure consistency*/
|
||||||
|
uint8_t update_no;
|
||||||
|
} ;
|
||||||
|
typedef struct buzz_updater_elem_s* buzz_updater_elem_t;
|
||||||
|
|
||||||
|
/**************************************************************************/
|
||||||
|
/*Updater routine from msg processing to file checks to be called from main*/
|
||||||
|
/**************************************************************************/
|
||||||
|
int update_routine(const char* bcfname,
|
||||||
|
const char* dbgfname, int destroy);
|
||||||
|
|
||||||
|
/************************************************/
|
||||||
|
/*Initalizes the updater */
|
||||||
|
/************************************************/
|
||||||
|
void init_update_monitor(const char* bo_filename,int barrier);
|
||||||
|
|
||||||
|
|
||||||
|
/*********************************************************/
|
||||||
|
/*Appends buffer of given size to in msg queue of updater*/
|
||||||
|
/*********************************************************/
|
||||||
|
|
||||||
|
void code_message_inqueue_append(uint8_t* msg,uint16_t size);
|
||||||
|
|
||||||
|
/*********************************************************/
|
||||||
|
/*Processes messages inside the queue of the updater*/
|
||||||
|
/*********************************************************/
|
||||||
|
|
||||||
|
void code_message_inqueue_process();
|
||||||
|
|
||||||
|
/*****************************************************/
|
||||||
|
/* obtains messages from out msgs queue of the updater*/
|
||||||
|
/*******************************************************/
|
||||||
|
uint8_t* getupdater_out_msg();
|
||||||
|
|
||||||
|
/******************************************************/
|
||||||
|
/*obtains out msg queue size*/
|
||||||
|
/*****************************************************/
|
||||||
|
uint8_t* getupdate_out_msg_size();
|
||||||
|
|
||||||
|
/**************************************************/
|
||||||
|
/*destroys the out msg queue*/
|
||||||
|
/*************************************************/
|
||||||
|
void destroy_out_msg_queue();
|
||||||
|
|
||||||
|
/***************************************************/
|
||||||
|
/*obatins updater state*/
|
||||||
|
/***************************************************/
|
||||||
|
int get_update_mode();
|
||||||
|
|
||||||
|
/***************************************************/
|
||||||
|
/*sets bzz file name*/
|
||||||
|
/***************************************************/
|
||||||
|
void set_bzz_file(const char* in_bzz_file);
|
||||||
|
|
||||||
|
/****************************************************/
|
||||||
|
/*Destroys the updater*/
|
||||||
|
/***************************************************/
|
||||||
|
|
||||||
|
void destroy_updater();
|
||||||
|
|
||||||
|
#endif
|
|
@ -2,6 +2,7 @@
|
||||||
#include <stdio.h>
|
#include <stdio.h>
|
||||||
#include "buzz_utility.h"
|
#include "buzz_utility.h"
|
||||||
#include "buzzuav_closures.h"
|
#include "buzzuav_closures.h"
|
||||||
|
#include "buzz_update.h"
|
||||||
#include <buzz/buzzdebug.h>
|
#include <buzz/buzzdebug.h>
|
||||||
#include <stdlib.h>
|
#include <stdlib.h>
|
||||||
#include <string.h>
|
#include <string.h>
|
||||||
|
@ -35,10 +36,20 @@ uint64_t* out_msg_process();
|
||||||
|
|
||||||
int buzz_script_set(const char* bo_filename,
|
int buzz_script_set(const char* bo_filename,
|
||||||
const char* bdbg_filename, int robot_id);
|
const char* bdbg_filename, int robot_id);
|
||||||
|
|
||||||
|
int buzz_update_set(uint8_t* UP_BO_BUF, const char* bdbg_filename,size_t bcode_size);
|
||||||
|
|
||||||
|
int buzz_update_init_test(uint8_t* UP_BO_BUF, const char* bdbg_filename,size_t bcode_size);
|
||||||
|
|
||||||
void buzz_script_step();
|
void buzz_script_step();
|
||||||
|
|
||||||
void buzz_script_destroy();
|
void buzz_script_destroy();
|
||||||
|
|
||||||
int buzz_script_done();
|
int buzz_script_done();
|
||||||
|
|
||||||
|
int update_step_test();
|
||||||
|
|
||||||
|
uint16_t get_robotid();
|
||||||
|
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
|
@ -74,5 +74,6 @@ int buzzuav_update_prox(buzzvm_t vm);
|
||||||
|
|
||||||
int bzz_cmd();
|
int bzz_cmd();
|
||||||
|
|
||||||
|
int dummy_closure(buzzvm_t vm);
|
||||||
//#endif
|
//#endif
|
||||||
}
|
}
|
||||||
|
|
|
@ -42,6 +42,7 @@ private:
|
||||||
int robot_id=0;
|
int robot_id=0;
|
||||||
//int oldcmdID=0;
|
//int oldcmdID=0;
|
||||||
int rc_cmd;
|
int rc_cmd;
|
||||||
|
int barrier;
|
||||||
std::string bzzfile_name, fcclient_name, rcservice_name,bcfname,dbgfname,out_payload,in_payload; //, rcclient;
|
std::string bzzfile_name, fcclient_name, rcservice_name,bcfname,dbgfname,out_payload,in_payload; //, rcclient;
|
||||||
bool rcclient;
|
bool rcclient;
|
||||||
ros::ServiceClient mav_client;
|
ros::ServiceClient mav_client;
|
||||||
|
|
|
@ -10,6 +10,7 @@
|
||||||
<!-- param name="in_payload" value="/rosbuzz_node1/outMavlink"-->
|
<!-- param name="in_payload" value="/rosbuzz_node1/outMavlink"-->
|
||||||
<param name="out_payload" value="/outMavlink"/>
|
<param name="out_payload" value="/outMavlink"/>
|
||||||
<param name="robot_id" value="1"/>
|
<param name="robot_id" value="1"/>
|
||||||
|
<param name="No_of_Robots" value="1"/>
|
||||||
</node>
|
</node>
|
||||||
<!-- node name="rosbuzz_node1" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" -->
|
<!-- node name="rosbuzz_node1" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" -->
|
||||||
<!-- param name="bzzfile_name" value="/home/vivek/catkin_ws/src/rosbuzz/src/test1.bzz" -->
|
<!-- param name="bzzfile_name" value="/home/vivek/catkin_ws/src/rosbuzz/src/test1.bzz" -->
|
||||||
|
|
|
@ -9,6 +9,7 @@
|
||||||
<param name="in_payload" value="/inMavlink"/>
|
<param name="in_payload" value="/inMavlink"/>
|
||||||
<param name="out_payload" value="/outMavlink"/>
|
<param name="out_payload" value="/outMavlink"/>
|
||||||
<param name="robot_id" value="3"/>
|
<param name="robot_id" value="3"/>
|
||||||
|
<param name="No_of_Robots" value="2"/>
|
||||||
</node>
|
</node>
|
||||||
|
|
||||||
|
|
||||||
|
|
|
@ -0,0 +1,486 @@
|
||||||
|
#include <stdio.h>
|
||||||
|
#include <stdlib.h>
|
||||||
|
#include <fcntl.h>
|
||||||
|
#include <unistd.h>
|
||||||
|
#include <sys/inotify.h>
|
||||||
|
#include "buzz_update.h"
|
||||||
|
#include <buzz_utility.h>
|
||||||
|
#include <string.h>
|
||||||
|
#include <buzz/buzzdebug.h>
|
||||||
|
#include <sys/time.h>
|
||||||
|
#define MAX_BUCKETS 10
|
||||||
|
|
||||||
|
/*Temp for data collection*/
|
||||||
|
//static int neigh=-1;
|
||||||
|
//static struct timeval t1, t2;
|
||||||
|
//static clock_t begin;
|
||||||
|
//static clock_t end;
|
||||||
|
//void collect_data();
|
||||||
|
/*Temp end */
|
||||||
|
static int fd,wd =0;
|
||||||
|
static int old_update =0;
|
||||||
|
static buzz_updater_elem_t updater;
|
||||||
|
static int no_of_robot;
|
||||||
|
static char* dbgf_name;
|
||||||
|
static const char* bzz_file;
|
||||||
|
|
||||||
|
uint32_t buzz_updater_hashfunp (const void* key){
|
||||||
|
return (*(uint16_t*)key);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
int buzz_updater_key_cmp(const void* a, const void* b){
|
||||||
|
if( *(uint16_t*)a < *(uint16_t*)b ) return -1;
|
||||||
|
if( *(uint16_t*)a > *(uint16_t*)b ) return 1;
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
void updater_entry_destroy(const void* key, void* data, void* params) {
|
||||||
|
// fprintf(stdout,"freeing element.\n");
|
||||||
|
free((void*)key);
|
||||||
|
free((void*)data);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
void standby_barrier_test(const void* key, const void* data, void* tmp){
|
||||||
|
//fprintf(stdout,"Checking barrier for robot :%i it has barrier : %i.\n",*(uint16_t*) key,*(uint8_t*) data );
|
||||||
|
if(*(uint8_t*) data == CODE_STANDBY || *(uint8_t*) data == CODE_RUNNING ){
|
||||||
|
if(updater->update_no==*(uint8_t*) (data+sizeof(uint8_t)) )
|
||||||
|
(*(uint8_t*)tmp)+=1;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void init_update_monitor(const char* bo_filename,int barrier){
|
||||||
|
fprintf(stdout,"intiialized file monitor.\n");
|
||||||
|
fd=inotify_init1(IN_NONBLOCK);
|
||||||
|
if ( fd < 0 ) {
|
||||||
|
perror( "inotify_init error" );
|
||||||
|
}
|
||||||
|
/* watch /.bzz file for any activity and report it back to me */
|
||||||
|
wd=inotify_add_watch(fd, bzz_file,IN_ALL_EVENTS );
|
||||||
|
|
||||||
|
uint8_t* BO_BUF = 0;
|
||||||
|
FILE* fp = fopen(bo_filename, "rb");
|
||||||
|
|
||||||
|
if(!fp) {
|
||||||
|
perror(bo_filename);
|
||||||
|
|
||||||
|
}
|
||||||
|
fseek(fp, 0, SEEK_END);
|
||||||
|
|
||||||
|
size_t bcode_size = ftell(fp);
|
||||||
|
rewind(fp);
|
||||||
|
|
||||||
|
BO_BUF = (uint8_t*)malloc(bcode_size);
|
||||||
|
if(fread(BO_BUF, 1, bcode_size, fp) < bcode_size) {
|
||||||
|
perror(bo_filename);
|
||||||
|
|
||||||
|
fclose(fp);
|
||||||
|
//return 0;
|
||||||
|
}
|
||||||
|
fclose(fp);
|
||||||
|
|
||||||
|
updater = (buzz_updater_elem_t)malloc(sizeof(struct buzz_updater_elem_s));
|
||||||
|
/* Create a new table for updater*/
|
||||||
|
updater->bcode = BO_BUF;
|
||||||
|
updater->bcode_size = bcode_size;
|
||||||
|
updater->mode=CODE_RUNNING;
|
||||||
|
updater->robotid= buzz_utility::get_robotid();
|
||||||
|
updater->state_dict=buzzdict_new(MAX_BUCKETS,
|
||||||
|
sizeof(uint16_t),
|
||||||
|
sizeof(uint16_t),
|
||||||
|
buzz_updater_hashfunp,
|
||||||
|
buzz_updater_key_cmp,
|
||||||
|
updater_entry_destroy);
|
||||||
|
updater->outmsg_queue = NULL;
|
||||||
|
updater->inmsg_queue = NULL;
|
||||||
|
updater->update_no=0;
|
||||||
|
no_of_robot=barrier;
|
||||||
|
//updater->outmsg_queue=
|
||||||
|
// update_table->barrier=nvs;
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
int check_update(){
|
||||||
|
|
||||||
|
struct inotify_event *event;
|
||||||
|
char buf[1024];
|
||||||
|
int check =0;
|
||||||
|
int i=0;
|
||||||
|
int len=read(fd,buf,1024);
|
||||||
|
while(i<len){
|
||||||
|
event=(struct inotify_event *) &buf[i];
|
||||||
|
|
||||||
|
/* file was modified this flag is true in nano and self delet in gedit and other editors */
|
||||||
|
//fprintf(stdout,"inside file monitor.\n");
|
||||||
|
if(event->mask & (IN_MODIFY| IN_DELETE_SELF)){
|
||||||
|
/*respawn watch if the file is self deleted */
|
||||||
|
inotify_rm_watch(fd,wd);
|
||||||
|
close(fd);
|
||||||
|
fd=inotify_init1(IN_NONBLOCK);
|
||||||
|
wd=inotify_add_watch(fd,bzz_file,IN_ALL_EVENTS);
|
||||||
|
//fprintf(stdout,"event.\n");
|
||||||
|
/* To mask multiple writes from editors*/
|
||||||
|
if(!old_update){
|
||||||
|
check=1;
|
||||||
|
old_update =1;
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
/* update index to start of next event */
|
||||||
|
i+=sizeof(struct inotify_event)+event->len;
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
if (!check) old_update=0;
|
||||||
|
/*if(update){
|
||||||
|
buzz_script_set(update_bo, update_bdbg);
|
||||||
|
update = 0;
|
||||||
|
}*/
|
||||||
|
return check;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
void code_message_outqueue_append(int type){
|
||||||
|
uint16_t size =0;
|
||||||
|
//fprintf(stdout,"updater queue append.\n");
|
||||||
|
//if(updater->outmsg_queue == NULL)
|
||||||
|
//updater->outmsg_queue=(updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s));
|
||||||
|
//else {
|
||||||
|
// destroy_out_msg_queue();
|
||||||
|
updater->outmsg_queue=(updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s));
|
||||||
|
// }
|
||||||
|
if(type==SEND_CODE){
|
||||||
|
updater->outmsg_queue->queue = (uint8_t*)malloc((6*sizeof(uint8_t))+updater->bcode_size);
|
||||||
|
updater->outmsg_queue->size = (uint8_t*)malloc(sizeof(uint16_t));
|
||||||
|
//*(uint16_t*)updater->outmsg_queue->size =(3*sizeof(uint8_t))+sizeof(size_t)+updater->bcode_size;
|
||||||
|
|
||||||
|
*(uint16_t*)(updater->outmsg_queue->queue+size) = (uint16_t) updater->robotid;
|
||||||
|
size+=sizeof(uint16_t);
|
||||||
|
*(uint8_t*)(updater->outmsg_queue->queue+size) = (uint8_t) updater->mode;
|
||||||
|
size+=sizeof(uint8_t);
|
||||||
|
*(uint8_t*)(updater->outmsg_queue->queue+size) = updater->update_no;
|
||||||
|
size+=sizeof(uint8_t);
|
||||||
|
*(uint16_t*)(updater->outmsg_queue->queue+size) = (uint16_t) updater->bcode_size;
|
||||||
|
size+=sizeof(uint16_t);
|
||||||
|
memcpy(updater->outmsg_queue->queue+size, updater->bcode, updater->bcode_size);
|
||||||
|
size+=updater->bcode_size;
|
||||||
|
FILE *fp;
|
||||||
|
fp=fopen("update.bo", "wb");
|
||||||
|
fwrite((updater->bcode), updater->bcode_size, 1, fp);
|
||||||
|
fclose(fp);
|
||||||
|
//fprintf(stdout,"[Debug : ]size = %i \n",(int)sizeof(size_t));
|
||||||
|
*(uint16_t*)updater->outmsg_queue->size=size;
|
||||||
|
/*Update local dictionary*/
|
||||||
|
uint8_t* dict_update=(uint8_t*)malloc(sizeof(uint16_t));
|
||||||
|
*(uint8_t*)dict_update=updater->mode;
|
||||||
|
*(uint8_t*)(dict_update+sizeof(uint8_t))=updater->update_no;
|
||||||
|
buzzdict_set(updater->state_dict,(uint8_t*) &(updater->robotid), (uint8_t*)dict_update);
|
||||||
|
delete_p(dict_update);
|
||||||
|
|
||||||
|
}
|
||||||
|
else if(type==STATE_MSG){
|
||||||
|
updater->outmsg_queue->queue = (uint8_t*)malloc(3*sizeof(uint8_t));
|
||||||
|
updater->outmsg_queue->size = (uint8_t*)malloc(sizeof(uint16_t));
|
||||||
|
//*(uint16_t*)updater->outmsg_queue->size = 3*sizeof(uint8_t);
|
||||||
|
*(uint16_t*)(updater->outmsg_queue->queue+size) = (uint16_t) updater->robotid;
|
||||||
|
size+=sizeof(uint16_t);
|
||||||
|
*(uint8_t*)(updater->outmsg_queue->queue+size) = (uint8_t) updater->mode;
|
||||||
|
size+=sizeof(uint8_t);
|
||||||
|
*(uint8_t*)(updater->outmsg_queue->queue+size) = updater->update_no;
|
||||||
|
size+=sizeof(uint8_t);
|
||||||
|
*(uint16_t*)updater->outmsg_queue->size=size;
|
||||||
|
/*Update local dictionary*/
|
||||||
|
uint8_t* dict_update=(uint8_t*)malloc(sizeof(uint16_t));
|
||||||
|
*(uint8_t*)dict_update=updater->mode;
|
||||||
|
*(uint8_t*)(dict_update+sizeof(uint8_t))=updater->update_no;
|
||||||
|
buzzdict_set(updater->state_dict,(uint8_t*) &(updater->robotid), (uint8_t*)dict_update);
|
||||||
|
delete_p(dict_update);
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
void code_message_inqueue_append(uint8_t* msg,uint16_t size){
|
||||||
|
updater->inmsg_queue=(updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s));
|
||||||
|
updater->inmsg_queue->queue = (uint8_t*)malloc(size);
|
||||||
|
updater->inmsg_queue->size = (uint8_t*)malloc(sizeof(uint16_t));
|
||||||
|
memcpy(updater->inmsg_queue->queue, msg, size);
|
||||||
|
*(uint16_t*)updater->inmsg_queue->size = size;
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
void code_message_inqueue_process(){
|
||||||
|
int size=0;
|
||||||
|
//fprintf(stdout,"received state : %i from robot : %i\n",*(uint8_t*)(updater->inmsg_queue->queue+sizeof(uint16_t)),*(uint16_t*)updater->inmsg_queue->queue);
|
||||||
|
if(*(uint8_t*)(updater->inmsg_queue->queue+sizeof(uint16_t)) == CODE_UPDATE){
|
||||||
|
buzzdict_set(updater->state_dict, updater->inmsg_queue->queue,(updater->inmsg_queue->queue+sizeof(uint16_t)));
|
||||||
|
size +=3*sizeof(uint8_t);
|
||||||
|
if(*(uint8_t*)(updater->inmsg_queue->queue+size) > (updater->update_no)){
|
||||||
|
updater->update_no=*(uint8_t*)(updater->inmsg_queue->queue+size);
|
||||||
|
size+=sizeof(uint8_t);
|
||||||
|
if(updater->mode==CODE_RUNNING){
|
||||||
|
uint16_t update_bcode_size =*(uint16_t*)(updater->inmsg_queue->queue+size);
|
||||||
|
size +=sizeof(uint16_t);
|
||||||
|
FILE *fp;
|
||||||
|
fp=fopen("update.bo", "wb");
|
||||||
|
fwrite((updater->inmsg_queue->queue+size), update_bcode_size, 1, fp);
|
||||||
|
fclose(fp);
|
||||||
|
if(buzz_utility::buzz_update_init_test((updater->inmsg_queue->queue+size), dbgf_name,(size_t)update_bcode_size)){
|
||||||
|
fprintf(stdout,"Initializtion of script test passed\n");
|
||||||
|
if(buzz_utility::update_step_test()){
|
||||||
|
/*data logging*/
|
||||||
|
//neigh=1;
|
||||||
|
//gettimeofday(&t1, NULL);
|
||||||
|
//fprintf(stdout,"start and end time in queue process of update Info : %f,%f",(double)begin,(double)end);
|
||||||
|
/*data logging*/
|
||||||
|
fprintf(stdout,"Step test passed\n");
|
||||||
|
updater->mode=CODE_UPDATE;
|
||||||
|
//fprintf(stdout,"updater value = %i\n",updater->mode);
|
||||||
|
free(updater->bcode);
|
||||||
|
updater->bcode = (uint8_t*)malloc(update_bcode_size);
|
||||||
|
memcpy(updater->bcode, updater->inmsg_queue->queue+size, update_bcode_size);
|
||||||
|
//updater->bcode = BO_BUF;
|
||||||
|
updater->bcode_size = (size_t)update_bcode_size;
|
||||||
|
code_message_outqueue_append(SEND_CODE);
|
||||||
|
updater->mode=CODE_NEIGHBOUR;
|
||||||
|
|
||||||
|
//return updater->mode;
|
||||||
|
//return 0;
|
||||||
|
}
|
||||||
|
/*Unable to step something wrong*/
|
||||||
|
else{
|
||||||
|
fprintf(stdout,"Step test failed, stick to old script\n");
|
||||||
|
buzz_utility::buzz_update_set((updater)->bcode, dbgf_name, (updater)->bcode_size);
|
||||||
|
|
||||||
|
//return updater->mode;
|
||||||
|
//return 1;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
fprintf(stdout,"Initialiation test failed, stick to old script\n");
|
||||||
|
buzz_utility::buzz_update_set((updater)->bcode, dbgf_name, (updater)->bcode_size);
|
||||||
|
//return updater->mode;
|
||||||
|
//return 1;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
buzzdict_set(updater->state_dict,updater->inmsg_queue->queue, updater->inmsg_queue->queue+sizeof(uint16_t));
|
||||||
|
}
|
||||||
|
//fprintf(stdout,"in queue freed\n");
|
||||||
|
delete_p(updater->inmsg_queue->queue);
|
||||||
|
delete_p(updater->inmsg_queue->size);
|
||||||
|
delete_p(updater->inmsg_queue);
|
||||||
|
}
|
||||||
|
int update_routine(const char* bcfname,
|
||||||
|
const char* dbgfname, int destroy){
|
||||||
|
dbgf_name=(char*)dbgfname;
|
||||||
|
//fprintf(stdout,"[Debug : ]updater value = %i \n",updater->mode);
|
||||||
|
if(updater->mode==CODE_RUNNING){
|
||||||
|
if(check_update()){
|
||||||
|
std::string bzzfile_name(bzz_file);
|
||||||
|
stringstream bzzfile_in_compile;
|
||||||
|
std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/"));
|
||||||
|
bzzfile_in_compile<<path<<"/";
|
||||||
|
path = bzzfile_in_compile.str();
|
||||||
|
bzzfile_in_compile.str("");
|
||||||
|
std::string name = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1);
|
||||||
|
name = name.substr(0,name.find_last_of("."));
|
||||||
|
bzzfile_in_compile << "bzzparse "<<bzzfile_name<<" "<<path<< name<<".basm";
|
||||||
|
|
||||||
|
FILE *fp;
|
||||||
|
int comp=0;
|
||||||
|
char buf[128];
|
||||||
|
fprintf(stdout,"Update found \nUpdating script ...\n");
|
||||||
|
|
||||||
|
if ((fp = popen(bzzfile_in_compile.str().c_str(), "r")) == NULL) { // to change file edit
|
||||||
|
fprintf(stdout,"Error opening pipe!\n");
|
||||||
|
}
|
||||||
|
|
||||||
|
while (fgets(buf, 128, fp) != NULL) {
|
||||||
|
fprintf(stdout,"OUTPUT: %s \n", buf);
|
||||||
|
comp=1;
|
||||||
|
}
|
||||||
|
bzzfile_in_compile.str("");
|
||||||
|
bzzfile_in_compile <<"bzzasm "<<path<<name<<".basm "<<path<<name<<".bo "<<path<<name<<".bdbg";
|
||||||
|
|
||||||
|
if ((fp = popen(bzzfile_in_compile.str().c_str(), "r")) == NULL) { // to change file edit
|
||||||
|
fprintf(stdout,"Error opening pipe!\n");
|
||||||
|
}
|
||||||
|
while (fgets(buf, 128, fp) != NULL) {
|
||||||
|
fprintf(stdout,"OUTPUT: %s \n", buf);
|
||||||
|
}
|
||||||
|
|
||||||
|
if(pclose(fp) || comp) {
|
||||||
|
fprintf(stdout,"Errors in comipilg script so staying on old script\n");
|
||||||
|
// return -1;
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
|
||||||
|
uint8_t* BO_BUF = 0;
|
||||||
|
FILE* fp = fopen(bcfname, "rb"); // to change file edit
|
||||||
|
if(!fp) {
|
||||||
|
perror(bcfname);
|
||||||
|
|
||||||
|
}
|
||||||
|
fseek(fp, 0, SEEK_END);
|
||||||
|
size_t bcode_size = ftell(fp);
|
||||||
|
rewind(fp);
|
||||||
|
BO_BUF = (uint8_t*)malloc(bcode_size);
|
||||||
|
if(fread(BO_BUF, 1, bcode_size, fp) < bcode_size) {
|
||||||
|
perror(bcfname);
|
||||||
|
|
||||||
|
fclose(fp);
|
||||||
|
|
||||||
|
}
|
||||||
|
fclose(fp);
|
||||||
|
if(buzz_utility::buzz_update_init_test(BO_BUF, dbgfname,bcode_size)){
|
||||||
|
fprintf(stdout,"Initializtion of script test passed\n");
|
||||||
|
if(buzz_utility::update_step_test()){
|
||||||
|
/*data logging*/
|
||||||
|
//neigh=0;
|
||||||
|
//gettimeofday(&t1, NULL);
|
||||||
|
//fprintf(stdout,"start and end time in code running state Info : %f,%f",(double)begin,(double)end);
|
||||||
|
/*data logging*/
|
||||||
|
fprintf(stdout,"Step test passed\n");
|
||||||
|
updater->update_no+=1;
|
||||||
|
updater->mode=CODE_UPDATE;
|
||||||
|
//fprintf(stdout,"updater value = %i\n",updater->mode);
|
||||||
|
delete_p(updater->bcode);
|
||||||
|
updater->bcode = (uint8_t*)malloc(bcode_size);
|
||||||
|
memcpy(updater->bcode, BO_BUF, bcode_size);
|
||||||
|
updater->bcode_size = bcode_size;
|
||||||
|
code_message_outqueue_append(SEND_CODE);
|
||||||
|
delete_p(BO_BUF);
|
||||||
|
return updater->mode;
|
||||||
|
//return 0;
|
||||||
|
}
|
||||||
|
/*Unable to step something wrong*/
|
||||||
|
else{
|
||||||
|
fprintf(stdout,"step test failed, stick to old script\n");
|
||||||
|
buzz_utility::buzz_update_set((updater)->bcode, dbgfname, (updater)->bcode_size);
|
||||||
|
code_message_outqueue_append(STATE_MSG);
|
||||||
|
delete_p(BO_BUF);
|
||||||
|
return updater->mode;
|
||||||
|
//return 1;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
fprintf(stdout,"unable to set new script to switch to old new script\n");
|
||||||
|
delete_p(BO_BUF);
|
||||||
|
buzz_utility::buzz_update_set((updater)->bcode, dbgfname, (updater)->bcode_size);
|
||||||
|
code_message_outqueue_append(STATE_MSG);
|
||||||
|
return updater->mode;
|
||||||
|
//return 1;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
code_message_outqueue_append(STATE_MSG);
|
||||||
|
}
|
||||||
|
|
||||||
|
else if (updater->mode==CODE_UPDATE){
|
||||||
|
|
||||||
|
code_message_outqueue_append(SEND_CODE);
|
||||||
|
updater->mode=CODE_STANDBY;
|
||||||
|
|
||||||
|
}
|
||||||
|
else if (updater->mode==CODE_NEIGHBOUR){
|
||||||
|
updater->mode=CODE_STANDBY;
|
||||||
|
}
|
||||||
|
else if (updater->mode==CODE_STANDBY){
|
||||||
|
|
||||||
|
uint8_t* tmp =(uint8_t*)malloc(sizeof(uint8_t));
|
||||||
|
*(uint8_t*)tmp=0;
|
||||||
|
buzzdict_foreach( updater->state_dict,reinterpret_cast<buzzdict_elem_funp>(standby_barrier_test), tmp);
|
||||||
|
fprintf(stdout,"Standby barrier ................... %i\n",*(uint8_t*)tmp);
|
||||||
|
code_message_outqueue_append(STATE_MSG);
|
||||||
|
if(*(uint8_t*)tmp==no_of_robot) {
|
||||||
|
updater->mode=CODE_RUNNING;
|
||||||
|
//gettimeofday(&t2, NULL);
|
||||||
|
//fprintf(stdout,"start and end time in standby state Info : %f,%f",(double)begin,(double)end);
|
||||||
|
//collect_data();
|
||||||
|
buzz_utility::buzz_update_set((updater)->bcode, dbgf_name, (updater)->bcode_size);
|
||||||
|
}
|
||||||
|
|
||||||
|
delete_p(tmp);
|
||||||
|
//fprintf(stdout,"freed tmp\n");
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
if(destroy){
|
||||||
|
|
||||||
|
destroy_updater();
|
||||||
|
fprintf(stdout,"updater destoryed.\n");
|
||||||
|
}
|
||||||
|
return updater->mode;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
uint8_t* getupdater_out_msg(){
|
||||||
|
return (uint8_t*)updater->outmsg_queue->queue;
|
||||||
|
}
|
||||||
|
|
||||||
|
uint8_t* getupdate_out_msg_size(){
|
||||||
|
//fprintf(stdout,"[Debug : ]size = %i \n",*(uint16_t*)updater->outmsg_queue->size);
|
||||||
|
|
||||||
|
return updater->outmsg_queue->size;
|
||||||
|
}
|
||||||
|
|
||||||
|
void destroy_out_msg_queue(){
|
||||||
|
//fprintf(stdout,"out queue freed\n");
|
||||||
|
delete_p(updater->outmsg_queue->queue);
|
||||||
|
delete_p(updater->outmsg_queue->size);
|
||||||
|
delete_p(updater->outmsg_queue);
|
||||||
|
}
|
||||||
|
|
||||||
|
int get_update_mode(){
|
||||||
|
return updater->mode;
|
||||||
|
}
|
||||||
|
|
||||||
|
void destroy_updater(){
|
||||||
|
buzz_utility::buzz_script_destroy();
|
||||||
|
//dictonary
|
||||||
|
//fprintf(stdout,"freeing dict.\n");
|
||||||
|
buzzdict_t* updater_dict_ptr=&(updater->state_dict);
|
||||||
|
buzzdict_destroy((updater_dict_ptr));
|
||||||
|
//fprintf(stdout,"freeing complete dict.\n");
|
||||||
|
delete_p(updater->bcode);
|
||||||
|
//fprintf(stdout,"freeing complete bcode.\n");
|
||||||
|
|
||||||
|
if(updater->outmsg_queue){
|
||||||
|
delete_p(updater->outmsg_queue->queue);
|
||||||
|
delete_p(updater->outmsg_queue->size);
|
||||||
|
delete_p(updater->outmsg_queue);
|
||||||
|
}
|
||||||
|
if(updater->inmsg_queue){
|
||||||
|
delete_p(updater->inmsg_queue->queue);
|
||||||
|
delete_p(updater->inmsg_queue->size);
|
||||||
|
delete_p(updater->inmsg_queue);
|
||||||
|
}
|
||||||
|
//
|
||||||
|
inotify_rm_watch(fd,wd);
|
||||||
|
close(fd);
|
||||||
|
}
|
||||||
|
|
||||||
|
void set_bzz_file(const char* in_bzz_file){
|
||||||
|
bzz_file=in_bzz_file;
|
||||||
|
}
|
||||||
|
void collect_data(){
|
||||||
|
//fprintf(stdout,"start and end time in data collection Info : %f,%f",(double)begin,(double)end);
|
||||||
|
//double time_spent = (t2.tv_sec - t1.tv_sec) * 1000.0; //(double)(end - begin) / CLOCKS_PER_SEC;
|
||||||
|
//time_spent += (t2.tv_usec - t1.tv_usec) / 1000.0;
|
||||||
|
//fprintf(stdout,"Data logger Info : %i,%i,%f,%i,%i,%i\n",(int)updater->robotid,neigh,time_spent,(int)updater->bcode_size,(int)no_of_robot,(int)updater->update_no);
|
||||||
|
//FILE *Fileptr;
|
||||||
|
//Fileptr=fopen("logger.csv", "a");
|
||||||
|
//fprintf(Fileptr,"%i,%i,%f,%i,%i,%i\n",(int)updater->robotid,neigh,time_spent,(int)updater->bcode_size,(int)no_of_robot,(int)updater->update_no);
|
||||||
|
//fclose(Fileptr);
|
||||||
|
}
|
||||||
|
|
|
@ -17,7 +17,8 @@ namespace buzz_utility{
|
||||||
static char* BO_FNAME = 0;
|
static char* BO_FNAME = 0;
|
||||||
static uint8_t* BO_BUF = 0;
|
static uint8_t* BO_BUF = 0;
|
||||||
static buzzdebug_t DBG_INFO = 0;
|
static buzzdebug_t DBG_INFO = 0;
|
||||||
static unsigned int MSG_SIZE = 32;
|
static uint8_t MSG_SIZE = 100; // Only 100 bytes of Buzz messages every step
|
||||||
|
static int MAX_MSG_SIZE = 10100; // Maximum Msg size for sending update packets
|
||||||
|
|
||||||
|
|
||||||
/****************************************/
|
/****************************************/
|
||||||
|
@ -35,9 +36,9 @@ namespace buzz_utility{
|
||||||
(it->second).z);
|
(it->second).z);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
/***************************************/
|
/**************************************************************************/
|
||||||
/*Reinterprets uint64_t into 4 uint16_t*/
|
/*Deserializes uint64_t into 4 uint16_t, freeing out is left to the user */
|
||||||
/***************************************/
|
/**************************************************************************/
|
||||||
uint16_t* u64_cvt_u16(uint64_t u64){
|
uint16_t* u64_cvt_u16(uint64_t u64){
|
||||||
uint16_t* out = new uint16_t[4];
|
uint16_t* out = new uint16_t[4];
|
||||||
uint32_t int32_1 = u64 & 0xFFFFFFFF;
|
uint32_t int32_1 = u64 & 0xFFFFFFFF;
|
||||||
|
@ -54,17 +55,26 @@ namespace buzz_utility{
|
||||||
/*Appends obtained messages to buzz in message Queue*/
|
/*Appends obtained messages to buzz in message Queue*/
|
||||||
/***************************************************/
|
/***************************************************/
|
||||||
|
|
||||||
|
/*******************************************************************************************************************/
|
||||||
|
/* Message format of payload (Each slot is uint64_t) */
|
||||||
|
/* _______________________________________________________________________________________________________________ */
|
||||||
|
/*| | |*/
|
||||||
|
/*|Size in Uint64_t(but size is Uint16_t)|robot_id|Update msg size|Update msg|Update msgs+Buzz_msgs with size.....|*/
|
||||||
|
/*|__________________________________________________________________________|____________________________________|*/
|
||||||
|
/*******************************************************************************************************************/
|
||||||
|
|
||||||
void in_msg_process(uint64_t* payload){
|
void in_msg_process(uint64_t* payload){
|
||||||
|
|
||||||
/* Go through messages and add them to the FIFO */
|
/* Go through messages and add them to the FIFO */
|
||||||
uint16_t* data= u64_cvt_u16((uint64_t)payload[0]);
|
uint16_t* data= u64_cvt_u16((uint64_t)payload[0]);
|
||||||
|
/*Size is at first 2 bytes*/
|
||||||
uint16_t size=data[0]*sizeof(uint64_t);
|
uint16_t size=data[0]*sizeof(uint64_t);
|
||||||
delete[] data;
|
delete[] data;
|
||||||
uint8_t* pl =(uint8_t*)malloc(size);
|
uint8_t* pl =(uint8_t*)malloc(size);
|
||||||
memset(pl, 0,size);
|
memset(pl, 0,size);
|
||||||
/* Copy packet into temporary buffer */
|
/* Copy packet into temporary buffer */
|
||||||
memcpy(pl, payload ,size);
|
memcpy(pl, payload ,size);
|
||||||
|
/*size and robot id read*/
|
||||||
size_t tot = sizeof(uint32_t);
|
size_t tot = sizeof(uint32_t);
|
||||||
/*for(int i=0;i<data[0];i++){
|
/*for(int i=0;i<data[0];i++){
|
||||||
uint16_t* out = u64_cvt_u16(payload[i]);
|
uint16_t* out = u64_cvt_u16(payload[i]);
|
||||||
|
@ -74,18 +84,31 @@ namespace buzz_utility{
|
||||||
}*/
|
}*/
|
||||||
/* Go through the messages until there's nothing else to read */
|
/* Go through the messages until there's nothing else to read */
|
||||||
uint16_t unMsgSize;
|
uint16_t unMsgSize;
|
||||||
do {
|
unMsgSize = *(uint16_t*)(pl + tot);
|
||||||
/* Get payload size */
|
tot += sizeof(uint16_t);
|
||||||
unMsgSize = *(uint16_t*)(pl + tot);
|
code_message_inqueue_append(pl+tot,unMsgSize);
|
||||||
tot += sizeof(uint16_t);
|
tot += unMsgSize;
|
||||||
/* Append message to the Buzz input message queue */
|
code_message_inqueue_process();
|
||||||
if(unMsgSize > 0 && unMsgSize <= size - tot ) {
|
unMsgSize=0;
|
||||||
buzzinmsg_queue_append(VM,
|
/*Check for Buzz messages only when the code is running*/
|
||||||
buzzmsg_payload_frombuffer(pl +tot, unMsgSize));
|
if(get_update_mode()==CODE_RUNNING){
|
||||||
tot += unMsgSize;
|
uint8_t buzz_msg_pre=*(uint8_t*)(pl + tot);
|
||||||
}
|
tot+= sizeof(uint8_t);
|
||||||
}while(size - tot > sizeof(uint16_t) && unMsgSize > 0);
|
/*Obtain Buzz messages only when they are present*/
|
||||||
|
if(buzz_msg_pre){
|
||||||
|
do {
|
||||||
|
/* Get payload size */
|
||||||
|
unMsgSize = *(uint16_t*)(pl + tot);
|
||||||
|
tot += sizeof(uint16_t);
|
||||||
|
/* Append message to the Buzz input message queue */
|
||||||
|
if(unMsgSize > 0 && unMsgSize <= size - tot ) {
|
||||||
|
buzzinmsg_queue_append(VM,
|
||||||
|
buzzmsg_payload_frombuffer(pl +tot, unMsgSize));
|
||||||
|
tot += unMsgSize;
|
||||||
|
}
|
||||||
|
}while(size - tot > sizeof(uint16_t) && unMsgSize > 0);
|
||||||
|
}
|
||||||
|
}
|
||||||
/* Process messages */
|
/* Process messages */
|
||||||
buzzvm_process_inmsgs(VM);
|
buzzvm_process_inmsgs(VM);
|
||||||
}
|
}
|
||||||
|
@ -95,26 +118,39 @@ namespace buzz_utility{
|
||||||
|
|
||||||
uint64_t* out_msg_process(){
|
uint64_t* out_msg_process(){
|
||||||
buzzvm_process_outmsgs(VM);
|
buzzvm_process_outmsgs(VM);
|
||||||
uint8_t* buff_send =(uint8_t*)malloc(MSG_SIZE);
|
uint8_t* buff_send =(uint8_t*)malloc(MAX_MSG_SIZE);
|
||||||
memset(buff_send, 0, MSG_SIZE);
|
memset(buff_send, 0, MAX_MSG_SIZE);
|
||||||
ssize_t tot = sizeof(uint16_t);
|
ssize_t tot = sizeof(uint16_t);
|
||||||
/* Send robot id */
|
/* Send robot id */
|
||||||
*(uint16_t*)(buff_send+tot) = (uint16_t) VM->robot;
|
*(uint16_t*)(buff_send+tot) = (uint16_t) VM->robot;
|
||||||
tot += sizeof(uint16_t);
|
tot += sizeof(uint16_t);
|
||||||
/* Send messages from FIFO */
|
/*Append updater msg size*/
|
||||||
do {
|
memcpy(buff_send + tot, (uint8_t*)getupdate_out_msg_size(), sizeof(uint16_t));
|
||||||
/* Are there more messages? */
|
// fprintf(stdout,"Msg size : %i\n",(int) *(uint16_t*)(STREAM_SEND_BUF + tot));
|
||||||
if(buzzoutmsg_queue_isempty(VM)) break;
|
tot+= sizeof(uint16_t);
|
||||||
/* Get first message */
|
/*Append updater msgs*/
|
||||||
buzzmsg_payload_t m = buzzoutmsg_queue_first(VM);
|
memcpy(buff_send + tot, (uint8_t*) getupdater_out_msg(),*(uint16_t*)getupdate_out_msg_size());
|
||||||
/* Make sure the next message fits the data buffer */
|
// fprintf(stdout,"Msg 1 : %i , and Msg 2: %i\n",(int) *(uint16_t*)(STREAM_SEND_BUF + tot),(int) *(uint8_t*)(STREAM_SEND_BUF + tot+sizeof(uint16_t)));
|
||||||
if(tot + buzzmsg_payload_size(m) + sizeof(uint16_t)
|
tot+= *(uint16_t*)getupdate_out_msg_size();
|
||||||
>
|
/*destroy the updater out msg queue*/
|
||||||
MSG_SIZE) {
|
destroy_out_msg_queue();
|
||||||
buzzmsg_payload_destroy(&m);
|
if(get_update_mode()==CODE_RUNNING){
|
||||||
break;
|
*(uint8_t*)(buff_send + tot) = 1;
|
||||||
}
|
tot+= sizeof(uint8_t);
|
||||||
|
/* Send messages from FIFO */
|
||||||
|
do {
|
||||||
|
/* Are there more messages? */
|
||||||
|
if(buzzoutmsg_queue_isempty(VM)) break;
|
||||||
|
/* Get first message */
|
||||||
|
buzzmsg_payload_t m = buzzoutmsg_queue_first(VM);
|
||||||
|
/* Make sure the next message makes the data buffer with buzz messages to be less than 100 Bytes */
|
||||||
|
if(tot + buzzmsg_payload_size(m) + sizeof(uint16_t)
|
||||||
|
>
|
||||||
|
MSG_SIZE) {
|
||||||
|
buzzmsg_payload_destroy(&m);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
/* Add message length to data buffer */
|
/* Add message length to data buffer */
|
||||||
*(uint16_t*)(buff_send + tot) = (uint16_t)buzzmsg_payload_size(m);
|
*(uint16_t*)(buff_send + tot) = (uint16_t)buzzmsg_payload_size(m);
|
||||||
tot += sizeof(uint16_t);
|
tot += sizeof(uint16_t);
|
||||||
|
@ -126,11 +162,14 @@ namespace buzz_utility{
|
||||||
/* Get rid of message */
|
/* Get rid of message */
|
||||||
buzzoutmsg_queue_next(VM);
|
buzzoutmsg_queue_next(VM);
|
||||||
buzzmsg_payload_destroy(&m);
|
buzzmsg_payload_destroy(&m);
|
||||||
} while(1);
|
} while(1);
|
||||||
|
}
|
||||||
int total_size =(ceil((float)tot/(float)sizeof(uint64_t)));
|
else{
|
||||||
|
*(uint8_t*)(buff_send + tot) = 0;
|
||||||
|
tot+= sizeof(uint8_t);
|
||||||
|
}
|
||||||
|
uint16_t total_size =(ceil((float)tot/(float)sizeof(uint64_t)));
|
||||||
*(uint16_t*)buff_send = (uint16_t) total_size;
|
*(uint16_t*)buff_send = (uint16_t) total_size;
|
||||||
|
|
||||||
|
|
||||||
uint64_t* payload_64 = new uint64_t[total_size];
|
uint64_t* payload_64 = new uint64_t[total_size];
|
||||||
|
|
||||||
|
@ -140,7 +179,8 @@ namespace buzz_utility{
|
||||||
}*/
|
}*/
|
||||||
/* Send message */
|
/* Send message */
|
||||||
return payload_64;
|
return payload_64;
|
||||||
}
|
}
|
||||||
|
|
||||||
/****************************************/
|
/****************************************/
|
||||||
/*Buzz script not able to load*/
|
/*Buzz script not able to load*/
|
||||||
/****************************************/
|
/****************************************/
|
||||||
|
@ -193,6 +233,33 @@ namespace buzz_utility{
|
||||||
return BUZZVM_STATE_READY;
|
return BUZZVM_STATE_READY;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/**************************************************/
|
||||||
|
/*Register dummy Buzz hooks for test during update*/
|
||||||
|
/**************************************************/
|
||||||
|
|
||||||
|
static int testing_buzz_register_hooks() {
|
||||||
|
buzzvm_pushs(VM, buzzvm_string_register(VM, "print", 1));
|
||||||
|
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print));
|
||||||
|
buzzvm_gstore(VM);
|
||||||
|
buzzvm_pushs(VM, buzzvm_string_register(VM, "log", 1));
|
||||||
|
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print));
|
||||||
|
buzzvm_gstore(VM);
|
||||||
|
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_goto", 1));
|
||||||
|
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
|
||||||
|
buzzvm_gstore(VM);
|
||||||
|
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_takeoff", 1));
|
||||||
|
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
|
||||||
|
buzzvm_gstore(VM);
|
||||||
|
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_gohome", 1));
|
||||||
|
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
|
||||||
|
buzzvm_gstore(VM);
|
||||||
|
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_land", 1));
|
||||||
|
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
|
||||||
|
buzzvm_gstore(VM);
|
||||||
|
return BUZZVM_STATE_READY;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
/****************************************/
|
/****************************************/
|
||||||
/*Sets the .bzz and .bdbg file*/
|
/*Sets the .bzz and .bdbg file*/
|
||||||
/****************************************/
|
/****************************************/
|
||||||
|
@ -231,29 +298,123 @@ namespace buzz_utility{
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
fclose(fd);
|
fclose(fd);
|
||||||
|
/* Read debug information */
|
||||||
|
//if(!buzzdebug_fromfile(DBG_INFO, bdbg_filename)) {
|
||||||
|
// buzzvm_destroy(&VM);
|
||||||
|
// buzzdebug_destroy(&DBG_INFO);
|
||||||
|
// perror(bdbg_filename);
|
||||||
|
// return 0;
|
||||||
|
//}
|
||||||
|
/* Set byte code */
|
||||||
|
//if(buzzvm_set_bcode(VM, BO_BUF, bcode_size) != BUZZVM_STATE_READY) {
|
||||||
|
// buzzvm_destroy(&VM);
|
||||||
|
// buzzdebug_destroy(&DBG_INFO);
|
||||||
|
// fprintf(stdout, "%s: Error loading Buzz script\n\n", bo_filename);
|
||||||
|
// return 0;
|
||||||
|
//}
|
||||||
|
/* Register hook functions */
|
||||||
|
/*if(buzz_register_hooks() != BUZZVM_STATE_READY) {
|
||||||
|
buzzvm_destroy(&VM);
|
||||||
|
buzzdebug_destroy(&DBG_INFO);
|
||||||
|
fprintf(stdout, "%s: Error registering hooks\n\n", bo_filename);
|
||||||
|
return 0;
|
||||||
|
}*/
|
||||||
|
/* Save bytecode file name */
|
||||||
|
//BO_FNAME = strdup(bo_filename);
|
||||||
|
/* Execute the global part of the script */
|
||||||
|
//buzzvm_execute_script(VM);
|
||||||
|
/* Call the Init() function */
|
||||||
|
//buzzvm_function_call(VM, "init", 0);
|
||||||
|
/* All OK */
|
||||||
|
return buzz_update_set(BO_BUF, bdbg_filename, bcode_size);
|
||||||
|
}
|
||||||
|
|
||||||
|
/****************************************/
|
||||||
|
/*Sets a new update */
|
||||||
|
/****************************************/
|
||||||
|
int buzz_update_set(uint8_t* UP_BO_BUF, const char* bdbg_filename,size_t bcode_size){
|
||||||
|
/* Get hostname */
|
||||||
|
char hstnm[30];
|
||||||
|
gethostname(hstnm, 30);
|
||||||
|
/* Make numeric id from hostname */
|
||||||
|
/* NOTE: here we assume that the hostname is in the format Knn */
|
||||||
|
int id = strtol(hstnm + 1, NULL, 10);
|
||||||
|
/* Reset the Buzz VM */
|
||||||
|
if(VM) buzzvm_destroy(&VM);
|
||||||
|
VM = buzzvm_new(id);
|
||||||
|
/* Get rid of debug info */
|
||||||
|
if(DBG_INFO) buzzdebug_destroy(&DBG_INFO);
|
||||||
|
DBG_INFO = buzzdebug_new();
|
||||||
|
|
||||||
/* Read debug information */
|
/* Read debug information */
|
||||||
if(!buzzdebug_fromfile(DBG_INFO, bdbg_filename)) {
|
if(!buzzdebug_fromfile(DBG_INFO, bdbg_filename)) {
|
||||||
buzzvm_destroy(&VM);
|
buzzvm_destroy(&VM);
|
||||||
buzzdebug_destroy(&DBG_INFO);
|
buzzdebug_destroy(&DBG_INFO);
|
||||||
perror(bdbg_filename);
|
perror(bdbg_filename);
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
/* Set byte code */
|
/* Set byte code */
|
||||||
if(buzzvm_set_bcode(VM, BO_BUF, bcode_size) != BUZZVM_STATE_READY) {
|
if(buzzvm_set_bcode(VM, UP_BO_BUF, bcode_size) != BUZZVM_STATE_READY) {
|
||||||
buzzvm_destroy(&VM);
|
buzzvm_destroy(&VM);
|
||||||
buzzdebug_destroy(&DBG_INFO);
|
buzzdebug_destroy(&DBG_INFO);
|
||||||
fprintf(stdout, "%s: Error loading Buzz script\n\n", bo_filename);
|
fprintf(stdout, "%s: Error loading Buzz script\n\n", BO_FNAME);
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
/* Register hook functions */
|
/* Register hook functions */
|
||||||
if(buzz_register_hooks() != BUZZVM_STATE_READY) {
|
if(buzz_register_hooks() != BUZZVM_STATE_READY) {
|
||||||
buzzvm_destroy(&VM);
|
buzzvm_destroy(&VM);
|
||||||
buzzdebug_destroy(&DBG_INFO);
|
buzzdebug_destroy(&DBG_INFO);
|
||||||
fprintf(stdout, "%s: Error registering hooks\n\n", bo_filename);
|
fprintf(stdout, "%s: Error registering hooks\n\n", BO_FNAME);
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
/* Save bytecode file name */
|
|
||||||
BO_FNAME = strdup(bo_filename);
|
/* Execute the global part of the script */
|
||||||
|
buzzvm_execute_script(VM);
|
||||||
|
/* Call the Init() function */
|
||||||
|
buzzvm_function_call(VM, "init", 0);
|
||||||
|
/* All OK */
|
||||||
|
return 1;
|
||||||
|
}
|
||||||
|
|
||||||
|
/****************************************/
|
||||||
|
/*Performs a initialization test */
|
||||||
|
/****************************************/
|
||||||
|
int buzz_update_init_test(uint8_t* UP_BO_BUF, const char* bdbg_filename,size_t bcode_size){
|
||||||
|
/* Get hostname */
|
||||||
|
char hstnm[30];
|
||||||
|
gethostname(hstnm, 30);
|
||||||
|
/* Make numeric id from hostname */
|
||||||
|
/* NOTE: here we assume that the hostname is in the format Knn */
|
||||||
|
int id = strtol(hstnm + 1, NULL, 10);
|
||||||
|
/* Reset the Buzz VM */
|
||||||
|
if(VM) buzzvm_destroy(&VM);
|
||||||
|
VM = buzzvm_new(id);
|
||||||
|
/* Get rid of debug info */
|
||||||
|
if(DBG_INFO) buzzdebug_destroy(&DBG_INFO);
|
||||||
|
DBG_INFO = buzzdebug_new();
|
||||||
|
|
||||||
|
/* Read debug information */
|
||||||
|
if(!buzzdebug_fromfile(DBG_INFO, bdbg_filename)) {
|
||||||
|
buzzvm_destroy(&VM);
|
||||||
|
buzzdebug_destroy(&DBG_INFO);
|
||||||
|
perror(bdbg_filename);
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
/* Set byte code */
|
||||||
|
if(buzzvm_set_bcode(VM, UP_BO_BUF, bcode_size) != BUZZVM_STATE_READY) {
|
||||||
|
buzzvm_destroy(&VM);
|
||||||
|
buzzdebug_destroy(&DBG_INFO);
|
||||||
|
fprintf(stdout, "%s: Error loading Buzz script\n\n", BO_FNAME);
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
/* Register hook functions */
|
||||||
|
if(testing_buzz_register_hooks() != BUZZVM_STATE_READY) {
|
||||||
|
buzzvm_destroy(&VM);
|
||||||
|
buzzdebug_destroy(&DBG_INFO);
|
||||||
|
fprintf(stdout, "%s: Error registering hooks\n\n", BO_FNAME);
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
/* Execute the global part of the script */
|
/* Execute the global part of the script */
|
||||||
buzzvm_execute_script(VM);
|
buzzvm_execute_script(VM);
|
||||||
/* Call the Init() function */
|
/* Call the Init() function */
|
||||||
|
@ -266,72 +427,72 @@ namespace buzz_utility{
|
||||||
/*Swarm struct*/
|
/*Swarm struct*/
|
||||||
/****************************************/
|
/****************************************/
|
||||||
|
|
||||||
struct buzzswarm_elem_s {
|
struct buzzswarm_elem_s {
|
||||||
buzzdarray_t swarms;
|
buzzdarray_t swarms;
|
||||||
uint16_t age;
|
uint16_t age;
|
||||||
};
|
};
|
||||||
typedef struct buzzswarm_elem_s* buzzswarm_elem_t;
|
typedef struct buzzswarm_elem_s* buzzswarm_elem_t;
|
||||||
|
|
||||||
void check_swarm_members(const void* key, void* data, void* params) {
|
void check_swarm_members(const void* key, void* data, void* params) {
|
||||||
buzzswarm_elem_t e = *(buzzswarm_elem_t*)data;
|
buzzswarm_elem_t e = *(buzzswarm_elem_t*)data;
|
||||||
int* status = (int*)params;
|
int* status = (int*)params;
|
||||||
if(*status == 3) return;
|
if(*status == 3) return;
|
||||||
if(buzzdarray_size(e->swarms) != 1) {
|
if(buzzdarray_size(e->swarms) != 1) {
|
||||||
fprintf(stderr, "Swarm list size is not 1\n");
|
fprintf(stderr, "Swarm list size is not 1\n");
|
||||||
*status = 3;
|
*status = 3;
|
||||||
}
|
}
|
||||||
else {
|
else {
|
||||||
int sid = 1;
|
int sid = 1;
|
||||||
if(*buzzdict_get(VM->swarms, &sid, uint8_t) &&
|
if(*buzzdict_get(VM->swarms, &sid, uint8_t) &&
|
||||||
buzzdarray_get(e->swarms, 0, uint16_t) != sid) {
|
buzzdarray_get(e->swarms, 0, uint16_t) != sid) {
|
||||||
fprintf(stderr, "I am in swarm #%d and neighbor is in %d\n",
|
fprintf(stderr, "I am in swarm #%d and neighbor is in %d\n",
|
||||||
sid,
|
sid,
|
||||||
buzzdarray_get(e->swarms, 0, uint16_t));
|
buzzdarray_get(e->swarms, 0, uint16_t));
|
||||||
*status = 3;
|
*status = 3;
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
sid = 2;
|
sid = 2;
|
||||||
if(*buzzdict_get(VM->swarms, &sid, uint8_t) &&
|
if(*buzzdict_get(VM->swarms, &sid, uint8_t) &&
|
||||||
buzzdarray_get(e->swarms, 0, uint16_t) != sid) {
|
buzzdarray_get(e->swarms, 0, uint16_t) != sid) {
|
||||||
fprintf(stderr, "I am in swarm #%d and neighbor is in %d\n",
|
fprintf(stderr, "I am in swarm #%d and neighbor is in %d\n",
|
||||||
sid,
|
sid,
|
||||||
buzzdarray_get(e->swarms, 0, uint16_t));
|
buzzdarray_get(e->swarms, 0, uint16_t));
|
||||||
*status = 3;
|
*status = 3;
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
/*Step through the buzz script*/
|
/*Step through the buzz script*/
|
||||||
|
|
||||||
void buzz_script_step() {
|
void buzz_script_step() {
|
||||||
/*
|
/*
|
||||||
* Update sensors
|
* Update sensors
|
||||||
*/
|
*/
|
||||||
buzzuav_closures::buzzuav_update_battery(VM);
|
buzzuav_closures::buzzuav_update_battery(VM);
|
||||||
buzzuav_closures::buzzuav_update_prox(VM);
|
buzzuav_closures::buzzuav_update_prox(VM);
|
||||||
buzzuav_closures::buzzuav_update_currentpos(VM);
|
buzzuav_closures::buzzuav_update_currentpos(VM);
|
||||||
buzzuav_closures::buzzuav_update_flight_status(VM);
|
buzzuav_closures::buzzuav_update_flight_status(VM);
|
||||||
/*
|
/*
|
||||||
* Call Buzz step() function
|
* Call Buzz step() function
|
||||||
*/
|
*/
|
||||||
if(buzzvm_function_call(VM, "step", 0) != BUZZVM_STATE_READY) {
|
if(buzzvm_function_call(VM, "step", 0) != BUZZVM_STATE_READY) {
|
||||||
fprintf(stderr, "%s: execution terminated abnormally: %s\n\n",
|
fprintf(stderr, "%s: execution terminated abnormally: %s\n\n",
|
||||||
BO_FNAME,
|
BO_FNAME,
|
||||||
buzz_error_info());
|
buzz_error_info());
|
||||||
buzzvm_dump(VM);
|
buzzvm_dump(VM);
|
||||||
}
|
}
|
||||||
/* Print swarm */
|
/* Print swarm */
|
||||||
//buzzswarm_members_print(stdout, VM->swarmmembers, VM->robot);
|
//buzzswarm_members_print(stdout, VM->swarmmembers, VM->robot);
|
||||||
/* Check swarm state */
|
/* Check swarm state */
|
||||||
/* int status = 1;
|
/* int status = 1;
|
||||||
buzzdict_foreach(VM->swarmmembers, check_swarm_members, &status);
|
buzzdict_foreach(VM->swarmmembers, check_swarm_members, &status);
|
||||||
if(status == 1 &&
|
if(status == 1 &&
|
||||||
buzzdict_size(VM->swarmmembers) < 9)
|
buzzdict_size(VM->swarmmembers) < 9)
|
||||||
status = 2;
|
status = 2;
|
||||||
buzzvm_pushs(VM, buzzvm_string_register(VM, "swarm_status", 1));
|
buzzvm_pushs(VM, buzzvm_string_register(VM, "swarm_status", 1));
|
||||||
buzzvm_pushi(VM, status);
|
buzzvm_pushi(VM, status);
|
||||||
buzzvm_gstore(VM);*/
|
buzzvm_gstore(VM);*/
|
||||||
}
|
}
|
||||||
|
|
||||||
/****************************************/
|
/****************************************/
|
||||||
/*Destroy the bvm and other resorces*/
|
/*Destroy the bvm and other resorces*/
|
||||||
|
@ -365,6 +526,33 @@ int buzz_script_done() {
|
||||||
return VM->state != BUZZVM_STATE_READY;
|
return VM->state != BUZZVM_STATE_READY;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
int update_step_test(){
|
||||||
|
|
||||||
|
buzzuav_closures::buzzuav_update_battery(VM);
|
||||||
|
buzzuav_closures::buzzuav_update_prox(VM);
|
||||||
|
buzzuav_closures::buzzuav_update_currentpos(VM);
|
||||||
|
buzzuav_closures::buzzuav_update_flight_status(VM);
|
||||||
|
|
||||||
|
int a = buzzvm_function_call(VM, "step", 0);
|
||||||
|
if(a != BUZZVM_STATE_READY){
|
||||||
|
fprintf(stdout, "step test VM state %i\n",a);
|
||||||
|
fprintf(stdout, " execution terminated abnormally: %s\n\n",
|
||||||
|
buzz_error_info());
|
||||||
|
}
|
||||||
|
return a == BUZZVM_STATE_READY;
|
||||||
|
}
|
||||||
|
|
||||||
|
uint16_t get_robotid(){
|
||||||
|
/* Get hostname */
|
||||||
|
char hstnm[30];
|
||||||
|
gethostname(hstnm, 30);
|
||||||
|
/* Make numeric id from hostname */
|
||||||
|
/* NOTE: here we assume that the hostname is in the format Knn */
|
||||||
|
int id = strtol(hstnm + 1, NULL, 10);
|
||||||
|
return (uint16_t)id;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
|
@ -263,7 +263,7 @@ int buzzuav_update_prox(buzzvm_t vm) {
|
||||||
/****************************************/
|
/****************************************/
|
||||||
/****************************************/
|
/****************************************/
|
||||||
|
|
||||||
|
int dummy_closure(buzzvm_t vm){ return buzzvm_ret0(vm);}
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -14,6 +14,8 @@ namespace rosbzz_node{
|
||||||
Initialize_pub_sub(n_c);
|
Initialize_pub_sub(n_c);
|
||||||
/*Compile the .bzz file to .basm, .bo and .bdbg*/
|
/*Compile the .bzz file to .basm, .bo and .bdbg*/
|
||||||
Compile_bzz();
|
Compile_bzz();
|
||||||
|
set_bzz_file(bzzfile_name.c_str());
|
||||||
|
init_update_monitor(bcfname.c_str(),barrier);
|
||||||
}
|
}
|
||||||
|
|
||||||
/***Destructor***/
|
/***Destructor***/
|
||||||
|
@ -21,7 +23,7 @@ namespace rosbzz_node{
|
||||||
{
|
{
|
||||||
/* All done */
|
/* All done */
|
||||||
/* Cleanup */
|
/* Cleanup */
|
||||||
buzz_utility::buzz_script_destroy();
|
//buzz_utility::buzz_script_destroy();
|
||||||
/* Stop the robot */
|
/* Stop the robot */
|
||||||
uav_done();
|
uav_done();
|
||||||
}
|
}
|
||||||
|
@ -35,6 +37,8 @@ namespace rosbzz_node{
|
||||||
{
|
{
|
||||||
/*Update neighbors position inside Buzz*/
|
/*Update neighbors position inside Buzz*/
|
||||||
buzz_utility::neighbour_pos_callback(neighbours_pos_map);
|
buzz_utility::neighbour_pos_callback(neighbours_pos_map);
|
||||||
|
/*Check updater state and step code*/
|
||||||
|
if(update_routine(bcfname.c_str(), dbgfname.c_str(),0)==CODE_RUNNING)
|
||||||
/*Step buzz script */
|
/*Step buzz script */
|
||||||
buzz_utility::buzz_script_step();
|
buzz_utility::buzz_script_step();
|
||||||
/*Prepare messages and publish them in respective topic*/
|
/*Prepare messages and publish them in respective topic*/
|
||||||
|
@ -49,6 +53,8 @@ namespace rosbzz_node{
|
||||||
maintain_pos(timer_step);
|
maintain_pos(timer_step);
|
||||||
|
|
||||||
}
|
}
|
||||||
|
/* Destroy updater and Cleanup */
|
||||||
|
update_routine(bcfname.c_str(), dbgfname.c_str(),1);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -79,7 +85,8 @@ namespace rosbzz_node{
|
||||||
n_c.getParam("out_payload", out_payload);
|
n_c.getParam("out_payload", out_payload);
|
||||||
/*Obtain in payload name*/
|
/*Obtain in payload name*/
|
||||||
n_c.getParam("in_payload", in_payload);
|
n_c.getParam("in_payload", in_payload);
|
||||||
|
/*Obtain Number of robots for barrier*/
|
||||||
|
n_c.getParam("No_of_Robots", barrier);
|
||||||
|
|
||||||
|
|
||||||
}
|
}
|
||||||
|
@ -121,7 +128,14 @@ namespace rosbzz_node{
|
||||||
dbgfname = bzzfile_in_compile.str();
|
dbgfname = bzzfile_in_compile.str();
|
||||||
|
|
||||||
}
|
}
|
||||||
|
/*Prepare messages for each step and publish*/
|
||||||
|
/*******************************************************************************************************/
|
||||||
|
/* Message format of payload (Each slot is uint64_t) */
|
||||||
|
/* _________________________________________________________________________________________________ */
|
||||||
|
/*| | | | | | */
|
||||||
|
/*|Pos x|Pos y|Pos z|Size in Uint64_t|robot_id|Buzz_msg_size|Buzz_msg|Buzz_msgs with size......... | */
|
||||||
|
/*|_____|_____|_____|________________________________________________|______________________________| */
|
||||||
|
/*******************************************************************************************************/
|
||||||
void roscontroller::prepare_msg_and_publish(){
|
void roscontroller::prepare_msg_and_publish(){
|
||||||
|
|
||||||
/* flight controller client call if requested from Buzz*/
|
/* flight controller client call if requested from Buzz*/
|
||||||
|
@ -161,7 +175,6 @@ namespace rosbzz_node{
|
||||||
for(std::vector<long unsigned int>::const_iterator it = payload_out.payload64.begin();
|
for(std::vector<long unsigned int>::const_iterator it = payload_out.payload64.begin();
|
||||||
it != payload_out.payload64.end(); ++it){
|
it != payload_out.payload64.end(); ++it){
|
||||||
message_obt[i] =(uint64_t) *it;
|
message_obt[i] =(uint64_t) *it;
|
||||||
|
|
||||||
i++;
|
i++;
|
||||||
}
|
}
|
||||||
for(i=0;i<payload_out.payload64.size();i++){
|
for(i=0;i<payload_out.payload64.size();i++){
|
||||||
|
@ -237,6 +250,15 @@ namespace rosbzz_node{
|
||||||
}
|
}
|
||||||
|
|
||||||
/*payload callback callback*/
|
/*payload callback callback*/
|
||||||
|
|
||||||
|
/*******************************************************************************************************/
|
||||||
|
/* Message format of payload (Each slot is uint64_t) */
|
||||||
|
/* _________________________________________________________________________________________________ */
|
||||||
|
/*| | | | | | */
|
||||||
|
/*|Pos x|Pos y|Pos z|Size in Uint64_t|robot_id|Buzz_msg_size|Buzz_msg|Buzz_msgs with size......... | */
|
||||||
|
/*|_____|_____|_____|________________________________________________|______________________________| */
|
||||||
|
/*******************************************************************************************************/
|
||||||
|
|
||||||
void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg){
|
void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg){
|
||||||
uint64_t message_obt[msg->payload64.size()];
|
uint64_t message_obt[msg->payload64.size()];
|
||||||
int i = 0;
|
int i = 0;
|
||||||
|
|
Loading…
Reference in New Issue