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

stable version of xbee with ros buzz but has lots of things to fix

This commit is contained in:
vivek-shankar 2017-01-27 13:00:11 -05:00
parent bd6bb41d25
commit e85c10a84a
1 changed files with 21 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

42
src/CommunicationManager.cpp
View File

@ -372,7 +372,7 @@ inline void CommunicationManager::Check_In_Messages_and_Transfer_To_Topics()
sscanf(in_message->c_str(), "%" PRIu64 " ", sscanf(in_message->c_str(), "%" PRIu64 " ",
&current_int64); &current_int64);
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;
if(header[3]==1 && header[0]==0 && header[1]>1 && header[2]==1){ if(header[3]==1 && header[0]==0 && header[1]>1 && header[2]==1){
for (std::size_t i = 1; i < in_message->size()-1; i++) for (std::size_t i = 1; i < in_message->size()-1; i++)
{ {
@ -385,15 +385,15 @@ inline void CommunicationManager::Check_In_Messages_and_Transfer_To_Topics()
} }
} }
std::cout << "Single packet message received" << std::endl; //std::cout << "Single packet message received" << std::endl;
mavlink_publisher_.publish(mavlink_msg); mavlink_publisher_.publish(mavlink_msg);
//delete[] header; //delete[] header;
} }
else if (header[3]>1 && header[0]==0 && header[1]>1){ else if (header[3]>1 && header[0]==0 && header[1]>1){
std::cout << "Multi packet: check_cur:"<<cur_checksum<< std::endl; //std::cout << "Multi packet: check_cur:"<<cur_checksum<< std::endl;
if (multi_msgs.empty()){ if (multi_msgs.empty()){
std::cout << "first message" << std::endl; //std::cout << "first message" << std::endl;
multi_msgs.insert(make_pair(header[2], in_message)); multi_msgs.insert(make_pair(header[2], in_message));
cur_checksum=header[1]; cur_checksum=header[1];
counter=1; counter=1;
@ -408,15 +408,15 @@ inline void CommunicationManager::Check_In_Messages_and_Transfer_To_Topics()
multi_msgs.insert(make_pair(header[2], in_message)); multi_msgs.insert(make_pair(header[2], in_message));
counter++; counter++;
} }
std::cout << "multi msg counter" <<counter << std::endl; //std::cout << "multi msg counter" <<counter << std::endl;
/*If the total size of msg reached transfer to topic*/ /*If the total size of msg reached transfer to topic*/
if(counter==header[3]){ if(counter==header[3]){
for(uint16_t i =1; i<=header[3];i++){ for(uint16_t i =1; i<=header[3];i++){
it = multi_msgs.find(i); it = multi_msgs.find(i);
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++)
{ {
@ -425,14 +425,14 @@ inline void CommunicationManager::Check_In_Messages_and_Transfer_To_Topics()
{ {
sscanf(it->second->c_str() + j, "%" PRIu64 " ", sscanf(it->second->c_str() + j, "%" PRIu64 " ",
&current_int64); &current_int64);
std::cout << "received Frame:" << current_int64 << std::endl; //std::cout << "received Frame:" << current_int64 << std::endl;
mavlink_msg.payload64.push_back(current_int64); mavlink_msg.payload64.push_back(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;
mavlink_publisher_.publish(mavlink_msg); mavlink_publisher_.publish(mavlink_msg);
multi_msgs.clear(); multi_msgs.clear();
cur_checksum=0; cur_checksum=0;
@ -494,7 +494,7 @@ inline void CommunicationManager::Send_Mavlink_Message_Callback(
const mavros_msgs::Mavlink::ConstPtr& mavlink_msg) const mavros_msgs::Mavlink::ConstPtr& mavlink_msg)
{ {
const unsigned short MAX_BUFFER_SIZE = 211; /* 20 (length(uint64_t)) * 10 (max int number) + 10 (spaces) + 1 */ const unsigned short MAX_BUFFER_SIZE = 211; /* 20 (length(uint64_t)) * 10 (max int number) + 10 (spaces) + 1 */
const unsigned short MAX_NBR_OF_INT64 = 20; //const unsigned short MAX_NBR_OF_INT64 = 20;
char temporary_buffer[MAX_BUFFER_SIZE]; char temporary_buffer[MAX_BUFFER_SIZE];
std::string frame; std::string frame;
int converted_bytes = 0; int converted_bytes = 0;
@ -528,13 +528,13 @@ inline void CommunicationManager::Send_Mavlink_Message_Callback(
uint16_t cnt=0; uint16_t cnt=0;
uint16_t number=1; uint16_t number=1;
uint16_t total =ceil((double)((double)mavlink_msg->payload64.size()/(double)10)); uint16_t total =ceil((double)((double)mavlink_msg->payload64.size()/(double)10));
std::cout <<"Payload size" <<mavlink_msg->payload64.size() << std::endl; //std::cout <<"Payload size" <<mavlink_msg->payload64.size() << std::endl;
uint64_t header = (uint64_t)0 | ((uint64_t)check_sum << 16) | ((uint64_t)number << 32) |((uint64_t) total << 48) ; uint64_t header = (uint64_t)0 | ((uint64_t)check_sum << 16) | ((uint64_t)number << 32) |((uint64_t) total << 48) ;
std::cout << "Total chunks:" <<check_sum << std::endl; //std::cout << "Total chunks:" <<check_sum << std::endl;
//temporary_buffer[MAX_BUFFER_SIZE]=""; //temporary_buffer[MAX_BUFFER_SIZE]="";
frame=""; frame="";
uint16_t* header_16 = u64_cvt_u16(header); uint16_t* header_16 = u64_cvt_u16(header);
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( converted_bytes= sprintf(
temporary_buffer, "%" PRIu64 " ", temporary_buffer, "%" PRIu64 " ",
(uint64_t)header); (uint64_t)header);
@ -547,24 +547,24 @@ inline void CommunicationManager::Send_Mavlink_Message_Callback(
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 << "Frame:"<<mavlink_msg->payload64.at(i) << std::endl; //std::cout << "Frame:"<<mavlink_msg->payload64.at(i) << std::endl;
} }
if(cnt==10) if(cnt==10)
{ 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); Generate_Transmit_Request_Frame(temporary_buffer, &frame);
serial_device_.Send_Frame(frame); serial_device_.Send_Frame(frame);
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;
number++; number++;
cnt=0; cnt=0;
frame = ""; frame = "";
std::cout << "total:" <<total << std::endl; //std::cout << "total:" <<total << std::endl;
header=0; header=0;
header = 0 | ((uint64_t)check_sum << 16) | ((uint64_t)number << 32) |((uint64_t) total << 48) ; header = 0 | ((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 << "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;
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 " ",
@ -574,7 +574,7 @@ inline void CommunicationManager::Send_Mavlink_Message_Callback(
} }
} }
if(total==1){ if(total==1){
std::cout << "Single frame" << std::endl; //std::cout << "Single frame" << std::endl;
Generate_Transmit_Request_Frame(temporary_buffer, &frame); Generate_Transmit_Request_Frame(temporary_buffer, &frame);
serial_device_.Send_Frame(frame); serial_device_.Send_Frame(frame);
} }