Spiri
/
ROSBuzz_MISTLab
13
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

update back and working with diff version upto IEEE Software

This commit is contained in:
vivek-shankar 2017-12-07 21:49:27 -05:00
parent 6018b681d7
commit 7e99692178
4 changed files with 379 additions and 230 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

37
include/buzz_update.h
View File

@ -1,5 +1,8 @@
#ifndef BUZZ_UPDATE_H #ifndef BUZZ_UPDATE_H
#define BUZZ_UPDATE_H #define BUZZ_UPDATE_H
/*Simulation or robot check*/
#define SIMULATION 1
#include <stdlib.h> #include <stdlib.h>
#include <stdio.h> #include <stdio.h>
#include <buzz/buzztype.h> #include <buzz/buzztype.h>
@ -9,8 +12,10 @@
#include <fstream> #include <fstream>
#define delete_p(p) do { free(p); p = NULL; } while(0) #define delete_p(p) do { free(p); p = NULL; } while(0)
static const uint16_t CODE_REQUEST_PADDING=250;
static const uint16_t MIN_UPDATE_PACKET=251;
static const uint16_t UPDATE_CODE_HEADER_SIZE=5;
static const uint16_t TIMEOUT_FOR_ROLLBACK=50;
/*********************/ /*********************/
/* Updater states */ /* Updater states */
/********************/ /********************/
@ -25,8 +30,8 @@ typedef enum {
/********************/ /********************/
typedef enum { typedef enum {
SEND_CODE = 0, // Broadcast code with state SENT_CODE = 0, // Broadcast code
STATE_MSG, // Broadcast state RESEND_CODE, // ReBroadcast request
} code_message_e; } code_message_e;
/*************************/ /*************************/
@ -39,6 +44,12 @@ struct updater_msgqueue_s {
} ; } ;
typedef struct updater_msgqueue_s* updater_msgqueue_t; typedef struct updater_msgqueue_s* updater_msgqueue_t;
struct updater_code_s {
uint8_t* bcode;
uint8_t* bcode_size;
} ;
typedef struct updater_code_s* updater_code_t;
/**************************/ /**************************/
/*Updater data*/ /*Updater data*/
/**************************/ /**************************/
@ -48,8 +59,14 @@ struct buzz_updater_elem_s {
//uint16_t robotid; //uint16_t robotid;
/*current Bytecode content */ /*current Bytecode content */
uint8_t* bcode; uint8_t* bcode;
/*old Bytecode name */
const char* old_bcode;
/*current bcode size*/ /*current bcode size*/
size_t* bcode_size; size_t* bcode_size;
/*Update patch*/
uint8_t* patch;
/* Update patch size*/
size_t* patch_size;
/*current Bytecode content */ /*current Bytecode content */
uint8_t* standby_bcode; uint8_t* standby_bcode;
/*current bcode size*/ /*current bcode size*/
@ -61,19 +78,19 @@ struct buzz_updater_elem_s {
/*Current state of the updater one in code_states_e ENUM*/ /*Current state of the updater one in code_states_e ENUM*/
int* mode; int* mode;
uint8_t* update_no; uint8_t* update_no;
} ; };
typedef struct buzz_updater_elem_s* buzz_updater_elem_t; typedef struct buzz_updater_elem_s* buzz_updater_elem_t;
/**************************************************************************/ /**************************************************************************/
/*Updater routine from msg processing to file checks to be called from main*/ /*Updater routine from msg processing to file checks to be called from main*/
/**************************************************************************/ /**************************************************************************/
void update_routine(const char* bcfname, void update_routine();
const char* dbgfname);
/************************************************/ /************************************************/
/*Initalizes the updater */ /*Initalizes the updater */
/************************************************/ /************************************************/
void init_update_monitor(const char* bo_filename,const char* stand_by_script); void init_update_monitor(const char* bo_filename,const char* stand_by_script,
const char* dbgfname, int robot_id);
/*********************************************************/ /*********************************************************/
@ -129,9 +146,11 @@ int get_update_status();
void set_read_update_status(); void set_read_update_status();
int compile_bzz(); int compile_bzz(std::string bzz_file);
void updates_set_robots(int robots); void updates_set_robots(int robots);
void set_packet_id(int packet_id);
void collect_data(std::ofstream &logger); void collect_data(std::ofstream &logger);
#endif #endif

398
src/buzz_update.cpp
View File

