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

merge conflicts resolved

This commit is contained in:
vivek-shankar 2017-02-17 15:42:35 -05:00
parent d2b7655f1a
commit 5196f7048c
6 changed files with 0 additions and 656 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

578
src/CommunicationManager.cpp
View File

@ -9,33 +9,6 @@
#include "CommunicationManager.h"
<<<<<<< HEAD
uint16_t* u64_cvt_u16(uint64_t u64){
uint16_t* out = new uint16_t[4];
uint32_t int32_1 = u64 & 0xFFFFFFFF;
uint32_t int32_2 = (u64 & 0xFFFFFFFF00000000 ) >> 32;
out[0] = int32_1 & 0xFFFF;
out[1] = (int32_1 & (0xFFFF0000) ) >> 16;
out[2] = int32_2 & 0xFFFF;
out[3] = (int32_2 & (0xFFFF0000) ) >> 16;
//cout << " values " <<out[0] <<" "<<out[1] <<" "<<out[2] <<" "<<out[3] <<" ";
return out;
}
uint16_t get_deviceid(){
/* Get hostname */
char hstnm[30];
gethostname(hstnm, 30);
/* Make numeric id from hostname */
/* NOTE: here we assume that the hostname is in the format M100X */
int id = strtol(hstnm + 4, NULL, 10);
//fprintf(stdout, "Robot id from get rid buzz util: %i\n",id);
return (uint16_t)id;
}
=======
>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1
namespace Mist
{
@ -47,11 +20,7 @@ namespace Xbee
//*****************************************************************************
CommunicationManager::CommunicationManager():
START_DLIMITER(static_cast<unsigned char>(0x7E)),
<<<<<<< HEAD
LOOP_RATE(50) /* 10 fps */
=======
LOOP_RATE(10) /* 10 fps */
>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1
{
}
@ -68,10 +37,6 @@ bool CommunicationManager::Init(
{
if (serial_device_.Init(device, baud_rate))
{
<<<<<<< HEAD
in_messages_ = serial_device_.Get_In_Messages_Pointer();
return true;
=======
serial_device_.Set_In_Messages_Pointers(&in_std_messages_, &in_fragments_,
&in_Acks_and_Pings_, &command_responses_);
@ -96,18 +61,14 @@ bool CommunicationManager::Init(
if (!device_ID_loaded)
return false;
>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1
}
else
{
Display_Init_Communication_Failure();
return false;
}
<<<<<<< HEAD
=======
return true;
>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1
}
@ -116,11 +77,7 @@ bool CommunicationManager::Init(
void CommunicationManager::Run(DRONE_TYPE drone_type,
RUNNING_MODE running_mode)
{
<<<<<<< HEAD
std::thread thread_service(&SerialDevice::Run_Service, &serial_device_);
=======
std::thread thread_packets_handler(&PacketsHandler::Run, &packets_handler_);
>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1
Display_Drone_Type_and_Running_Mode(drone_type, running_mode);
@ -130,14 +87,10 @@ void CommunicationManager::Run(DRONE_TYPE drone_type,
Run_In_Solo_Mode(drone_type);
serial_device_.Stop_Service();
<<<<<<< HEAD
thread_service.join();
=======
serial_device_.Close_Serial_Port();
packets_handler_.Quit();
service_thread_->join();
thread_packets_handler.join();
>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1
}
@ -151,11 +104,7 @@ void CommunicationManager::Run_In_Solo_Mode(DRONE_TYPE drone_type)
{
if (node_handle_.getParam("Xbee_In_From_Controller", service_name))
{
<<<<<<< HEAD
mav_dji_server_ = node_handle_.advertiseService(service_name.c_str(), &CommunicationManager::Serve_Flight_Controller, this);
=======
mav_dji_server_ = node_handle_.advertiseService(service_name.c_str(), &CommunicationManager::Serve_Flight_Controller, this);
>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1
success = true;
}
else
@ -213,25 +162,6 @@ void CommunicationManager::Run_In_Swarm_Mode()
else
std::cout << "Failed to Get Topic Name: param 'Xbee_Out_To_Buzz' Not Found." << std::endl;
<<<<<<< HEAD
/*Get no of devices*/
node_handle_.getParam("No_of_dev", no_of_dev);
/*Get device Id feom host name*/
device_id=get_deviceid();
std::cout << "Device Id" <<device_id << std::endl;
if (success_1 && success_2)
{
ros::Rate loop_rate(LOOP_RATE);
counter=0;
while (ros::ok())
{
Check_In_Messages_and_Transfer_To_Topics();
Send_multi_msg();
ros::spinOnce();
loop_rate.sleep();
=======
if (success_1 && success_2)
{
ros::Rate loop_rate(LOOP_RATE);
@ -245,7 +175,6 @@ void CommunicationManager::Run_In_Swarm_Mode()
packets_handler_.Delete_Packets_With_Time_Out();
ros::spinOnce();
loop_rate.sleep();
>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1
}
}
}
@ -255,20 +184,12 @@ void CommunicationManager::Run_In_Swarm_Mode()
inline bool CommunicationManager::Serve_Flight_Controller(mavros_msgs::CommandInt::
Request& request, mavros_msgs::CommandInt::Response& response)
{
<<<<<<< HEAD
const std::size_t MAX_BUFFER_SIZE = 255;
=======
const std::size_t MAX_BUFFER_SIZE = 256;
>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1
unsigned int command = 0;
char temporary_buffer[MAX_BUFFER_SIZE];
std::string frame;
<<<<<<< HEAD
switch(request.command)
=======
/*switch(request.command)
>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1
{
case mavros_msgs::CommandCode::NAV_TAKEOFF:
{
@ -312,15 +233,9 @@ inline bool CommunicationManager::Serve_Flight_Controller(mavros_msgs::CommandIn
if (command != 0)
{
<<<<<<< HEAD
//Generate_Transmit_Request_Frame(temporary_buffer, &frame);
serial_device_.Send_Frame(frame);
}
=======
Generate_Transmit_Request_Frame(temporary_buffer, &frame);
serial_device_.Send_Frame(frame);
}*/
>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1
return true;
}
@ -341,16 +256,9 @@ void CommunicationManager::Display_Init_Communication_Failure()
//*****************************************************************************
<<<<<<< HEAD
inline void CommunicationManager::Generate_Transmit_Request_Frame(
const char* const message,
std::string * frame,
int tot,
=======
/*inline void CommunicationManager::Generate_Transmit_Request_Frame(
const char* const message,
std::string * frame,
>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1
const unsigned char frame_ID,
const std::string& destination_adssress,
const std::string& short_destination_adress,
@ -358,11 +266,7 @@ inline void CommunicationManager::Generate_Transmit_Request_Frame(
const std::string& options)
{
const unsigned char FAME_TYPE = static_cast<unsigned char>(0x10); /* Transmit Request Frame */
<<<<<<< HEAD
std::string frame_parameters;
=======
/*std::string frame_parameters;
>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1
std::string bytes_frame_parameters;
frame_parameters.append(destination_adssress);
@ -375,17 +279,6 @@ inline void CommunicationManager::Generate_Transmit_Request_Frame(
frame->push_back(FAME_TYPE);
frame->push_back(frame_ID);
frame->append(bytes_frame_parameters);
<<<<<<< HEAD
frame->append(message, tot);
Calculate_and_Append_Checksum(frame);
Add_Length_and_Start_Delimiter(frame);
}
//*****************************************************************************
inline void CommunicationManager::Convert_HEX_To_Bytes(
=======
frame->append(message, std::strlen(message));
Calculate_and_Append_Checksum(frame);
@ -395,7 +288,6 @@ inline void CommunicationManager::Convert_HEX_To_Bytes(
//*****************************************************************************
/*inline void CommunicationManager::Convert_HEX_To_Bytes(
>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1
const std::string& HEX_data, std::string* converted_data)
{
const unsigned short TWO_BYTES = 2;
@ -411,19 +303,11 @@ inline void CommunicationManager::Convert_HEX_To_Bytes(
temporary_char = static_cast<unsigned char>(temporary_HEX);
converted_data->push_back(temporary_char);
}
<<<<<<< HEAD
}
//*****************************************************************************
inline void CommunicationManager::Calculate_and_Append_Checksum(
=======
}*/
//*****************************************************************************
/*inline void CommunicationManager::Calculate_and_Append_Checksum(
>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1
std::string* frame)
{
unsigned short bytes_sum = 0;
@ -438,35 +322,11 @@ inline void CommunicationManager::Calculate_and_Append_Checksum(
checksum = 0xFF - lowest_8_bits;
checksum_byte = static_cast<unsigned char>(checksum);
frame->push_back(checksum_byte);
<<<<<<< HEAD
}
unsigned short CommunicationManager::Caculate_Checksum(std::string* frame)
{
unsigned short bytes_sum = 0;
unsigned lowest_8_bits = 0;
unsigned short checksum = 0;
//unsigned char checksum_byte;
for (unsigned short i = 0; i < frame->size(); i++)
bytes_sum += static_cast<unsigned short>(frame->at(i));
lowest_8_bits = bytes_sum & 0xFF;
checksum = 0xFF - lowest_8_bits;
return checksum;
}
//*****************************************************************************
inline void CommunicationManager::Add_Length_and_Start_Delimiter(
=======
}*/
//*****************************************************************************
/*inline void CommunicationManager::Add_Length_and_Start_Delimiter(
>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1
std::string* frame)
{
const unsigned short FIVE_BYTES = 5;
@ -477,11 +337,7 @@ inline void CommunicationManager::Add_Length_and_Start_Delimiter(
std::string header;
int frame_length = frame->size() - 1; /* frame_length = frame_size - checksum byte */
<<<<<<< HEAD
header.push_back(START_DLIMITER);
=======
/*header.push_back(START_DLIMITER);
>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1
sprintf(temporary_buffer, "%04X", frame_length);
sscanf(temporary_buffer, "%02X%02X", &Hex_frame_length_1,
&Hex_frame_length_2);
@ -490,30 +346,6 @@ inline void CommunicationManager::Add_Length_and_Start_Delimiter(
temporary_char = static_cast<unsigned char>(Hex_frame_length_2);
header.push_back(temporary_char);
frame->insert(0, header);
<<<<<<< HEAD
}
//*****************************************************************************
inline void CommunicationManager::Check_In_Messages_and_Transfer_To_Topics()
{
std::size_t size_in_messages = in_messages_->Get_Size();
uint16_t* header;
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;
=======
}*/
@ -526,188 +358,11 @@ inline void CommunicationManager::Check_In_Messages_and_Transfer_To_Topics()
{
uint64_t current_int64 = 0;
>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1
for (std::size_t j = 0; j < size_in_messages; j++)
{
std::shared_ptr<std::string> in_message =
in_messages_->Pop_Front();
mavros_msgs::Mavlink mavlink_msg;
<<<<<<< HEAD
int tot = 0;
//uint64_t header=0;
/*Copy the header*/
memcpy(&current_int64,in_message->c_str(),sizeof(uint64_t));
tot+=sizeof(uint64_t);
/*sscanf(in_message->c_str(), "%" PRIu64 " ",
&current_int64);*/
//std::cout<<in_message<< std::endl;
header = u64_cvt_u16(current_int64);
//std::cout << "Received header" <<header[0]<<" "<<header[1]<<" "<<header[2]<<" "<<header[3]<<" "<< std::endl;
/*Check header for msgs or ack msg */
if(header[0]==(uint16_t)MESSAGE_CONSTANT){
if(header[3]==1 && header[1]>1 && header[2]==1){
/*copy msg size*/
uint16_t tmp_size=0;
memcpy(&tmp_size,in_message->c_str()+tot,sizeof(uint16_t));
tot+=sizeof(uint16_t);
//std::cout<<"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,in_message->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]);
/*if (' ' == in_message->at(i) || 0 == i)
{
sscanf(in_message->c_str() + i, "%" PRIu64 " ",
&current_int64);
mavlink_msg.payload64.push_back(current_int64);
}*/
}
//std::cout << "Single packet message received size"<<mavlink_msg.payload64.size()<< std::endl;
mavlink_publisher_.publish(mavlink_msg);
//delete[] header;
}
else if (header[3]>1 && header[1]>1){
/*multimsg received send ack msg*/
char temporary_buffer[20];
std::string frame;
std::cout << "Multi msg Received header " <<header[0]<<" "<<header[1]<<" "<<header[2]<<" "<<header[3]<<" "<< std::endl;
if (header[2]==1){
//std::cout << "first message" << std::endl;
multi_msgs_receive.clear();
multi_msgs_receive.insert(make_pair(header[2], in_message));
receiver_cur_checksum=header[1];
//counter=1;
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) ;
//sprintf(temporary_buffer, "%" PRIu64 " ",(uint64_t)ack_msg);
memcpy(temporary_buffer, &ack_msg,sizeof(uint64_t));
Generate_Transmit_Request_Frame(temporary_buffer, &frame,tot);
serial_device_.Send_Frame(frame);
}
else if (header[1]==receiver_cur_checksum) {
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) ;
memcpy(temporary_buffer, &ack_msg,sizeof(uint64_t));
Generate_Transmit_Request_Frame(temporary_buffer, &frame,tot);
serial_device_.Send_Frame(frame);
/*tmp*/
//uint64_t tmp_printer;
//sscanf(frame.c_str(), "%" PRIu64 " ",&tmp_printer);
//uint16_t* tmp_printer_16 =u64_cvt_u16(tmp_printer);
//std::cout << "Send ACK for " <<ACK_MESSAGE_CONSTANT<<" "<<header[1] <<" "<<header[2]<<" "<<device_id<<" "<< std::endl;
//delete[] tmp_printer_16;
/*tmp*/
std::map< std::size_t, std::shared_ptr<std::string> >::iterator it = multi_msgs_receive.find(header[2]);
if(it!=multi_msgs_receive.end()){
multi_msgs_receive.erase(it);
multi_msgs_receive.insert(make_pair(header[2], in_message));
}
else{
multi_msgs_receive.insert(make_pair(header[2], in_message));
//counter++;
}
std::cout << "multi msg counter " <<multi_msgs_receive.size() << std::endl;
/*If the total size of msg reached transfer to topic*/
if(multi_msgs_receive.size()==header[3]){
for(uint16_t i =1; i<=header[3];i++){
it = multi_msgs_receive.find(i);
/*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 << "received Frame:"<<(void *) it->second->c_str() << std::endl;
//std::cout<<"Size of map : "<< multi_msgs.size()<< std::endl;
/* for (std::size_t j = 1; j < it->second->size()-1; j++)
{
if (' ' == it->second->at(j) || 0 == j)
{*/
//current_int64=0
//sscanf(it->second->c_str() + j, "%" PRIu64 " ",
// &current_int64);
/*Copy obt msg*/
//memcpy(current_int64, it->second->c_str()+tot, tmp_size*sizeof(uint64_t));
//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;
mavlink_publisher_.publish(mavlink_msg);
multi_msgs_receive.clear();
receiver_cur_checksum=0;
counter=10;
}
steps=0;
}
}
}
else if(header[0]==(uint16_t)ACK_MESSAGE_CONSTANT){
//std::cout << "ACK Received header " <<header[0]<<" "<<header[1]<<" "<<header[2]<<" "<<header[3]<<" "<< std::endl;
//std::cout << "size of ack map before adding" << ack_received_dict.size()<< std::endl;
/*Ack message about a message packet find wheather that matches with your current expectation*/
if(header[1]==Sender_cur_checksum && header[2]== (sending_chunk_no+1)){
std::map< uint16_t, uint16_t >::iterator it = ack_received_dict.find(header[3]);
if(it!=ack_received_dict.end()){
ack_received_dict.erase(it);
ack_received_dict.insert(std::make_pair((uint16_t)header[3], (uint16_t)ACK_MESSAGE_CONSTANT));
}
else{
ack_received_dict.insert( std::make_pair( (uint16_t)header[3], (uint16_t)ACK_MESSAGE_CONSTANT ) );
}
}
//std::cout << "ACK added and size of ack map " << ack_received_dict.size()<< std::endl;
}
delete[] header;
}
}
}
//*****************************************************************************
inline void CommunicationManager::Check_In_Messages_and_Transfer_To_Server()
{
std::size_t size_in_messages = in_messages_->Get_Size();
if (size_in_messages > 0)
=======
for (std::size_t i = 0; i < in_message->size(); i++)
{
@ -733,7 +388,6 @@ inline void CommunicationManager::Check_In_Messages_and_Transfer_To_Server() //
//std::size_t size_in_messages = in_messages_->Get_Size();
/*if (size_in_messages > 0)
>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1
{
for (std::size_t j = 0; j < size_in_messages; j++)
{
@ -765,11 +419,7 @@ inline void CommunicationManager::Check_In_Messages_and_Transfer_To_Server() //
ROS_INFO("XBeeMav: Faild to Tranfer Command.");
}
<<<<<<< HEAD
}
=======
}*/
>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1
}
@ -777,229 +427,10 @@ inline void CommunicationManager::Check_In_Messages_and_Transfer_To_Server() //
inline void CommunicationManager::Send_Mavlink_Message_Callback(
const mavros_msgs::Mavlink::ConstPtr& mavlink_msg)
{
<<<<<<< HEAD
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 = 24;
char temporary_buffer[MAX_BUFFER_SIZE];
std::string frame;
int converted_bytes = 0;
if((uint64_t)mavlink_msg->payload64.at(0)==(uint64_t)XBEE_STOP_TRANSMISSION && mavlink_msg->payload64.size() == 1){
std::cout << "clearing multi msg queue after request from buzz"<< std::endl;
multi_msgs_send_dict.clear();
ack_received_dict.clear();
sending_chunk_no=0;
/*Multi message packets stoped tell rosbuzz this*/
mavros_msgs::Mavlink mavlink_msg;
mavlink_msg.payload64.push_back(XBEE_MESSAGE_CONSTANT);
mavlink_publisher_.publish(mavlink_msg);
}
else if(mavlink_msg->payload64.size() > MAX_NBR_OF_INT64 && !( multi_msgs_send_dict.empty() ) ){
std::cout << "Sending previous multi message not complete yet, so dropping message"<<std::endl;
}
else{
char temporary_buffer_check[600000];
/* create a whole check sum to handel multi chunk msg*/
for(std::size_t i=0; i<mavlink_msg->payload64.size(); i++)
{
converted_bytes += sprintf(
temporary_buffer_check + converted_bytes, "%" PRIu64 " ",
(uint64_t)mavlink_msg->payload64.at(i));
}
frame.append(temporary_buffer_check, std::strlen(temporary_buffer_check));
uint16_t check_sum = (uint16_t)Caculate_Checksum(&frame);
uint16_t number=1;
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;
/*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) ;
frame="";
memset(temporary_buffer, 0, MAX_BUFFER_SIZE);
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;
delete[] header_16;
/*Single frame message*/
if(mavlink_msg->payload64.size() <= MAX_NBR_OF_INT64){
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() ) );
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++)
{
message_obt[i] =(uint64_t)mavlink_msg->payload64[i];
}
/*Copy the header*/
memcpy(cpy_buff,&header,sizeof(uint64_t));
tot+=sizeof(uint64_t);
/*copy msg size*/
uint16_t tmp_size=(uint16_t)mavlink_msg->payload64.size();
memcpy(cpy_buff+tot,&tmp_size,sizeof(uint64_t));
tot+=sizeof(uint16_t);
std::cout<<"tmp size in sender"<<tmp_size<< std::endl;
/*Copy obt msg*/
memcpy(cpy_buff+tot,message_obt,( sizeof(uint64_t) )*tmp_size);
tot+=( sizeof(uint64_t) )*tmp_size;
/*Copy the data to char buff*/
memcpy((void*)temporary_buffer,(void*)cpy_buff,tot);
delete[] cpy_buff;
//std::cout << "Single packet message sent size"<<mavlink_msg->payload64.size()<<" Tot size: "<< tot<< std::endl;
Generate_Transmit_Request_Frame(temporary_buffer, &frame,tot);
serial_device_.Send_Frame(frame);
}
else{
/*clear all the related parameter and get ready to send multi msg*/
sending_chunk_no=0;
Sender_cur_checksum = check_sum;
ack_received_dict.clear();
multi_msgs_send_dict.clear();
/*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)*MAX_NBR_OF_INT64 ) );
memset(cpy_buff, 0,sizeof(uint64_t)+sizeof(uint16_t) + ( sizeof(uint64_t)*MAX_NBR_OF_INT64 ));
for (std::size_t i =0; i<mavlink_msg->payload64.size(); i++)
{
message_obt[i] =(uint64_t)mavlink_msg->payload64[i];
}
//std::cout << "put header in dict" <<header_16[0]<<" "<<header_16[1]<<" "<<header_16[2]<<" "<<header_16[3]<<" "<< std::endl;
/*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<total; 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(
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;
//if(cnt==MAX_NBR_OF_INT64)
//{
//std::cout << "Multi frame sent no:"<<number << std::endl;
Generate_Transmit_Request_Frame(temporary_buffer, &frame,tot);
//std::S
multi_msgs_send_dict.push_back(frame);
//serial_device_.Send_Frame(frame);
/*Sleep for some time in order not to confuse Xbee, a try to reduce errors*/
//usleep(1000);
//std::cout << "Frame:"<<frame << std::endl;
//std::cout << "size of frame:"<<std::strlen(temporary_buffer)<< std::endl;
tot =0;
number++;
frame = "";
memset(cpy_buff, 0,sizeof(uint64_t)+sizeof(uint16_t) + ( sizeof(uint64_t)*MAX_NBR_OF_INT64 ));
//std::cout << "total:" <<total << std::endl;
header=0;
header = (uint64_t) MESSAGE_CONSTANT | ((uint64_t)check_sum << 16) | ((uint64_t)number << 32) |((uint64_t) total << 48) ;
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;
memset(temporary_buffer, 0, MAX_BUFFER_SIZE);
/*converted_bytes = sprintf(
temporary_buffer, "%" PRIu64 " ",
(uint64_t)header);*/
//}
}
delete[] header_16;
if(uint64_counter!=mavlink_msg->payload64.size()){
tmp_size=mavlink_msg->payload64.size() - uint64_counter;
/*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);
Generate_Transmit_Request_Frame(temporary_buffer, &frame,tot);
multi_msgs_send_dict.push_back(frame);
}
delete[] cpy_buff;
//std::cout << " Received size: " <<mavlink_msg->payload64.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*/
//serial_device_.Send_Frame(multi_msgs_send_dict[0]);
}
}
/*check for chunk to send, if multi chunk message present*/
/*first msg*/
/* if(sending_chunk_no==0 && !( multi_msgs_send_dict.empty() ) ){
serial_device_.Send_Frame(multi_msgs_send_dict.at(sending_chunk_no));
}
else */
//}
}
void CommunicationManager::Send_multi_msg(){
if( !( multi_msgs_send_dict.empty() ) ){
/*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){
std::cout << "clearing multi msg queue and telling buzz"<< std::endl;
multi_msgs_send_dict.clear();
sending_chunk_no=0;
/*Multi message packet sent tell rosbuzz this*/
mavros_msgs::Mavlink mavlink_msg;
mavlink_msg.payload64.push_back(XBEE_MESSAGE_CONSTANT);
mavlink_publisher_.publish(mavlink_msg);
}
else{
std::cout << "current size of ack in sender " <<ack_received_dict.size()<<"No. of devices"<<(uint16_t) no_of_dev<< std::endl;
if((uint16_t)ack_received_dict.size() == (uint16_t) (no_of_dev)-1){
sending_chunk_no++;
ack_received_dict.clear();
//std::cout << "sending next msg"<< std::endl;
}
std::cout << "Sent frame no. " <<sending_chunk_no+1 << std::endl;
//uint64_t tmp_printer;
//std::cout<<"Current msg in string "<<multi_msgs_send_dict.at(sending_chunk_no)<< std::endl;
//sscanf(multi_msgs_send_dict.at(sending_chunk_no).c_str(), "%" PRIu64 " ",&tmp_printer);
//uint16_t* tmp_printer_16 =u64_cvt_u16(tmp_printer);
//std::cout << "Send header" <<tmp_printer_16[0]<<" "<<tmp_printer_16[1]<<" "<<tmp_printer_16[2]<<" "<<tmp_printer_16[3]<<" "<< std::endl;
//delete[] tmp_printer_16;
serial_device_.Send_Frame(multi_msgs_send_dict.at(sending_chunk_no));
}
}
}
=======
std::cout << "Received Message From Buzz" << std::endl; // TO DO delete
packets_handler_.Handle_Mavlink_Message(mavlink_msg);
}
>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1
//*****************************************************************************
void CommunicationManager::Display_Drone_Type_and_Running_Mode(
@ -1017,14 +448,6 @@ void CommunicationManager::Display_Drone_Type_and_Running_Mode(
}
<<<<<<< HEAD
}
}
=======
//*****************************************************************************
void CommunicationManager::Process_In_Standard_Messages()
{
@ -1123,4 +546,3 @@ void CommunicationManager::Process_Command_Responses()
}
>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1

