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

stable version of updater with xbee

This commit is contained in:
vivek-shankar 2017-01-27 12:55:53 -05:00
parent d1809a6a7c
commit a7e7c24906
3 changed files with 16 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
src/buzz_update.cpp
View File

@ -157,12 +157,12 @@ void code_message_outqueue_append(){
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);
//fprintf(stdout,"out msg append transfer code size %d\n", (int)*(size_t*) updater->bcode_size);
}
void code_message_inqueue_append(uint8_t* msg,uint16_t size){
updater->inmsg_queue=(updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s));
fprintf(stdout,"in ms append code size %d\n", (int) size);
//fprintf(stdout,"in ms append code size %d\n", (int) size);
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);
@ -178,8 +178,8 @@ if(*(int*)updater->mode==CODE_RUNNING){
size +=sizeof(uint16_t);
uint16_t update_bcode_size =*(uint16_t*)(updater->inmsg_queue->queue+size);
size +=sizeof(uint16_t);
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);
//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);
@ -311,7 +311,7 @@ return (uint8_t*)updater->outmsg_queue->queue;
}
uint8_t* getupdate_out_msg_size(){
fprintf(stdout,"[Debug from get out size in util: ]size = %i \n",*(uint16_t*)updater->outmsg_queue->size);
//fprintf(stdout,"[Debug from get out size in util: ]size = %i \n",*(uint16_t*)updater->outmsg_queue->size);
return (uint8_t*)updater->outmsg_queue->size;
}

14
src/buzz_utility.cpp
View File

@ -83,11 +83,11 @@ namespace buzz_utility{
}
}*/
/* Go through the messages until there's nothing else to read */
fprintf(stdout,"Total data size : utils : %d\n",(int)size);
//fprintf(stdout,"Total data size : utils : %d\n",(int)size);
uint16_t unMsgSize;
uint8_t is_msg_pres=*(uint8_t*)(pl + tot);
tot+=sizeof(uint8_t);
fprintf(stdout,"is_updater msg present : %i\n",(int)is_msg_pres);
//fprintf(stdout,"is_updater msg present : %i\n",(int)is_msg_pres);
if(is_msg_pres){
unMsgSize = *(uint16_t*)(pl + tot);
tot+=sizeof(uint16_t);
@ -95,9 +95,9 @@ namespace buzz_utility{
if(unMsgSize>0){
code_message_inqueue_append((uint8_t*)(pl + tot),unMsgSize);
tot+=unMsgSize;
fprintf(stdout,"before in queue process : utils\n");
//fprintf(stdout,"before in queue process : utils\n");
code_message_inqueue_process();
fprintf(stdout,"after in queue process : utils\n");
//fprintf(stdout,"after in queue process : utils\n");
}
}
unMsgSize=0;
@ -133,7 +133,7 @@ namespace buzz_utility{
uint8_t updater_msg_pre = 0;
uint16_t updater_msgSize= 0;
if((int)get_update_mode()!=CODE_RUNNING && is_msg_present()==1){
fprintf(stdout,"transfer code \n");
// fprintf(stdout,"transfer code \n");
updater_msg_pre =1;
//transfer_code=0;
*(uint8_t*)(buff_send + tot) = (uint8_t)updater_msg_pre;
@ -142,7 +142,7 @@ namespace buzz_utility{
//*(uint16_t*)(buff_send + tot) = *(uint16_t*) (getupdate_out_msg_size());
updater_msgSize=*(uint16_t*) (getupdate_out_msg_size());
*(uint16_t*)(buff_send + tot)=updater_msgSize;
fprintf(stdout,"Updater sent msg size : %i \n", (int)updater_msgSize);
//fprintf(stdout,"Updater sent msg size : %i \n", (int)updater_msgSize);
tot += sizeof(uint16_t);
/*Append updater msgs*/
memcpy(buff_send + tot, (uint8_t*)(getupdater_out_msg()), updater_msgSize);
@ -566,7 +566,7 @@ uint16_t get_robotid(){
/* Make numeric id from hostname */
/* NOTE: here we assume that the hostname is in the format Knn */
int id = strtol(hstnm + 4, NULL, 10);
fprintf(stdout, "Robot id from get rid buzz util: %i\n",id);
//fprintf(stdout, "Robot id from get rid buzz util: %i\n",id);
return (uint16_t)id;
}

8
src/roscontroller.cpp
View File

@ -56,14 +56,14 @@ namespace rosbzz_node{
//std::cout<<"long obt"<<neigh_tmp.longitude<<endl;
}
neigh_pos_pub.publish(neigh_pos_array);
fprintf(stdout, "before update routine\n");
//fprintf(stdout, "before update routine\n");
/*Check updater state and step code*/
update_routine(bcfname.c_str(), dbgfname.c_str());
/*Step buzz script */
fprintf(stdout, "before code step\n");
//fprintf(stdout, "before code step\n");
buzz_utility::buzz_script_step();
/*Prepare messages and publish them in respective topic*/
fprintf(stdout, "before publish msg\n");
//fprintf(stdout, "before publish msg\n");
prepare_msg_and_publish();
/*run once*/
ros::spinOnce();
@ -184,7 +184,7 @@ namespace rosbzz_node{
else ROS_ERROR("Failed to call service from flight controller");
}
/*obtain Pay load to be sent*/
fprintf(stdout, "before getting msg from utility\n");
//fprintf(stdout, "before getting msg from utility\n");
uint64_t* payload_out_ptr= buzz_utility::out_msg_process();
uint64_t position[3];
/*Appened current position to message*/