@ -21,21 +21,40 @@ static int fd,wd =0;
static int old_update =0; static int old_update =0;
static buzz_updater_elem_t updater; static buzz_updater_elem_t updater;
static int no_of_robot; static int no_of_robot;
static char* dbgf_name; static const char* dbgf_name;
static const char* bcfname;
static const char* old_bcfname="old_bcode.bo";
static const char* bzz_file; static const char* bzz_file;
static int Robot_id=0;
static int neigh=-1; static int neigh=-1;
static int updater_msg_ready ; static int updater_msg_ready ;
static uint16_t update_try_counter=0;
static int updated=0; static int updated=0;
static const uint16_t MAX_UPDATE_TRY=5;
static int packet_id_=0;
static size_t old_byte_code_size=0;
/*Initialize updater*/ /*Initialize updater*/
void init_update_monitor(const char* bo_filename, const char* stand_by_script){ void init_update_monitor(const char* bo_filename, const char* stand_by_script,
const char* dbgfname, int robot_id){
Robot_id=robot_id;
dbgf_name=dbgfname;
bcfname=bo_filename;
ROS_INFO("Initializing file monitor..."); ROS_INFO("Initializing file monitor...");
fd=inotify_init1(IN_NONBLOCK); fd=inotify_init1(IN_NONBLOCK);
if ( fd < 0 ) { if ( fd < 0 ) {
perror( "inotify_init error" ); perror( "inotify_init error" );
} }
/* watch /.bzz file for any activity and report it back to update */ /*If simulation set the file monitor only for robot 1*/
wd=inotify_add_watch(fd, bzz_file,IN_ALL_EVENTS ); if(SIMULATION==1 && robot_id==0){
/* watch /.bzz file for any activity and report it back to update */
wd=inotify_add_watch(fd, bzz_file,IN_ALL_EVENTS );
}
else if (SIMULATION==0){
/* watch /.bzz file for any activity and report it back to update */
wd=inotify_add_watch(fd, bzz_file,IN_ALL_EVENTS );
}
/*load the .bo under execution into the updater*/ /*load the .bo under execution into the updater*/
uint8_t* BO_BUF = 0; uint8_t* BO_BUF = 0;
FILE* fp = fopen(bo_filename, "rb"); FILE* fp = fopen(bo_filename, "rb");
@ -73,8 +92,11 @@ void init_update_monitor(const char* bo_filename, const char* stand_by_script){
updater = (buzz_updater_elem_t)malloc(sizeof(struct buzz_updater_elem_s)); updater = (buzz_updater_elem_t)malloc(sizeof(struct buzz_updater_elem_s));
/*Intialize the updater with the required data*/ /*Intialize the updater with the required data*/
updater->bcode = BO_BUF; updater->bcode = BO_BUF;
/*Store a copy of the Bcode for rollback*/
updater->outmsg_queue = NULL; updater->outmsg_queue = NULL;
updater->inmsg_queue = NULL; updater->inmsg_queue = NULL;
updater->patch = NULL;
updater->patch_size = (size_t*) malloc(sizeof(size_t));
updater->bcode_size = (size_t*) malloc(sizeof(size_t)); updater->bcode_size = (size_t*) malloc(sizeof(size_t));
updater->update_no = (uint8_t*) malloc(sizeof(uint16_t)); updater->update_no = (uint8_t*) malloc(sizeof(uint16_t));
*(uint16_t*)(updater->update_no) =0; *(uint16_t*)(updater->update_no) =0;
@ -86,10 +108,11 @@ void init_update_monitor(const char* bo_filename, const char* stand_by_script){
*(int*)(updater->mode)=CODE_RUNNING; *(int*)(updater->mode)=CODE_RUNNING;
//no_of_robot=barrier; //no_of_robot=barrier;
updater_msg_ready=0; updater_msg_ready=0;
//neigh = 0;
//updater->outmsg_queue= /*Write the bcode to a file for rollback*/
// update_table->barrier=nvs; fp=fopen("old_bcode.bo", "wb");
// open logger fwrite((updater->bcode), *(size_t*)updater->bcode_size, 1, fp);
fclose(fp);
} }
/*Check for .bzz file chages*/ /*Check for .bzz file chages*/
@ -123,27 +146,131 @@ int check_update(){
return check; return check;
} }
int test_patch(std::string path, std::string name1,size_t update_patch_size, uint8_t* patch){
if(SIMULATION==1){
return 1;
}
else {
/*Patch the old bo with new patch*/
std::stringstream patch_writefile;
patch_writefile<< path<<name1<<"tmp_patch.bo";
/*Write the patch to a file*/
FILE *fp=fopen(patch_writefile.str().c_str(), "wb");
fwrite(patch, update_patch_size, 1, fp);
fclose(fp);
std::stringstream patch_exsisting;
patch_exsisting<< "bspatch "<< path << name1<<".bo "<< path<<name1 << "-patched.bo "<< path<<name1<<"tmp_patch.bo";
fprintf(stdout,"Launching bspatch command: %s \n", patch_exsisting.str().c_str());
if(system(patch_exsisting.str().c_str()) ) return 0;
else return 1;
}
}
updater_code_t obtain_patched_bo(std::string path, std::string name1){
if(SIMULATION==1){
/*Read the exsisting file to simulate the patching*/
std::stringstream read_patched;
read_patched<<path<<name1<<".bo";
fprintf(stdout,"read file name %s \n", read_patched.str().c_str());
uint8_t* patched_BO_Buf = 0;
FILE *fp = fopen(read_patched.str().c_str(), "rb");
if(!fp) {
perror(read_patched.str().c_str());
}
fseek(fp, 0, SEEK_END);
size_t patched_size = ftell(fp);
rewind(fp);
patched_BO_Buf = (uint8_t*)malloc(patched_size);
if(fread(patched_BO_Buf, 1, patched_size, fp) < patched_size) {
perror(read_patched.str().c_str());
fclose(fp);
}
fclose(fp);
/*Write the patched to a code struct and return*/
updater_code_t update_code = (updater_code_t)malloc(sizeof(struct updater_code_s));
update_code->bcode = patched_BO_Buf;
update_code->bcode_size = (uint8_t*) malloc(sizeof(uint16_t));
*(uint16_t*) (update_code->bcode_size) = patched_size;
return update_code;
}
else{
/*Read the new patched file*/
std::stringstream read_patched;
read_patched<<path<<name1<<"-patched.bo";
fprintf(stdout,"read file name %s \n", read_patched.str().c_str());
uint8_t* patched_BO_Buf = 0;
FILE *fp = fopen(read_patched.str().c_str(), "rb");
if(!fp) {
perror(read_patched.str().c_str());
}
fseek(fp, 0, SEEK_END);
size_t patched_size = ftell(fp);
rewind(fp);
patched_BO_Buf = (uint8_t*)malloc(patched_size);
if(fread(patched_BO_Buf, 1, patched_size, fp) < patched_size) {
perror(read_patched.str().c_str());
fclose(fp);
}
fclose(fp);
/* delete old bo file & rename new bo file */
remove((path + name1 + ".bo").c_str());
rename((path + name1 + "-patched.bo").c_str(), (path + name1 + ".bo").c_str());
/*Write the patched to a code struct and return*/
updater_code_t update_code = (updater_code_t)malloc(sizeof(struct updater_code_s));
update_code->bcode = patched_BO_Buf;
update_code->bcode_size = (uint8_t*) malloc(sizeof(uint16_t));
*(uint16_t*) (update_code->bcode_size) = patched_size;
return update_code;
}
}
void code_message_outqueue_append(){ void code_message_outqueue_append(){
updater->outmsg_queue=(updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s)); updater->outmsg_queue=(updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s));
uint16_t size =0; /* if size less than 250 Pad with zeors to assure transmission*/
updater->outmsg_queue->queue = (uint8_t*)malloc(2*sizeof(uint16_t)+ *(size_t*)(updater->bcode_size)); uint16_t size = UPDATE_CODE_HEADER_SIZE + *(size_t*)(updater->patch_size);
updater->outmsg_queue->size = (uint8_t*)malloc(sizeof(uint16_t)); uint16_t padding_size= ( size > MIN_UPDATE_PACKET) ? 0 : MIN_UPDATE_PACKET-size;
/*append the update no, code size and code to out msg*/ size +=padding_size;
updater->outmsg_queue->queue = (uint8_t*)malloc(size);
memset(updater->outmsg_queue->queue, 0, size);
updater->outmsg_queue->size = (uint8_t*)malloc(sizeof(uint16_t));
*(uint16_t*)(updater->outmsg_queue->size)=size;
size=0;
/*Append message type*/
*(uint8_t*)(updater->outmsg_queue->queue+size) = SENT_CODE;
size+=sizeof(uint8_t);
/*Append the update no, code size and code to out msg*/
*(uint16_t*)(updater->outmsg_queue->queue+size) = *(uint16_t*) (updater->update_no); *(uint16_t*)(updater->outmsg_queue->queue+size) = *(uint16_t*) (updater->update_no);
size+=sizeof(uint16_t); size+=sizeof(uint16_t);
*(uint16_t*)(updater->outmsg_queue->queue+size) = *(size_t*) (updater->bcode_size); *(uint16_t*)(updater->outmsg_queue->queue+size) =(uint16_t) *(size_t*) (updater->patch_size);
size+=sizeof(uint16_t); size+=sizeof(uint16_t);
memcpy(updater->outmsg_queue->queue+size, updater->bcode, *(size_t*)(updater->bcode_size)); memcpy(updater->outmsg_queue->queue+size, updater->patch, *(size_t*)(updater->patch_size));
size+=(uint16_t)*(size_t*)(updater->bcode_size); size+=(uint16_t)*(size_t*)(updater->patch_size);
/*FILE *fp;
fp=fopen("update.bo", "wb");
fwrite((updater->bcode), updater->bcode_size, 1, fp);
fclose(fp);*/
updater_msg_ready=1; updater_msg_ready=1;
*(uint16_t*)(updater->outmsg_queue->size)=size; }
//fprintf(stdout,"out msg append transfer code size %d\n", (int)*(size_t*) updater->bcode_size); void outqueue_append_code_request(uint16_t update_no){
updater->outmsg_queue=(updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s));
uint16_t size =0;
updater->outmsg_queue->queue = (uint8_t*)malloc(2*sizeof(uint16_t) + sizeof(uint8_t) +CODE_REQUEST_PADDING);
updater->outmsg_queue->size = (uint8_t*)malloc(sizeof(uint16_t));
memset(updater->outmsg_queue->queue, 0, sizeof(uint16_t) + sizeof(uint8_t) +CODE_REQUEST_PADDING);
/*Append message type*/
*(uint8_t*)(updater->outmsg_queue->queue+size) = RESEND_CODE;
size+=sizeof(uint8_t);
/*Append the update no, code size and code to out msg*/
*(uint16_t*)(updater->outmsg_queue->queue+size) = update_no;
size+=sizeof(uint16_t);
*(uint16_t*)(updater->outmsg_queue->queue+size) = update_try_counter;
size+=sizeof(uint16_t);
*(uint16_t*)(updater->outmsg_queue->size)=size+CODE_REQUEST_PADDING;
updater_msg_ready=1;
ROS_INFO("[Debug] Requested update no. %u with try: %u \n",update_no,update_try_counter);
} }
void code_message_inqueue_append(uint8_t* msg,uint16_t size){ void code_message_inqueue_append(uint8_t* msg,uint16_t size){
@ -155,43 +282,158 @@ void code_message_inqueue_append(uint8_t* msg,uint16_t size){
*(uint16_t*)(updater->inmsg_queue->size) = size; *(uint16_t*)(updater->inmsg_queue->size) = size;
} }
void set_packet_id(int packet_id){
/*Used for data logging*/
packet_id_=packet_id;
}
void code_message_inqueue_process(){ void code_message_inqueue_process(){
int size=0; int size=0;
updater_code_t out_code=NULL;
ROS_INFO("[Debug] Updater processing in msg with mode %d \n", *(int*)(updater->mode) ); ROS_INFO("[Debug] Updater processing in msg with mode %d \n", *(int*)(updater->mode) );
ROS_INFO("[Debug] %u : Current update number, %u : Received update no \n",( *(uint16_t*) (updater->update_no) ), (*(uint16_t*)(updater->inmsg_queue->queue)) ); ROS_INFO("[Debug] %u : Current update number, %u : Received update no \n",( *(uint16_t*) (updater->update_no) ), (*(uint16_t*)(updater->inmsg_queue->queue+sizeof(uint8_t))) );
ROS_INFO("[Debug] Updater received code of size %u \n",(*(uint16_t*)(updater->inmsg_queue->queue+sizeof(uint16_t)) ) ); ROS_INFO("[Debug] Updater received patch of size %u \n",(*(uint16_t*)(updater->inmsg_queue->queue+sizeof(uint16_t)+sizeof(uint8_t)) ) );
if( *(int*) (updater->mode) == CODE_RUNNING){ if( *(int*) (updater->mode) == CODE_RUNNING){
//fprintf(stdout,"[debug]Inside inmsg code running"); //fprintf(stdout,"[debug]Inside inmsg code running");
if( *(uint16_t*)(updater->inmsg_queue->queue) > *(uint16_t*) (updater->update_no) ){ if(*(uint8_t*)(updater->inmsg_queue->queue) == SENT_CODE){
//fprintf(stdout,"[debug]Inside update number comparision"); size+=sizeof(uint8_t);
uint16_t update_no=*(uint16_t*)(updater->inmsg_queue->queue); if( *(uint16_t*)(updater->inmsg_queue->queue+size) > *(uint16_t*) (updater->update_no) ){
size +=sizeof(uint16_t); //fprintf(stdout,"[debug]Inside update number comparision");
uint16_t update_bcode_size =*(uint16_t*)(updater->inmsg_queue->queue+size); uint16_t update_no=*(uint16_t*)(updater->inmsg_queue->queue+size);
size +=sizeof(uint16_t); size +=sizeof(uint16_t);
//fprintf(stdout,"in queue process Update no %d\n", (int) update_no); uint16_t update_patch_size =*(uint16_t*)(updater->inmsg_queue->queue+size);
//fprintf(stdout,"in queue process bcode size %d\n", (int) update_bcode_size); size +=sizeof(uint16_t);
//FILE *fp; /*Generate patch*/
//fp=fopen("update.bo", "wb"); std::string bzzfile_name(bzz_file);
//fwrite((updater->inmsg_queue->queue+size), update_bcode_size, 1, fp); std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/";
//fclose(fp); std::string name1 = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1);
if( test_set_code((uint8_t*)(updater->inmsg_queue->queue+size), name1 = name1.substr(0,name1.find_last_of("."));
(char*) dbgf_name,(size_t)update_bcode_size) ) { if(test_patch(path, name1,update_patch_size,(updater->inmsg_queue->queue+size)) ){
*(uint16_t*)(updater->update_no)=update_no; out_code = obtain_patched_bo(path, name1);
neigh=1;
//gettimeofday(&t1, NULL); //fprintf(stdout,"in queue process Update no %d\n", (int) update_no);
//fprintf(stdout,"in queue process bcode size %d\n", (int) update_bcode_size);
//FILE *fp;
//fp=fopen("update.bo", "wb");
//fwrite((updater->inmsg_queue->queue+size), update_bcode_size, 1, fp);
//fclose(fp);
if(test_set_code( (out_code->bcode),
(char*) dbgf_name, (size_t) (*(uint16_t*)out_code->bcode_size) ) ){
printf("TEST PASSED!\n");
*(uint16_t*)updater->update_no=update_no;
/*Clear exisiting patch if any*/
delete_p(updater->patch);
/*copy the patch into the updater*/
updater->patch = (uint8_t*)malloc(update_patch_size);
memcpy(updater->patch, (updater->inmsg_queue->queue+size), update_patch_size);
*(size_t*) (updater->patch_size) = update_patch_size;
//code_message_outqueue_append();
neigh=1;
}
/*clear the temp code buff*/
delete_p(out_code->bcode);
delete_p(out_code->bcode_size);
delete_p(out_code);
}
else{
ROS_ERROR("Patching the .bo file failed, could be corrupt patch\n");
update_try_counter++;
outqueue_append_code_request(update_no);
}
} }
} }
}
size=0;
if(*(uint8_t*)(updater->inmsg_queue->queue) == RESEND_CODE){
size+=sizeof(uint8_t);
if( *(uint16_t*)(updater->inmsg_queue->queue+size) == *(uint16_t*) (updater->update_no) ){
size+=sizeof(uint16_t);
if(*(uint16_t*)(updater->inmsg_queue->queue+size) > update_try_counter){
update_try_counter=*(uint16_t*)(updater->inmsg_queue->queue+size);
ROS_ERROR("Code appended! update try : %u \n",update_try_counter);
code_message_outqueue_append();
}
if(update_try_counter > MAX_UPDATE_TRY) ROS_ERROR("TODO: ROLL BACK !!");
}
} }
//fprintf(stdout,"in queue freed\n"); //fprintf(stdout,"in queue freed\n");
delete_p(updater->inmsg_queue->queue); delete_p(updater->inmsg_queue->queue);
delete_p(updater->inmsg_queue->size); delete_p(updater->inmsg_queue->size);
delete_p(updater->inmsg_queue); delete_p(updater->inmsg_queue);
} }
void update_routine(const char* bcfname,
const char* dbgfname){
dbgf_name=(char*)dbgfname;
void create_update_patch(){
std::stringstream genpatch;
std::stringstream usepatch;
std::string bzzfile_name(bzz_file);
std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/";
std::string name1 = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1);
name1 = name1.substr(0,name1.find_last_of("."));
std::string name2 = name1 + "-update";
// CALL BSDIFF CMD
genpatch<< "bsdiff "<< path << name1 <<".bo "<< path << name2 <<".bo "<< path<<"diff.bo";
fprintf(stdout,"Launching bsdiff command: %s \n", genpatch.str().c_str());
system(genpatch.str().c_str());
// BETTER: CALL THE FUNCTION IN BSDIFF.CPP
//bsdiff_do(path + name1 + ".bo", path + name2 + ".bo", path + "diff.bo");
/* delete old files & rename new files */
remove((path + name1 + ".bo").c_str());
remove((path + name1 + ".bdb").c_str());
rename((path + name2 + ".bo").c_str(), (path + name1 + ".bo").c_str());
rename((path + name2 + ".bdb").c_str(), (path + name1 + ".bdb").c_str());
/*Read the diff file */
std::stringstream patchfileName;
patchfileName<<path<<"diff.bo";
uint8_t* patch_buff = 0;
FILE* fp = fopen(patchfileName.str().c_str(), "rb");
if(!fp) {
perror(patchfileName.str().c_str());
}
fseek(fp, 0, SEEK_END);
size_t patch_size = ftell(fp);
rewind(fp);
patch_buff = (uint8_t*)malloc(patch_size);
if(fread(patch_buff, 1, patch_size, fp) < patch_size) {
perror(patchfileName.str().c_str());
fclose(fp);
}
fclose(fp);
delete_p(updater->patch);
updater->patch = patch_buff;
*(size_t*) (updater->patch_size) = patch_size;
/* Delete the diff file */
remove(patchfileName.str().c_str());
}
void update_routine(){
buzzvm_t VM = buzz_utility::get_vm(); buzzvm_t VM = buzz_utility::get_vm();
buzzvm_pushs(VM, buzzvm_string_register(VM, "update_no", 1)); buzzvm_pushs(VM, buzzvm_string_register(VM, "update_no", 1));
buzzvm_pushi(VM, *(uint16_t*)(updater->update_no)); buzzvm_pushi(VM, *(uint16_t*)(updater->update_no));
@ -199,19 +441,26 @@ void update_routine(const char* bcfname,
//fprintf(stdout,"[Debug : ]updater value = %i \n",updater->mode); //fprintf(stdout,"[Debug : ]updater value = %i \n",updater->mode);
if(*(int*)updater->mode==CODE_RUNNING){ if(*(int*)updater->mode==CODE_RUNNING){
buzzvm_function_call(VM, "updated_neigh", 0); buzzvm_function_call(VM, "updated_neigh", 0);
// Check for update
if(check_update()){ if(check_update()){
ROS_INFO("Update found \nUpdating script ...\n"); ROS_INFO("Update found \nUpdating script ...\n");
if(compile_bzz()){ if(compile_bzz(bzz_file)){
ROS_WARN("Errors in comipilg script so staying on old script\n"); ROS_WARN("Errors in comipilg script so staying on old script\n");
} }
else { else {
std::string bzzfile_name(bzz_file);
stringstream bzzfile_in_compile;
std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/";
std::string name = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1);
name = name.substr(0,name.find_last_of("."));
bzzfile_in_compile<<path<<name<<"-update.bo";
uint8_t* BO_BUF = 0; uint8_t* BO_BUF = 0;
FILE* fp = fopen(bcfname, "rb"); // to change file edit FILE* fp = fopen(bzzfile_in_compile.str().c_str(), "rb"); // to change file edit
if(!fp) { if(!fp) {
perror(bcfname); perror(bzzfile_in_compile.str().c_str());
} }
fseek(fp, 0, SEEK_END); fseek(fp, 0, SEEK_END);
size_t bcode_size = ftell(fp); size_t bcode_size = ftell(fp);
@ -222,10 +471,11 @@ void update_routine(const char* bcfname,
fclose(fp); fclose(fp);
} }
fclose(fp); fclose(fp);
if(test_set_code(BO_BUF, dbgfname,bcode_size)){ if(test_set_code(BO_BUF, dbgf_name,bcode_size)){
uint16_t update_no =*(uint16_t*)(updater->update_no); uint16_t update_no =*(uint16_t*)(updater->update_no);
*(uint16_t*)(updater->update_no) =update_no +1; *(uint16_t*)(updater->update_no) =update_no +1;
code_message_outqueue_append();
create_update_patch();
VM = buzz_utility::get_vm(); VM = buzz_utility::get_vm();
ROS_INFO("Current Update no %d\n", *(uint16_t*)(updater->update_no)); ROS_INFO("Current Update no %d\n", *(uint16_t*)(updater->update_no));
buzzvm_pushs(VM, buzzvm_string_register(VM, "update_no", 1)); buzzvm_pushs(VM, buzzvm_string_register(VM, "update_no", 1));
@ -244,11 +494,7 @@ void update_routine(const char* bcfname,
else{ else{
if(neigh==0 && (!is_msg_present())){
ROS_INFO("Sending code... \n");
code_message_outqueue_append();
}
timer_steps++; timer_steps++;
buzzvm_pushs(VM, buzzvm_string_register(VM, "barrier_val",1)); buzzvm_pushs(VM, buzzvm_string_register(VM, "barrier_val",1));
buzzvm_gload(VM); buzzvm_gload(VM);
@ -259,10 +505,22 @@ void update_routine(const char* bcfname,
*(int*)(updater->mode) = CODE_RUNNING; *(int*)(updater->mode) = CODE_RUNNING;
gettimeofday(&t2, NULL); gettimeofday(&t2, NULL);
//collect_data(); //collect_data();
buzz_utility::buzz_update_set((updater)->bcode, (char*)dbgfname, *(size_t*)(updater->bcode_size)); buzz_utility::buzz_update_set((updater)->bcode, (char*)dbgf_name, *(size_t*)(updater->bcode_size));
//buzzvm_function_call(m_tBuzzVM, "updated", 0); //buzzvm_function_call(m_tBuzzVM, "updated", 0);
updated=1; updated=1;
update_try_counter=0;
timer_steps=0;
} }
else if (timer_steps>TIMEOUT_FOR_ROLLBACK){
ROS_ERROR("TIME OUT Reached, decided to roll back");
/*Time out hit deceided to roll back*/
*(int*)(updater->mode) = CODE_RUNNING;
buzz_utility::buzz_script_set(old_bcfname, dbgf_name,
(int)VM->robot);
updated=1;
update_try_counter=0;
timer_steps=0;
}
} }
@ -285,7 +543,7 @@ int test_set_code(uint8_t* BO_BUF, const char* dbgfname,size_t bcode_size ){
ROS_WARN("Initializtion of script test passed\n"); ROS_WARN("Initializtion of script test passed\n");
if(buzz_utility::update_step_test()){ if(buzz_utility::update_step_test()){
/*data logging*/ /*data logging*/
//start =1; old_byte_code_size=*(size_t*)updater->bcode_size;
/*data logging*/ /*data logging*/
ROS_WARN("Step test passed\n"); ROS_WARN("Step test passed\n");
*(int*) (updater->mode) = CODE_STANDBY; *(int*) (updater->mode) = CODE_STANDBY;
@ -383,7 +641,6 @@ void destroy_updater(){
delete_p(updater->inmsg_queue->size); delete_p(updater->inmsg_queue->size);
delete_p(updater->inmsg_queue); delete_p(updater->inmsg_queue);
} }
//
inotify_rm_watch(fd,wd); inotify_rm_watch(fd,wd);
close(fd); close(fd);
} }
@ -399,7 +656,7 @@ void updates_set_robots(int robots){
/*-------------------------------------------------------- /*--------------------------------------------------------
/ Create Buzz bytecode from the bzz script inputed / Create Buzz bytecode from the bzz script inputed
/-------------------------------------------------------*/ /-------------------------------------------------------*/
int compile_bzz(){ int compile_bzz(std::string bzz_file){
/*Compile the buzz code .bzz to .bo*/ /*Compile the buzz code .bzz to .bo*/
std::string bzzfile_name(bzz_file); std::string bzzfile_name(bzz_file);
stringstream bzzfile_in_compile; stringstream bzzfile_in_compile;
@ -407,24 +664,21 @@ int compile_bzz(){
std::string name = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1); std::string name = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1);
name = name.substr(0,name.find_last_of(".")); name = name.substr(0,name.find_last_of("."));
bzzfile_in_compile << "bzzc -I " << path << "include/"; //<<" "<<path<< name<<".basm"; bzzfile_in_compile << "bzzc -I " << path << "include/"; //<<" "<<path<< name<<".basm";
bzzfile_in_compile << " -b " << path << name << ".bo"; bzzfile_in_compile << " -b " << path << name << "-update.bo";
bzzfile_in_compile << " -d " << path << name << ".bdb "; bzzfile_in_compile << " -d " << path << name << "-update.bdb ";
bzzfile_in_compile << bzzfile_name; bzzfile_in_compile << bzzfile_name;
ROS_WARN("Launching buzz compilation for update: %s", bzzfile_in_compile.str().c_str()); ROS_WARN("Launching buzz compilation for update: %s", bzzfile_in_compile.str().c_str());
return system(bzzfile_in_compile.str().c_str()); return system(bzzfile_in_compile.str().c_str());
} }
void collect_data(std::ofstream &logger){ void collect_data(std::ofstream &logger){
//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; 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; time_spent += (t2.tv_usec - t1.tv_usec) / 1000.0;
//int bytecodesize=(int); //RID,update trigger,time steps taken,old byte code size, new bytecode size, patch size,update number,
logger<<(int)no_of_robot<<","<<neigh<<","<<(double)time_spent<<","<<(int)timer_steps<<","<<*(size_t*)updater->bcode_size<<","<<(int)*(uint8_t*)updater->update_no; //Patch_packets_received_counter,Patch_request_packets_received,Patch_packets_sent_counter,Patch_request_packets_sent_counter
timer_steps=0; logger<<(int)no_of_robot<<","<<neigh<<","<<(double)time_spent<<","<<(int)timer_steps<<","<<old_byte_code_size<<","<<*(size_t*)updater->bcode_size<<","
neigh=0; <<*(size_t*)updater->patch_size<<","<<(int)*(uint8_t*)updater->update_no<<","<<(int)packet_id_;
//fprintf(stdout,"Data logger Info : %i , %i , %f , %ld , %i , %d \n",(int)no_of_robot,neigh,time_spent,*(size_t*)updater->bcode_size,(int)no_of_robot,*(uint8_t*)updater->update_no);
//FILE *Fileptr;
//Fileptr=fopen("/home/ubuntu/ROS_WS/update_logger.csv", "a");
//fprintf(Fileptr,"%i,%i,%i,%i,%i,%u\n",(int)buzz_utility::get_robotid(),neigh,timer_steps,(int)*(size_t*)updater->bcode_size,(int)no_of_robot, *(uint8_t*)updater->update_no);
//fclose(Fileptr);
}
}

66
src/buzz_utility.cpp
View File

@ -462,14 +462,8 @@ int create_stig_tables() {
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) {
ROS_INFO(" Robot ID: %i" , robot_id); ROS_INFO(" Robot ID: %i" , robot_id);
/* Reset the Buzz VM */ Robot_id=robot_id;
if(VM) buzzvm_destroy(&VM); /* Read bytecode from file and fill the bo buffer*/
Robot_id = robot_id;
VM = buzzvm_new((int)robot_id);
/* Get rid of debug info */
if(DBG_INFO) buzzdebug_destroy(&DBG_INFO);
DBG_INFO = buzzdebug_new();
/* Read bytecode and fill in data structure */
FILE* fd = fopen(bo_filename, "rb"); FILE* fd = fopen(bo_filename, "rb");
if(!fd) { if(!fd) {
perror(bo_filename); perror(bo_filename);
@ -487,61 +481,11 @@ int create_stig_tables() {
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);
ROS_ERROR("[%i] %s: Error loading Buzz script", Robot_id, bo_filename);
return 0;
}
/* Register hook functions */
if(buzz_register_hooks() != BUZZVM_STATE_READY) {
buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO);
ROS_ERROR("[%i] Error registering hooks", Robot_id);
return 0;
}
// Initialize UAVSTATE variable
buzzvm_pushs(VM, buzzvm_string_register(VM, "UAVSTATE", 1));
buzzvm_pushs(VM, buzzvm_string_register(VM, "TURNEDOFF", 1));
buzzvm_gstore(VM);
/* Create vstig tables
if(create_stig_tables() != BUZZVM_STATE_READY) {
buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO);
ROS_ERROR("[%i] Error creating stigmergy tables", Robot_id);
//cout << "ERROR!!!! ---------- " << VM->errormsg << endl;
//cout << "ERROR!!!! ---------- " << buzzvm_strerror(VM) << endl;
return 0;
}*/
/* Save bytecode file name */ /* Save bytecode file name */
BO_FNAME = strdup(bo_filename); BO_FNAME = strdup(bo_filename);
// Execute the global part of the script return buzz_update_set(BO_BUF, bdbg_filename, bcode_size);
if(buzzvm_execute_script(VM)!= BUZZVM_STATE_DONE){
ROS_ERROR("Error executing global part, VM state : %i",VM->state);
return 0;
}
// Call the Init() function
if(buzzvm_function_call(VM, "init", 0) != BUZZVM_STATE_READY){
ROS_ERROR("Error in calling init, VM state : %i", VM->state);
return 0;
}
/* All OK */
ROS_INFO("[%i] INIT DONE!!!", Robot_id);
return 1;//buzz_update_set(BO_BUF, bdbg_filename, bcode_size);
} }
/****************************************/ /****************************************/
@ -717,7 +661,7 @@ int create_stig_tables() {
void update_sensors(){ void update_sensors(){
/* Update sensors*/ /* Update sensors*/
buzzuav_closures::buzzuav_update_battery(VM); buzzuav_closures::buzzuav_update_battery(VM);
buzzuav_closures::buzzuav_update_xbee_status(VM); buzzuav_closures::buzzuav_update_xbee_status(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::update_neighbors(VM); buzzuav_closures::update_neighbors(VM);

108
src/roscontroller.cpp
View File

@ -48,17 +48,6 @@ roscontroller::roscontroller(ros::NodeHandle &n_c, ros::NodeHandle &n_c_priv)
} else { } else {
robot_id = strtol(robot_name.c_str() + 5, NULL, 10); robot_id = strtol(robot_name.c_str() + 5, NULL, 10);
} }
std::string path =
bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/";
path = path.substr(0, bzzfile_name.find_last_of("\\/"))+"/log/";
std::string folder_check="mkdir -p "+path;
system(folder_check.c_str());
for(int i=5;i>0;i--){
rename((path +"logger_"+ std::to_string((uint8_t)robot_id)+"_"+std::to_string(i-1)+".log").c_str(),
(path +"logger_"+ std::to_string((uint8_t)robot_id)+"_"+std::to_string(i)+".log").c_str());
}
path += "logger_"+std::to_string(robot_id)+"_0.log";
log.open(path.c_str(), std::ios_base::trunc | std::ios_base::out);
} }
/*--------------------- /*---------------------
@ -196,11 +185,13 @@ void roscontroller::RosControllerRun()
/* Set the Buzz bytecode */ /* Set the Buzz bytecode */
if (buzz_utility::buzz_script_set(bcfname.c_str(), dbgfname.c_str(), if (buzz_utility::buzz_script_set(bcfname.c_str(), dbgfname.c_str(),
robot_id)) { robot_id)) {
ROS_INFO("[%i] INIT DONE!!!", robot_id);
int packet_loss_tmp,time_step=0; int packet_loss_tmp,time_step=0;
double cur_packet_loss=0; double cur_packet_loss=0;
ROS_INFO("[%i] Bytecode file found and set", robot_id); ROS_INFO("[%i] Bytecode file found and set", robot_id);
std::string standby_bo = Compile_bzz(stand_by) + ".bo"; std::string standby_bo = Compile_bzz(stand_by) + ".bo";
//init_update_monitor(bcfname.c_str(), standby_bo.c_str()); /*Intialize the update monitor*/
init_update_monitor(bcfname.c_str(), standby_bo.c_str(),dbgfname.c_str(),robot_id);
/*loop rate of ros*/ /*loop rate of ros*/
ros::Rate loop_rate(BUZZRATE); ros::Rate loop_rate(BUZZRATE);
/////////////////////////////////////////////////////// ///////////////////////////////////////////////////////
@ -208,31 +199,11 @@ void roscontroller::RosControllerRun()
////////////////////////////////////////////////////// //////////////////////////////////////////////////////
//ROS_WARN("[%i] -----------------------STARTING MAIN LOOP!", robot_id); //ROS_WARN("[%i] -----------------------STARTING MAIN LOOP!", robot_id);
while (ros::ok() && !buzz_utility::buzz_script_done()) { while (ros::ok() && !buzz_utility::buzz_script_done()) {
/*Update neighbors position inside Buzz*/
// buzz_closure::neighbour_pos_callback(neighbours_pos_map);
/*log ROS Time stamp, pos (lat,long,alt), Swarm size, no. of neigh,
neigh pos, RSSI val, Packet loss, filtered packet loss*/
log<<ros::Time::now()<<",";
log<<cur_pos.latitude << "," << cur_pos.longitude << ","
<< cur_pos.altitude << ",";
log << (int)no_of_robots<<",";
map<int, buzz_utility::Pos_struct>::iterator it =
neighbours_pos_map.begin();
log << neighbours_pos_map.size()<< ",";
for (; it != neighbours_pos_map.end(); ++it)
{
log << (double)it->second.x << "," << (double)it->second.y
<< "," << (double)it->second.z << ",";
}
const uint8_t shrt_id= 0xFF;
float result;
/*Neighbours of the robot published with id in respective topic*/ /*Neighbours of the robot published with id in respective topic*/
neighbours_pos_publisher(); neighbours_pos_publisher();
send_MPpayload(); send_MPpayload();
/*Check updater state and step code*/ /*Check updater state and step code*/
// update_routine(bcfname.c_str(), dbgfname.c_str()); update_routine();
if(time_step==BUZZRATE){ if(time_step==BUZZRATE){
time_step=0; time_step=0;
cur_packet_loss= 1 -( (double)packet_loss_tmp/(BUZZRATE*( (int)no_of_robots-1 )) ); cur_packet_loss= 1 -( (double)packet_loss_tmp/(BUZZRATE*( (int)no_of_robots-1 )) );
@ -245,8 +216,6 @@ void roscontroller::RosControllerRun()
time_step++; time_step++;
} }
if(debug) ROS_WARN("CURRENT PACKET DROP : %f ",cur_packet_loss); if(debug) ROS_WARN("CURRENT PACKET DROP : %f ",cur_packet_loss);
/*Log In Msg queue size*/
log<<(int)buzz_utility::get_inmsg_size()<<",";
/*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*/
@ -254,37 +223,14 @@ void roscontroller::RosControllerRun()
/*call flight controler service to set command long*/ /*call flight controler service to set command long*/
flight_controller_service_call(); flight_controller_service_call();
/*Set multi message available after update*/ /*Set multi message available after update*/
//if (get_update_status()) if (get_update_status())
//{ {
/* set_read_update_status(); set_read_update_status();
multi_msg = true; }
log << cur_pos.latitude << "," << cur_pos.longitude << ","
<< cur_pos.altitude << ",";
collect_data(log);
map<int, buzz_utility::Pos_struct>::iterator it =
neighbours_pos_map.begin();
log << "," << neighbours_pos_map.size();
for (; it != neighbours_pos_map.end(); ++it)
{
log << "," << (double)it->second.x << "," << (double)it->second.y
<< "," << (double)it->second.z;
}
log << std::endl;*/
//}
/*Set ROBOTS variable for barrier in .bzz from neighbours count*/ /*Set ROBOTS variable for barrier in .bzz from neighbours count*/
// no_of_robots=get_number_of_robots();
get_number_of_robots(); get_number_of_robots();
buzz_utility::set_robot_var(no_of_robots); buzz_utility::set_robot_var(no_of_robots);
/*Retrive the state of the graph and uav and log*/ updates_set_robots(no_of_robots);
std::stringstream state_buff;
state_buff << buzzuav_closures::getuavstate();
log<<state_buff.str()<<std::endl;
// if(neighbours_pos_map.size() >0) no_of_robots
// =neighbours_pos_map.size()+1;
// buzz_utility::set_robot_var(no_of_robots);
/*Set no of robots for updates TODO only when not updating*/
// if(multi_msg)
//updates_set_robots(no_of_robots);
//get_xbee_status(); // commented out because it may slow down the node too much, to be tested //get_xbee_status(); // commented out because it may slow down the node too much, to be tested
/*run once*/ /*run once*/
ros::spinOnce(); ros::spinOnce();
@ -299,11 +245,8 @@ void roscontroller::RosControllerRun()
/*sleep for the mentioned loop rate*/ /*sleep for the mentioned loop rate*/
timer_step += 1; timer_step += 1;
maintain_pos(timer_step); maintain_pos(timer_step);
// std::cout<< "HOME: " << home.latitude << ", " << home.longitude; // std::cout<< "HOME: " << home.latitude << ", " << home.longitude;
} }
/* Destroy updater and Cleanup */
// update_routine(bcfname.c_str(), dbgfname.c_str(),1);
} }
} }
@ -605,6 +548,7 @@ void roscontroller::prepare_msg_and_publish()
tmp[2] = cur_pos.altitude; tmp[2] = cur_pos.altitude;
memcpy(position, tmp, 3 * sizeof(uint64_t)); memcpy(position, tmp, 3 * sizeof(uint64_t));
mavros_msgs::Mavlink payload_out; mavros_msgs::Mavlink payload_out;
payload_out.payload64.push_back(XBEE_MESSAGE_CONSTANT);
payload_out.payload64.push_back(position[0]); payload_out.payload64.push_back(position[0]);
payload_out.payload64.push_back(position[1]); payload_out.payload64.push_back(position[1]);
payload_out.payload64.push_back(position[2]); payload_out.payload64.push_back(position[2]);
@ -621,30 +565,25 @@ void roscontroller::prepare_msg_and_publish()
payload_out.sysid = (uint8_t)robot_id; payload_out.sysid = (uint8_t)robot_id;
payload_out.msgid = (uint32_t)message_number; payload_out.msgid = (uint32_t)message_number;
/*Log out message id and message size*/
log<<(int)message_number<<",";
log<<(int)out[0]<<",";
/*publish prepared messages in respective topic*/ /*publish prepared messages in respective topic*/
payload_pub.publish(payload_out); payload_pub.publish(payload_out);
delete[] out; delete[] out;
delete[] payload_out_ptr; delete[] payload_out_ptr;
/*Check for updater message if present send*/ /*Check for updater message if present send*/
/* if ((int)get_update_mode() != CODE_RUNNING && is_msg_present() == 1 && if (is_msg_present())
multi_msg)
{ {
uint8_t *buff_send = 0; uint8_t *buff_send = 0;
uint16_t updater_msgSize = *(uint16_t *)(getupdate_out_msg_size()); uint16_t updater_msgSize = *(uint16_t *)(getupdate_out_msg_size());
; ;
int tot = 0; int tot = 0;
mavros_msgs::Mavlink update_packets; mavros_msgs::Mavlink update_packets;
fprintf(stdout, "Transfering code \n"); fprintf(stdout, "Appending code into message ...\n");
fprintf(stdout, "Sent Update packet Size: %u \n", updater_msgSize); fprintf(stdout, "Sent Update packet Size: %u \n", updater_msgSize);
// allocate mem and clear it // allocate mem and clear it
buff_send = (uint8_t *)malloc(sizeof(uint16_t) + updater_msgSize); buff_send = (uint8_t *)malloc(sizeof(uint16_t) + updater_msgSize);
memset(buff_send, 0, sizeof(uint16_t) + updater_msgSize); memset(buff_send, 0, sizeof(uint16_t) + updater_msgSize);
// Append updater msg size // Append updater msg size
*(uint16_t *)(buff_send + tot) = updater_msgSize; *(uint16_t *)(buff_send + tot) = updater_msgSize;
// fprintf(stdout,"Updater sent msg size : %i \n", (int)updater_msgSize);
tot += sizeof(uint16_t); tot += sizeof(uint16_t);
// Append updater msgs // Append updater msgs
memcpy(buff_send + tot, (uint8_t *)(getupdater_out_msg()), updater_msgSize); memcpy(buff_send + tot, (uint8_t *)(getupdater_out_msg()), updater_msgSize);
@ -669,9 +608,9 @@ void roscontroller::prepare_msg_and_publish()
update_packets.sysid = (uint8_t)robot_id; update_packets.sysid = (uint8_t)robot_id;
update_packets.msgid = (uint32_t)message_number; update_packets.msgid = (uint32_t)message_number;
payload_pub.publish(update_packets); payload_pub.publish(update_packets);
multi_msg = false; //multi_msg = false;
delete[] payload_64; delete[] payload_64;
}*/ }
} }
/*--------------------------------------------------------------------------------- /*---------------------------------------------------------------------------------
/Flight controller service call every step if there is a command set from bzz /Flight controller service call every step if there is a command set from bzz
@ -1041,8 +980,7 @@ void roscontroller::SetStreamRate(int id, int rate, int on_off) {
/*******************************************************************************************************/ /*******************************************************************************************************/
void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr &msg) { void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr &msg) {
/*Check for Updater message, if updater message push it into updater FIFO*/ /*Check for Updater message, if updater message push it into updater FIFO*/
if ((uint64_t)msg->payload64[0] == (uint64_t)UPDATER_MESSAGE_CONSTANT && if ((uint64_t)msg->payload64[0] == (uint64_t)UPDATER_MESSAGE_CONSTANT) {
msg->payload64.size() > 5) {
uint16_t obt_msg_size = sizeof(uint64_t) * (msg->payload64.size()); uint16_t obt_msg_size = sizeof(uint64_t) * (msg->payload64.size());
uint64_t message_obt[obt_msg_size]; uint64_t message_obt[obt_msg_size];
/* Go throught the obtained payload*/ /* Go throught the obtained payload*/
@ -1055,26 +993,20 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr &msg) {
/* Copy packet into temporary buffer neglecting update constant */ /* Copy packet into temporary buffer neglecting update constant */
memcpy((void *)pl, (void *)(message_obt + 1), obt_msg_size); memcpy((void *)pl, (void *)(message_obt + 1), obt_msg_size);
uint16_t unMsgSize = *(uint16_t *)(pl); uint16_t unMsgSize = *(uint16_t *)(pl);
// uint16_t tot; fprintf(stdout, "Update packet received, read msg size : %u \n", unMsgSize);
// tot+=sizeof(uint16_t);
fprintf(stdout, "Update packet, read msg size : %u \n", unMsgSize);
if (unMsgSize > 0) { if (unMsgSize > 0) {
code_message_inqueue_append((uint8_t *)(pl + sizeof(uint16_t)), code_message_inqueue_append((uint8_t *)(pl + sizeof(uint16_t)),
unMsgSize); unMsgSize);
// fprintf(stdout,"before in queue process : utils\n");
code_message_inqueue_process(); code_message_inqueue_process();
// fprintf(stdout,"after in queue process : utils\n");
} }
free(pl); free(pl);
} }
/*BVM FIFO message*/ /*BVM FIFO message*/
else if (msg->payload64.size() > 3) { else if (msg->payload64[0]==(uint64_t)XBEE_MESSAGE_CONSTANT) {
uint64_t message_obt[msg->payload64.size()]; uint64_t message_obt[msg->payload64.size()-1];
/* Go throught the obtained payload*/ /* Go throught the obtained payload*/
for (int i = 0; i < (int)msg->payload64.size(); i++) { for (int i = 1; i < (int)msg->payload64.size(); i++) {
message_obt[i] = (uint64_t)msg->payload64[i]; message_obt[i-1] = (uint64_t)msg->payload64[i];
// cout<<"[Debug:] obtaind message "<<message_obt[i]<<endl;
// i++;
} }
/* Extract neighbours position from payload*/ /* Extract neighbours position from payload*/
double neighbours_pos_payload[3]; double neighbours_pos_payload[3];