Modifications in Updater
This commit is contained in:
parent
ac444df03c
commit
8992f0b13b
|
@ -162,7 +162,7 @@ void code_message_outqueue_append(){
|
||||||
|
|
||||||
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);
|
||||||
|
@ -172,13 +172,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,"[debug]Updater mode %d",(*(int*)(updater->mode)) );
|
fprintf(stdout,"[debug]Updater mode %d \n", *(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] %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",(*(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");
|
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");
|
fprintf(stdout,"[debug]Inside update number comparision");
|
||||||
uint16_t update_no=*(uint16_t*)(updater->inmsg_queue->queue);
|
uint16_t update_no=*(uint16_t*)(updater->inmsg_queue->queue);
|
||||||
size +=sizeof(uint16_t);
|
size +=sizeof(uint16_t);
|
||||||
|
@ -324,7 +324,7 @@ uint8_t* getupdate_out_msg_size(){
|
||||||
return (uint8_t*)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){
|
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)){
|
if(buzz_utility::buzz_update_init_test(BO_BUF, dbgfname,bcode_size)){
|
||||||
fprintf(stdout,"Initializtion of script test passed\n");
|
fprintf(stdout,"Initializtion of script test passed\n");
|
||||||
if(buzz_utility::update_step_test()){
|
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;
|
//start =1;
|
||||||
/*data logging*/
|
/*data logging*/
|
||||||
fprintf(stdout,"Step test passed\n");
|
fprintf(stdout,"Step test passed\n");
|
||||||
*(int*)updater->mode = CODE_STANDBY;
|
*(int*) (updater->mode) = CODE_STANDBY;
|
||||||
//fprintf(stdout,"updater value = %i\n",updater->mode);
|
//fprintf(stdout,"updater value = %i\n",updater->mode);
|
||||||
delete_p(updater->bcode);
|
delete_p(updater->bcode);
|
||||||
updater->bcode = (uint8_t*)malloc(bcode_size);
|
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*/
|
/*Unable to step something wrong*/
|
||||||
else{
|
else{
|
||||||
|
if(*(int*) (updater->mode) == CODE_RUNNING){
|
||||||
fprintf(stdout,"step test failed, stick to old script\n");
|
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));
|
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;
|
return 0;
|
||||||
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
else {
|
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));
|
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;
|
return 0;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
@ -232,6 +232,7 @@ namespace rosbzz_node{
|
||||||
mavros_msgs::Mavlink update_packets;
|
mavros_msgs::Mavlink update_packets;
|
||||||
fprintf(stdout,"Transfering code \n");
|
fprintf(stdout,"Transfering code \n");
|
||||||
fprintf(stdout,"Sent Update packet Size: %u \n",updater_msgSize);
|
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);
|
buff_send =(uint8_t*)malloc(sizeof(uint16_t)+updater_msgSize);
|
||||||
memset(buff_send, 0,sizeof(uint16_t)+updater_msgSize);
|
memset(buff_send, 0,sizeof(uint16_t)+updater_msgSize);
|
||||||
/*Append updater msg size*/
|
/*Append updater msg size*/
|
||||||
|
@ -243,7 +244,7 @@ namespace rosbzz_node{
|
||||||
tot += updater_msgSize;
|
tot += updater_msgSize;
|
||||||
/*Destroy the updater out msg queue*/
|
/*Destroy the updater out msg queue*/
|
||||||
destroy_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];
|
uint64_t* payload_64 = new uint64_t[total_size];
|
||||||
memcpy((void*)payload_64, (void*)buff_send, total_size*sizeof(uint64_t));
|
memcpy((void*)payload_64, (void*)buff_send, total_size*sizeof(uint64_t));
|
||||||
delete[] buff_send;
|
delete[] buff_send;
|
||||||
|
@ -446,11 +447,11 @@ namespace rosbzz_node{
|
||||||
void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg){
|
void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg){
|
||||||
|
|
||||||
if((uint64_t)msg->payload64[0]==(uint64_t)UPDATER_MESSAGE_CONSTANT){
|
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];
|
uint64_t message_obt[obt_msg_size];
|
||||||
/* Go throught the obtained payload*/
|
/* Go throught the obtained payload*/
|
||||||
for(int i=1;i < (int)msg->payload64.size();i++){
|
for(int i=0;i < (int)msg->payload64.size();i++){
|
||||||
message_obt[i-1] =(uint64_t)msg->payload64[i];
|
message_obt[i] =(uint64_t)msg->payload64[i];
|
||||||
//cout<<"[Debug:] obtaind message "<<message_obt[i]<<endl;
|
//cout<<"[Debug:] obtaind message "<<message_obt[i]<<endl;
|
||||||
//i++;
|
//i++;
|
||||||
}
|
}
|
||||||
|
@ -458,7 +459,7 @@ namespace rosbzz_node{
|
||||||
uint8_t* pl =(uint8_t*)malloc(obt_msg_size);
|
uint8_t* pl =(uint8_t*)malloc(obt_msg_size);
|
||||||
memset(pl, 0,obt_msg_size);
|
memset(pl, 0,obt_msg_size);
|
||||||
/* Copy packet into temporary buffer neglecting update constant */
|
/* 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 unMsgSize = *(uint16_t*)(pl);
|
||||||
//uint16_t tot;
|
//uint16_t tot;
|
||||||
//tot+=sizeof(uint16_t);
|
//tot+=sizeof(uint16_t);
|
||||||
|
|
Loading…
Reference in New Issue