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

fixes in updater

Browse Source
This commit is contained in:
vivek-shankar 2017-01-27 04:14:57 -05:00
parent 02c1593738
commit 4b45a43dc2
4 changed files with 22 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
src/buzz_update.cpp
View File

@ -171,7 +171,10 @@ uint16_t size =0;
fp=fopen("update.bo", "wb"); fp=fopen("update.bo", "wb");
fwrite((updater->bcode), updater->bcode_size, 1, fp); fwrite((updater->bcode), updater->bcode_size, 1, fp);
fclose(fp); fclose(fp);
//fprintf(stdout,"[Debug : ]size = %i \n",(int)sizeof(size_t)); 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; *(uint16_t*)updater->outmsg_queue->size=size;
/*Update local dictionary*/ /*Update local dictionary*/
uint8_t* dict_update=(uint8_t*)malloc(sizeof(uint16_t)); uint8_t* dict_update=(uint8_t*)malloc(sizeof(uint16_t));
@ -182,7 +185,7 @@ uint16_t size =0;
} }
else if(type==STATE_MSG){ else if(type==STATE_MSG){
updater->outmsg_queue->queue = (uint8_t*)malloc(3*sizeof(uint8_t)); updater->outmsg_queue->queue = (uint8_t*)malloc(4*sizeof(uint8_t));
updater->outmsg_queue->size = (uint8_t*)malloc(sizeof(uint16_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->size = 3*sizeof(uint8_t);
*(uint16_t*)(updater->outmsg_queue->queue+size) = (uint16_t) updater->robotid; *(uint16_t*)(updater->outmsg_queue->queue+size) = (uint16_t) updater->robotid;
@ -192,6 +195,10 @@ uint16_t size =0;
*(uint8_t*)(updater->outmsg_queue->queue+size) = updater->update_no; *(uint8_t*)(updater->outmsg_queue->queue+size) = updater->update_no;
size+=sizeof(uint8_t); size+=sizeof(uint8_t);
*(uint16_t*)updater->outmsg_queue->size=size; *(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*/ /*Update local dictionary*/
uint8_t* dict_update=(uint8_t*)malloc(sizeof(uint16_t)); uint8_t* dict_update=(uint8_t*)malloc(sizeof(uint16_t));
*(uint8_t*)dict_update=updater->mode; *(uint8_t*)dict_update=updater->mode;
@ -215,13 +222,13 @@ memcpy(updater->inmsg_queue->queue, msg, size);
void code_message_inqueue_process(){ void code_message_inqueue_process(){
int size=0; 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); //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){ 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))); buzzdict_set(updater->state_dict, updater->inmsg_queue->queue,(updater->inmsg_queue->queue+sizeof(uint16_t)));
size +=3*sizeof(uint8_t); size +=3*sizeof(uint8_t);
if(*(uint8_t*)(updater->inmsg_queue->queue+size) > (updater->update_no)){ 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){ 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); uint16_t update_bcode_size =*(uint16_t*)(updater->inmsg_queue->queue+size);
size +=sizeof(uint16_t); size +=sizeof(uint16_t);
FILE *fp; FILE *fp;
@ -237,6 +244,7 @@ int size=0;
//fprintf(stdout,"start and end time in queue process of update Info : %f,%f",(double)begin,(double)end); //fprintf(stdout,"start and end time in queue process of update Info : %f,%f",(double)begin,(double)end);
/*data logging*/ /*data logging*/
fprintf(stdout,"Step test passed\n"); fprintf(stdout,"Step test passed\n");
updater->update_no=update_no_obt;
updater->mode=CODE_UPDATE; updater->mode=CODE_UPDATE;
//fprintf(stdout,"updater value = %i\n",updater->mode); //fprintf(stdout,"updater value = %i\n",updater->mode);
free(updater->bcode); free(updater->bcode);
@ -245,7 +253,7 @@ int size=0;
//updater->bcode = BO_BUF; //updater->bcode = BO_BUF;
updater->bcode_size = (size_t)update_bcode_size; updater->bcode_size = (size_t)update_bcode_size;
code_message_outqueue_append(STATE_MSG); code_message_outqueue_append(STATE_MSG);
updater->mode=CODE_NEIGHBOUR; updater->mode=CODE_STANDBY;
//return updater->mode; //return updater->mode;
//return 0; //return 0;

3
src/buzz_utility.cpp
View File

@ -89,8 +89,8 @@ namespace buzz_utility{
fprintf(stdout,"received Msg size : %i\n",(int) *(uint16_t*)(pl + tot)); fprintf(stdout,"received Msg size : %i\n",(int) *(uint16_t*)(pl + tot));
tot += sizeof(uint16_t); tot += sizeof(uint16_t);
code_message_inqueue_append(pl+tot,unMsgSize); code_message_inqueue_append(pl+tot,unMsgSize);
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))); 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(); code_message_inqueue_process();
unMsgSize=0; unMsgSize=0;
/*Check for Buzz messages only when the code is running*/ /*Check for Buzz messages only when the code is running*/
@ -554,6 +554,7 @@ uint16_t get_robotid(){
/* Make numeric id from hostname */ /* Make numeric id from hostname */
/* NOTE: here we assume that the hostname is in the format Knn */ /* NOTE: here we assume that the hostname is in the format Knn */
int id = strtol(hstnm + 4, NULL, 10); int id = strtol(hstnm + 4, NULL, 10);
fprintf(stdout, "Robot id from get rid buzz util: %i\n",id);
return (uint16_t)id; return (uint16_t)id;
} }

7
src/roscontroller.cpp
View File

@ -15,7 +15,7 @@ namespace rosbzz_node{
/*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()); set_bzz_file(bzzfile_name.c_str());
init_update_monitor(bcfname.c_str(),barrier);
} }
/***Destructor***/ /***Destructor***/
@ -33,6 +33,7 @@ namespace rosbzz_node{
/* Set the Buzz bytecode */ /* Set the Buzz bytecode */
if(buzz_utility::buzz_script_set(bcfname.c_str(), dbgfname.c_str(),robot_id)) { if(buzz_utility::buzz_script_set(bcfname.c_str(), dbgfname.c_str(),robot_id)) {
fprintf(stdout, "Bytecode file found and set\n"); fprintf(stdout, "Bytecode file found and set\n");
init_update_monitor(bcfname.c_str(),barrier);
while (ros::ok() && !buzz_utility::buzz_script_done()) while (ros::ok() && !buzz_utility::buzz_script_done())
{ {
/*Update neighbors position inside Buzz*/ /*Update neighbors position inside Buzz*/
@ -198,8 +199,8 @@ namespace rosbzz_node{
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++){
out = buzz_utility::u64_cvt_u16(message_obt[i]); out = buzz_utility::u64_cvt_u16(message_obt[i]);
for(int k=0;k<4;k++){ for(int k=0;k<4;k++){
cout<<" [Debug:] sent message "<<out[k]<<endl; cout<<" [Debug:] sent message "<<out[k]<<endl;

4
src/test1.bzz
View File

@ -1,9 +1,7 @@
# We need this for 2D vectors # We need this for 2D vectors
# Make sure you pass the correct include path to "bzzc -I <path1:path2> ..." # Make sure you pass the correct include path to "bzzc -I <path1:path2> ..."
include "/home/ubuntu/buzz/src/include/vec2.bzz" include "/home/vivek/catkin_ws/src/rosbuzz/src/vec2.bzz"
TARGET_ALTITUDE = 10.1 TARGET_ALTITUDE = 10.1