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

changes in clearing vect buff

This commit is contained in:
vivek-shankar 2017-01-29 18:54:11 -05:00
parent 1e6afbf004
commit 67162b00ba
1 changed files with 16 additions and 51 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

67
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(20) /* 10 fps */ LOOP_RATE(10) /* 10 fps */
{ {
} }
@ -170,13 +170,10 @@ void CommunicationManager::Run_In_Swarm_Mode()
ros::Rate loop_rate(LOOP_RATE); ros::Rate loop_rate(LOOP_RATE);
counter=0; counter=0;
while (ros::ok()) while (ros::ok())
{ {
Check_In_Messages_and_Transfer_To_Topics(); Check_In_Messages_and_Transfer_To_Topics();
Send_multi_msg(); Send_multi_msg();
//if(counter==10) {test_1=5;}
//else counter++;
ros::spinOnce(); ros::spinOnce();
loop_rate.sleep(); loop_rate.sleep();
} }
@ -375,20 +372,19 @@ inline void CommunicationManager::Check_In_Messages_and_Transfer_To_Topics()
{ {
std::size_t size_in_messages = in_messages_->Get_Size(); std::size_t size_in_messages = in_messages_->Get_Size();
uint16_t* header; uint16_t* header;
//if(!multi_msgs_receive.empty()) steps++;
if(steps>200){
steps=0;
multi_msgs_receive.clear();
receiver_cur_checksum=0;
}
/*T0 overcome mesages after the stop transmission of multi packet*/
if(counter!=0){
multi_msgs_receive.clear();
counter--;
}
if (size_in_messages > 0) if (size_in_messages > 0)
{ {
/*if(!multi_msgs_receive.empty()) steps++;
if(steps>500){
steps=0;
multi_msgs_receive.clear();
receiver_cur_checksum=0;
}*/
/*T0 overcome mesages after the stop transmission of multi packet*/
if(counter!=0){
multi_msgs_receive.clear();
counter--;
}
uint64_t current_int64 = 0; uint64_t current_int64 = 0;
for (std::size_t j = 0; j < size_in_messages; j++) for (std::size_t j = 0; j < size_in_messages; j++)
{ {
@ -402,26 +398,10 @@ inline void CommunicationManager::Check_In_Messages_and_Transfer_To_Topics()
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<<in_message<< std::endl; //std::cout<<in_message<< 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 */
char temporary_buffer[20];
std::string frame;
/*if(current_int64 <= 3){
Generate_Transmit_Request_Frame(in_message->c_str(), &frame,sizeof(uint64_t) * );
serial_device_.Send_Frame(frame);
}
if(current_int64 == (uint64_t)device_id ){
gettimeofday(&t2, NULL);
double time_spent = (t2.tv_sec - t1.tv_sec) * 1000.0; //(double)(end - begin) / CLOCKS_PER_SEC;
time_spent += (t2.tv_usec - t1.tv_usec) / 1000.0;
std::cout<<"Time taken for send and receive : "<<time_spent<<std::endl;
test_1=1;
}*/
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*/
@ -453,8 +433,8 @@ inline void CommunicationManager::Check_In_Messages_and_Transfer_To_Topics()
else if (header[3]>1 && header[1]>1){ else if (header[3]>1 && header[1]>1){
/*multimsg received send ack msg*/ /*multimsg received send ack msg*/
//char temporary_buffer[20]; char temporary_buffer[20];
//std::string frame; std::string frame;
std::cout << "Multi msg Received header " <<header[0]<<" "<<header[1]<<" "<<header[2]<<" "<<header[3]<<" "<< std::endl; std::cout << "Multi msg Received header " <<header[0]<<" "<<header[1]<<" "<<header[2]<<" "<<header[3]<<" "<< std::endl;
if (header[2]==1){ if (header[2]==1){
//std::cout << "first message" << std::endl; //std::cout << "first message" << std::endl;
@ -804,22 +784,7 @@ inline void CommunicationManager::Send_Mavlink_Message_Callback(
} }
void CommunicationManager::Send_multi_msg(){ void CommunicationManager::Send_multi_msg(){
/*if(test_1>0){
test_1--;
char temporary_buffer[sizeof(uint64_t) * 5];
std::string frame;
uint64_t constant_msg[3];
constant_msg[1]=245253253253532;
constant_msg[2]=245253253253532;
constant_msg[3]=245253253253532;
constant_msg[0]=(uint64_t) device_id;
constant_msg[4]=(uint64_t) test_t;
gettimeofday(&t1, NULL);
/*Copy the data to char buff*/
/*memcpy((void*)temporary_buffer,(void*)constant_msg,sizeof(uint64_t) * 5);
Generate_Transmit_Request_Frame(temporary_buffer, &frame,sizeof(uint64_t) * 5);
multi_msgs_send_dict.push_back(frame);
}*/
if( !( multi_msgs_send_dict.empty() ) ){ if( !( multi_msgs_send_dict.empty() ) ){
/*If the sent message chunk not the last message then send else clear the dict*/ /*If the sent message chunk not the last message then send else clear the dict*/
if( (uint16_t)(multi_msgs_send_dict.size() ) - 1 == sending_chunk_no && (uint16_t)ack_received_dict.size() == (uint16_t)(no_of_dev)-1){ if( (uint16_t)(multi_msgs_send_dict.size() ) - 1 == sending_chunk_no && (uint16_t)ack_received_dict.size() == (uint16_t)(no_of_dev)-1){