36
src/SerialDevice.cpp
View File

@ -39,10 +39,6 @@ bool SerialDevice::Init(
{
Set_Port_Options(baud_rate);
Init_Frame_Type_Keys();
<<<<<<< HEAD
Read_Frame_Header();
=======
>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1
return true;
}
else
@ -85,10 +81,7 @@ void SerialDevice::Send_Frame(const std::string& frame)
//*****************************************************************************
void SerialDevice::Run_Service()
{
<<<<<<< HEAD
=======
Read_Frame_Header();
>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1
io_service_.run();
}
@ -97,25 +90,17 @@ void SerialDevice::Run_Service()
void SerialDevice::Stop_Service()
{
io_service_.post([this]() {io_service_.stop(); });
<<<<<<< HEAD
=======
}
//*****************************************************************************
void SerialDevice::Close_Serial_Port()
{
>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1
io_service_.post([this]() {serial_port_.close(); });
}
//*****************************************************************************
<<<<<<< HEAD
Thread_Safe_Deque * SerialDevice::Get_In_Messages_Pointer()
{
return &in_messages_;
=======
void SerialDevice::Set_In_Messages_Pointers(Thread_Safe_Deque* in_std_messages,
Thread_Safe_Deque* in_fragments,
Thread_Safe_Deque* in_Acks_and_Pings,
@ -125,7 +110,6 @@ void SerialDevice::Set_In_Messages_Pointers(Thread_Safe_Deque* in_std_messages,
in_fragments_ = in_fragments;
in_Acks_and_Pings_ = in_Acks_and_Pings;
command_responses_ = command_responses;
>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1
}
@ -184,15 +168,10 @@ void SerialDevice::Read_Frame_Header()
Read_Frame_Body();
}
else
<<<<<<< HEAD
/* The header is totally corrupted, read another header. */
Read_Frame_Header();
=======
{
/* The header is totally corrupted, read another header. */
Read_Frame_Header();
}
>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1
}
else
{
@ -218,17 +197,6 @@ void SerialDevice::Read_Frame_Body()
if (current_frame_.Get_Frame_Type() ==
FRAME_TYPE_KEYS[RECEIVE_PACKET])
{
<<<<<<< HEAD
const unsigned short ELEVEN_BYTES = 11;
const unsigned short TWELVE_BYTES = 12;
std::shared_ptr<std::string> in_message =
std::make_shared<std::string>();
in_message->append(current_frame_.Get_Frame_Body()
+ ELEVEN_BYTES,
current_frame_.Get_Frame_Body_Length() - TWELVE_BYTES);
in_messages_.Push_Pack(in_message);
=======
char msg_type = current_frame_.Get_Message_Type();
std::shared_ptr<std::string> in_message =
std::make_shared<std::string>();
@ -265,7 +233,6 @@ void SerialDevice::Read_Frame_Body()
current_frame_.Get_Frame_Body_Length() - 2);
command_responses_->Push_Back(in_message);
>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1
}
Read_Frame_Header();
@ -300,8 +267,6 @@ void SerialDevice::Write_Frame()
}
<<<<<<< HEAD
=======
//*****************************************************************************
bool SerialDevice::Is_IO_Service_Stopped()
{
@ -316,7 +281,6 @@ void SerialDevice::Reset_IO_Service()
}
>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1
}

