Spiri
/
ROSBuzz_MISTLab
12
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; updater_msg_ready=1;
*(uint16_t*)updater->outmsg_queue->size=size; *(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){ 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=(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->queue = (uint8_t*)malloc(size);
updater->inmsg_queue->size = (uint8_t*)malloc(sizeof(uint16_t)); updater->inmsg_queue->size = (uint8_t*)malloc(sizeof(uint16_t));
memcpy(updater->inmsg_queue->queue, msg, size); memcpy(updater->inmsg_queue->queue, msg, size);
@ -178,8 +178,8 @@ if(*(int*)updater->mode==CODE_RUNNING){
size +=sizeof(uint16_t); size +=sizeof(uint16_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);
fprintf(stdout,"in queue process Update no %d\n", (int) update_no); //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 bcode size %d\n", (int) update_bcode_size);
//FILE *fp; //FILE *fp;
//fp=fopen("update.bo", "wb"); //fp=fopen("update.bo", "wb");
//fwrite((updater->inmsg_queue->queue+size), update_bcode_size, 1, fp); //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(){ 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; 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 */ /* 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; uint16_t unMsgSize;
uint8_t is_msg_pres=*(uint8_t*)(pl + tot); uint8_t is_msg_pres=*(uint8_t*)(pl + tot);
tot+=sizeof(uint8_t); 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){ if(is_msg_pres){
unMsgSize = *(uint16_t*)(pl + tot); unMsgSize = *(uint16_t*)(pl + tot);
tot+=sizeof(uint16_t); tot+=sizeof(uint16_t);
@ -95,9 +95,9 @@ namespace buzz_utility{
if(unMsgSize>0){ if(unMsgSize>0){
code_message_inqueue_append((uint8_t*)(pl + tot),unMsgSize); code_message_inqueue_append((uint8_t*)(pl + tot),unMsgSize);
tot+=unMsgSize; tot+=unMsgSize;
fprintf(stdout,"before in queue process : utils\n"); //fprintf(stdout,"before in queue process : utils\n");
code_message_inqueue_process(); code_message_inqueue_process();
fprintf(stdout,"after in queue process : utils\n"); //fprintf(stdout,"after in queue process : utils\n");
} }
} }
unMsgSize=0; unMsgSize=0;
@ -133,7 +133,7 @@ namespace buzz_utility{
uint8_t updater_msg_pre = 0; uint8_t updater_msg_pre = 0;
uint16_t updater_msgSize= 0; uint16_t updater_msgSize= 0;
if((int)get_update_mode()!=CODE_RUNNING && is_msg_present()==1){ 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; updater_msg_pre =1;
//transfer_code=0; //transfer_code=0;
*(uint8_t*)(buff_send + tot) = (uint8_t)updater_msg_pre; *(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()); //*(uint16_t*)(buff_send + tot) = *(uint16_t*) (getupdate_out_msg_size());
updater_msgSize=*(uint16_t*) (getupdate_out_msg_size()); updater_msgSize=*(uint16_t*) (getupdate_out_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); //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);
@ -566,7 +566,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); //fprintf(stdout, "Robot id from get rid buzz util: %i\n",id);
return (uint16_t)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; //std::cout<<"long obt"<<neigh_tmp.longitude<<endl;
} }
neigh_pos_pub.publish(neigh_pos_array); 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*/ /*Check updater state and step code*/
update_routine(bcfname.c_str(), dbgfname.c_str()); update_routine(bcfname.c_str(), dbgfname.c_str());
/*Step buzz script */ /*Step buzz script */
fprintf(stdout, "before code step\n"); //fprintf(stdout, "before code step\n");
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*/
fprintf(stdout, "before publish msg\n"); //fprintf(stdout, "before publish msg\n");
prepare_msg_and_publish(); prepare_msg_and_publish();
/*run once*/ /*run once*/
ros::spinOnce(); ros::spinOnce();
@ -184,7 +184,7 @@ namespace rosbzz_node{
else ROS_ERROR("Failed to call service from flight controller"); else ROS_ERROR("Failed to call service from flight controller");
} }
/*obtain Pay load to be sent*/ /*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* payload_out_ptr= buzz_utility::out_msg_process();
uint64_t position[3]; uint64_t position[3];
/*Appened current position to message*/ /*Appened current position to message*/