diff --git a/CMakeLists.txt b/CMakeLists.txt index 962e5a5..0bd4af3 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -11,6 +11,7 @@ set (BOOST_INCLUDEDIR "/usr/include") find_package(catkin REQUIRED COMPONENTS roscpp std_msgs + mavros_msgs ) ## System dependencies are found with CMake's conventions @@ -37,14 +38,18 @@ find_package(catkin REQUIRED COMPONENTS catkin_package( INCLUDE_DIRS include # LIBRARIES xbee_ros_node - CATKIN_DEPENDS roscpp std_msgs + CATKIN_DEPENDS roscpp std_msgs mavros_msgs # DEPENDS system_lib ) ########### ## Build ## ########### - +## Add definitions +add_definitions( + -DDATABASE_PATH="${CMAKE_CURRENT_SOURCE_DIR}/Resources/database.xml" + -DXBEE_CONFIG_PATH="${CMAKE_CURRENT_SOURCE_DIR}/Resources/XBee_Config.xml" +) ## Specify additional locations of header files include_directories( @@ -53,19 +58,19 @@ include_directories( ) -add_executable(xbee_mav src/Xbee.cpp src/XBeeFrame.cpp src/SerialDevice.cpp src/CommunicationManager.cpp) + +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}) +add_executable(xbee_config src/main.cpp src/XBeeModule.cpp src/XMLConfigParser.cpp) +target_link_libraries(xbee_config ${catkin_LIBRARIES}) #add_executable(test_controller src/TestController.cpp) #target_link_libraries(test_controller ${catkin_LIBRARIES}) -#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}) ############# ## Install ## @@ -77,5 +82,3 @@ target_link_libraries(config ${catkin_LIBRARIES}) ## Testing ## ############# - - diff --git a/Resources/XBee_Config.xml b/Resources/XBee_Config.xml index f7f4b2d..17284b1 100644 --- a/Resources/XBee_Config.xml +++ b/Resources/XBee_Config.xml @@ -2,7 +2,7 @@ - 00FFFFFFFFFFFF7FFF + 00FFFFFFFFFFF7FFFF 1 5FFF 3 diff --git a/Resources/database.xml b/Resources/database.xml new file mode 100644 index 0000000..3257dc9 --- /dev/null +++ b/Resources/database.xml @@ -0,0 +1,18 @@ + + + + 0013A20040D8CA1E + 0013A200415278B8 + 0013A2004103B363 + 0013A200415278AD + 0013A2004103B356 + 0013A200415278B7 + 0013A2004098A7A7 + 0013A2004098A7BA + 0013A200415A9DDD + 0013A200415A9DE4 + 0013A200415A9DDF + 0013A200415A9DDE + 0013A200415A9DE8 + + diff --git a/include/CommunicationManager.h b/include/CommunicationManager.h index 880eb5f..74dbfba 100644 --- a/include/CommunicationManager.h +++ b/include/CommunicationManager.h @@ -1,6 +1,6 @@ /* CommunicationManager.h -- Communication Manager class for XBee: Handles all communications with other ROS nodes - and the serial port -- */ + and the serial port -- */ /* ------------------------------------------------------------------------- */ /* September 20, 2016 -- @Copyright Aymen Soussia. All rights reserved. */ /* (aymen.soussia@gmail.com) */ @@ -10,12 +10,14 @@ #include #include - +#include #include #include #include #include - +#include +#include +#include"PacketsHandler.h" #include"SerialDevice.h" @@ -58,7 +60,7 @@ private: void Run_In_Solo_Mode(DRONE_TYPE drone_type); void Run_In_Swarm_Mode(); - void Generate_Transmit_Request_Frame( + /*void Generate_Transmit_Request_Frame( const char* const message, std::string* frame, const unsigned char frame_ID = @@ -66,13 +68,13 @@ private: 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 Check_In_Messages_and_Transfer_To_Topics(); + 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); + //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 Send_Mavlink_Message_Callback( const mavros_msgs::Mavlink::ConstPtr& mavlink_msg); void Display_Drone_Type_and_Running_Mode(DRONE_TYPE drone_type, @@ -80,14 +82,28 @@ private: bool Serve_Flight_Controller(mavros_msgs::CommandInt:: Request& request, mavros_msgs::CommandInt::Response& response); void Check_In_Messages_and_Transfer_To_Server(); + void Process_In_Standard_Messages(); + void Process_In_Acks_and_Pings(); + void Process_In_Fragments(); + void Process_In_Packets(); + void Process_Command_Responses(); + bool Get_Param(mavros_msgs::ParamGet::Request& req, mavros_msgs::ParamGet::Response& res); Mist::Xbee::SerialDevice serial_device_; - Thread_Safe_Deque* in_messages_; + 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_; ros::NodeHandle node_handle_; ros::Subscriber mavlink_subscriber_; ros::Publisher mavlink_publisher_; ros::ServiceClient mav_dji_client_; ros::ServiceServer mav_dji_server_; + ros::ServiceServer StatusSrv_; + std_msgs::UInt8 device_id_out; + std::shared_ptr service_thread_; // TO DO delete !? }; diff --git a/include/MultithreadingDeque.hpp b/include/MultithreadingDeque.hpp index 8de0acf..ea9aa6a 100644 --- a/include/MultithreadingDeque.hpp +++ b/include/MultithreadingDeque.hpp @@ -36,7 +36,7 @@ public: //**************************************************************************** - void Push_Pack(const _T& new_data) + void Push_Back(const _T& new_data) { 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..178cea9 --- /dev/null +++ b/include/PacketsHandler.h @@ -0,0 +1,153 @@ +/* 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); + uint8_t get_device_id(); + + +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..25333cf 100644 --- a/include/SerialDevice.h +++ b/include/SerialDevice.h @@ -25,7 +25,14 @@ namespace Xbee { +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; //***************************************************************************** @@ -39,7 +46,13 @@ public: void Send_Frame(const std::string& frame); void Run_Service(); void Stop_Service(); - 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(); private: @@ -71,7 +84,10 @@ private: boost::asio::io_service io_service_; boost::asio::serial_port serial_port_; std::deque out_messages_; - 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_; 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..50ed2b3 100644 --- a/include/XBeeFrame.h +++ b/include/XBeeFrame.h @@ -27,7 +27,7 @@ public: ~Frame(); enum {FRAME_HEADER_LENGTH = 4}; - enum {MAX_FRAME_BODY_LENGTH = 280}; // TO DO check value + enum {MAX_FRAME_BODY_LENGTH = 270}; // TO DO check value const char* Get_Frame_Data() const; char* Get_Frame_Data(); @@ -39,6 +39,7 @@ public: std::size_t Get_Frame_Type() const; int Get_Start_Delimiter_Position(); void Rearrange_Corrupted_Header(); + char Get_Message_Type(); // TO DO const !? private: diff --git a/launch/xbeemav.launch b/launch/xbeemav.launch index ead9515..4d789ce 100644 --- a/launch/xbeemav.launch +++ b/launch/xbeemav.launch @@ -1,12 +1,14 @@ - - + + + + diff --git a/package.xml b/package.xml index d009ff0..2d1f3c1 100644 --- a/package.xml +++ b/package.xml @@ -13,6 +13,9 @@ std_msgs roscpp std_msgs + + mavros_msgs + mavros_msgs diff --git a/src/CommunicationManager.cpp b/src/CommunicationManager.cpp index 7457422..3f1156d 100644 --- a/src/CommunicationManager.cpp +++ b/src/CommunicationManager.cpp @@ -37,14 +37,38 @@ bool CommunicationManager::Init( { if (serial_device_.Init(device, baud_rate)) { - 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 <= 300000) + { + Process_Command_Responses(); + + if (packets_handler_.Init_Device_ID()) + { + device_ID_loaded = true; + break; + } + } + + if (!device_ID_loaded) + return false; } else { Display_Init_Communication_Failure(); return false; } + + return true; } @@ -53,7 +77,7 @@ bool CommunicationManager::Init( void CommunicationManager::Run(DRONE_TYPE drone_type, RUNNING_MODE running_mode) { - std::thread thread_service(&SerialDevice::Run_Service, &serial_device_); + std::thread thread_packets_handler(&PacketsHandler::Run, &packets_handler_); Display_Drone_Type_and_Running_Mode(drone_type, running_mode); @@ -63,7 +87,10 @@ void CommunicationManager::Run(DRONE_TYPE drone_type, Run_In_Solo_Mode(drone_type); serial_device_.Stop_Service(); - thread_service.join(); + serial_device_.Close_Serial_Port(); + packets_handler_.Quit(); + service_thread_->join(); + thread_packets_handler.join(); } @@ -77,7 +104,7 @@ void CommunicationManager::Run_In_Solo_Mode(DRONE_TYPE drone_type) { if (node_handle_.getParam("Xbee_In_From_Controller", service_name)) { - 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); success = true; } else @@ -116,7 +143,7 @@ void CommunicationManager::Run_In_Swarm_Mode() std::string in_messages_topic; bool success_1 = false; bool success_2 = false; - + StatusSrv_ = node_handle_.advertiseService("/xbee_status", &CommunicationManager::Get_Param, this); if (node_handle_.getParam("Xbee_In_From_Buzz", out_messages_topic)) { mavlink_subscriber_ = node_handle_.subscribe(out_messages_topic.c_str(), 1000, @@ -137,14 +164,17 @@ void CommunicationManager::Run_In_Swarm_Mode() if (success_1 && success_2) { - - ros::Rate loop_rate(LOOP_RATE); + ros::Rate loop_rate(LOOP_RATE); while (ros::ok()) { - Check_In_Messages_and_Transfer_To_Topics(); + 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(); + loop_rate.sleep(); } } } @@ -154,12 +184,12 @@ void CommunicationManager::Run_In_Swarm_Mode() inline bool CommunicationManager::Serve_Flight_Controller(mavros_msgs::CommandInt:: Request& request, mavros_msgs::CommandInt::Response& response) { - const std::size_t MAX_BUFFER_SIZE = 255; + const std::size_t MAX_BUFFER_SIZE = 256; unsigned int command = 0; char temporary_buffer[MAX_BUFFER_SIZE]; std::string frame; - switch(request.command) + /*switch(request.command) { case mavros_msgs::CommandCode::NAV_TAKEOFF: { @@ -205,7 +235,7 @@ inline bool CommunicationManager::Serve_Flight_Controller(mavros_msgs::CommandIn { Generate_Transmit_Request_Frame(temporary_buffer, &frame); serial_device_.Send_Frame(frame); - } + }*/ return true; } @@ -226,7 +256,7 @@ void CommunicationManager::Display_Init_Communication_Failure() //***************************************************************************** -inline void CommunicationManager::Generate_Transmit_Request_Frame( +/*inline void CommunicationManager::Generate_Transmit_Request_Frame( const char* const message, std::string * frame, const unsigned char frame_ID, @@ -236,7 +266,7 @@ inline void CommunicationManager::Generate_Transmit_Request_Frame( const std::string& options) { const unsigned char FAME_TYPE = static_cast(0x10); /* Transmit Request Frame */ - std::string frame_parameters; + /*std::string frame_parameters; std::string bytes_frame_parameters; frame_parameters.append(destination_adssress); @@ -253,11 +283,11 @@ inline void CommunicationManager::Generate_Transmit_Request_Frame( Calculate_and_Append_Checksum(frame); Add_Length_and_Start_Delimiter(frame); -} +}*/ //***************************************************************************** -inline void CommunicationManager::Convert_HEX_To_Bytes( +/*inline void CommunicationManager::Convert_HEX_To_Bytes( const std::string& HEX_data, std::string* converted_data) { const unsigned short TWO_BYTES = 2; @@ -273,11 +303,11 @@ inline void CommunicationManager::Convert_HEX_To_Bytes( temporary_char = static_cast(temporary_HEX); converted_data->push_back(temporary_char); } -} +}*/ //***************************************************************************** -inline void CommunicationManager::Calculate_and_Append_Checksum( +/*inline void CommunicationManager::Calculate_and_Append_Checksum( std::string* frame) { unsigned short bytes_sum = 0; @@ -292,11 +322,11 @@ inline void CommunicationManager::Calculate_and_Append_Checksum( checksum = 0xFF - lowest_8_bits; checksum_byte = static_cast(checksum); frame->push_back(checksum_byte); -} +}*/ //***************************************************************************** -inline void CommunicationManager::Add_Length_and_Start_Delimiter( +/*inline void CommunicationManager::Add_Length_and_Start_Delimiter( std::string* frame) { const unsigned short FIVE_BYTES = 5; @@ -307,7 +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 */ - header.push_back(START_DLIMITER); + /*header.push_back(START_DLIMITER); sprintf(temporary_buffer, "%04X", frame_length); sscanf(temporary_buffer, "%02X%02X", &Hex_frame_length_1, &Hex_frame_length_2); @@ -316,11 +346,11 @@ 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); -} +}*/ //***************************************************************************** -inline void CommunicationManager::Check_In_Messages_and_Transfer_To_Topics() +/*inline void CommunicationManager::Check_In_Messages_and_Transfer_To_Topics() // TO DO delete { std::size_t size_in_messages = in_messages_->Get_Size(); @@ -349,15 +379,15 @@ inline void CommunicationManager::Check_In_Messages_and_Transfer_To_Topics() } } -} +}*/ //***************************************************************************** -inline void CommunicationManager::Check_In_Messages_and_Transfer_To_Server() +inline void CommunicationManager::Check_In_Messages_and_Transfer_To_Server() // TO DO change it { - std::size_t size_in_messages = in_messages_->Get_Size(); + //std::size_t size_in_messages = in_messages_->Get_Size(); - if (size_in_messages > 0) + /*if (size_in_messages > 0) { for (std::size_t j = 0; j < size_in_messages; j++) { @@ -389,7 +419,7 @@ inline void CommunicationManager::Check_In_Messages_and_Transfer_To_Server() ROS_INFO("XBeeMav: Faild to Tranfer Command."); } - } + }*/ } @@ -397,26 +427,8 @@ inline void CommunicationManager::Check_In_Messages_and_Transfer_To_Server() inline void CommunicationManager::Send_Mavlink_Message_Callback( 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_NBR_OF_INT64 = 10; - char temporary_buffer[MAX_BUFFER_SIZE]; - std::string frame; - int converted_bytes = 0; - - /* Check the payload is not too long. Otherwise ignore it. */ - if (mavlink_msg->payload64.size() <= MAX_NBR_OF_INT64) - { - for (unsigned short i = 0; i < mavlink_msg->payload64.size(); i++) - { - converted_bytes += sprintf( - temporary_buffer + converted_bytes, "%" PRIu64 " ", - (uint64_t)mavlink_msg->payload64.at(i)); - - } - - Generate_Transmit_Request_Frame(temporary_buffer, &frame); - serial_device_.Send_Frame(frame); - } + //std::cout << "Received Message From Buzz" << std::endl; // TO DO delete + packets_handler_.Handle_Mavlink_Message(mavlink_msg); } @@ -436,6 +448,113 @@ void CommunicationManager::Display_Drone_Type_and_Running_Mode( } +//***************************************************************************** +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()); + } + } +} + +bool CommunicationManager::Get_Param (mavros_msgs::ParamGet::Request& req, mavros_msgs::ParamGet::Response& res){ +mavros_msgs::ParamValue val; +if(req.param_id=="id"){ + val.integer=packets_handler_.get_device_id(); +} else if(req.param_id=="rssi"){ + val.integer=-1; +} else if(req.param_id=="pl"){ + val.integer=-1; +} +res.value= val; +res.success=true; +return true; +} + } diff --git a/src/PacketsHandler.cpp b/src/PacketsHandler.cpp new file mode 100644 index 0000000..f662614 --- /dev/null +++ b/src/PacketsHandler.cpp @@ -0,0 +1,775 @@ +/* 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; + + 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) +{ + 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) +{ + uint8_t packet_ID = frame->at(12); + uint8_t node_8_bits_address = frame->at(13); + + if (frame->at(11) == 'A') + { + mutex_.lock(); + + connected_network_nodes_it_ = connected_network_nodes_.find(node_8_bits_address); + + if (connected_network_nodes_it_ == connected_network_nodes_.end()) + { + mutex_.unlock(); + Add_New_Node_To_Network(node_8_bits_address); + mutex_.lock(); + 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)); + } + } + + mutex_.unlock(); + } + 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()); + 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)) + { + new_node_address = static_cast( + 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)) + { + 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)) + { + 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_); + current_processed_packet_ID_ = packet.packet_ID_; + + std::clock_t start_time = std::clock(); + + while (std::clock() - start_time <= MAX_TIME_TO_SEND_PACKET && !Check_Packet_Transmitted_To_All_Nodes()) + { + NBR_of_transmission++; + Transmit_Fragments(frames); + Send_End_Of_Packet_Ping(packet.packet_ID_, packet.packet_buffer_->size()); + usleep(500 * 1000); + } + + Adjust_Optimum_MT_Number(std::clock() - start_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 = DATABASE_PATH; + + if (!boost::filesystem::exists(FILE_PATH)) + { + std::cout << "database.xml Not Found with path: " << FILE_PATH << 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_); + } + + fragments_indexes_to_transmit_.clear(); +} + + +//***************************************************************************** +void PacketsHandler::Adjust_Optimum_MT_Number(const std::clock_t elapsed_time, + const std::size_t NBR_of_transmission) +{ + if (NBR_of_transmission > 1 && elapsed_time < MAX_TIME_TO_SEND_PACKET) + delay_interframes_ += 5000; + else if (NBR_of_transmission == 1 && delay_interframes_ >= 5000) + delay_interframes_ -= 5000; +} + + +//***************************************************************************** +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); +} + +uint8_t PacketsHandler::get_device_id(){ +return device_address_; +} + +} + + +} diff --git a/src/SerialDevice.cpp b/src/SerialDevice.cpp index 33dc7a5..5b00d0e 100644 --- a/src/SerialDevice.cpp +++ b/src/SerialDevice.cpp @@ -39,7 +39,6 @@ bool SerialDevice::Init( { Set_Port_Options(baud_rate); Init_Frame_Type_Keys(); - Read_Frame_Header(); return true; } else @@ -82,6 +81,7 @@ void SerialDevice::Send_Frame(const std::string& frame) //***************************************************************************** void SerialDevice::Run_Service() { + Read_Frame_Header(); io_service_.run(); } @@ -90,14 +90,26 @@ void SerialDevice::Run_Service() void SerialDevice::Stop_Service() { io_service_.post([this]() {io_service_.stop(); }); +} + + +//***************************************************************************** +void SerialDevice::Close_Serial_Port() +{ io_service_.post([this]() {serial_port_.close(); }); } //***************************************************************************** -Thread_Safe_Deque * SerialDevice::Get_In_Messages_Pointer() +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) { - return &in_messages_; + in_std_messages_ = in_std_messages; + in_fragments_ = in_fragments; + in_Acks_and_Pings_ = in_Acks_and_Pings; + command_responses_ = command_responses; } @@ -156,8 +168,10 @@ void SerialDevice::Read_Frame_Header() Read_Frame_Body(); } else + { /* The header is totally corrupted, read another header. */ Read_Frame_Header(); + } } else { @@ -183,15 +197,42 @@ void SerialDevice::Read_Frame_Body() if (current_frame_.Get_Frame_Type() == FRAME_TYPE_KEYS[RECEIVE_PACKET]) { - const unsigned short ELEVEN_BYTES = 11; - const unsigned short TWELVE_BYTES = 12; - + char msg_type = current_frame_.Get_Message_Type(); std::shared_ptr in_message = std::make_shared(); - in_message->append(current_frame_.Get_Frame_Body() - + ELEVEN_BYTES, - current_frame_.Get_Frame_Body_Length() - TWELVE_BYTES); - in_messages_.Push_Pack(in_message); + + 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); } Read_Frame_Header(); @@ -226,6 +267,20 @@ void SerialDevice::Write_Frame() } +//***************************************************************************** +bool SerialDevice::Is_IO_Service_Stopped() +{ + return io_service_.stopped(); +} + + +//***************************************************************************** +void SerialDevice::Reset_IO_Service() +{ + io_service_.reset(); +} + + } diff --git a/src/TestBuzz.cpp b/src/TestBuzz.cpp index 1337742..3bcffb9 100644 --- a/src/TestBuzz.cpp +++ b/src/TestBuzz.cpp @@ -8,7 +8,7 @@ #include #include -#include +#include #include @@ -47,25 +47,28 @@ void Init_Random_Seed() //***************************************************************************** int main(int argc, char **argv) { - const unsigned int MIN_PAYLOAD_SIZE = 1; - const unsigned int MAX_PAYLOAD_SIZE = 10; + const unsigned int MIN_PAYLOAD_SIZE = 750; + const unsigned int MAX_PAYLOAD_SIZE = 752; - ros::init(argc, argv, "flight_controller"); + ros::init(argc, argv, "test_buzz"); 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); - ros::Rate loop_rate(0.2); - Init_Random_Seed(); int count = 0; + const std::size_t MAX_BUFFER_SIZE = 64; + char line[MAX_BUFFER_SIZE]; - - while (ros::ok()) + 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; unsigned int payload_size = Get_Random_Size(MIN_PAYLOAD_SIZE, MAX_PAYLOAD_SIZE); @@ -87,9 +90,8 @@ int main(int argc, char **argv) ros::spinOnce(); - loop_rate.sleep(); - count++; + std::cout << "Press Enter to Send New Mavlink Message..." << std::endl; } return 0; diff --git a/src/XBeeFrame.cpp b/src/XBeeFrame.cpp index 2e2fad1..1a99176 100644 --- a/src/XBeeFrame.cpp +++ b/src/XBeeFrame.cpp @@ -141,6 +141,13 @@ void Frame::Rearrange_Corrupted_Header() } +//***************************************************************************** +char Frame::Get_Message_Type() +{ + return data_buffer_[15]; +} + + } diff --git a/src/XMLConfigParser.cpp b/src/XMLConfigParser.cpp index 42ad314..dfd527a 100644 --- a/src/XMLConfigParser.cpp +++ b/src/XMLConfigParser.cpp @@ -7,6 +7,7 @@ #include "XMLConfigParser.h" + //***************************************************************************** XMLConfigParser::XMLConfigParser(): config_loaded_successfully_(false) @@ -23,7 +24,7 @@ XMLConfigParser::~XMLConfigParser() //***************************************************************************** bool XMLConfigParser::Load_Config() { - const std::string FILE_NAME = "/home/ubuntu/ROS_WS/src/xbeemav/Resources/XBee_Config.xml"; + const std::string FILE_NAME = XBEE_CONFIG_PATH; if (Check_Config_File_Exists(FILE_NAME)) { diff --git a/src/Xbee.cpp b/src/Xbee.cpp index d71e3a6..53b2669 100644 --- a/src/Xbee.cpp +++ b/src/Xbee.cpp @@ -14,10 +14,11 @@ int main(int argc, char* argv[]) try { ros::init(argc, argv, "xbee"); + ros::NodeHandle node_handle; Mist::Xbee::CommunicationManager communication_manager; - const std::string& device = "/dev/ttyUSB0"; // TO DO can be introduced as command. - const std::size_t baud_rate = 230400; // TO DO Can be introduced as command. + std::string device; + double baud_rate; Mist::Xbee::CommunicationManager::DRONE_TYPE drone_type = Mist::Xbee::CommunicationManager::DRONE_TYPE::SLAVE; Mist::Xbee::CommunicationManager::RUNNING_MODE running_mode = @@ -34,9 +35,18 @@ int main(int argc, char* argv[]) running_mode = Mist::Xbee::CommunicationManager::RUNNING_MODE::SWARM; } } - + if (!node_handle.getParam("USB_port", device)) + { + std::cout << "Failed to Get Topic Name: param 'USB_port' Not Found." << std::endl; + return EXIT_FAILURE; + } - if (communication_manager.Init(device, baud_rate)) + if (!node_handle.getParam("Baud_rate", baud_rate)) + { + std::cout << "Failed to Get Topic Name: param 'Baud_rate' Not Found." << std::endl; + return EXIT_FAILURE; + } + if (communication_manager.Init(device, static_cast(baud_rate))) communication_manager.Run(drone_type, running_mode); } catch (const std::exception& e)