Spiri
/
services-ros1-xbee_mav
12
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Optimization to reduce multi data packets

This commit is contained in:
vivek-shankar 2017-01-29 13:26:30 -05:00
parent 7e1be92538
commit e7c2d489df
1 changed files with 111 additions and 76 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

187
src/CommunicationManager.cpp
View File

@ -44,7 +44,7 @@ namespace Xbee
//***************************************************************************** //*****************************************************************************
CommunicationManager::CommunicationManager(): CommunicationManager::CommunicationManager():
START_DLIMITER(static_cast<unsigned char>(0x7E)), START_DLIMITER(static_cast<unsigned char>(0x7E)),
LOOP_RATE(60) /* 10 fps */ LOOP_RATE(10) /* 10 fps */
{ {
} }
@ -396,32 +396,26 @@ inline void CommunicationManager::Check_In_Messages_and_Transfer_To_Topics()
/*Copy the header*/ /*Copy the header*/
memcpy(&current_int64,in_message->c_str(),sizeof(uint64_t)); memcpy(&current_int64,in_message->c_str(),sizeof(uint64_t));
tot+=sizeof(uint64_t); tot+=sizeof(uint64_t);
/*sscanf(in_message->c_str(), "%" PRIu64 " ", /*sscanf(in_message->c_str(), "%" PRIu64 " ",
&current_int64);*/ &current_int64);*/
std::cout<<"Size of char"<<sizeof(char)<< std::endl; std::cout<<in_message<< std::endl;
std::cout<<in_message->c_str()<< std::endl;
header = u64_cvt_u16(current_int64); header = u64_cvt_u16(current_int64);
std::cout << "Received header" <<header[0]<<" "<<header[1]<<" "<<header[2]<<" "<<header[3]<<" "<< std::endl; std::cout << "Received header" <<header[0]<<" "<<header[1]<<" "<<header[2]<<" "<<header[3]<<" "<< std::endl;
/*Check header for msgs or ack msg */ /*Check header for msgs or ack msg */
if(header[0]==(uint16_t)MESSAGE_CONSTANT){ if(header[0]==(uint16_t)MESSAGE_CONSTANT){
if(header[3]==1 && header[1]>1 && header[2]==1){ if(header[3]==1 && header[1]>1 && header[2]==1){
/*copy msg size*/ /*copy msg size*/
uint16_t tmp_size; uint16_t tmp_size=0;
memcpy(&tmp_size,in_message->c_str()+tot,sizeof(uint16_t)); memcpy(&tmp_size,in_message->c_str()+tot,sizeof(uint16_t));
tot+=sizeof(uint16_t); tot+=sizeof(uint16_t);
int uint64_size=tmp_size/sizeof(uint64_t); std::cout<<"received size in bytes: "<<tmp_size << std::endl;
uint64_t message_obt[uint64_size]; //int uint64_size=tmp_size/sizeof(uint64_t);
uint64_t message_obt[tmp_size];
/*Copy obt msg*/ /*Copy obt msg*/
memcpy(message_obt,in_message->c_str()+tot,tmp_size); memcpy(message_obt,in_message->c_str()+tot,tmp_size*sizeof(uint64_t));
tot+=tmp_size; tot+=tmp_size*sizeof(uint64_t);
uint16_t test_end_char; std::cout<<"tot size : "<<tot<< std::endl;
memcpy(&test_end_char,in_message->c_str()+tot,sizeof(uint16_t)); for (uint16_t i = 0; i < tmp_size; i++)
tot+=sizeof(uint16_t);
std::cout<<"test end char: "<< std::endl;
for (uint16_t i = 0; i < uint64_size; i++)
{ {
mavlink_msg.payload64.push_back(message_obt[i]); mavlink_msg.payload64.push_back(message_obt[i]);
/*if (' ' == in_message->at(i) || 0 == i) /*if (' ' == in_message->at(i) || 0 == i)
@ -447,16 +441,17 @@ inline void CommunicationManager::Check_In_Messages_and_Transfer_To_Topics()
multi_msgs_receive.insert(make_pair(header[2], in_message)); multi_msgs_receive.insert(make_pair(header[2], in_message));
receiver_cur_checksum=header[1]; receiver_cur_checksum=header[1];
//counter=1; //counter=1;
int tot =0; tot = sizeof(uint64_t);
uint64_t ack_msg = (uint64_t)ACK_MESSAGE_CONSTANT | ((uint64_t)header[1] << 16) | ((uint64_t)header[2] << 32) |((uint64_t) device_id << 48) ; uint64_t ack_msg = (uint64_t)ACK_MESSAGE_CONSTANT | ((uint64_t)header[1] << 16) | ((uint64_t)header[2] << 32) |((uint64_t) device_id << 48) ;
sprintf(temporary_buffer, "%" PRIu64 " ",(uint64_t)ack_msg); //sprintf(temporary_buffer, "%" PRIu64 " ",(uint64_t)ack_msg);
memcpy(temporary_buffer, &ack_msg,sizeof(uint64_t));
Generate_Transmit_Request_Frame(temporary_buffer, &frame,tot); Generate_Transmit_Request_Frame(temporary_buffer, &frame,tot);
serial_device_.Send_Frame(frame); serial_device_.Send_Frame(frame);
} }
else if (header[1]==receiver_cur_checksum) { else if (header[1]==receiver_cur_checksum) {
int tot =0; tot =sizeof(uint64_t);
uint64_t ack_msg = (uint64_t)ACK_MESSAGE_CONSTANT | ((uint64_t)header[1] << 16) | ((uint64_t)header[2] << 32) |((uint64_t) device_id << 48) ; uint64_t ack_msg = (uint64_t)ACK_MESSAGE_CONSTANT | ((uint64_t)header[1] << 16) | ((uint64_t)header[2] << 32) |((uint64_t) device_id << 48) ;
sprintf(temporary_buffer, "%" PRIu64 " ",(uint64_t)ack_msg); memcpy(temporary_buffer, &ack_msg,sizeof(uint64_t));
Generate_Transmit_Request_Frame(temporary_buffer, &frame,tot); Generate_Transmit_Request_Frame(temporary_buffer, &frame,tot);
serial_device_.Send_Frame(frame); serial_device_.Send_Frame(frame);
/*tmp*/ /*tmp*/
@ -470,7 +465,7 @@ inline void CommunicationManager::Check_In_Messages_and_Transfer_To_Topics()
if(it!=multi_msgs_receive.end()){ if(it!=multi_msgs_receive.end()){
multi_msgs_receive.erase(it); multi_msgs_receive.erase(it);
multi_msgs_receive.insert(make_pair(header[2], in_message)); multi_msgs_receive.insert(make_pair(header[2], in_message));
} }
else{ else{
multi_msgs_receive.insert(make_pair(header[2], in_message)); multi_msgs_receive.insert(make_pair(header[2], in_message));
//counter++; //counter++;
@ -481,27 +476,49 @@ inline void CommunicationManager::Check_In_Messages_and_Transfer_To_Topics()
for(uint16_t i =1; i<=header[3];i++){ for(uint16_t i =1; i<=header[3];i++){
it = multi_msgs_receive.find(i); it = multi_msgs_receive.find(i);
uint64_t previous_int64=0; /*Escape the header*/
tot =sizeof(uint64_t);
/*copy msg size*/
uint16_t tmp_size=0;
memcpy(&tmp_size,it->second->c_str()+tot,sizeof(uint16_t));
tot+=sizeof(uint16_t);
std::cout<<"multi publisher received size in bytes: "<<tmp_size << std::endl;
//int uint64_size=tmp_size/sizeof(uint64_t);
uint64_t message_obt[tmp_size];
/*Copy obt msg*/
memcpy(message_obt,it->second->c_str()+tot,tmp_size*sizeof(uint64_t));
tot+=tmp_size*sizeof(uint64_t);
std::cout<<"tot size : "<<tot<< std::endl;
for (uint16_t i = 0; i < tmp_size; i++)
{
mavlink_msg.payload64.push_back(message_obt[i]);
}
//uint64_t previous_int64=0;
//std::cout<<"Transfering to topic chunk no. :"<<it->first << "Size of current map" <<it->second->size()<< std::endl; //std::cout<<"Transfering to topic chunk no. :"<<it->first << "Size of current map" <<it->second->size()<< std::endl;
//std::cout << "received Frame:"<<(void *) it->second->c_str() << std::endl; //std::cout << "received Frame:"<<(void *) it->second->c_str() << std::endl;
//std::cout<<"Size of map : "<< multi_msgs.size()<< std::endl; //std::cout<<"Size of map : "<< multi_msgs.size()<< std::endl;
for (std::size_t j = 1; j < it->second->size()-1; j++) /* for (std::size_t j = 1; j < it->second->size()-1; j++)
{ {
if (' ' == it->second->at(j) || 0 == j) if (' ' == it->second->at(j) || 0 == j)
{ {*/
sscanf(it->second->c_str() + j, "%" PRIu64 " ", //current_int64=0
&current_int64); //sscanf(it->second->c_str() + j, "%" PRIu64 " ",
// &current_int64);
std::cout << "received Frame:" << current_int64 << std::endl; /*Copy obt msg*/
if(previous_int64 != current_int64){
mavlink_msg.payload64.push_back(current_int64); //memcpy(current_int64, it->second->c_str()+tot, tmp_size*sizeof(uint64_t));
previous_int64=current_int64; std::cout << "received Frame:" << current_int64 << std::endl;
} //if(previous_int64 != current_int64){
} //mavlink_msg.payload64.push_back(current_int64);
//previous_int64=current_int64;
//}
/* }
} } */
} }
std::cout << "one multi message published in topic with size :" <<mavlink_msg.payload64.size() << std::endl; std::cout << "one multi message published in topic with size :" <<mavlink_msg.payload64.size() << std::endl;
@ -606,6 +623,7 @@ inline void CommunicationManager::Send_Mavlink_Message_Callback(
} }
else{ else{
char temporary_buffer_check[6000]; char temporary_buffer_check[6000];
/* create a whole check sum to handel multi chunk msg*/
for(std::size_t i=0; i<mavlink_msg->payload64.size(); i++) for(std::size_t i=0; i<mavlink_msg->payload64.size(); i++)
{ {
converted_bytes += sprintf( converted_bytes += sprintf(
@ -616,30 +634,25 @@ inline void CommunicationManager::Send_Mavlink_Message_Callback(
frame.append(temporary_buffer_check, std::strlen(temporary_buffer_check)); frame.append(temporary_buffer_check, std::strlen(temporary_buffer_check));
uint16_t check_sum = (uint16_t)Caculate_Checksum(&frame); uint16_t check_sum = (uint16_t)Caculate_Checksum(&frame);
uint16_t cnt=0;
uint16_t number=1; uint16_t number=1;
uint16_t total =ceil((double)((double)mavlink_msg->payload64.size()/(double)MAX_NBR_OF_INT64)); uint16_t total =ceil((double)((double)mavlink_msg->payload64.size()/(double)MAX_NBR_OF_INT64));
//std::cout <<"Payload size" <<mavlink_msg->payload64.size() << std::endl; //std::cout <<"Payload size" <<mavlink_msg->payload64.size() << std::endl;
/*Create a header for the msgs*/
uint64_t header = (uint64_t)MESSAGE_CONSTANT | ((uint64_t)check_sum << 16) | ((uint64_t)number << 32) |((uint64_t) total << 48) ; uint64_t header = (uint64_t)MESSAGE_CONSTANT | ((uint64_t)check_sum << 16) | ((uint64_t)number << 32) |((uint64_t) total << 48) ;
//std::cout << "Total chunks:" <<check_sum << std::endl;
//temporary_buffer[MAX_BUFFER_SIZE]="";
frame=""; frame="";
memset(temporary_buffer, 0, MAX_BUFFER_SIZE); memset(temporary_buffer, 0, MAX_BUFFER_SIZE);
uint16_t* header_16 = u64_cvt_u16(header); uint16_t* header_16 = u64_cvt_u16(header);
/*buffer byte counter*/
int tot=0;
//std::cout << "Sent header" <<header_16[0]<<" "<<header_16[1]<<" "<<header_16[2]<<" "<<header_16[3]<<" "<< std::endl; //std::cout << "Sent header" <<header_16[0]<<" "<<header_16[1]<<" "<<header_16[2]<<" "<<header_16[3]<<" "<< std::endl;
/*converted_bytes= sprintf(
temporary_buffer, "%" PRIu64 " ",
(uint64_t)header);*/
delete[] header_16; delete[] header_16;
/*Single frame message*/ /*Single frame message*/
if(mavlink_msg->payload64.size() <= MAX_NBR_OF_INT64){ if(mavlink_msg->payload64.size() <= MAX_NBR_OF_INT64){
uint64_t message_obt[mavlink_msg->payload64.size()]; uint64_t message_obt[mavlink_msg->payload64.size()];
uint8_t* cpy_buff = (uint8_t*)malloc( sizeof(uint64_t)+sizeof(uint16_t) + ( sizeof(uint64_t)*mavlink_msg->payload64.size() ) ); uint8_t* cpy_buff = (uint8_t*)malloc( sizeof(uint64_t)+sizeof(uint16_t) + ( sizeof(uint64_t)*mavlink_msg->payload64.size() ) );
memset(cpy_buff, 0,sizeof(uint64_t) + ( sizeof(uint64_t)*mavlink_msg->payload64.size() )); memset(cpy_buff, 0,sizeof(uint64_t) + ( sizeof(uint64_t)*mavlink_msg->payload64.size() ));
int tot=0;
for (std::size_t i =0; i<mavlink_msg->payload64.size(); i++) for (std::size_t i =0; i<mavlink_msg->payload64.size(); i++)
{ {
message_obt[i] =(uint64_t)mavlink_msg->payload64[i]; message_obt[i] =(uint64_t)mavlink_msg->payload64[i];
@ -647,31 +660,18 @@ inline void CommunicationManager::Send_Mavlink_Message_Callback(
} }
/*Copy the header*/ /*Copy the header*/
memcpy(cpy_buff,&header,sizeof(uint64_t)); memcpy(cpy_buff,&header,sizeof(uint64_t));
tot+=sizeof(uint64_t); tot+=sizeof(uint64_t);
/*copy msg size*/ /*copy msg size*/
uint16_t tmp_size=(uint16_t)mavlink_msg->payload64.size(); uint16_t tmp_size=(uint16_t)mavlink_msg->payload64.size();
memcpy(cpy_buff+tot,&tmp_size,sizeof(uint64_t)); memcpy(cpy_buff+tot,&tmp_size,sizeof(uint64_t));
tot+=sizeof(uint16_t); tot+=sizeof(uint16_t);
std::cout<<"tmp size in sender"<<tmp_size<< std::endl;
/*Copy obt msg*/ /*Copy obt msg*/
memcpy(cpy_buff+tot,message_obt,sizeof(uint64_t)*tmp_size); memcpy(cpy_buff+tot,message_obt,( sizeof(uint64_t) )*tmp_size);
tot+=sizeof(uint64_t)*tmp_size; tot+=( sizeof(uint64_t) )*tmp_size;
uint16_t test_end_char = 1234;
memcpy(cpy_buff+tot,&test_end_char,sizeof(uint16_t));
tot+=sizeof(uint16_t);
/*Copy the data to char buff*/ /*Copy the data to char buff*/
memcpy((void*)temporary_buffer,(void*)cpy_buff,tot); memcpy((void*)temporary_buffer,(void*)cpy_buff,tot);
delete[] cpy_buff; delete[] cpy_buff;
/*for (std::size_t i =0; i<mavlink_msg->payload64.size(); i++)
{
cnt++;
converted_bytes += sprintf(
temporary_buffer+converted_bytes, "%" PRIu64 " ",
(uint64_t)mavlink_msg->payload64.at(i));
//std::cout << "Sent Frame in (uint64):"<<mavlink_msg->payload64.at(i) << std::endl;
//std::cout << "Sent Frame in string"<<temporary_buffer<<std::endl;
}*/
//std::cout << "Single frame" << std::endl;
std::cout << "Single packet message sent size"<<mavlink_msg->payload64.size()<<" Tot size: "<< tot<< std::endl; std::cout << "Single packet message sent size"<<mavlink_msg->payload64.size()<<" Tot size: "<< tot<< std::endl;
Generate_Transmit_Request_Frame(temporary_buffer, &frame,tot); Generate_Transmit_Request_Frame(temporary_buffer, &frame,tot);
serial_device_.Send_Frame(frame); serial_device_.Send_Frame(frame);
@ -683,19 +683,42 @@ inline void CommunicationManager::Send_Mavlink_Message_Callback(
Sender_cur_checksum = check_sum; Sender_cur_checksum = check_sum;
ack_received_dict.clear(); ack_received_dict.clear();
multi_msgs_send_dict.clear(); multi_msgs_send_dict.clear();
/*Multi message frame received, split them into chunks and store them in dict*/ /*Copy the msgs into a 64bit array*/
uint64_t message_obt[mavlink_msg->payload64.size()];
/*buffer for easy handel operation*/
uint8_t* cpy_buff = (uint8_t*)malloc( sizeof(uint64_t)+sizeof(uint16_t) + ( sizeof(uint64_t)*mavlink_msg->payload64.size() ) );
memset(cpy_buff, 0,sizeof(uint64_t) + ( sizeof(uint64_t)*mavlink_msg->payload64.size() ));
for (std::size_t i =0; i<mavlink_msg->payload64.size(); i++) for (std::size_t i =0; i<mavlink_msg->payload64.size(); i++)
{ {
message_obt[i] =(uint64_t)mavlink_msg->payload64[i];
cnt++; }
/*copy msg size*/
uint16_t tmp_size=(uint16_t)MAX_NBR_OF_INT64;
uint16_t uint64_counter=0;
/*Multi message frame received, split them into chunks and store them in dict*/
for (uint16_t i =1; i<number; i++)
{
/*Copy the header*/
memcpy(cpy_buff,&header,sizeof(uint64_t));
tot+=sizeof(uint64_t);
memcpy(cpy_buff+tot,&tmp_size,sizeof(uint64_t));
tot+=sizeof(uint16_t);
/*Copy obt msg*/
memcpy(cpy_buff+tot, message_obt+uint64_counter, ( sizeof(uint64_t) )*tmp_size);
uint64_counter+=tmp_size;
tot+=( sizeof(uint64_t) )*tmp_size;
/*Copy the data to char buff*/
memcpy((void*)temporary_buffer,(void*)cpy_buff,tot);
/*cnt++;
converted_bytes += sprintf( converted_bytes += sprintf(
temporary_buffer+converted_bytes, "%" PRIu64 " ", temporary_buffer+converted_bytes, "%" PRIu64 " ",
(uint64_t)mavlink_msg->payload64.at(i)); (uint64_t)mavlink_msg->payload64.at(i));
*/
//std::cout << "Sent Frame in (uint64):"<<mavlink_msg->payload64.at(i) << std::endl; //std::cout << "Sent Frame in (uint64):"<<mavlink_msg->payload64.at(i) << std::endl;
//std::cout << "Sent Frame in string"<<temporary_buffer<<std::endl; //std::cout << "Sent Frame in string"<<temporary_buffer<<std::endl;
if(cnt==MAX_NBR_OF_INT64) //if(cnt==MAX_NBR_OF_INT64)
{ int tot =0; //{
//std::cout << "Multi frame sent no:"<<number << std::endl; //std::cout << "Multi frame sent no:"<<number << std::endl;
Generate_Transmit_Request_Frame(temporary_buffer, &frame,tot); Generate_Transmit_Request_Frame(temporary_buffer, &frame,tot);
//std::S //std::S
@ -705,30 +728,42 @@ inline void CommunicationManager::Send_Mavlink_Message_Callback(
//usleep(1000); //usleep(1000);
//std::cout << "Frame:"<<frame << std::endl; //std::cout << "Frame:"<<frame << std::endl;
//std::cout << "size of frame:"<<std::strlen(temporary_buffer)<< std::endl; //std::cout << "size of frame:"<<std::strlen(temporary_buffer)<< std::endl;
tot =0;
number++; number++;
cnt=0;
frame = ""; frame = "";
memset(cpy_buff, 0,sizeof(uint64_t)+sizeof(uint16_t) + ( sizeof(uint64_t)*mavlink_msg->payload64.size() ));
//std::cout << "total:" <<total << std::endl; //std::cout << "total:" <<total << std::endl;
header=0; header=0;
header = (uint64_t) MESSAGE_CONSTANT | ((uint64_t)check_sum << 16) | ((uint64_t)number << 32) |((uint64_t) total << 48) ; header = (uint64_t) MESSAGE_CONSTANT | ((uint64_t)check_sum << 16) | ((uint64_t)number << 32) |((uint64_t) total << 48) ;
header_16 = u64_cvt_u16(header); header_16 = u64_cvt_u16(header);
std::cout << "put header in dict" <<header_16[0]<<" "<<header_16[1]<<" "<<header_16[2]<<" "<<header_16[3]<<" "<< std::endl; std::cout << "put header in dict" <<header_16[0]<<" "<<header_16[1]<<" "<<header_16[2]<<" "<<header_16[3]<<" "<< std::endl;
memset(temporary_buffer, 0, MAX_BUFFER_SIZE); memset(temporary_buffer, 0, MAX_BUFFER_SIZE);
converted_bytes = sprintf( /*converted_bytes = sprintf(
temporary_buffer, "%" PRIu64 " ", temporary_buffer, "%" PRIu64 " ",
(uint64_t)header); (uint64_t)header);*/
delete[] header_16;
//delete[] header_16; //}
}
} }
if(cnt!=MAX_NBR_OF_INT64 && cnt!=0){ if(uint64_counter!=mavlink_msg->payload64.size()){
int tot =0; tmp_size=mavlink_msg->payload64.size() - uint64_counter;
Generate_Transmit_Request_Frame(temporary_buffer, &frame,tot); /*Copy the header*/
multi_msgs_send_dict.push_back(frame); memcpy(cpy_buff,&header,sizeof(uint64_t));
tot+=sizeof(uint64_t);
memcpy(cpy_buff+tot,&tmp_size,sizeof(uint64_t));
tot+=sizeof(uint16_t);
/*Copy obt msg*/
memcpy(cpy_buff+tot, message_obt+uint64_counter, ( sizeof(uint64_t) )*tmp_size);
uint64_counter+=tmp_size;
tot+=( sizeof(uint64_t) )*tmp_size;
/*Copy the data to char buff*/
memcpy((void*)temporary_buffer,(void*)cpy_buff,tot);
Generate_Transmit_Request_Frame(temporary_buffer, &frame,tot);
multi_msgs_send_dict.push_back(frame);
} }
std::cout << "total size of multi msg dict after geting it from rosbuzz:" <<multi_msgs_send_dict.size() << std::endl; std::cout << "total size of multi msg dict mavlink size:" <<multi_msgs_send_dict.size() << std::endl;
std::cout << "uint64_counter size:" <<uint64_counter << std::endl;
/*Send the first message chunk*/ /*Send the first message chunk*/
//serial_device_.Send_Frame(multi_msgs_send_dict[0]); //serial_device_.Send_Frame(multi_msgs_send_dict[0]);
} }