diff --git a/CMakeLists.txt b/CMakeLists.txt index 962e5a5..ae971cb 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -53,17 +53,30 @@ include_directories( ) +<<<<<<< HEAD add_executable(xbee_mav src/Xbee.cpp src/XBeeFrame.cpp src/SerialDevice.cpp src/CommunicationManager.cpp) target_link_libraries(xbee_mav ${catkin_LIBRARIES}) add_executable(config src/main.cpp src/XBeeModule.cpp src/XMLConfigParser.cpp) target_link_libraries(config ${catkin_LIBRARIES}) +======= +add_executable(xbee_mav src/Xbee.cpp src/XBeeFrame.cpp src/SerialDevice.cpp src/CommunicationManager.cpp src/PacketsHandler) +target_link_libraries(xbee_mav ${catkin_LIBRARIES}) + +#add_executable(config src/main.cpp src/XBeeModule.cpp src/XMLConfigParser.cpp) +#target_link_libraries(config ${catkin_LIBRARIES}) +>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1 #add_executable(test_controller src/TestController.cpp) #target_link_libraries(test_controller ${catkin_LIBRARIES}) +<<<<<<< HEAD #add_executable(test_buzz src/TestBuzz.cpp) #target_link_libraries(test_buzz ${catkin_LIBRARIES}) +======= +add_executable(test_buzz src/TestBuzz.cpp) +target_link_libraries(test_buzz ${catkin_LIBRARIES}) +>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1 @@ -76,6 +89,9 @@ target_link_libraries(config ${catkin_LIBRARIES}) ############# ## Testing ## ############# +<<<<<<< HEAD +======= +>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1 diff --git a/Resources/XBee_Config.xml b/Resources/XBee_Config.xml index 8b7af61..da8ec54 100644 --- a/Resources/XBee_Config.xml +++ b/Resources/XBee_Config.xml @@ -22,7 +22,11 @@ 11 0 +<<<<<<< HEAD 7 +======= + 8 +>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1 0 0 3 diff --git a/Resources/database.xml b/Resources/database.xml new file mode 100644 index 0000000..be63e56 --- /dev/null +++ b/Resources/database.xml @@ -0,0 +1,14 @@ + + + + 0013A20040D8CA1E + 0013A200415278B8 + 0013A2004103B363 + 0013A200415278AD + 0013A2004103B356 + 0013A200415278B7 + 0013A2004098A7A7 + 0013A2004098A7BA + 0013A200415A9DDD + 0013A200415A9DE4 + diff --git a/include/CommunicationManager.h b/include/CommunicationManager.h index 313f823..cfa0da1 100644 --- a/include/CommunicationManager.h +++ b/include/CommunicationManager.h @@ -1,6 +1,10 @@ /* CommunicationManager.h -- Communication Manager class for XBee: Handles all communications with other ROS nodes +<<<<<<< HEAD and the serial port -- */ +======= + and the serial port -- */ +>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1 /* ------------------------------------------------------------------------- */ /* September 20, 2016 -- @Copyright Aymen Soussia. All rights reserved. */ /* (aymen.soussia@gmail.com) */ @@ -16,12 +20,18 @@ #include #include +<<<<<<< HEAD #include"SerialDevice.h" #define MESSAGE_CONSTANT 238 #define ACK_MESSAGE_CONSTANT 911 #define XBEE_MESSAGE_CONSTANT 586782343 #define XBEE_STOP_TRANSMISSION 4355356352 +======= +#include"PacketsHandler.h" +#include"SerialDevice.h" + +>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1 namespace Mist { @@ -62,15 +72,22 @@ private: void Run_In_Solo_Mode(DRONE_TYPE drone_type); void Run_In_Swarm_Mode(); +<<<<<<< HEAD void Generate_Transmit_Request_Frame( const char* const message, std::string* frame, int tot, +======= + /*void Generate_Transmit_Request_Frame( + const char* const message, + std::string* frame, +>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1 const unsigned char frame_ID = static_cast(0x01), const std::string& destination_adssress = "000000000000FFFF", const std::string& short_destination_adress = "FFFF", const std::string& broadcast_radius = "00", +<<<<<<< HEAD const std::string& options = "00"); void Check_In_Messages_and_Transfer_To_Topics(); void Display_Init_Communication_Failure(); @@ -78,6 +95,15 @@ private: std::string* converted_data); void Calculate_and_Append_Checksum(std::string* frame); void Add_Length_and_Start_Delimiter(std::string* frame); +======= + const std::string& options = "00");*/ + //void Check_In_Messages_and_Transfer_To_Topics(); + void Display_Init_Communication_Failure(); + //void Convert_HEX_To_Bytes(const std::string& HEX_data, + //std::string* converted_data); + //void Calculate_and_Append_Checksum(std::string* frame); + //void Add_Length_and_Start_Delimiter(std::string* frame); +>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1 void Send_Mavlink_Message_Callback( const mavros_msgs::Mavlink::ConstPtr& mavlink_msg); void Display_Drone_Type_and_Running_Mode(DRONE_TYPE drone_type, @@ -85,15 +111,32 @@ private: bool Serve_Flight_Controller(mavros_msgs::CommandInt:: Request& request, mavros_msgs::CommandInt::Response& response); void Check_In_Messages_and_Transfer_To_Server(); +<<<<<<< HEAD unsigned short Caculate_Checksum(std::string* frame); void Send_multi_msg(); Mist::Xbee::SerialDevice serial_device_; Thread_Safe_Deque* in_messages_; +======= + void Process_In_Standard_Messages(); + void Process_In_Acks_and_Pings(); + void Process_In_Fragments(); + void Process_In_Packets(); + void Process_Command_Responses(); + + Mist::Xbee::SerialDevice serial_device_; + Mist::Xbee::PacketsHandler packets_handler_; + Thread_Safe_Deque in_std_messages_; + Thread_Safe_Deque in_fragments_; + Thread_Safe_Deque in_Acks_and_Pings_; + Thread_Safe_Deque command_responses_; + Thread_Safe_Deque in_packets_; +>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1 ros::NodeHandle node_handle_; ros::Subscriber mavlink_subscriber_; ros::Publisher mavlink_publisher_; ros::ServiceClient mav_dji_client_; ros::ServiceServer mav_dji_server_; +<<<<<<< HEAD /*No of robots*/ int no_of_dev; int device_id; @@ -114,6 +157,9 @@ private: uint64_t message_obtmulti[600]; //int test_1; //struct timeval t1, t2; +======= + std::shared_ptr service_thread_; // TO DO delete !? +>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1 }; diff --git a/include/MultithreadingDeque.hpp b/include/MultithreadingDeque.hpp index 8de0acf..6d0e80d 100644 --- a/include/MultithreadingDeque.hpp +++ b/include/MultithreadingDeque.hpp @@ -36,7 +36,11 @@ public: //**************************************************************************** +<<<<<<< HEAD void Push_Pack(const _T& new_data) +======= + void Push_Back(const _T& new_data) +>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1 { std::lock_guard guard(mutex_); deque_.push_back(new_data); diff --git a/include/PacketsHandler.h b/include/PacketsHandler.h new file mode 100644 index 0000000..a450350 --- /dev/null +++ b/include/PacketsHandler.h @@ -0,0 +1,152 @@ +/* PacketsHandler.h-- Packets Handler class for XBee: + Serialize, deserialize, fragment and reassemly mavlink + messages -- */ +/* ------------------------------------------------------------------------- */ +/* February 06, 2017 -- @Copyright Aymen Soussia. All rights reserved. */ +/* (aymen.soussia@gmail.com) */ + + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include"MultithreadingDeque.hpp" +#include"SerialDevice.h" + + +//***************************************************************************** +using boost::property_tree::ptree; + + +namespace Mist +{ + + +namespace Xbee +{ + + +//***************************************************************************** +class PacketsHandler +{ +public: + PacketsHandler(); + ~PacketsHandler(); + + bool Init(SerialDevice* serial_device, Thread_Safe_Deque* in_packets); + bool Init_Device_ID(); + void Run(); + void Handle_Mavlink_Message(const mavros_msgs::Mavlink::ConstPtr& mavlink_msg); + void Process_Fragment(std::shared_ptr fragment); + void Process_Ping_Or_Acknowledgement(std::shared_ptr frame); + void Process_Command_Response(const char* command_response); + void Quit(); + void Delete_Packets_With_Time_Out(); + void Deserialize_Mavlink_Message(const char * bytes, + mavros_msgs::Mavlink* mavlink_msg, const std::size_t msg_size); + + + +private: + const std::size_t MAX_PACEKT_SIZE; /* MAX packet size in bytes = 63750 bytes */ + const std::size_t XBEE_NETWORK_MTU; /* Maximum Transmission Unit of Xbee netwrok = 256 bytes (max payload) - 6 bytes (Header size of each fragment) = 250 bytes */ + const std::size_t FRAGMENT_HEADER_SIZE; /* Header size of each fragment = 6 bytes */ + const std::clock_t MAX_TIME_TO_SEND_PACKET; /* Maximum time before dropping a packet = 30 seconds*/ + const unsigned char START_DLIMITER; + + struct Reassembly_Packet_S + { + uint8_t packet_ID_; + std::string packet_buffer_; // TO DO make it shared ptr + std::set received_fragments_IDs_; + std::clock_t time_since_creation_; // TO DO use it to delete packets with time out + }; + + void Insert_Fragment_In_Packet_Buffer(std::string* buffer, + const char* fragment, const uint16_t offset, const std::size_t length); + void Add_New_Node_To_Network(const uint8_t new_node_address); + void Serialize_Mavlink_Message(const mavros_msgs::Mavlink::ConstPtr& + mavlink_msg, std::shared_ptr serialized_packet); + void Insert_Fragment_Header(bool single_fragment, + std::shared_ptr fragment, const uint8_t packet_ID, + const uint8_t fragment_ID, const uint16_t offset); + void Process_Out_Standard_Messages(); + void Process_Out_Packets(); + void Send_Packet(const Out_Packet_S& packet); + void Send_End_Of_Packet_Ping(const uint8_t packet_ID, const uint8_t total_NBR_of_fragments); + bool Load_Database_Addresses(); + bool Get_Local_Address(); + bool Check_Packet_Transmitted_To_All_Nodes(); + void Init_Network_Nodes_For_New_Transmission(const uint8_t packet_ID, + std::vector* frames, + std::shared_ptr>> packet); + void Transmit_Fragments(const std::vector& frames); + void Adjust_Optimum_MT_Number(const std::clock_t elapsed_time, + const std::size_t NBR_of_transmission); + void Send_SL_and_SH_Commands(); + void Generate_Transmit_Request_Frame( + const char* message, + std::string* frame, + const std::size_t message_size, + const unsigned char frame_ID = + static_cast(0x01), + const std::string& destination_adssress = "000000000000FFFF", + const std::string& short_destination_adress = "FFFF", + const std::string& broadcast_radius = "00", + const std::string& options = "00"); + void Convert_HEX_To_Bytes(const std::string& HEX_data, + std::string* converted_data); + void Calculate_and_Append_Checksum(std::string* frame); + void Add_Length_and_Start_Delimiter(std::string* frame); + void Generate_AT_Command(const char* command, + std::string* frame, + const unsigned char frame_ID = + static_cast(0x01)); + + struct On_Line_Node_S + { + bool received_hole_packet_; + std::string address_64_bits_; + }; + + std::set fragments_indexes_to_transmit_; + SerialDevice* serial_device_; + std::atomic quit_; + Thread_Safe_Deque out_std_messages_; + Thread_Safe_Deque_Of_Vectors out_packets_; + Thread_Safe_Deque* in_packets_; + std::map connected_network_nodes_; + std::map::iterator connected_network_nodes_it_; + std::map packets_assembly_map_; + std::map::iterator assembly_map_it_; + std::map database_addresses_; + std::map::iterator database_addresses_it_; + std::mutex mutex_; + uint8_t device_address_; + std::string device_64_bits_address_; + bool loaded_SL_; + bool loaded_SH_; + uint8_t current_processed_packet_ID_; + std::size_t optimum_MT_NBR_; + // TO DO & after auto !? + useconds_t delay_interframes_; +}; + + +} + + +} diff --git a/include/SerialDevice.h b/include/SerialDevice.h index 8d34947..0012c13 100644 --- a/include/SerialDevice.h +++ b/include/SerialDevice.h @@ -25,7 +25,18 @@ namespace Xbee { +<<<<<<< HEAD typedef MultithreadingDeque> Thread_Safe_Deque; +======= +struct Out_Packet_S +{ + uint8_t packet_ID_; + std::shared_ptr>> packet_buffer_; +}; + +typedef MultithreadingDeque> Thread_Safe_Deque; +typedef MultithreadingDeque Thread_Safe_Deque_Of_Vectors; +>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1 //***************************************************************************** @@ -39,7 +50,17 @@ public: void Send_Frame(const std::string& frame); void Run_Service(); void Stop_Service(); +<<<<<<< HEAD Thread_Safe_Deque* Get_In_Messages_Pointer(); +======= + void Set_In_Messages_Pointers(Thread_Safe_Deque* in_std_messages, + Thread_Safe_Deque* in_fragments, + Thread_Safe_Deque* in_Acks_and_Pings, + Thread_Safe_Deque* command_responses); + bool Is_IO_Service_Stopped(); // TO DO delete this function + void Reset_IO_Service(); // TO DO delete this function + void Close_Serial_Port(); +>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1 private: @@ -71,7 +92,14 @@ private: boost::asio::io_service io_service_; boost::asio::serial_port serial_port_; std::deque out_messages_; +<<<<<<< HEAD Thread_Safe_Deque in_messages_; +======= + Thread_Safe_Deque* in_std_messages_; + Thread_Safe_Deque* in_fragments_; + Thread_Safe_Deque* in_Acks_and_Pings_; + Thread_Safe_Deque* command_responses_; +>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1 Mist::Xbee::Frame current_frame_; unsigned int FRAME_TYPE_KEYS[REMOTE_AT_COMMAND_RESPONSE + 1]; }; diff --git a/include/XBeeFrame.h b/include/XBeeFrame.h index 5512385..c7073b0 100644 --- a/include/XBeeFrame.h +++ b/include/XBeeFrame.h @@ -27,7 +27,11 @@ public: ~Frame(); enum {FRAME_HEADER_LENGTH = 4}; +<<<<<<< HEAD enum {MAX_FRAME_BODY_LENGTH = 280}; // TO DO check value +======= + enum {MAX_FRAME_BODY_LENGTH = 270}; // TO DO check value +>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1 const char* Get_Frame_Data() const; char* Get_Frame_Data(); @@ -39,6 +43,10 @@ public: std::size_t Get_Frame_Type() const; int Get_Start_Delimiter_Position(); void Rearrange_Corrupted_Header(); +<<<<<<< HEAD +======= + char Get_Message_Type(); // TO DO const !? +>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1 private: diff --git a/launch/xbeemav.launch b/launch/xbeemav.launch index 8a448ab..e2b9730 100644 --- a/launch/xbeemav.launch +++ b/launch/xbeemav.launch @@ -7,7 +7,10 @@ +<<<<<<< HEAD +======= +>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1 diff --git a/src/CommunicationManager.cpp b/src/CommunicationManager.cpp index 3d779f9..6385972 100644 --- a/src/CommunicationManager.cpp +++ b/src/CommunicationManager.cpp @@ -9,6 +9,7 @@ #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; @@ -33,6 +34,8 @@ uint16_t get_deviceid(){ return (uint16_t)id; } +======= +>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1 namespace Mist { @@ -44,7 +47,11 @@ namespace Xbee //***************************************************************************** CommunicationManager::CommunicationManager(): START_DLIMITER(static_cast(0x7E)), +<<<<<<< HEAD LOOP_RATE(50) /* 10 fps */ +======= + LOOP_RATE(10) /* 10 fps */ +>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1 { } @@ -61,14 +68,46 @@ 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_); + + service_thread_ = std::make_shared(std::thread(&SerialDevice::Run_Service, &serial_device_)); + + if (!packets_handler_.Init(&serial_device_, &in_packets_)) + return false; + + std::clock_t elapsed_time = std::clock(); + bool device_ID_loaded = false; + + while (std::clock() - elapsed_time <= 30000) + { + Process_Command_Responses(); + + if (packets_handler_.Init_Device_ID()) + { + device_ID_loaded = true; + break; + } + } + + if (!device_ID_loaded) + return false; +>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1 } else { Display_Init_Communication_Failure(); return false; } +<<<<<<< HEAD +======= + + return true; +>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1 } @@ -77,7 +116,11 @@ 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); @@ -87,7 +130,14 @@ 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 } @@ -101,7 +151,11 @@ 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 @@ -159,6 +213,7 @@ 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*/ @@ -176,6 +231,21 @@ void CommunicationManager::Run_In_Swarm_Mode() Send_multi_msg(); ros::spinOnce(); loop_rate.sleep(); +======= + if (success_1 && success_2) + { + ros::Rate loop_rate(LOOP_RATE); + + while (ros::ok()) + { + Process_In_Standard_Messages(); + Process_In_Fragments(); + Process_In_Acks_and_Pings(); + Process_In_Packets(); + packets_handler_.Delete_Packets_With_Time_Out(); + ros::spinOnce(); + loop_rate.sleep(); +>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1 } } } @@ -185,12 +255,20 @@ 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: { @@ -234,9 +312,15 @@ 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; } @@ -257,10 +341,16 @@ 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, @@ -268,7 +358,11 @@ inline void CommunicationManager::Generate_Transmit_Request_Frame( const std::string& options) { const unsigned char FAME_TYPE = static_cast(0x10); /* Transmit Request Frame */ +<<<<<<< HEAD std::string frame_parameters; +======= + /*std::string frame_parameters; +>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1 std::string bytes_frame_parameters; frame_parameters.append(destination_adssress); @@ -281,6 +375,7 @@ 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); @@ -290,6 +385,17 @@ inline void CommunicationManager::Generate_Transmit_Request_Frame( //***************************************************************************** inline void CommunicationManager::Convert_HEX_To_Bytes( +======= + frame->append(message, std::strlen(message)); + + Calculate_and_Append_Checksum(frame); + Add_Length_and_Start_Delimiter(frame); +}*/ + + +//***************************************************************************** +/*inline void CommunicationManager::Convert_HEX_To_Bytes( +>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1 const std::string& HEX_data, std::string* converted_data) { const unsigned short TWO_BYTES = 2; @@ -305,11 +411,19 @@ inline void CommunicationManager::Convert_HEX_To_Bytes( temporary_char = static_cast(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; @@ -324,6 +438,7 @@ inline void CommunicationManager::Calculate_and_Append_Checksum( checksum = 0xFF - lowest_8_bits; checksum_byte = static_cast(checksum); frame->push_back(checksum_byte); +<<<<<<< HEAD } unsigned short CommunicationManager::Caculate_Checksum(std::string* frame) @@ -345,6 +460,13 @@ 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; @@ -355,7 +477,11 @@ 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); @@ -364,6 +490,7 @@ inline void CommunicationManager::Add_Length_and_Start_Delimiter( temporary_char = static_cast(Hex_frame_length_2); header.push_back(temporary_char); frame->insert(0, header); +<<<<<<< HEAD } @@ -386,11 +513,26 @@ inline void CommunicationManager::Check_In_Messages_and_Transfer_To_Topics() counter--; } uint64_t current_int64 = 0; +======= +}*/ + + +//***************************************************************************** +/*inline void CommunicationManager::Check_In_Messages_and_Transfer_To_Topics() // TO DO delete +{ + std::size_t size_in_messages = in_messages_->Get_Size(); + + if (size_in_messages > 0) + { + uint64_t current_int64 = 0; + +>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1 for (std::size_t j = 0; j < size_in_messages; j++) { std::shared_ptr in_message = in_messages_->Pop_Front(); mavros_msgs::Mavlink mavlink_msg; +<<<<<<< HEAD int tot = 0; //uint64_t header=0; /*Copy the header*/ @@ -565,6 +707,33 @@ 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++) + { + if (' ' == in_message->at(i) || 0 == i) + { + sscanf(in_message->c_str() + i, "%" PRIu64 " ", + ¤t_int64); + mavlink_msg.payload64.push_back(current_int64); + } + + } + + mavlink_publisher_.publish(mavlink_msg); + } + + } +}*/ + + +//***************************************************************************** +inline void CommunicationManager::Check_In_Messages_and_Transfer_To_Server() // TO DO change it +{ + //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++) { @@ -596,7 +765,11 @@ inline void CommunicationManager::Check_In_Messages_and_Transfer_To_Server() ROS_INFO("XBeeMav: Faild to Tranfer Command."); } +<<<<<<< HEAD } +======= + }*/ +>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1 } @@ -604,6 +777,7 @@ 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]; @@ -820,6 +994,12 @@ void CommunicationManager::Send_multi_msg(){ } } +======= + 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( @@ -837,9 +1017,110 @@ void CommunicationManager::Display_Drone_Type_and_Running_Mode( } +<<<<<<< HEAD } } +======= +//***************************************************************************** +void CommunicationManager::Process_In_Standard_Messages() +{ + std::size_t in_messages_size = in_std_messages_.Get_Size(); + + if (in_messages_size > 0) + { + for (std::size_t j = 0; j < in_messages_size; j++) + { + std::shared_ptr in_message = + in_std_messages_.Pop_Front(); + mavros_msgs::Mavlink mavlink_msg; + + packets_handler_.Deserialize_Mavlink_Message(in_message->c_str(), &mavlink_msg, in_message->size()); + mavlink_publisher_.publish(mavlink_msg); + } + } +} + + +//***************************************************************************** +void CommunicationManager::Process_In_Acks_and_Pings() +{ + std::size_t in_messages_size = in_Acks_and_Pings_.Get_Size(); + + if (in_messages_size > 0) + { + for (std::size_t j = 0; j < in_messages_size; j++) + { + std::shared_ptr in_message = + in_Acks_and_Pings_.Pop_Front(); + + packets_handler_.Process_Ping_Or_Acknowledgement(in_message); + } + } +} + + +//***************************************************************************** +void CommunicationManager::Process_In_Fragments() +{ + std::size_t in_messages_size = in_fragments_.Get_Size(); + + if (in_messages_size > 0) + { + for (std::size_t j = 0; j < in_messages_size; j++) + { + std::shared_ptr in_message = + in_fragments_.Pop_Front(); + + packets_handler_.Process_Fragment(in_message); + } + } +} + + +//***************************************************************************** +void CommunicationManager::Process_In_Packets() +{ + std::size_t in_messages_size = in_packets_.Get_Size(); + + if (in_messages_size > 0) + { + for (std::size_t j = 0; j < in_messages_size; j++) + { + std::shared_ptr in_message = + in_packets_.Pop_Front(); + mavros_msgs::Mavlink mavlink_msg; + + packets_handler_.Deserialize_Mavlink_Message(in_message->c_str(), &mavlink_msg, in_message->size()); + mavlink_publisher_.publish(mavlink_msg); + } + } +} + + +//***************************************************************************** +void CommunicationManager::Process_Command_Responses() +{ + std::size_t in_messages_size = command_responses_.Get_Size(); + + if (in_messages_size > 0) + { + for (std::size_t j = 0; j < in_messages_size; j++) + { + std::shared_ptr in_message = + command_responses_.Pop_Front(); + + packets_handler_.Process_Command_Response(in_message->c_str()); + } + } +} + + +} + + +} +>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1 diff --git a/src/PacketsHandler.cpp b/src/PacketsHandler.cpp new file mode 100644 index 0000000..94c6525 --- /dev/null +++ b/src/PacketsHandler.cpp @@ -0,0 +1,778 @@ +/* PacketsHandler.cpp -- Packets Handler class for XBee: + Serialize, deserialize, fragment and reassemly mavlink + messages (packets and std messages) -- */ +/* ------------------------------------------------------------------------- */ +/* February 06, 2017 -- @Copyright Aymen Soussia. All rights reserved. */ +/* (aymen.soussia@gmail.com) */ + + +#include"PacketsHandler.h" + + +namespace Mist +{ + + +namespace Xbee +{ + + +//***************************************************************************** +PacketsHandler::PacketsHandler(): + MAX_PACEKT_SIZE(64000), + XBEE_NETWORK_MTU(250), + FRAGMENT_HEADER_SIZE(6), + MAX_TIME_TO_SEND_PACKET(30000), + START_DLIMITER(static_cast(0x7E)), + device_64_bits_address_("12345678"), + loaded_SL_(false), + loaded_SH_(false), + optimum_MT_NBR_(3), + delay_interframes_(100 * 1000) +{ +} + + +//***************************************************************************** +PacketsHandler::~PacketsHandler() +{ +} + + +//***************************************************************************** +bool PacketsHandler::Init(SerialDevice* serial_device, + Thread_Safe_Deque* in_packets) +{ + serial_device_ = serial_device; + + if (!Load_Database_Addresses()) + return false; + + // TO DO node dicov + + Send_SL_and_SH_Commands(); + in_packets_ = in_packets; + + return true; +} + + +//***************************************************************************** +bool PacketsHandler::Init_Device_ID() +{ + if (Get_Local_Address()) + return true; + else + return false; +} + + +//***************************************************************************** +void PacketsHandler::Run() +{ + quit_.store(false); + + while (!quit_.load()) + { + Process_Out_Standard_Messages(); + Process_Out_Packets(); + } +} + + +//***************************************************************************** +void PacketsHandler::Handle_Mavlink_Message(const mavros_msgs::Mavlink::ConstPtr& mavlink_msg) +{ + std::shared_ptr serialized_packet = + std::make_shared(); + + Serialize_Mavlink_Message(mavlink_msg, serialized_packet); + + if (serialized_packet->size() > XBEE_NETWORK_MTU && serialized_packet->size() <= MAX_PACEKT_SIZE) + { + std::shared_ptr>> fragmented_packet = + std::make_shared>>(); + + std::size_t offset = 0; + std::size_t NBR_of_bytes = 0; + std::size_t NBR_of_fragments = std::ceil( + static_cast(serialized_packet->size()) / XBEE_NETWORK_MTU); + + for (uint8_t i = 0; i < NBR_of_fragments; i++) + { + fragmented_packet->push_back(std::make_shared()); + NBR_of_bytes = std::min(XBEE_NETWORK_MTU, serialized_packet->size() - offset); + Insert_Fragment_Header(false, fragmented_packet->at(i), mavlink_msg->msgid, i, offset); + fragmented_packet->at(i)->append(serialized_packet->c_str() + offset, NBR_of_bytes); + offset += NBR_of_bytes; + } + + out_packets_.Push_Back({mavlink_msg->msgid, fragmented_packet}); + } + else if (serialized_packet->size() < XBEE_NETWORK_MTU) + { + serialized_packet->insert(0, 1, 'S'); + out_std_messages_.Push_Back(serialized_packet); + } +} + + +//***************************************************************************** +void PacketsHandler::Process_Fragment(std::shared_ptr fragment) +{ + uint8_t packet_ID = fragment->at(1); + uint8_t node_8_bits_address = fragment->at(2); + uint8_t fragment_ID = fragment->at(3); + uint16_t offset = static_cast( + static_cast(static_cast(fragment->at(4))) << 8 | + static_cast(static_cast(fragment->at(5)))); + + assembly_map_it_ = packets_assembly_map_.find(node_8_bits_address); + + if (assembly_map_it_ != packets_assembly_map_.end()) + { + if (assembly_map_it_->second.packet_ID_ == packet_ID) + { + std::set::iterator it = assembly_map_it_->second.received_fragments_IDs_.find(fragment_ID); + + if (it == assembly_map_it_->second.received_fragments_IDs_.end()) + { + if (assembly_map_it_->second.received_fragments_IDs_.size() == 0) + assembly_map_it_->second.time_since_creation_ = std::clock(); + + Insert_Fragment_In_Packet_Buffer(&assembly_map_it_->second.packet_buffer_, fragment->c_str(), offset, fragment->size()); + assembly_map_it_->second.received_fragments_IDs_.insert(fragment_ID); + } + } + else + { + assembly_map_it_->second = {}; + assembly_map_it_->second.packet_ID_ = packet_ID; + Insert_Fragment_In_Packet_Buffer(&assembly_map_it_->second.packet_buffer_, fragment->c_str(), offset, fragment->size()); + assembly_map_it_->second.received_fragments_IDs_.insert(fragment_ID); + assembly_map_it_->second.time_since_creation_ = std::clock(); + } + } + else + { + Add_New_Node_To_Network(node_8_bits_address); + assembly_map_it_ = packets_assembly_map_.find(node_8_bits_address); + assembly_map_it_->second.packet_ID_ = packet_ID; + Insert_Fragment_In_Packet_Buffer(&assembly_map_it_->second.packet_buffer_, fragment->c_str(), offset, fragment->size()); + assembly_map_it_->second.received_fragments_IDs_.insert(fragment_ID); + assembly_map_it_->second.time_since_creation_ = std::clock(); + } +} + + +//***************************************************************************** +void PacketsHandler::Insert_Fragment_In_Packet_Buffer(std::string* buffer, const char* fragment, const uint16_t offset, const std::size_t length) // TO DO delete length +{ + if (offset >= buffer->size()) + buffer->append(fragment + FRAGMENT_HEADER_SIZE, length - FRAGMENT_HEADER_SIZE); + else + buffer->insert(offset, fragment + FRAGMENT_HEADER_SIZE, length - FRAGMENT_HEADER_SIZE); +} + + +//***************************************************************************** +void PacketsHandler::Process_Ping_Or_Acknowledgement(std::shared_ptr frame) // TO DO change useless std::shared_ptr frame args to ->c_str() +{ + uint8_t packet_ID = frame->at(12); + uint8_t node_8_bits_address = frame->at(13); + + if (frame->at(11) == 'A') + { + /*std::lock_guard guard(mutex_); + + connected_network_nodes_it_ = connected_network_nodes_.find(node_8_bits_address); + + if (connected_network_nodes_it_ == connected_network_nodes_.end()) + { + Add_New_Node_To_Network(node_8_bits_address); + connected_network_nodes_it_ = connected_network_nodes_.find(node_8_bits_address); + } + + if (packet_ID == current_processed_packet_ID_) + { + if (frame->size() < 15) + connected_network_nodes_it_->second = true; + else + { + for (uint8_t i = 14; i < frame->size(); i++) + fragments_indexes_to_transmit_.insert(frame->at(i)); + } + }*/ + } + else if (frame->at(11) == 'P') + { + assembly_map_it_ = packets_assembly_map_.find(node_8_bits_address); + + if (assembly_map_it_ == packets_assembly_map_.end()) + { + Add_New_Node_To_Network(node_8_bits_address); + assembly_map_it_ = packets_assembly_map_.find(node_8_bits_address); + assembly_map_it_->second.packet_ID_ = packet_ID; + assembly_map_it_->second.time_since_creation_ = std::clock(); + } + + if (assembly_map_it_->second.packet_ID_ == packet_ID) + { + std::string Acknowledgement = "A"; + Acknowledgement.push_back(packet_ID); + Acknowledgement.push_back(device_address_); + uint8_t packet_size = frame->at(14); + + if (assembly_map_it_->second.received_fragments_IDs_.size() == packet_size) + { + in_packets_->Push_Back(std::make_shared(assembly_map_it_->second.packet_buffer_)); + assembly_map_it_->second.packet_buffer_.clear(); + assembly_map_it_->second.received_fragments_IDs_.clear(); + assembly_map_it_->second.time_since_creation_ = 0; + } + else + { + std::set::iterator it = assembly_map_it_->second.received_fragments_IDs_.begin(); + uint8_t j = 0; + + while (j <= packet_size - 1) + { + if (j != *it) + Acknowledgement.push_back(j); + else if (it != std::prev(assembly_map_it_->second.received_fragments_IDs_.end())) + it++; + + j++; + } + } + + std::string Ack_frame; + Generate_Transmit_Request_Frame(Acknowledgement.c_str(), &Ack_frame, Acknowledgement.size()); // TO Do send as unicast + serial_device_->Send_Frame(Ack_frame); + usleep(delay_interframes_); + + } + else + { + assembly_map_it_->second = {}; + assembly_map_it_->second.packet_ID_ = packet_ID; + } + } +} + + +//***************************************************************************** +void PacketsHandler::Add_New_Node_To_Network(const uint8_t new_node_address) +{ + std::set empty_set; + packets_assembly_map_.insert(std::pair(new_node_address, {})); + + std::lock_guard guard(mutex_); + connected_network_nodes_.insert(std::pair(new_node_address, false)); +} + + +//***************************************************************************** +void PacketsHandler::Process_Command_Response(const char* command_response) +{ + if (command_response[0] == 'N' && command_response[1] == 'D') + { + uint64_t new_node_address = 0; + std::lock_guard guard(mutex_); + + if (command_response[2] == static_cast(0)) // TO DO check this + { + new_node_address = static_cast( // TO DO correct this + static_cast(command_response[5]) << 56 | + static_cast(command_response[6]) << 48 | + static_cast(command_response[7]) << 40 | + static_cast(command_response[8]) << 32 | + static_cast(command_response[9]) << 24 | + static_cast(command_response[10]) << 16 | + static_cast(command_response[11]) << 8 | + static_cast(command_response[12])); + } + + database_addresses_it_ = database_addresses_.find(new_node_address); + + if (database_addresses_it_ != database_addresses_.end()) + device_address_ = database_addresses_it_->second; + else + std::cout << "Remote Node Not in Database" << std::endl; + } + else if (command_response[0] == 'S' && command_response[1] == 'H') + { + if (command_response[2] == static_cast(0)) // TO DO check this + { + loaded_SH_ = true; + + for (std::size_t i = 0; i < 4; i++) + device_64_bits_address_[i] = command_response[3 + i]; + } + } + else if (command_response[0] == 'S' && command_response[1] == 'L') + { + if (command_response[2] == static_cast(0)) // TO DO check this + { + loaded_SL_ = true; + + for (std::size_t i = 0; i < 4; i++) + device_64_bits_address_[4 + i] = command_response[3 + i]; + } + } +} + + +//***************************************************************************** +void PacketsHandler::Quit() +{ + quit_.store(true); +} + + +//***************************************************************************** +void PacketsHandler::Serialize_Mavlink_Message(const mavros_msgs::Mavlink::ConstPtr& + mavlink_msg, std::shared_ptr serialized_packet) +{ + serialized_packet->push_back(mavlink_msg->sysid); + serialized_packet->push_back(mavlink_msg->msgid); + + std::string bytes="12345678"; + + for (std::size_t j = 0; j < mavlink_msg->payload64.size(); j++) + { + for (std::size_t i = 0; i < 8; i++) + bytes[7 - i] = (mavlink_msg->payload64.at(j) >> (i * 8)); + + serialized_packet->append(bytes); + } +} + + +//***************************************************************************** +void PacketsHandler::Insert_Fragment_Header(bool single_fragment, + std::shared_ptr fragment, const uint8_t packet_ID, + const uint8_t fragment_ID, const uint16_t offset) +{ + if (!single_fragment) + { + fragment->push_back('F'); + fragment->push_back(packet_ID); + fragment->push_back(device_address_); + fragment->push_back(fragment_ID); + fragment->push_back(offset >> (1 * 8)); + fragment->push_back(offset >> (0 * 8)); + } + else + fragment->push_back('S'); +} + + +//***************************************************************************** +void PacketsHandler::Delete_Packets_With_Time_Out() +{ + for(auto& iterator: packets_assembly_map_) + { + if (std::clock_t() - iterator.second.time_since_creation_ > MAX_TIME_TO_SEND_PACKET && iterator.second.time_since_creation_ != 0) + iterator.second = {}; + } +} + + +//***************************************************************************** +void PacketsHandler::Process_Out_Standard_Messages() +{ + std::size_t deque_size = out_std_messages_.Get_Size(); + + if (deque_size > 0) + { + std::string frame; + std::shared_ptr out_message; + + for (std::size_t i = 0; i < deque_size; i++) + { + frame.clear(); + out_message = out_std_messages_.Pop_Front(); + + Generate_Transmit_Request_Frame(out_message->c_str(), &frame, out_message->size()); + serial_device_->Send_Frame(frame); + usleep(delay_interframes_); + } + } +} + + +//***************************************************************************** +void PacketsHandler::Process_Out_Packets() +{ + std::size_t deque_size = out_packets_.Get_Size(); + + if (deque_size > 0) + { + Out_Packet_S out_packet; + + for (std::size_t i = 0; i < deque_size; i++) + { + Process_Out_Standard_Messages(); + out_packet = out_packets_.Pop_Front(); + + Send_Packet(out_packet); + } + } +} + + +//***************************************************************************** +void PacketsHandler::Send_Packet(const Out_Packet_S& packet) +{ + std::size_t NBR_of_transmission = 0; + std::vector frames; + + Init_Network_Nodes_For_New_Transmission(packet.packet_ID_, &frames, packet.packet_buffer_); + + std::clock_t elapsed_time = std::clock(); + + //while (NBR_of_transmission <= optimum_MT_NBR_ && !Check_Packet_Transmitted_To_All_Nodes()) + //{ + //NBR_of_transmission++; + + Transmit_Fragments(frames); + + // TO DO usleep(flow_control_time) + // TO DO clear fragments_IDs_to_transmit_set + Send_End_Of_Packet_Ping(packet.packet_ID_, packet.packet_buffer_->size()); + // TO DO sleep after ping + //} + + elapsed_time = std::clock() - elapsed_time; + Adjust_Optimum_MT_Number(elapsed_time, NBR_of_transmission); +} + + +//***************************************************************************** +void PacketsHandler::Send_End_Of_Packet_Ping(const uint8_t packet_ID, const uint8_t total_NBR_of_fragments) +{ + std::string ping_message = "P"; + std::string ping_frame; + + ping_message.push_back(packet_ID); + ping_message.push_back(device_address_); + ping_message.push_back(total_NBR_of_fragments); + + Generate_Transmit_Request_Frame(ping_message.c_str(), &ping_frame, ping_message.size()); + serial_device_->Send_Frame(ping_frame); + usleep(delay_interframes_); +} + + +//***************************************************************************** +bool PacketsHandler::Load_Database_Addresses() +{ + const std::string FILE_PATH = "/home/aymen/catkin_ws/src/xbee_ros_node/Resources/database.xml"; + + if (!boost::filesystem::exists(FILE_PATH)) + { + std::cout << "database.xml Not Found." << std::endl; + return false; + } + + ptree pt; + boost::property_tree::read_xml(FILE_PATH, pt); + std::string short_address; + std::string address_64_bits; + unsigned int short_address_int; + uint64_t address_64_bits_int; + + BOOST_FOREACH(ptree::value_type const&v, pt.get_child("Addresses")) + { + if (v.first == "Device") + { + short_address = v.second.get(".Address"); + address_64_bits = v.second.data(); + + if (sscanf(short_address.c_str(), "%3u", &short_address_int) < 0) + { + std::cout << "Short Address Error. Please Check database.xml For Possible Errors." << std::endl; + return false; + } + + if (sscanf(address_64_bits.c_str(), "%" SCNx64, &address_64_bits_int) < 0) + { + std::cout << "64 bits Address Error. Please Check database.xml For Possible Errors." << std::endl; + return false; + } + + database_addresses_.insert(std::pair(address_64_bits_int, static_cast(short_address_int))); + } + } + + return true; +} + + +//***************************************************************************** +bool PacketsHandler::Get_Local_Address() +{ + const useconds_t ONE_SECOND = 1*1000*1000; /* 1s = 1 * 10⁶ microseconds. */ + usleep(ONE_SECOND); + + if (loaded_SH_ && loaded_SL_) + { + uint64_t local_64_bits_address = ( + static_cast(static_cast(device_64_bits_address_[0])) << 56 | + static_cast(static_cast(device_64_bits_address_[1])) << 48 | + static_cast(static_cast(device_64_bits_address_[2])) << 40 | + static_cast(static_cast(device_64_bits_address_[3])) << 32 | + static_cast(static_cast(device_64_bits_address_[4])) << 24 | + static_cast(static_cast(device_64_bits_address_[5])) << 16 | + static_cast(static_cast(device_64_bits_address_[6])) << 8 | + static_cast(static_cast(device_64_bits_address_[7]))); + + database_addresses_it_ = database_addresses_.find(local_64_bits_address); + + if (database_addresses_it_ != database_addresses_.end()) + { + device_address_ = database_addresses_it_->second; + std::cout << "Loaded Short Device Address : " << static_cast(device_address_) << std::endl; + return true; + } + else + { + std::cout << "Local Address Not Found In Database. Please Add The Xbee Address to database.xml." << std::endl; + return false; + } + } + else + { + Send_SL_and_SH_Commands(); + return false; + } +} + + +//***************************************************************************** +bool PacketsHandler::Check_Packet_Transmitted_To_All_Nodes() +{ + std::lock_guard guard(mutex_); + + if (connected_network_nodes_.size() == 0) + return false; + + for (auto it : connected_network_nodes_) + { + if (!it.second) + return false; + } + + return true; +} + + +//***************************************************************************** +void PacketsHandler::Init_Network_Nodes_For_New_Transmission(const uint8_t packet_ID, std::vector* frames, std::shared_ptr>> packet) +{ + std::lock_guard guard(mutex_); + fragments_indexes_to_transmit_.clear(); + + for (auto it : connected_network_nodes_) + it.second = false; + + current_processed_packet_ID_ = packet_ID; + + for (uint8_t i = 0; i < packet->size(); i++) + { + frames->push_back(""); + fragments_indexes_to_transmit_.insert(i); + Generate_Transmit_Request_Frame(packet->at(i)->c_str(), &frames->at(i), packet->at(i)->size()); + } +} + + +//***************************************************************************** +void PacketsHandler::Transmit_Fragments(const std::vector& frames) +{ + std::lock_guard guard(mutex_); + + for (auto index: fragments_indexes_to_transmit_) + { + serial_device_->Send_Frame(frames.at(index)); + usleep(delay_interframes_); + } + +} + + +//***************************************************************************** +void PacketsHandler::Adjust_Optimum_MT_Number(const std::clock_t elapsed_time, + const std::size_t NBR_of_transmission) +{ + if (NBR_of_transmission < optimum_MT_NBR_) + optimum_MT_NBR_ = NBR_of_transmission; + else if (NBR_of_transmission == optimum_MT_NBR_) + { + std::lock_guard guard(mutex_); + + if (elapsed_time <= MAX_TIME_TO_SEND_PACKET && fragments_indexes_to_transmit_.size() > 0) + optimum_MT_NBR_++; + } +} + + +//***************************************************************************** +void PacketsHandler::Send_SL_and_SH_Commands() +{ + std::string command; + std::string frame; + + command = "SL"; + Generate_AT_Command(command.c_str(), &frame); + serial_device_->Send_Frame(frame); + + command = "SH"; + frame = ""; + Generate_AT_Command(command.c_str(), &frame); + serial_device_->Send_Frame(frame); +} + + +//***************************************************************************** +void PacketsHandler::Deserialize_Mavlink_Message(const char * bytes, + mavros_msgs::Mavlink* mavlink_msg, const std::size_t msg_size) +{ + mavlink_msg->sysid = bytes[0]; + mavlink_msg->msgid = bytes[1]; + uint64_t current_int64 = 0; + + for (std::size_t i = 2; i < msg_size; i += 8) + { + current_int64 = ( + static_cast(static_cast(bytes[i])) << 56 | + static_cast(static_cast(bytes[i + 1])) << 48 | + static_cast(static_cast(bytes[i + 2])) << 40 | + static_cast(static_cast(bytes[i + 3])) << 32 | + static_cast(static_cast(bytes[i + 4])) << 24 | + static_cast(static_cast(bytes[i + 5])) << 16 | + static_cast(static_cast(bytes[i + 6])) << 8 | + static_cast(static_cast(bytes[i + 7]))); + + mavlink_msg->payload64.push_back(current_int64); + } +} + + +//***************************************************************************** +inline void PacketsHandler::Generate_Transmit_Request_Frame( + const char* message, + std::string * frame, + const std::size_t message_size, + const unsigned char frame_ID, + const std::string& destination_adssress, + const std::string& short_destination_adress, + const std::string& broadcast_radius, + const std::string& options) +{ + const unsigned char FAME_TYPE = static_cast(0x10); /* Transmit Request Frame */ + std::string frame_parameters; + std::string bytes_frame_parameters; + + frame_parameters.append(destination_adssress); + frame_parameters.append(short_destination_adress); + frame_parameters.append(broadcast_radius); + frame_parameters.append(options); + + Convert_HEX_To_Bytes(frame_parameters, &bytes_frame_parameters); + + frame->push_back(FAME_TYPE); + frame->push_back(frame_ID); + frame->append(bytes_frame_parameters); + frame->append(message, message_size); + + Calculate_and_Append_Checksum(frame); + Add_Length_and_Start_Delimiter(frame); +} + + +//***************************************************************************** +inline void PacketsHandler::Add_Length_and_Start_Delimiter( + std::string* frame) +{ + const unsigned short FIVE_BYTES = 5; + char temporary_buffer[FIVE_BYTES]; + unsigned char temporary_char; + int Hex_frame_length_1; + int Hex_frame_length_2; + std::string header; + int frame_length = frame->size() - 1; /* frame_length = frame_size - checksum byte */ + + header.push_back(START_DLIMITER); + sprintf(temporary_buffer, "%04X", frame_length); + sscanf(temporary_buffer, "%02X%02X", &Hex_frame_length_1, + &Hex_frame_length_2); + temporary_char = static_cast(Hex_frame_length_1); + header.push_back(temporary_char); + temporary_char = static_cast(Hex_frame_length_2); + header.push_back(temporary_char); + frame->insert(0, header); +} + + +//***************************************************************************** +inline void PacketsHandler::Calculate_and_Append_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(frame->at(i)); + + lowest_8_bits = bytes_sum & 0xFF; + checksum = 0xFF - lowest_8_bits; + checksum_byte = static_cast(checksum); + frame->push_back(checksum_byte); +} + + +//***************************************************************************** +inline void PacketsHandler::Convert_HEX_To_Bytes( + const std::string& HEX_data, std::string* converted_data) +{ + const unsigned short TWO_BYTES = 2; + char temporary_buffer[TWO_BYTES]; + unsigned char temporary_char; + int temporary_HEX; + + for (unsigned short i = 0; i <= HEX_data.size() - TWO_BYTES; + i += TWO_BYTES) + { + memcpy(temporary_buffer, HEX_data.c_str() + i, TWO_BYTES); + sscanf(temporary_buffer, "%02X", &temporary_HEX); + temporary_char = static_cast(temporary_HEX); + converted_data->push_back(temporary_char); + } +} + + +//***************************************************************************** +void PacketsHandler::Generate_AT_Command(const char* command, + std::string* frame, + const unsigned char frame_ID) +{ + const unsigned char FAME_TYPE = static_cast(0x09);/* AT Command Frame */ + std::string temporary_parameter_value; + + frame->push_back(FAME_TYPE); + frame->push_back(frame_ID); + frame->append(command); + + Calculate_and_Append_Checksum(frame); + Add_Length_and_Start_Delimiter(frame); +} + + +} + + +} diff --git a/src/SerialDevice.cpp b/src/SerialDevice.cpp index 33dc7a5..4735352 100644 --- a/src/SerialDevice.cpp +++ b/src/SerialDevice.cpp @@ -39,7 +39,10 @@ bool SerialDevice::Init( { Set_Port_Options(baud_rate); Init_Frame_Type_Keys(); +<<<<<<< HEAD Read_Frame_Header(); +======= +>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1 return true; } else @@ -82,6 +85,10 @@ void SerialDevice::Send_Frame(const std::string& frame) //***************************************************************************** void SerialDevice::Run_Service() { +<<<<<<< HEAD +======= + Read_Frame_Header(); +>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1 io_service_.run(); } @@ -90,14 +97,35 @@ 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, + Thread_Safe_Deque* command_responses) +{ + in_std_messages_ = in_std_messages; + in_fragments_ = in_fragments; + in_Acks_and_Pings_ = in_Acks_and_Pings; + command_responses_ = command_responses; +>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1 } @@ -156,8 +184,15 @@ 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 { @@ -183,6 +218,7 @@ 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; @@ -192,6 +228,44 @@ void SerialDevice::Read_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 in_message = + std::make_shared(); + + if (msg_type == 'F') + { + in_message->append(current_frame_.Get_Frame_Body() + + 11, + current_frame_.Get_Frame_Body_Length() - 12); + in_fragments_->Push_Back(in_message); + } + + else if (msg_type == 'A' || msg_type == 'P') + { + in_message->append(current_frame_.Get_Frame_Body(), + current_frame_.Get_Frame_Body_Length() - 1); + in_Acks_and_Pings_->Push_Back(in_message); + } + + else if (msg_type == 'S') + { + in_message->append(current_frame_.Get_Frame_Body() + 12, + current_frame_.Get_Frame_Body_Length() - 13); + in_std_messages_->Push_Back(in_message); + } + } + else if (current_frame_.Get_Frame_Type() == + FRAME_TYPE_KEYS[AT_COMMAND_RESPONSE]) + { + std::shared_ptr in_message = + std::make_shared(); + + in_message->append(current_frame_.Get_Frame_Body() + 1, + current_frame_.Get_Frame_Body_Length() - 2); + + command_responses_->Push_Back(in_message); +>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1 } Read_Frame_Header(); @@ -226,6 +300,23 @@ void SerialDevice::Write_Frame() } +<<<<<<< HEAD +======= +//***************************************************************************** +bool SerialDevice::Is_IO_Service_Stopped() +{ + return io_service_.stopped(); +} + + +//***************************************************************************** +void SerialDevice::Reset_IO_Service() +{ + io_service_.reset(); +} + + +>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1 } diff --git a/src/TestBuzz.cpp b/src/TestBuzz.cpp index 1337742..b39c9a8 100644 --- a/src/TestBuzz.cpp +++ b/src/TestBuzz.cpp @@ -47,15 +47,23 @@ 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("outMavlink", 1000); ros::Subscriber mavlink_sub = node_handle.subscribe("inMavlink", 1000, Receive_Payload_Callback); +<<<<<<< HEAD ros::Rate loop_rate(0.2); Init_Random_Seed(); @@ -66,6 +74,21 @@ int main(int argc, char **argv) while (ros::ok()) { mavros_msgs::Mavlink mavlink_msg_; +======= + Init_Random_Seed(); + + int count = 0; + const std::size_t MAX_BUFFER_SIZE = 64; + char line[MAX_BUFFER_SIZE]; + + std::cout << "Press Enter to Send New Mavlink Message..." << std::endl; + + while (ros::ok() && std::cin.getline(line, MAX_BUFFER_SIZE)) + { + 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); @@ -87,9 +110,14 @@ 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; diff --git a/src/XBeeFrame.cpp b/src/XBeeFrame.cpp index 2e2fad1..91761a5 100644 --- a/src/XBeeFrame.cpp +++ b/src/XBeeFrame.cpp @@ -141,6 +141,16 @@ void Frame::Rearrange_Corrupted_Header() } +<<<<<<< HEAD +======= +//***************************************************************************** +char Frame::Get_Message_Type() +{ + return data_buffer_[15]; +} + + +>>>>>>> a16cf8b196cb6b63ef52ea26b8cb9a8e861d84d1 } diff --git a/src/XMLConfigParser.cpp b/src/XMLConfigParser.cpp index 42ad314..56a1760 100644 --- a/src/XMLConfigParser.cpp +++ b/src/XMLConfigParser.cpp @@ -23,7 +23,11 @@ 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)) { diff --git a/src/Xbee.cpp b/src/Xbee.cpp index d4b1d38..c7a375a 100644 --- a/src/Xbee.cpp +++ b/src/Xbee.cpp @@ -17,7 +17,11 @@ 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 = @@ -34,8 +38,12 @@ 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); }