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

test

This commit is contained in:
vivek-shankar 2017-01-29 18:14:24 -05:00
parent 5b8aac2774
commit 88a6b12b14
2 changed files with 49 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
include/CommunicationManager.h
View File

@ -111,6 +111,9 @@ private:
uint16_t receiveing_cur_totalsize; uint16_t receiveing_cur_totalsize;
uint16_t steps; uint16_t steps;
//uint16_t multi_msg_size; //uint16_t multi_msg_size;
int test_1;
struct timeval t1, t2;
}; };

44
src/CommunicationManager.cpp
View File

@ -170,6 +170,7 @@ void CommunicationManager::Run_In_Swarm_Mode()
ros::Rate loop_rate(LOOP_RATE); ros::Rate loop_rate(LOOP_RATE);
counter=0; counter=0;
test_1=1;
while (ros::ok()) while (ros::ok())
{ {
Check_In_Messages_and_Transfer_To_Topics(); Check_In_Messages_and_Transfer_To_Topics();
@ -372,10 +373,8 @@ 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 (size_in_messages > 0)
{
if(!multi_msgs_receive.empty()) steps++; if(!multi_msgs_receive.empty()) steps++;
if(steps>500){ if(steps>200){
steps=0; steps=0;
multi_msgs_receive.clear(); multi_msgs_receive.clear();
receiver_cur_checksum=0; receiver_cur_checksum=0;
@ -385,6 +384,9 @@ inline void CommunicationManager::Check_In_Messages_and_Transfer_To_Topics()
multi_msgs_receive.clear(); multi_msgs_receive.clear();
counter--; counter--;
} }
if (size_in_messages > 0)
{
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,6 +404,22 @@ inline void CommunicationManager::Check_In_Messages_and_Transfer_To_Topics()
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) * 4);
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*/
@ -433,8 +451,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 (multi_msgs_receive.empty()){ if (multi_msgs_receive.empty()){
//std::cout << "first message" << std::endl; //std::cout << "first message" << std::endl;
@ -783,7 +801,21 @@ inline void CommunicationManager::Send_Mavlink_Message_Callback(
} }
void CommunicationManager::Send_multi_msg(){ void CommunicationManager::Send_multi_msg(){
if(test_1){
test_1=0;
char temporary_buffer[sizeof(uint64_t) * 4];
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;
gettimeofday(&t1, NULL);
/*Copy the data to char buff*/
memcpy((void*)temporary_buffer,(void*)constant_msg,sizeof(uint64_t) * 4);
Generate_Transmit_Request_Frame(temporary_buffer, &frame,sizeof(uint64_t) * 4);
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){