26
src/TestBuzz.cpp
View File

@ -47,34 +47,15 @@ void Init_Random_Seed()
//*****************************************************************************
int main(int argc, char **argv)
{
<<<<<<< HEAD
const unsigned int MIN_PAYLOAD_SIZE = 1;
const unsigned int MAX_PAYLOAD_SIZE = 10;
ros::init(argc, argv, "flight_controller");
=======
const unsigned int MIN_PAYLOAD_SIZE = 750;
const unsigned int MAX_PAYLOAD_SIZE = 752;
ros::init(argc, argv, "test_buzz");
>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1
ros::NodeHandle node_handle;
ros::Publisher mavlink_pub = node_handle.advertise<mavros_msgs::Mavlink>("outMavlink", 1000);
ros::Subscriber mavlink_sub = node_handle.subscribe("inMavlink", 1000, Receive_Payload_Callback);
<<<<<<< HEAD
ros::Rate loop_rate(0.2);
Init_Random_Seed();
int count = 0;
while (ros::ok())
{
mavros_msgs::Mavlink mavlink_msg_;
=======
Init_Random_Seed();
int count = 0;
@ -88,7 +69,6 @@ int main(int argc, char **argv)
mavros_msgs::Mavlink mavlink_msg_;
mavlink_msg_.sysid = 2;
mavlink_msg_.msgid = 1;
>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1
unsigned int payload_size = Get_Random_Size(MIN_PAYLOAD_SIZE, MAX_PAYLOAD_SIZE);
@ -110,14 +90,8 @@ int main(int argc, char **argv)
ros::spinOnce();
<<<<<<< HEAD
loop_rate.sleep();
count++;
=======
count++;
std::cout << "Press Enter to Send New Mavlink Message..." << std::endl;
>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1
}
return 0;

3
src/XBeeFrame.cpp
View File

@ -141,8 +141,6 @@ void Frame::Rearrange_Corrupted_Header()
}
<<<<<<< HEAD
=======
//*****************************************************************************
char Frame::Get_Message_Type()
{
@ -150,7 +148,6 @@ char Frame::Get_Message_Type()
}
>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1
}

