diff --git a/include/buzz_update.h b/include/buzz_update.h
index baf1b4a..3e99d86 100644
--- a/include/buzz_update.h
+++ b/include/buzz_update.h
@@ -16,8 +16,6 @@
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;
@@ -47,33 +45,34 @@ struct updater_msgqueue_s {
struct buzz_updater_elem_s {
/* robot id */
uint16_t robotid;
- /*current Bytecode content */
+ /*current Bytecode content */
uint8_t* bcode;
/*current bcode size*/
- size_t bcode_size;
+ size_t* bcode_size;
+ /*current Bytecode content */
+ uint8_t* standby_bcode;
+ /*current bcode size*/
+ size_t* standby_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;
+ int* mode;
+ 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);
+void update_routine(const char* bcfname,
+ const char* dbgfname);
/************************************************/
/*Initalizes the updater */
/************************************************/
-void init_update_monitor(const char* bo_filename,int barrier);
+void init_update_monitor(const char* bo_filename,const char* stand_by_script,int barrier);
/*********************************************************/
@@ -113,6 +112,8 @@ int get_update_mode();
/***************************************************/
void set_bzz_file(const char* in_bzz_file);
+int test_set_code(uint8_t* BO_BUF, const char* dbgfname,size_t bcode_size);
+
/****************************************************/
/*Destroys the updater*/
/***************************************************/
diff --git a/include/buzz_utility.h b/include/buzz_utility.h
index 54417ba..2675e8b 100644
--- a/include/buzz_utility.h
+++ b/include/buzz_utility.h
@@ -51,4 +51,5 @@ int update_step_test();
uint16_t get_robotid();
+buzzvm_t get_vm();
}
diff --git a/include/roscontroller.h b/include/roscontroller.h
index 90fdc42..105f56c 100644
--- a/include/roscontroller.h
+++ b/include/roscontroller.h
@@ -46,7 +46,7 @@ private:
//int oldcmdID=0;
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,stand_by; //, rcclient;
bool rcclient;
ros::ServiceClient mav_client;
ros::Publisher payload_pub;
diff --git a/launch/rosbuzz_2_parallel.launch b/launch/rosbuzz_2_parallel.launch
index 2c792c4..44841ea 100644
--- a/launch/rosbuzz_2_parallel.launch
+++ b/launch/rosbuzz_2_parallel.launch
@@ -11,6 +11,7 @@
+
diff --git a/launch/rosbuzzm100.launch b/launch/rosbuzzm100.launch
index 0c5c75c..3dd8394 100644
--- a/launch/rosbuzzm100.launch
+++ b/launch/rosbuzzm100.launch
@@ -10,6 +10,7 @@
+
diff --git a/src/buzz_update.cpp b/src/buzz_update.cpp
index 7131d4f..a877945 100644
--- a/src/buzz_update.cpp
+++ b/src/buzz_update.cpp
@@ -8,7 +8,7 @@
#include
#include
#include
-#define MAX_BUCKETS 10
+
/*Temp for data collection*/
//static int neigh=-1;
@@ -23,35 +23,9 @@ static buzz_updater_elem_t updater;
static int no_of_robot;
static char* dbgf_name;
static const char* bzz_file;
+static int neigh=0;
-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){
+void init_update_monitor(const char* bo_filename, const char* stand_by_script,int barrier){
fprintf(stdout,"intiialized file monitor.\n");
fd=inotify_init1(IN_NONBLOCK);
if ( fd < 0 ) {
@@ -80,23 +54,43 @@ void init_update_monitor(const char* bo_filename,int barrier){
//return 0;
}
fclose(fp);
+ uint8_t* STD_BO_BUF = 0;
+ fp = fopen(stand_by_script, "rb");
+
+ if(!fp) {
+ perror(stand_by_script);
+
+ }
+ fseek(fp, 0, SEEK_END);
+
+ size_t stdby_bcode_size = ftell(fp);
+ rewind(fp);
+
+ STD_BO_BUF = (uint8_t*)malloc(stdby_bcode_size);
+ if(fread(STD_BO_BUF, 1, stdby_bcode_size, fp) < stdby_bcode_size) {
+ perror(stand_by_script);
+
+ 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;
+ updater->bcode_size = (size_t*) malloc(sizeof(size_t));
+ updater->update_no = (uint8_t*) malloc(sizeof(uint16_t));
+ *(uint16_t*)updater->update_no =0;
+ *(size_t*)(updater->bcode_size)=bcode_size;
+ updater->standby_bcode = STD_BO_BUF;
+ updater->standby_bcode_size = (size_t*)malloc(sizeof(size_t));
+ *(size_t*)(updater->standby_bcode_size)=stdby_bcode_size;
+ updater->mode=(int*)malloc(sizeof(int));
+ *(int*)updater->mode=CODE_RUNNING;
no_of_robot=barrier;
+ //neigh = 0;
//updater->outmsg_queue=
// update_table->barrier=nvs;
@@ -143,71 +137,25 @@ int check_update(){
}
-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 : ]updater rid = %i \n",(int)updater->robotid);
- fprintf(stdout,"[Debug : ]updater mode = %i \n",(int)updater->mode);
- fprintf(stdout,"[Debug : ]update number = %i \n",(int)updater->update_no);
-
- *(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(4*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;
- fprintf(stdout,"[Debug : ]updater rid = %i \n",(int)updater->robotid);
- fprintf(stdout,"[Debug : ]updater mode = %i \n",(int)updater->mode);
- fprintf(stdout,"[Debug : ]update number = %i \n",(int)updater->update_no);
-
- /*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_outqueue_append(){
+ 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)+ *(size_t*)(updater->bcode_size));
+ updater->outmsg_queue->size = (uint8_t*)malloc(sizeof(uint16_t));
+ /*append the update no, code size and code to out msg*/
+ *(uint16_t*)(updater->outmsg_queue->queue+size) = *(uint16_t*) updater->update_no;
+ size+=sizeof(uint16_t);
+ *(uint16_t*)(updater->outmsg_queue->queue+size) = *(size_t*) updater->bcode_size;
+ size+=sizeof(uint16_t);
+ memcpy(updater->outmsg_queue->queue+size, updater->bcode, *(size_t*)updater->bcode_size);
+ size+=*(size_t*)updater->bcode_size;
+ /*FILE *fp;
+ fp=fopen("update.bo", "wb");
+ fwrite((updater->bcode), updater->bcode_size, 1, fp);
+ fclose(fp);*/
+ *(uint16_t*)updater->outmsg_queue->size=size;
+
+ //fprintf(stdout,"out mes append transfer code %d\n", transfer_code);
}
void code_message_inqueue_append(uint8_t* msg,uint16_t size){
@@ -221,75 +169,34 @@ memcpy(updater->inmsg_queue->queue, msg, 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_RUNNING){
- 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)){
- if(updater->mode==CODE_RUNNING){
- uint8_t update_no_obt=*(uint8_t*)(updater->inmsg_queue->queue+size);
- size+=sizeof(uint8_t);
- 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->update_no=update_no_obt;
- 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(STATE_MSG);
- updater->mode=CODE_STANDBY;
-
- //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;
- }
- }
+if(*(int*)updater->mode==CODE_RUNNING){
+ if(*(uint16_t*)(updater->inmsg_queue->queue+size) > *(uint16_t*) updater->update_no){
+ uint16_t update_no=*(uint16_t*)(updater->inmsg_queue->queue+size);
+ size +=sizeof(uint16_t);
+ 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(test_set_code((updater->inmsg_queue->queue+size),
+ (char*) dbgf_name,(size_t)update_bcode_size)) {
+ *(uint16_t*)updater->update_no=update_no;
+ neigh=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;
+void update_routine(const char* bcfname,
+ const char* dbgfname){
+//dbgf_name=(char*)dbgfname;
//fprintf(stdout,"[Debug : ]updater value = %i \n",updater->mode);
- if(updater->mode==CODE_RUNNING){
+ if(*(int*)updater->mode==CODE_RUNNING){
if(check_update()){
std::string bzzfile_name(bzz_file);
stringstream bzzfile_in_compile;
@@ -347,86 +254,38 @@ dbgf_name=(char*)dbgfname;
}
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;
- }
+ if(test_set_code(BO_BUF, dbgfname,bcode_size)){
+ uint16_t update_no =*(uint16_t*)(updater->update_no);
+ *(uint16_t*)(updater->update_no) =update_no +1;
+ code_message_outqueue_append();
+ fprintf(stdout,"Update no %d\n", update_no);
+ }
+ neigh=0;
+ delete_p(BO_BUF);
}
}
- code_message_outqueue_append(STATE_MSG);
+
}
- else if (updater->mode==CODE_UPDATE){
-
- code_message_outqueue_append(SEND_CODE);
- updater->mode=CODE_STANDBY;
+ else{
- }
- 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(standby_barrier_test), tmp);
- fprintf(stdout,"Standby barrier ................... %i\n",*(uint8_t*)tmp);
- code_message_outqueue_append(SEND_CODE);
- 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);
+ if(neigh==0) code_message_outqueue_append();
+ buzzvm_t VM = buzz_utility::get_vm();
+ buzzvm_pushs(VM, buzzvm_string_register(VM, "barrier_val",1));
+ buzzvm_gload(VM);
+ buzzobj_t tObj = buzzvm_stack_at(VM, 1);
+ buzzvm_pop(VM);
+ fprintf(stdout,"Barrier ..................... %i \n",tObj->i.value);
+ if(tObj->i.value==no_of_robot) {
+ *(int*)updater->mode=CODE_RUNNING;
//collect_data();
- buzz_utility::buzz_update_set((updater)->bcode, dbgf_name, (updater)->bcode_size);
+ buzz_utility::buzz_update_init_test((updater)->bcode, (char*)dbgfname, *(size_t*)(updater->bcode_size));
+ //buzzvm_function_call(m_tBuzzVM, "updated", 0);
}
-
- delete_p(tmp);
- //fprintf(stdout,"freed tmp\n");
-
+
}
- if(destroy){
-
- destroy_updater();
- fprintf(stdout,"updater destoryed.\n");
- }
-return updater->mode;
}
@@ -439,7 +298,44 @@ 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;
+return (uint8_t*)updater->outmsg_queue->size;
+}
+
+int test_set_code(uint8_t* BO_BUF, const char* dbgfname,size_t bcode_size){
+ 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*/
+ //start =1;
+ /*data logging*/
+ fprintf(stdout,"Step test passed\n");
+ *(int*)updater->mode = CODE_STANDBY;
+ //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);
+ *(size_t*)updater->bcode_size = bcode_size;
+ buzz_utility::buzz_update_init_test((updater)->standby_bcode,
+ (char*)dbgfname, *(size_t*)(updater->standby_bcode_size));
+ buzzvm_t VM = buzz_utility::get_vm();
+ buzzvm_pushs(VM, buzzvm_string_register(VM, "ROBOTS", 1));
+ buzzvm_pushi(VM, no_of_robot);
+ buzzvm_gstore(VM);
+ return 1;
+ }
+ /*Unable to step something wrong*/
+ else{
+ fprintf(stdout,"step test failed, stick to old script\n");
+ buzz_utility::buzz_update_init_test((updater)->bcode, dbgfname, *(size_t*)(updater->bcode_size));
+ return 0;
+
+ }
+ }
+ else {
+ fprintf(stdout,"Initialization test failed\n");
+ buzz_utility::buzz_update_init_test((updater)->bcode, dbgfname,*(size_t*) (updater->bcode_size));
+ return 0;
+ }
}
void destroy_out_msg_queue(){
@@ -450,19 +346,16 @@ delete_p(updater->outmsg_queue);
}
int get_update_mode(){
-return updater->mode;
+return *(int*)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");
-
+delete_p(updater->bcode_size);
+delete_p(updater->standby_bcode);
+delete_p(updater->standby_bcode_size);
+delete_p(updater->mode);
+delete_p(updater->update_no);
if(updater->outmsg_queue){
delete_p(updater->outmsg_queue->queue);
delete_p(updater->outmsg_queue->size);
diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp
index ffaf378..9932676 100644
--- a/src/buzz_utility.cpp
+++ b/src/buzz_utility.cpp
@@ -84,39 +84,35 @@ namespace buzz_utility{
}*/
/* Go through the messages until there's nothing else to read */
uint16_t unMsgSize;
+ uint8_t is_msg_pres=*(uint8_t*)(pl + tot);
+ tot+=sizeof(uint8_t);
+ if(is_msg_pres){
unMsgSize = *(uint16_t*)(pl + tot);
-
- fprintf(stdout,"received Msg size : %i\n",(int) *(uint16_t*)(pl + tot));
- /*Xbee seems to send a lots of unknown message check later, added to avoid it, safe anyways*/
- if(*(uint16_t*)(pl + tot) >= 4){
- tot += sizeof(uint16_t);
- code_message_inqueue_append(pl+tot,unMsgSize);
- fprintf(stdout,"Msg 1 : %i , and Msg 2: %i\n",(int) *(uint16_t*)(pl+tot),(int) *(uint8_t*)(pl+tot+sizeof(uint16_t)));
- tot += unMsgSize;
- code_message_inqueue_process();
- unMsgSize=0;
- /*Check for Buzz messages only when the code is running*/
- if(get_update_mode()==CODE_RUNNING){
- uint8_t buzz_msg_pre=*(uint8_t*)(pl + tot);
- tot+= sizeof(uint8_t);
- /*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);
- }
+ tot+=sizeof(uint16_t);
+ // fprintf(stdout,"%u : read msg size : %u \n",m_unRobotId,unMsgSize);
+ if(unMsgSize>0){
+ code_message_inqueue_append((uint8_t*)(pl + tot),unMsgSize);
+ tot+=unMsgSize;
+ code_message_inqueue_process();
}
- /* Process messages */
- buzzvm_process_inmsgs(VM);
}
+ unMsgSize=0;
+
+ /*Obtain Buzz messages only when they are present*/
+ 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 */
+ buzzvm_process_inmsgs(VM);
}
/***************************************************/
/*Obtains messages from buzz out message Queue*/
@@ -130,20 +126,28 @@ namespace buzz_utility{
/* Send robot id */
*(uint16_t*)(buff_send+tot) = (uint16_t) VM->robot;
tot += sizeof(uint16_t);
- /*Append updater msg size*/
- memcpy(buff_send + tot, (uint8_t*)getupdate_out_msg_size(), sizeof(uint16_t));
- fprintf(stdout,"Msg size : %i\n",(int) *(uint16_t*)(buff_send + tot));
- tot+= sizeof(uint16_t);
- /*Append updater msgs*/
- memcpy(buff_send + tot, (uint8_t*) getupdater_out_msg(),*(uint16_t*)getupdate_out_msg_size());
- fprintf(stdout,"Msg 1 : %i , and Msg 2: %i\n",(int) *(uint16_t*)(buff_send + tot),(int) *(uint8_t*)(buff_send + tot+sizeof(uint16_t)));
- tot+= *(uint16_t*)getupdate_out_msg_size();
- /*destroy the updater out msg queue*/
- destroy_out_msg_queue();
- if(get_update_mode()==CODE_RUNNING){
- *(uint8_t*)(buff_send + tot) = 1;
- tot+= sizeof(uint8_t);
- /* Send messages from FIFO */
+ uint8_t updater_msg_pre = 0;
+ uint16_t updater_msgSize= 0;
+ if((int)get_update_mode()!=CODE_RUNNING){
+ //fprintf(stdout,"transfer code %d\n", transfer_code);
+ updater_msg_pre =1;
+ //transfer_code=0;
+ *(uint8_t*)(buff_send + tot) = (uint8_t)updater_msg_pre;
+ tot += sizeof(uint8_t);
+ /*Append updater msg size*/
+ *(uint16_t*)(buff_send + tot) = *(uint16_t*) (getupdate_out_msg_size());
+ tot += sizeof(uint16_t);
+ /*Append updater msgs*/
+ memcpy(buff_send + tot, (uint8_t*)(getupdater_out_msg()), updater_msgSize);
+ tot += updater_msgSize;
+ /*destroy the updater out msg queue*/
+ destroy_out_msg_queue();
+ }
+ else{
+ *(uint8_t*)(buff_send + tot) = (uint8_t)updater_msg_pre;
+ tot += sizeof(uint8_t);
+ }
+ /* Send messages from FIFO */
do {
/* Are there more messages? */
if(buzzoutmsg_queue_isempty(VM)) break;
@@ -169,11 +173,7 @@ namespace buzz_utility{
buzzoutmsg_queue_next(VM);
buzzmsg_payload_destroy(&m);
} while(1);
- }
- 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;
@@ -489,7 +489,10 @@ namespace buzz_utility{
buzz_error_info());
buzzvm_dump(VM);
}
- /* Print swarm */
+ /*!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!*/
+ /* look into this currently we don't have swarm feature at all */
+ /*!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!*/
+ /*Print swarm*/
//buzzswarm_members_print(stdout, VM->swarmmembers, VM->robot);
/* Check swarm state */
/* int status = 1;
@@ -560,6 +563,9 @@ uint16_t get_robotid(){
return (uint16_t)id;
}
+buzzvm_t get_vm(){
+return VM;
+}
}
diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp
index d5decde..9f34cf7 100644
--- a/src/roscontroller.cpp
+++ b/src/roscontroller.cpp
@@ -33,7 +33,7 @@ namespace rosbzz_node{
/* Set the Buzz bytecode */
if(buzz_utility::buzz_script_set(bcfname.c_str(), dbgfname.c_str(),robot_id)) {
fprintf(stdout, "Bytecode file found and set\n");
- init_update_monitor(bcfname.c_str(),barrier);
+ init_update_monitor(bcfname.c_str(),stand_by.c_str(),barrier);
while (ros::ok() && !buzz_utility::buzz_script_done())
{
/*Update neighbors position inside Buzz*/
@@ -56,12 +56,14 @@ namespace rosbzz_node{
//std::cout<<"long obt"< ..."
include "/home/ubuntu/buzz/src/include/vec2.bzz"
+####################################################################################################
+# Updater related
+# This should be here for the updater to work, changing position of code will crash the updater
+####################################################################################################
+updated="update_ack"
+
+function updated_neigh(){
+neighbors.broadcast(updated, update_no)
+}
TARGET_ALTITUDE = 10.1
@@ -46,7 +55,7 @@ function hexagon() {
# Constants
#
BARRIER_VSTIG = 1
-ROBOTS = 3 # number of robots in the swarm
+#ROBOTS = 3 # number of robots in the swarm
#
# Sets a barrier