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

Modifications in Updater

This commit is contained in:
vivek-shankar 2017-01-28 05:50:46 -05:00
parent ac444df03c
commit 8992f0b13b
2 changed files with 38 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

41
src/buzz_update.cpp
View File

@ -162,7 +162,7 @@ void code_message_outqueue_append(){
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);
@ -172,13 +172,13 @@ memcpy(updater->inmsg_queue->queue, msg, size);
void code_message_inqueue_process(){
int size=0;
fprintf(stdout,"[debug]Updater mode %d",(*(int*)(updater->mode)) );
fprintf(stdout,"[debug] %u : current update number, %u : received update no ",( *(uint16_t*) (updater->update_no) ), (*(uint16_t*)(updater->inmsg_queue->queue)) );
fprintf(stdout,"[debug]Updater code size %u",(*(uint16_t*)(updater->inmsg_queue->queue) ) );
fprintf(stdout,"[debug]Updater mode %d \n", *(int*)(updater->mode) );
fprintf(stdout,"[debug] %u : current update number, %u : received update no \n",( *(uint16_t*) (updater->update_no) ), (*(uint16_t*)(updater->inmsg_queue->queue)) );
fprintf(stdout,"[debug]Updater code size %u \n",(*(uint16_t*)(updater->inmsg_queue->queue+sizeof(uint16_t)) ) );
if((*(int*)(updater->mode))==CODE_RUNNING){
if( *(int*) (updater->mode) == CODE_RUNNING){
fprintf(stdout,"[debug]Inside inmsg code running");
if( (*(uint16_t*)(updater->inmsg_queue->queue)) > ( *(uint16_t*) (updater->update_no) ) ){
if( *(uint16_t*)(updater->inmsg_queue->queue) > *(uint16_t*) (updater->update_no) ){
fprintf(stdout,"[debug]Inside update number comparision");
uint16_t update_no=*(uint16_t*)(updater->inmsg_queue->queue);
size +=sizeof(uint16_t);
@ -324,7 +324,7 @@ uint8_t* getupdate_out_msg_size(){
return (uint8_t*)updater->outmsg_queue->size;
}
int test_set_code(uint8_t* BO_BUF, const char* dbgfname,size_t bcode_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()){
@ -332,7 +332,7 @@ int test_set_code(uint8_t* BO_BUF, const char* dbgfname,size_t bcode_size){
//start =1;
/*data logging*/
fprintf(stdout,"Step test passed\n");
*(int*)updater->mode = CODE_STANDBY;
*(int*) (updater->mode) = CODE_STANDBY;
//fprintf(stdout,"updater value = %i\n",updater->mode);
delete_p(updater->bcode);
updater->bcode = (uint8_t*)malloc(bcode_size);
@ -348,15 +348,38 @@ int test_set_code(uint8_t* BO_BUF, const char* dbgfname,size_t bcode_size){
}
/*Unable to step something wrong*/
else{
if(*(int*) (updater->mode) == CODE_RUNNING){
fprintf(stdout,"step test failed, stick to old script\n");
buzz_utility::buzz_update_init_test((updater)->bcode, dbgfname, (size_t)*(size_t*)(updater->bcode_size));
}
else{
fprintf(stdout,"step test failed, Return to stand by\n");
buzz_utility::buzz_update_init_test((updater)->standby_bcode,
(char*)dbgfname,(size_t) *(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 0;
}
}
else {
fprintf(stdout,"Initialization test failed\n");
if(*(int*) (updater->mode) == CODE_RUNNING){
fprintf(stdout,"Initialization test failed, stick to old script\n");
buzz_utility::buzz_update_init_test((updater)->bcode, dbgfname,(int)*(size_t*) (updater->bcode_size));
}
else{
fprintf(stdout,"Initialization test failed, Return to stand by\n");
buzz_utility::buzz_update_init_test((updater)->standby_bcode,
(char*)dbgfname,(size_t) *(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 0;
}
}

11
src/roscontroller.cpp
View File

@ -232,6 +232,7 @@ namespace rosbzz_node{
mavros_msgs::Mavlink update_packets;
fprintf(stdout,"Transfering code \n");
fprintf(stdout,"Sent Update packet Size: %u \n",updater_msgSize);
/*allocate mem and clear it*/
buff_send =(uint8_t*)malloc(sizeof(uint16_t)+updater_msgSize);
memset(buff_send, 0,sizeof(uint16_t)+updater_msgSize);
/*Append updater msg size*/
@ -243,7 +244,7 @@ namespace rosbzz_node{
tot += updater_msgSize;
/*Destroy the updater out msg queue*/
destroy_out_msg_queue();
uint16_t total_size =(ceil((float)tot/(float)sizeof(uint64_t)));
uint16_t total_size =(ceil((float)(float)tot/(float)sizeof(uint64_t)));
uint64_t* payload_64 = new uint64_t[total_size];
memcpy((void*)payload_64, (void*)buff_send, total_size*sizeof(uint64_t));
delete[] buff_send;
@ -446,11 +447,11 @@ namespace rosbzz_node{
void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg){
if((uint64_t)msg->payload64[0]==(uint64_t)UPDATER_MESSAGE_CONSTANT){
uint16_t obt_msg_size=sizeof(uint64_t)*(msg->payload64.size()-1);
uint16_t obt_msg_size=sizeof(uint64_t)*(msg->payload64.size());
uint64_t message_obt[obt_msg_size];
/* Go throught the obtained payload*/
for(int i=1;i < (int)msg->payload64.size();i++){
message_obt[i-1] =(uint64_t)msg->payload64[i];
for(int i=0;i < (int)msg->payload64.size();i++){
message_obt[i] =(uint64_t)msg->payload64[i];
//cout<<"[Debug:] obtaind message "<<message_obt[i]<<endl;
//i++;
}
@ -458,7 +459,7 @@ namespace rosbzz_node{
uint8_t* pl =(uint8_t*)malloc(obt_msg_size);
memset(pl, 0,obt_msg_size);
/* Copy packet into temporary buffer neglecting update constant */
memcpy((void*)pl, (void*)(message_obt) ,obt_msg_size);
memcpy((void*)pl, (void*)(message_obt+1) ,obt_msg_size);
uint16_t unMsgSize = *(uint16_t*)(pl);
//uint16_t tot;
//tot+=sizeof(uint16_t);