4
src/XMLConfigParser.cpp
View File

@ -23,11 +23,7 @@ XMLConfigParser::~XMLConfigParser()
//*****************************************************************************
bool XMLConfigParser::Load_Config()
{
<<<<<<< HEAD
const std::string FILE_NAME = "/home/ubuntu/ROS_WS/src/xbeemav/Resources/XBee_Config.xml";
=======
const std::string FILE_NAME = "/home/mistlab/catkin_ws/src/xbee_ros_node/Resources/XBee_Config.xml";
>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1
if (Check_Config_File_Exists(FILE_NAME))
{

9
src/Xbee.cpp
View File

@ -17,11 +17,7 @@ int main(int argc, char* argv[])
Mist::Xbee::CommunicationManager communication_manager;
const std::string& device = "/dev/ttyUSB0"; // TO DO can be introduced as command.
<<<<<<< HEAD
const std::size_t baud_rate = 115200; // TO DO Can be introduced as command.
=======
const std::size_t baud_rate = 230400; // TO DO Can be introduced as command.
>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1
Mist::Xbee::CommunicationManager::DRONE_TYPE drone_type =
Mist::Xbee::CommunicationManager::DRONE_TYPE::SLAVE;
Mist::Xbee::CommunicationManager::RUNNING_MODE running_mode =
@ -38,12 +34,7 @@ int main(int argc, char* argv[])
running_mode = Mist::Xbee::CommunicationManager::RUNNING_MODE::SWARM;
}
}
<<<<<<< HEAD
=======
>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1
if (communication_manager.Init(device, baud_rate))
communication_manager.Run(drone_type, running_mode);
}