diff --git a/CMakeLists.txt b/CMakeLists.txt index 0bd4af3..33b391c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -34,12 +34,11 @@ find_package(catkin REQUIRED COMPONENTS ## catkin specific configuration ## ################################### - catkin_package( - INCLUDE_DIRS include -# LIBRARIES xbee_ros_node - CATKIN_DEPENDS roscpp std_msgs mavros_msgs -# DEPENDS system_lib +INCLUDE_DIRS include +LIBRARIES xbee_setup +CATKIN_DEPENDS roscpp std_msgs mavros_msgs +DEPENDS Boost ) ########### @@ -53,25 +52,34 @@ add_definitions( ## Specify additional locations of header files include_directories( - include ${xbee_ros_node_INCLUDE_DIRS} + include + ${xbee_ros_node_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} + ${BOOST_INCLUDEDIR} ) +add_library(xbee_setup + src/XBeeSetup.cpp + src/XBeeModule.cpp + src/XMLConfigParser.cpp +) +add_executable(xbee_mav src/Xbee.cpp src/XBeeFrame.cpp src/SerialDevice.cpp src/CommunicationManager.cpp src/PacketsHandler src/frame_generators.cpp) +target_link_libraries(xbee_mav xbee_setup ${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(xbee_config src/main.cpp src/XBeeModule.cpp src/XMLConfigParser.cpp) -target_link_libraries(xbee_config ${catkin_LIBRARIES}) +add_executable(xbee_config src/XMLConfigParser.cpp src/main.cpp) +target_link_libraries(xbee_config xbee_setup ${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_cyclic src/TestBuzzCyclic.cpp) +target_link_libraries(test_buzz_cyclic ${catkin_LIBRARIES}) + ############# ## Install ## ############# @@ -81,4 +89,3 @@ target_link_libraries(test_buzz ${catkin_LIBRARIES}) ############# ## Testing ## ############# - diff --git a/README.md b/README.md index f5d1e80..64a87c8 100644 --- a/README.md +++ b/README.md @@ -1,52 +1,43 @@ -##Description +# XbeeMav +## Description The "xbee_ros_node" package provides many tools (ROS nodes) to configure, test and communicate Xbee devices. Four nodes are provided in the package: - * config + * xbee_config * xbee_mav * test_controller * test_buzz -The "test_controller" and the "test_buzz" are dummy nodes. They are used only for testing purposes. The "config" node is used to configure the Xbees. The "xbee_mav" is used to communicates between Xbee devices in different modes (e.g. SOLO and SWARM). +The "test_controller" and the "test_buzz" are dummy nodes. They are used only for testing purposes. The "xbee_config" node is used to configure the Xbees. The "xbee_mav" is used to communicates between Xbee devices in different modes (e.g. SOLO and SWARM). -##Prerequisites +## Prerequisites * Linux OS * ROS * Serial device drivers. For FTDI, the Virtual COM port (VCP) drivers are mandatory (http://www.ftdichip.com/Drivers/VCP.htm). - -##Configuration of Xbee devices -To configure an Xbee device, the "config" node is required. To build the "config" node: +## Configuration of Xbee devices - 1. Uncomment the following lines in "xbee_ros_node/CMakeLists.txt": +To configure an Xbee device, the "xbee_config" node is required. To build the "xbee_config" node: - add_executable(config src/main.cpp src/XbeeModule.cpp src/XMLConfigParser.cpp) - target_link_libraries(config ${catkin_LIBRARIES}) - - 2. Insert the correct path to the config file “Resources/Xbee_Config.xml” in “XMLConfigParser.cpp” (line 26): - - FILE_NAME = “/home/mistlab/catkin_ws/src/xbee_ros_node/Resources/Xbee_Config.xml” - - 3. Build the package: + 1. Build the package: $ cd ~/catkin_ws $ catkin_make - -To configure an Xbee device run the "config" node. By default, the VCP and the baud rate are respectively "/dev/ttyUSB0" and "9600". Before running the “config” node, please make sure: + +To configure an Xbee device run the "xbee_config" node. By default, the VCP and the baud rate are respectively "/dev/ttyUSB0" and "9600". Before running the “config” node, please make sure: 1. The Xbee device is plugged. 2. The serial adapter driver is installed. 3. You have full access to the correspondent serial port (e.g. ttyUSB0): - - $ cd ~/../../dev/ - $ sudo chmod 666 ttyUSB0 - -Now you can run the “config” node (after running roscore): + + $ sudo chmod 666 /dev/ttyUSB0 + +Now you can run the "xbee_config" node (after running roscore): $ cd ~/catkin_ws - $ rosrun xbee_ros_node config /dev/ttyUSB0 9600 - + $ rosrun xbee_ros_node xbee_config /dev/ttyUSB0 9600 + If this command generates an error: * Error: open: No such file or directory: @@ -54,24 +45,17 @@ If this command generates an error: 2. Make sure the appropriate driver is installed correctly. 3. Make sure the path to the VCP (e.g. /dev/ttyUSB0) is correct. 4. Install the newest Xbee firmware with “XCTU” and Write the default config. - + * Error: open: Permission denied: - + You do not have full access to the serial port. Please execute these commands: - - $ cd ~/../../dev/ - $ sudo chmod 666 ttyUSB0 - + + $ sudo chmod 666 /dev/ttyUSB0 + * Time Out: The Xbee Module is Not Responding: - - Baud rate mismatch. Please make sure the introduced baud rate (e.g. 9600) matches the baud rate used by the Xbee (by default is 9600). If the Xbee was previously configured with the “config” node the baud rate will be 230400. - - * Error: Config File Not Found: - - The path to the config file is incorrect. Change it in “XMLConfigParser.cpp” (line 26): - - FILE_NAME = “/home/mistlab/catkin_ws/src/xbee_ros_node/Resources/Xbee_Config.xml” - + + Baud rate mismatch. Please make sure the introduced baud rate (e.g. 9600) matches the baud rate used by the Xbee (by default is 9600). If the Xbee was previously configured with the “xbee_config” node the baud rate will be 230400. + The configuration will be loaded from "Resources/Xbee_Config.xml". Most of the existing values in the config file are optimized for Digi-Mesh. Some parameters are default values. However, all parameters can be edited. Please refer to **Table.1** before editing any parameter. More details can be found in “Xbee_Manual.pdf”. The commands table is in Chapter 3 (page 28). **Table.1:** Most relevant Xbee Parameters. @@ -104,14 +88,13 @@ The configuration will be loaded from "Resources/Xbee_Config.xml". Most of the e |FT | Flow Control Threshold | 13F | Since we are using the highest baud rate, we might need to increase the flow control threshold. Testing will tell us if the integrity of the data is fine.| |AP | API Enable | 1 | The XBee has to be set to API Mode to receive API frames. This allows recovering useful information in the headers of the frames such as source and destination address. We shouldn't ever need to escape API mode so we chose option [1]. Otherwise, the escape bytes would have to be escaped each time they occur. | |AO | API Options | 0 | Will need to change to [1] (Explicit Rx Indicator) if we wand to be able to read Cluster ID. Otherwise, not very useful. | - + ## Test two Xbee devices We consider the following setup (**Fig.1**). The block (Drone + Manifold) can be replaced by any Desktop/Laptop meeting the prerequisites. -**Fig.1:** Experimental Setup: -![][fig1] -[fig1]: https://github.com/MISTLab/XbeeMav/tree/master/Resources/Fig1.png "Fig.1" +**Fig.1:** Experimental Setup: +![fig1](Resources/Fig1.png) One of the drones will behave as a Master while the other one will act as a Slave. The Master drone will send commands to the Slave drone. Each drone is running a dummy flight controller node "test_controller". According to the drone type Master/Slave, the "test_controller" node will respectively send or receive and display a command. The commands in the Master drone will be introduced with the keyboard. When a command is received in the Slave drone, it will be printed on the screen. The following table (**Table.2**) depicts the keys of each command: @@ -128,9 +111,8 @@ Each drone is running a dummy flight controller node "test_controller". Accordin | 23 | Start Mission | -**Fig.2:** ROS nodes running on the drone: -![][fig2] -[fig2]: https://github.com/MISTLab/XbeeMav/tree/master/Resources/Fig2.png "Fig.2" +**Fig.2:** ROS nodes running on the drone: +![fig2](Resources/Fig2.png) The communication between both drones is performed with Xbees. The “xbee_mav” node (**Fig.2**) will handle all communications with other ROS nodes (test_controller(Flight Controller) or test_buzz (ROS Buzz)) and the connected Xbee device. Therefore, both Xbees must be configured for Digi-Mesh with the maximum baud rate (230400). We recognize two modes of communications: @@ -141,7 +123,7 @@ We recognize two modes of communications: All topics and services names of the “xbee_mav” node can be edited in the launch file "launch/xbeemav.launch". For other nodes (test_controller and test_buzz), topics and services names are hardcoded. Thus, they need to be modified in the source code. -####SOLO mode +#### SOLO mode The "test_controller" node is needed: 1. Uncomment the following lines in "CMakeLists.txt": @@ -152,13 +134,13 @@ The "test_controller" node is needed: $ catkin_make 3. Run the "test controller" node in Matser/Slave (after running roscore): $ cd ~/catkin_ws - $ rosrun xbee_ros_node test_controller master + $ rosrun xbee_ros_node test_controller master 4. Run the "xbee_mav" node in SOLO mode. The drone type (Master/Slave) and the communication mode (SOLO/SWARM) need to be specified for the "xbee_mav". You can change these parameters in the launch file “launch/xbeemav.launch”: $ cd ~/catkin_ws $ roslaunch xbee_ros_node xbeemav.launch 5. Introduce some commands to the "test_controller". If the network was configured correctly, commands sent from one side should be received and displayed in the opposite side. -####SWARM mode +#### SWARM mode The "test_buzz" node is required: 1. Uncomment the following lines in "CMakeLists.txt": @@ -176,7 +158,7 @@ The "test_buzz" node is required: Random payloads (Mavlink messages) will be created in the Master drone and transferred through the Xbees to the Slave drone for display. Payloads are arrays with random sizes of random 64 bits integers. -## Communicate drones in a Swarm +## Communicate drones in a Swarm We consider the same setup in the testing phase (**Fig.1**). The "test_controller" and the "test_buzz" dummy nodes need to be replaced with real ones (“fligh_controller” and “ros_buzz”). You can download the real nodes by clicking on the correspondent link: @@ -184,14 +166,21 @@ You can download the real nodes by clicking on the correspondent link: * ros_buzz : (https://github.com/MISTLab/ROSBuzz.git) To run drones in SWARM mode you need to: - 1. Comment the following lines in “xbee_ros_node/CMakeLists.txt”: - - add_executable(config src/main.cpp src/XbeeModule.cpp src/XMLConfigParser.cpp) - target_link_libraries(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}) - - 2. Build the three packages. - 3. Run the launch file (this will run the flight_controller, ros_buzz and xbee_mav). The "xbee_mav" should run in SWARM mode. The drone type (Master/Slave) is not needed. + 1. Build the three packages. + 2. Run a launch file that will start the flight_controller, ros_buzz and xbee_mav. The "xbee_mav" should run in SWARM mode. The drone type (Master/Slave) is not needed. + +## ROS services +The node xbee_mav provides a ROS service xbee_status that will return information +about the xbee module. The information returned depends on the argument passed to +the service (example: rosservice call /xbee_status "param_id: 'ARGUMENT'"). The different arguments that can be passed are described below: +* id : Returns the xbee module short identifier. +* deque_full : returns 1 if the out messages queue is full and 0 otherwise +* rssi : Returns the average Received Signal Strength Indicator (RSSI). This RSSI value could be inaccurate since the average is obtained by getting the RSSI of the last message at a fixed frequency without any information about the sender. +* trig_rssi_api_avg : Triggers a link testing procedure to obtain the RSSI with all the known nodes in the network. Returns false if there is no other node connected to the network. +* trig_rssi_api_ID (where ID should be replaced by a number): Triggers a link testing procedure to obtain the RSSI with the corresponding ID. Returns false if the node is not present on the network. +* get_rssi_api_avg : Returns the average of the average RSSI results of the link testing procedures. +* get_rssi_api_ID (where ID should be replaced by a number): Returns the average RSSI result of the link testing procedure. +* pl_raw_avg : Returns the average of the raw packet loss value among the connected nodes. Returns false if there is no other node connected to the network. +* pl_raw_ID (where ID should be replaced by a number): Returns the raw packet loss value between this node and the node ID. Returns false if the node is not present on the network. +* pl_filter_avg : same thing as pl_raw_avg but for the filtered value. +* pl_filter_ID (where ID should be replaced by a number): same thing as pl_raw_ID but for the filtered value. diff --git a/Resources/XBee_Config.xml b/Resources/XBee_Config.xml index 17284b1..e33fdc8 100644 --- a/Resources/XBee_Config.xml +++ b/Resources/XBee_Config.xml @@ -5,11 +5,11 @@ 00FFFFFFFFFFF7FFFF 1 5FFF - 3 + 0 4 A 0 - 0 + 1 7 1 3 @@ -63,6 +63,5 @@ 2B 60 3E8 - B0000 diff --git a/Resources/XbeeModule_DataSheet.pdf b/Resources/XbeeModule_DataSheet.pdf new file mode 100644 index 0000000..41db1d3 Binary files /dev/null and b/Resources/XbeeModule_DataSheet.pdf differ diff --git a/Resources/database.xml b/Resources/database.xml index 3257dc9..52828fd 100644 --- a/Resources/database.xml +++ b/Resources/database.xml @@ -1,18 +1,39 @@ + + 0013A20040D8CA1E - 0013A200415278B8 + 0013A200415278B8 0013A2004103B363 0013A200415278AD - 0013A2004103B356 + 0013A2004103B356 0013A200415278B7 0013A2004098A7A7 0013A2004098A7BA - 0013A200415A9DDD + 0013A200415A9DDD 0013A200415A9DE4 0013A200415A9DDF 0013A200415A9DDE - 0013A200415A9DE8 - + 0013A200415A9DE8 + 0013A2004156FA60 + 0013A2004156FA61 + 0013A2004156FA57 + 0013A200417DFE1D + 0013A200417DFE2B + 0013A200417DFE04 + 0013A20041795D68 + 0013A20041795E68 + 0013A2004178AF0E + + 0013A20041795D68 + 0013A20041795E68 + 0013A2004178AF0E diff --git a/include/CommunicationManager.h b/include/CommunicationManager.h index 74dbfba..2398083 100644 --- a/include/CommunicationManager.h +++ b/include/CommunicationManager.h @@ -1,113 +1,103 @@ /* CommunicationManager.h -- Communication Manager class for XBee: - Handles all communications with other ROS nodes - and the serial port -- */ + Handles all communications with other ROS nodes + and the serial port -- */ /* ------------------------------------------------------------------------- */ /* September 20, 2016 -- @Copyright Aymen Soussia. All rights reserved. */ /* (aymen.soussia@gmail.com) */ - #pragma once +#include "PacketsHandler.h" +#include "SerialDevice.h" #include -#include -#include -#include -#include -#include -#include +#include +#include +#include #include #include -#include"PacketsHandler.h" -#include"SerialDevice.h" +#include +#include +#include +namespace Mist { -namespace Mist -{ - - -namespace Xbee -{ - +namespace Xbee { //***************************************************************************** -struct Waypoint_S -{ - unsigned int latitude; - unsigned int longitude; - double altitude; - unsigned int staytime; - unsigned int heading; +struct Waypoint_S { + unsigned int latitude; + unsigned int longitude; + double altitude; + unsigned int staytime; + unsigned int heading; }; - //***************************************************************************** class CommunicationManager { -public: - CommunicationManager(); - ~CommunicationManager(); + public: + CommunicationManager(); + ~CommunicationManager(); - enum class DRONE_TYPE {MASTER, SLAVE}; - enum class RUNNING_MODE {SWARM, SOLO}; + enum class DRONE_TYPE { MASTER, SLAVE }; + enum class RUNNING_MODE { SWARM, SOLO }; - bool Init(const std::string& device, const std::size_t baud_rate); - void Run(DRONE_TYPE drone_type, RUNNING_MODE running_mode); + bool Init(const std::string &device, const std::size_t baud_rate); + void Run(DRONE_TYPE drone_type, RUNNING_MODE running_mode); -private: + private: + const unsigned char START_DLIMITER; + const std::size_t LOOP_RATE; + const uint8_t DEFAULT_RATE_DIVIDER_RSSI; + const uint8_t DEFAULT_RATE_DIVIDER_PACKET_LOSS; + const uint16_t DEFAULT_RSSI_PAYLOAD_SIZE; + const uint16_t DEFAULT_RSSI_ITERATIONS; - const unsigned char START_DLIMITER; - const std::size_t LOOP_RATE; + void Run_In_Solo_Mode(DRONE_TYPE drone_type); + void Run_In_Swarm_Mode(); + void Display_Init_Communication_Failure(); + void Send_Mavlink_Message_Callback( + const mavros_msgs::Mavlink::ConstPtr &mavlink_msg); + void Display_Drone_Type_and_Running_Mode(DRONE_TYPE drone_type, + RUNNING_MODE running_mode); + 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(); + void Process_Packet_Loss(); + bool Get_Param(mavros_msgs::ParamGet::Request &req, + mavros_msgs::ParamGet::Response &res); + bool getRosParams(); + int getIntParam(std::string name, int default_value); + void triggerRssiUpdate(); + std::string safeSubStr(const std::string strg, + const unsigned int index_max) const; - void Run_In_Solo_Mode(DRONE_TYPE drone_type); - void Run_In_Swarm_Mode(); - /*void Generate_Transmit_Request_Frame( - const char* const message, - std::string* frame, - 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 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 Send_Mavlink_Message_Callback( - const mavros_msgs::Mavlink::ConstPtr& mavlink_msg); - void Display_Drone_Type_and_Running_Mode(DRONE_TYPE drone_type, - RUNNING_MODE running_mode); - 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_; - 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 !? + 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_; + Thread_Safe_Deque in_packet_loss_; + 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 !? + std::uint16_t packet_loss_timer_; + uint8_t rate_divider_rssi_; + uint8_t rate_divider_packet_loss_; + uint16_t rssi_payload_size_; + uint16_t rssi_iterations_; }; - - } - - } diff --git a/include/PacketsHandler.h b/include/PacketsHandler.h index 178cea9..d092f4b 100644 --- a/include/PacketsHandler.h +++ b/include/PacketsHandler.h @@ -1,6 +1,6 @@ /* PacketsHandler.h-- Packets Handler class for XBee: - Serialize, deserialize, fragment and reassemly mavlink - messages -- */ + Serialize, deserialize, fragment and reassemly mavlink + messages -- */ /* ------------------------------------------------------------------------- */ /* February 06, 2017 -- @Copyright Aymen Soussia. All rights reserved. */ /* (aymen.soussia@gmail.com) */ @@ -8,23 +8,24 @@ #pragma once -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include -#include"MultithreadingDeque.hpp" -#include"SerialDevice.h" +#include "MultithreadingDeque.hpp" +#include "SerialDevice.h" +#include "frame_generators.h" //***************************************************************************** @@ -38,114 +39,160 @@ namespace Mist namespace Xbee { + 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 + }; -//***************************************************************************** -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_; -}; + struct On_Line_Node_S + { + uint64_t device_64_bits_address_; + float packet_loss_raw_; + float packet_loss_filtered_; + uint16_t sent_packets_; + uint16_t received_packets_; + uint8_t packet_loss_sent_id_; + uint8_t packet_loss_received_id_; + }; + enum RSSI_API_STATUS + { + OLD_VALUE, + TRIGGERED, + NEW_VALUE + }; + + struct RSSI_Result + { + uint16_t payload_size; + uint16_t iterations; + uint16_t success; + uint16_t retries; + uint8_t result; + uint8_t rr; + uint8_t max_rssi; + uint8_t min_rssi; + uint8_t avg_rssi; + RSSI_API_STATUS status; + }; + + //***************************************************************************** + 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 getDeviceId(); + uint8_t getDequeFull(); + float getSignalStrength(); + float getAPISignalStrength(uint8_t short_node_id); + float getRawPacketLoss(uint8_t short_node_id); + float getPacketLoss(uint8_t short_node_id); + void triggerRssiUpdate(); + uint8_t triggerAPIRssiUpdate(uint16_t rssi_payload_size, + uint16_t rssi_iterations, + uint8_t target_id); + void processPacketLoss(const char* packet_loss); + void sendPacketLoss(); + + static uint16_t ucharToUint16(unsigned char msb, unsigned char lsb); + + static const uint16_t PACKET_LOSS_UNAVAILABLE; + static const uint8_t ALL_IDS; + static const uint8_t MAX_WAITING_OUT_MSG; + + 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*/ + ; + // RSSI constants + const std::string RSSI_COMMAND; + const float RSSI_FILTER_GAIN; //filter: old * (1.0-GAIN) + new * GAIN + + // Packet Loss + const std::string PACKET_LOSS_IDENTIFIER; + const uint8_t MAX_PACKET_LOSS_MSG_ID; // used for synchronisation + const float PACKET_LOSS_FILTER_GAIN; //filter: new * (1.0-GAIN) + old * GAIN + + 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(); + + + float computePercentage(const int16_t numerator, const int16_t denumerator) const; + float filterIIR(const float new_val, const float old_val, const float gain) const; + void updatePacketLoss(On_Line_Node_S& node, const uint16_t recieved_packet); + void processAPIRssi(const char* command_response); + uint64_t get64BitsAddress(const char* bytes, const int offset); + + 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::map database_addresses_inv_; + std::map::iterator database_addresses_inv_it_; + std::map packet_loss_map_; + std::map::iterator packet_loss_it_; + std::map rssi_result_map_; + std::map::iterator rssi_result_map_it_; + + std::mutex mutex_; + uint8_t device_address_; + uint64_t local_64_bits_address_; + std::string device_64_bits_address_; + bool loaded_SL_; + bool loaded_SH_; + bool deque_full_; + float rssi_float_; + 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 25333cf..5f842e7 100644 --- a/include/SerialDevice.h +++ b/include/SerialDevice.h @@ -9,7 +9,7 @@ #include #include -#include +#include #include @@ -49,11 +49,18 @@ public: 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); + Thread_Safe_Deque* command_responses, + Thread_Safe_Deque* in_packet_loss); bool Is_IO_Service_Stopped(); // TO DO delete this function void Reset_IO_Service(); // TO DO delete this function void Close_Serial_Port(); + const char FRAGMENT_MSG_ID = 'F'; + const char ACKNOWLEDGEMENT_MSG_ID = 'A'; + const char PING_MSG_ID = 'P'; + const char STANDARD_MSG_ID = 'S'; + const char PACKET_LOSS_MSG_ID = 'L'; + private: enum FRAME_TYPE @@ -88,6 +95,8 @@ private: Thread_Safe_Deque* in_fragments_; Thread_Safe_Deque* in_Acks_and_Pings_; Thread_Safe_Deque* command_responses_; + Thread_Safe_Deque* in_packet_loss_; + Mist::Xbee::Frame current_frame_; unsigned int FRAME_TYPE_KEYS[REMOTE_AT_COMMAND_RESPONSE + 1]; }; diff --git a/include/XBeeSetup.h b/include/XBeeSetup.h new file mode 100644 index 0000000..a0bf995 --- /dev/null +++ b/include/XBeeSetup.h @@ -0,0 +1,14 @@ +/* ---------------------------------------------------------------- + * File: XBeeSetup.cpp + * Date: 02/06/2017 + * Author: Pierre-Yves Breches + * Description: Implementation of the xbeeSetup function to + * configure the xbee module + * Copyright Humanitas Solutions. All rights reserved. + ------------------------------------------------------------------ */ +#include + +#include"XBeeModule.h" +#include"XMLConfigParser.h" + +bool setupXBee(const std::string& device_port, const unsigned int baud_rate); diff --git a/include/frame_generators.h b/include/frame_generators.h new file mode 100644 index 0000000..a045e4e --- /dev/null +++ b/include/frame_generators.h @@ -0,0 +1,59 @@ +/* ---------------------------------------------------------------- + * File: frame_generators.h + * Created on: 21/06/2017 + * Author: Pierre-Yves Breches + * Description: This file contains functions used for the the creation of xbee + * frame + * Copyright Humanitas Solutions. All rights reserved. + ------------------------------------------------------------------ */ + +#include +#include +#include +#include +#include + +namespace Mist +{ + +namespace Xbee +{ + + 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 generateLinkTestingFrame( + std::string* frame, + uint16_t rssi_payload_size, + uint16_t rssi_iterations, + std::string device_64_bits_address, + uint64_t target_64_bits_address); + + void Generate_AT_Command(const char* command, + std::string* frame, + const unsigned char frame_ID = + static_cast(0x01)); + + 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); + + template< typename T > + std::string int_to_hex(const T i, int size) + { + std::stringstream stream; + stream << std::setfill ('0') << std::setw(size) + << std::hex << i; + return stream.str(); + } +} + +} diff --git a/launch/xbeemav.launch b/launch/xbeemav.launch index 4d789ce..5698534 100644 --- a/launch/xbeemav.launch +++ b/launch/xbeemav.launch @@ -1,4 +1,5 @@ + @@ -9,6 +10,7 @@ - + + diff --git a/launch/xbeemav_test.launch b/launch/xbeemav_test.launch new file mode 100644 index 0000000..0ae3cd6 --- /dev/null +++ b/launch/xbeemav_test.launch @@ -0,0 +1,27 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/CommunicationManager.cpp b/src/CommunicationManager.cpp index 3f1156d..4fc86a5 100644 --- a/src/CommunicationManager.cpp +++ b/src/CommunicationManager.cpp @@ -1,26 +1,32 @@ /* CommunicationManager.cpp -- Communication Manager class for XBee: - Handles all communications with other ROS nodes - and the serial port -- */ + Handles all communications with other ROS nodes + and the serial port -- */ /* ------------------------------------------------------------------------- */ /* September 20, 2016 -- @Copyright Aymen Soussia. All rights reserved. */ /* (aymen.soussia@gmail.com) */ - - +/* ------------------------------------------------------------------------- */ +/* Revision + * Date: July 1st, 2017 + * Author: Pierre-Yves Breches + */ +/* ------------------------------------------------------------------------- */ #include "CommunicationManager.h" namespace Mist { - namespace Xbee { - //***************************************************************************** -CommunicationManager::CommunicationManager(): - START_DLIMITER(static_cast(0x7E)), - LOOP_RATE(10) /* 10 fps */ +CommunicationManager::CommunicationManager() : + START_DLIMITER(static_cast(0x7E)), + LOOP_RATE(10), /* 10 fps */ + DEFAULT_RATE_DIVIDER_RSSI(5), + DEFAULT_RATE_DIVIDER_PACKET_LOSS(20), + DEFAULT_RSSI_PAYLOAD_SIZE(10), + DEFAULT_RSSI_ITERATIONS(2) { } @@ -32,106 +38,106 @@ CommunicationManager::~CommunicationManager() //***************************************************************************** -bool CommunicationManager::Init( - const std::string& device, const std::size_t baud_rate) +bool CommunicationManager::Init(const std::string& device, + const std::size_t baud_rate) { - if (serial_device_.Init(device, baud_rate)) - { - 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; + if (serial_device_.Init(device, baud_rate)) + { + serial_device_.Set_In_Messages_Pointers(&in_std_messages_, &in_fragments_, + &in_Acks_and_Pings_, &command_responses_, + &in_packet_loss_); + + 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; } //***************************************************************************** void CommunicationManager::Run(DRONE_TYPE drone_type, - RUNNING_MODE running_mode) + RUNNING_MODE running_mode) { - std::thread thread_packets_handler(&PacketsHandler::Run, &packets_handler_); + std::thread thread_packets_handler(&PacketsHandler::Run, &packets_handler_); - Display_Drone_Type_and_Running_Mode(drone_type, running_mode); + Display_Drone_Type_and_Running_Mode(drone_type, running_mode); - if (RUNNING_MODE::SWARM == running_mode) - Run_In_Swarm_Mode(); - else - Run_In_Solo_Mode(drone_type); + if (RUNNING_MODE::SWARM == running_mode) + Run_In_Swarm_Mode(); + else + Run_In_Solo_Mode(drone_type); - serial_device_.Stop_Service(); - serial_device_.Close_Serial_Port(); - packets_handler_.Quit(); - service_thread_->join(); - thread_packets_handler.join(); + serial_device_.Stop_Service(); + serial_device_.Close_Serial_Port(); + packets_handler_.Quit(); + service_thread_->join(); + thread_packets_handler.join(); } //***************************************************************************** void CommunicationManager::Run_In_Solo_Mode(DRONE_TYPE drone_type) { - std::string service_name; - bool success = false; + std::string service_name; + bool success = false; - if (DRONE_TYPE::MASTER == 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); - success = true; - } - else - std::cout << "Failed to Get Service Name: param 'Xbee_In_From_Controller' Not Found." << std::endl; - } - else - { - if (node_handle_.getParam("Xbee_Out_To_Controller", service_name)) - { - mav_dji_client_ = node_handle_.serviceClient(service_name.c_str()); - success = true; - } - else - std::cout << "Failed to Get Service Name: param 'Xbee_Out_To_Controller' Not Found." << std::endl; - } + if (DRONE_TYPE::MASTER == 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); + success = true; + } + else + std::cout << "Failed to Get Service Name: param 'Xbee_In_From_Controller' Not Found." << std::endl; + } + else + { + if (node_handle_.getParam("Xbee_Out_To_Controller", service_name)) + { + mav_dji_client_ = node_handle_.serviceClient(service_name.c_str()); + success = true; + } + else + std::cout << "Failed to Get Service Name: param 'Xbee_Out_To_Controller' Not Found." << std::endl; + } - if (success) - { - ros::Rate loop_rate(LOOP_RATE); + if (success) + { + ros::Rate loop_rate(LOOP_RATE); - while (ros::ok()) - { - Check_In_Messages_and_Transfer_To_Server(); - ros::spinOnce(); - loop_rate.sleep(); - } - } + while (ros::ok()) + { + ros::spinOnce(); + loop_rate.sleep(); + } + } } @@ -139,423 +145,389 @@ void CommunicationManager::Run_In_Solo_Mode(DRONE_TYPE drone_type) //***************************************************************************** void CommunicationManager::Run_In_Swarm_Mode() { - std::string out_messages_topic; - 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, - &CommunicationManager::Send_Mavlink_Message_Callback, this); - success_1 = true; - } - else - std::cout << "Failed to Get Topic Name: param 'Xbee_In_From_Buzz' Not Found." << std::endl; - if (node_handle_.getParam("Xbee_Out_To_Buzz", in_messages_topic)) - { - mavlink_publisher_ = node_handle_.advertise( - in_messages_topic.c_str(), 1000); - success_2 = true; - } - else - std::cout << "Failed to Get Topic Name: param 'Xbee_Out_To_Buzz' Not Found." << std::endl; + if (getRosParams()) + { - if (success_1 && success_2) - { + ros::Rate loop_rate(LOOP_RATE); - 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(); - } - } + while (ros::ok()) + { + triggerRssiUpdate(); + Process_In_Standard_Messages(); + Process_In_Fragments(); + Process_In_Acks_and_Pings(); + Process_In_Packets(); + Process_Packet_Loss(); + Process_Command_Responses(); + packets_handler_.Delete_Packets_With_Time_Out(); + ros::spinOnce(); + loop_rate.sleep(); + } + } } //***************************************************************************** inline bool CommunicationManager::Serve_Flight_Controller(mavros_msgs::CommandInt:: - Request& request, mavros_msgs::CommandInt::Response& response) + Request& request, mavros_msgs::CommandInt::Response& response) // TODO to be cleaned { - const std::size_t MAX_BUFFER_SIZE = 256; - unsigned int command = 0; - char temporary_buffer[MAX_BUFFER_SIZE]; - std::string frame; - - /*switch(request.command) - { - case mavros_msgs::CommandCode::NAV_TAKEOFF: - { - command = static_cast(mavros_msgs::CommandCode::NAV_TAKEOFF); - sprintf(temporary_buffer, "%u ", command); - response.success = true; - break; - } - case mavros_msgs::CommandCode::NAV_LAND: - { - command = static_cast(mavros_msgs::CommandCode::NAV_LAND); - sprintf(temporary_buffer, "%u ", command); - response.success = true; - break; - } - case mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH: - { - command = static_cast(mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH); - sprintf(temporary_buffer, "%u ", command); - response.success = true; - break; - } - case mavros_msgs::CommandCode::NAV_WAYPOINT: - { - command = static_cast(mavros_msgs::CommandCode::NAV_WAYPOINT); - sprintf(temporary_buffer, "%u %u %u %lf %u %u ", - command, request.x, request.y, request.z, 0, 1); // TO DO change 0 and 1 - response.success = true; - break; - } - case mavros_msgs::CommandCode::CMD_MISSION_START: - { - command = static_cast(mavros_msgs::CommandCode::CMD_MISSION_START); - sprintf(temporary_buffer, "%u ", command); - response.success = true; - break; - } - default: - response.success = false; - } - - if (command != 0) - { - Generate_Transmit_Request_Frame(temporary_buffer, &frame); - serial_device_.Send_Frame(frame); - }*/ - - return true; + return true; } //***************************************************************************** void CommunicationManager::Display_Init_Communication_Failure() { - std::cout << "Failed to Init Communication With XBee." << std::endl; - std::cout << "Please Check The Following Parameters:" << std::endl; - std::cout << " 1) Device (e.g. /dev/ttyUSB0 for Linux. " << std::endl; - std::cout << " 2) Baud Rate." << std::endl; - std::cout << " 3) Press Reset Button on The XBee." << std::endl; - std::cout << " 4) Connect and Disconnect The XBee." << std::endl; - std::cout << " 5) Update The XBee Firmware." << std::endl; - std::cout << " 6) Reinstall The FTDI Driver." << std::endl; -} - - -//***************************************************************************** -/*inline void CommunicationManager::Generate_Transmit_Request_Frame( - const char* const message, - std::string * frame, - 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, std::strlen(message)); - - Calculate_and_Append_Checksum(frame); - Add_Length_and_Start_Delimiter(frame); -}*/ - - -//***************************************************************************** -/*inline void CommunicationManager::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); - } -}*/ - - -//***************************************************************************** -/*inline void CommunicationManager::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 CommunicationManager::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 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; - - 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; - - 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) - { - for (std::size_t j = 0; j < size_in_messages; j++) - { - std::shared_ptr in_message = - in_messages_->Pop_Front(); - mavros_msgs:: CommandInt command_msg; - unsigned int command = 0; - - sscanf(in_message->c_str(), "%u", &command); - - if (static_cast(mavros_msgs::CommandCode::NAV_WAYPOINT) == command) - { - Waypoint_S new_waypoint; - sscanf(in_message->c_str(), "%u %u %u %lf %u %u ", - &command, &new_waypoint.latitude, - &new_waypoint.longitude, &new_waypoint.altitude, - &new_waypoint.staytime, &new_waypoint.heading); - command_msg.request.x = new_waypoint.latitude; - command_msg.request.y = new_waypoint.longitude; - command_msg.request.z = new_waypoint.altitude; - // TO DO add staytime and heading; - } - - command_msg.request.command = command; - - if (mav_dji_client_.call(command_msg)) - ROS_INFO("XBeeMav: Command Successfully Tranferred to Dji Mav."); - else - ROS_INFO("XBeeMav: Faild to Tranfer Command."); - - } - }*/ + std::cout << "Failed to Init Communication With XBee." << std::endl; + std::cout << "Please Check The Following Parameters:" << std::endl; + std::cout << " 1) Device (e.g. /dev/ttyUSB0 for Linux. " << std::endl; + std::cout << " 2) Baud Rate." << std::endl; + std::cout << " 3) Press Reset Button on The XBee." << std::endl; + std::cout << " 4) Connect and Disconnect The XBee." << std::endl; + std::cout << " 5) Update The XBee Firmware." << std::endl; + std::cout << " 6) Reinstall The FTDI Driver." << std::endl; } //***************************************************************************** inline void CommunicationManager::Send_Mavlink_Message_Callback( - const mavros_msgs::Mavlink::ConstPtr& mavlink_msg) + const mavros_msgs::Mavlink::ConstPtr& mavlink_msg) { - //std::cout << "Received Message From Buzz" << std::endl; // TO DO delete - packets_handler_.Handle_Mavlink_Message(mavlink_msg); + packets_handler_.Handle_Mavlink_Message(mavlink_msg); } //***************************************************************************** void CommunicationManager::Display_Drone_Type_and_Running_Mode( - DRONE_TYPE drone_type, RUNNING_MODE running_mode) + DRONE_TYPE drone_type, RUNNING_MODE running_mode) { - if (DRONE_TYPE::MASTER == drone_type) - std::cout << "Drone: MASTER" << std::endl; - else - std::cout << "Drone: SLAVE" << std::endl; + if (DRONE_TYPE::MASTER == drone_type) + std::cout << "Drone: MASTER" << std::endl; + else + std::cout << "Drone: SLAVE" << std::endl; - if (RUNNING_MODE::SOLO == running_mode) - std::cout << "XBeeMav Running in SOLO Mode..." << std::endl; - else - std::cout << "XBeeMav Running in SWARM Mode..." << std::endl; + if (RUNNING_MODE::SOLO == running_mode) + std::cout << "XBeeMav Running in SOLO Mode..." << std::endl; + else + std::cout << "XBeeMav Running in SWARM Mode..." << std::endl; } //***************************************************************************** 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); - } - } + 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); - } - } + 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); - } - } + 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() + /* This function publish MAVROS msgs comming the xbee network. + */ { - 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); - } - } + 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()); - } - } + /* This function forwards the command responses to the packethandler to be + * processed. + */ +{ + 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; +//***************************************************************************** +bool CommunicationManager::Get_Param (mavros_msgs::ParamGet::Request& req, + mavros_msgs::ParamGet::Response& res) + /* This function processes the requests sent to the xbee_status rosservice + * The response success is set to true if the param_id of the request is known + * Return: true + */ + //TODO these ids need to be defined as constants +{ + mavros_msgs::ParamValue val; + res.success = true; + + if(req.param_id == "id") + { + val.integer=packets_handler_.getDeviceId(); + } + if(req.param_id == "deque_full") + { + val.integer=packets_handler_.getDequeFull(); + } + ///////////////////////////////////////////////////////// + // RSSI Section + else if(req.param_id == "rssi") + { + val.real=packets_handler_.getSignalStrength(); + } + else if(req.param_id == "trig_rssi_api_avg") + { + if(packets_handler_.triggerAPIRssiUpdate(rssi_payload_size_, + rssi_iterations_, + PacketsHandler::ALL_IDS) == 0) + { + res.success = false; + } + } + else if (safeSubStr(req.param_id, 14) == "trig_rssi_api_") + { + int short_id = std::strtol(req.param_id.substr(14).c_str(), NULL, 10); + if(packets_handler_.triggerAPIRssiUpdate(rssi_payload_size_, + rssi_iterations_, + static_cast(short_id)) == 0) + { + res.success = false; + } + } + else if (req.param_id == "get_rssi_api_avg") + { + val.real = packets_handler_.getAPISignalStrength(PacketsHandler::ALL_IDS); + if(val.real == 0){res.success = false;} + } + else if (safeSubStr(req.param_id, 13) == "get_rssi_api_") + { + int short_id = std::strtol(req.param_id.substr(13).c_str(), NULL, 10); + val.integer = short_id; + val.real = packets_handler_.getAPISignalStrength(static_cast(short_id)); + if(val.real == 0){res.success = false;} + } + ///////////////////////////////////////////////////////// + // Packet loss Section + else if (req.param_id == "pl_raw_avg") + { + val.real = packets_handler_.getRawPacketLoss(PacketsHandler::ALL_IDS); + if(val.real == PacketsHandler::PACKET_LOSS_UNAVAILABLE){res.success = false;} + } + else if (safeSubStr(req.param_id, 7) == "pl_raw_") + { + int short_id = std::strtol(req.param_id.substr(7).c_str(), NULL, 10); + val.integer = short_id; + val.real = packets_handler_.getRawPacketLoss(static_cast(short_id)); + if(val.real == PacketsHandler::PACKET_LOSS_UNAVAILABLE){res.success = false;} + } + else if (req.param_id == "pl_filtered_avg") + { + val.real = packets_handler_.getPacketLoss(PacketsHandler::ALL_IDS); + if(val.real == PacketsHandler::PACKET_LOSS_UNAVAILABLE){res.success = false;} + } + else if (safeSubStr(req.param_id, 12) == "pl_filtered_") + { + int short_id = std::strtol(req.param_id.substr(12).c_str(), NULL, 10); + val.integer = short_id; + val.real = packets_handler_.getPacketLoss(static_cast(short_id)); + if(val.real == PacketsHandler::PACKET_LOSS_UNAVAILABLE){res.success = false;} + } + else + { + res.success = false; + } + + res.value = val; + return true; } -res.value= val; -res.success=true; -return true; + +//***************************************************************************** +bool CommunicationManager::getRosParams() +/* This function queries all the ROS parameters. + * Return False if a mandatory parameter could not be queried. + */ +{ + std::string out_messages_topic; + std::string in_messages_topic; + bool success_get_param_in_topic = false; + bool success_get_param_out_topic = false; + + if (node_handle_.getParam("status_service", out_messages_topic)) + { + StatusSrv_ = node_handle_.advertiseService(out_messages_topic.c_str(), + &CommunicationManager::Get_Param, this); + } + else + { + std::cout << "Failed to Get Status Service Name: param 'status_service' Not Found." << std::endl; + } + + if (node_handle_.getParam("Xbee_In_From_Buzz", out_messages_topic)) + { + mavlink_subscriber_ = node_handle_.subscribe(out_messages_topic.c_str(), 100, + &CommunicationManager::Send_Mavlink_Message_Callback, this); + success_get_param_in_topic = true; + } + else + { + std::cout << "Failed to Get Topic Name: param 'Xbee_In_From_Buzz' Not Found." << std::endl; + } + + if (node_handle_.getParam("Xbee_Out_To_Buzz", in_messages_topic)) + { + mavlink_publisher_ = node_handle_.advertise( + in_messages_topic.c_str(), 100); + success_get_param_out_topic = true; + } + else + { + std::cout << "Failed to Get Topic Name: param 'Xbee_Out_To_Buzz' Not Found." << std::endl; + } + + rate_divider_rssi_ = static_cast(getIntParam("rate_divider_rssi", DEFAULT_RSSI_ITERATIONS)); + rate_divider_packet_loss_ = static_cast(getIntParam("rate_divider_packet_loss_", DEFAULT_RSSI_ITERATIONS)); + rssi_payload_size_ = static_cast(getIntParam("rssi_payload_size", DEFAULT_RSSI_ITERATIONS)); + rssi_iterations_ = static_cast(getIntParam("rssi_iterations", DEFAULT_RSSI_ITERATIONS)); + + return success_get_param_in_topic && success_get_param_out_topic; +} + +//***************************************************************************** +int CommunicationManager::getIntParam(std::string name, int default_value) +{ + int temp_param; + if(node_handle_.getParam(name, temp_param)) + { + return temp_param; + } + else + { + return default_value; + } +} + +//***************************************************************************** +void CommunicationManager::triggerRssiUpdate() +{ + /* This trigger the update of the rssi parameter (Signal strength) every + * rate_divider_rssi_ cycles + */ + static uint8_t rssi_update_count = 0; + + rssi_update_count++; + if(rssi_update_count >= rate_divider_rssi_){ + packets_handler_.triggerRssiUpdate(); + rssi_update_count = 0; + } +} + +//***************************************************************************** +void CommunicationManager::Process_Packet_Loss() +{ + /* This function passes the packet loss messages to the packethandler to be + * processed. + * It also sends the packet loss messages to the other every + * rate_divider_packet_loss_ cycles + */ + std::size_t in_messages_size = in_packet_loss_.Get_Size(); + + if (in_messages_size > 0) + { + for (std::size_t j = 0; j < in_messages_size; j++) + { + std::shared_ptr in_message = + in_packet_loss_.Pop_Front(); + + packets_handler_.processPacketLoss(in_message->c_str()); + } + } + + packet_loss_timer_++; + if (packet_loss_timer_ > rate_divider_packet_loss_) + { + packets_handler_.sendPacketLoss(); + packet_loss_timer_ = 0; + } +} + +//***************************************************************************** +std::string CommunicationManager::safeSubStr(const std::string strg, + const unsigned int index_max) const +/* Verify if the operation substr(0, index_max) is possible on the given string + * and return the substring if successful. + * Error value: empty string; + */ +{ + if(strg.size() > index_max) { + return strg.substr(0,index_max); + } + return std::string(""); + } } - } diff --git a/src/PacketsHandler.cpp b/src/PacketsHandler.cpp index f662614..cbc680f 100644 --- a/src/PacketsHandler.cpp +++ b/src/PacketsHandler.cpp @@ -1,9 +1,20 @@ /* PacketsHandler.cpp -- Packets Handler class for XBee: - Serialize, deserialize, fragment and reassemly mavlink - messages (packets and std messages) -- */ + Serialize, deserialize, fragment and reassemly mavlink + messages (packets and std messages) -- */ /* ------------------------------------------------------------------------- */ /* February 06, 2017 -- @Copyright Aymen Soussia. All rights reserved. */ /* (aymen.soussia@gmail.com) */ +/* Revision + * Date: July 1st, 2017 + * Author: Pierre-Yves Breches + */ +/* ------------------------------------------------------------------------- */ +/* TODO + * * Split PacketsHandler class (the last two could be instantiated by the first one) + * * class PacketsHandler + * * class PacketsLossHandler + * * class APIRssiHandler + */ #include"PacketsHandler.h" @@ -16,19 +27,27 @@ namespace Mist namespace Xbee { +const uint8_t PacketsHandler::ALL_IDS = 0xFF; +const uint16_t PacketsHandler::PACKET_LOSS_UNAVAILABLE = 0xFFFF; +const uint8_t PacketsHandler::MAX_WAITING_OUT_MSG = 10; //***************************************************************************** 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) + MAX_PACEKT_SIZE(64000), + XBEE_NETWORK_MTU(250), + FRAGMENT_HEADER_SIZE(6), + MAX_TIME_TO_SEND_PACKET(30000), // TO DO change it to packet time out + RSSI_COMMAND("DB"), + RSSI_FILTER_GAIN(0.5), + PACKET_LOSS_IDENTIFIER("L"), + MAX_PACKET_LOSS_MSG_ID(100), + PACKET_LOSS_FILTER_GAIN(0.7), + device_64_bits_address_("12345678"), + loaded_SL_(false), + loaded_SH_(false), + rssi_float_(0), + optimum_MT_NBR_(3), + delay_interframes_(100 * 1000) { } @@ -41,425 +60,493 @@ PacketsHandler::~PacketsHandler() //***************************************************************************** bool PacketsHandler::Init(SerialDevice* serial_device, - Thread_Safe_Deque* in_packets) + 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; + 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; + 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(); - } +{ + 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); - } + 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({static_cast(mavlink_msg->msgid & 0xFF), + 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(); - } + 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); + packet_loss_it_ = packet_loss_map_.find(node_8_bits_address); + + if (packet_loss_it_ != packet_loss_map_.end()) + packet_loss_it_->second.received_packets_ = packet_loss_it_->second.received_packets_ + 1; + + 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) +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); + 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) +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') - { - 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); - } + uint8_t packet_ID = frame->at(12); + uint8_t node_8_bits_address = frame->at(13); - 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++; + if (frame->at(11) == 'A') + { + mutex_.lock(); - 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; - } - } + 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) + { + // TO DO add test if the packet was already transmitted to rosbuzz dont transmit ack only if + 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)); + 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]; - } - } + 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 = get64BitsAddress(command_response, 5); + } + + 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]; + } + } + else if (command_response[0] == 'D' && command_response[1] == 'B') + { + // TODO implemetation of a handler for error value responses (outside valid range) + rssi_float_ = filterIIR(static_cast(static_cast(command_response[3])), + rssi_float_, + RSSI_FILTER_GAIN); + } + else if (command_response[0] == 'R' && command_response[1] == 'S') + { + processAPIRssi(command_response); + } + else + { + // nothing to do + } } +//***************************************************************************** +void PacketsHandler::processAPIRssi(const char* command_response) +{ + uint64_t node_address = get64BitsAddress(command_response, 2); + + database_addresses_it_ = database_addresses_.find(node_address); + if (database_addresses_it_ != database_addresses_.end()) + { + rssi_result_map_it_ = rssi_result_map_.find(database_addresses_it_->second); + if(rssi_result_map_it_ == rssi_result_map_.end()) + { + RSSI_Result result ={ucharToUint16(command_response[10], command_response[11]), + ucharToUint16(command_response[12], command_response[13]), + ucharToUint16(command_response[14], command_response[15]), + ucharToUint16(command_response[16], command_response[17]), + static_cast(command_response[18]), + static_cast(command_response[19]), + static_cast(command_response[20]), + static_cast(command_response[21]), + static_cast(command_response[22]), + NEW_VALUE + }; + rssi_result_map_[database_addresses_it_->second] = result; + } + else + { + rssi_result_map_it_->second.payload_size = ucharToUint16(command_response[10], command_response[11]); + rssi_result_map_it_->second.iterations = ucharToUint16(command_response[12], command_response[13]); + rssi_result_map_it_->second.success = ucharToUint16(command_response[14], command_response[15]); + rssi_result_map_it_->second.retries = ucharToUint16(command_response[16], command_response[17]); + rssi_result_map_it_->second.result = static_cast(command_response[18]); + rssi_result_map_it_->second.rr = static_cast(command_response[19]); + rssi_result_map_it_->second.max_rssi = static_cast(command_response[20]); + rssi_result_map_it_->second.min_rssi = static_cast(command_response[21]); + rssi_result_map_it_->second.avg_rssi = static_cast(command_response[22]); + rssi_result_map_it_->second.status = NEW_VALUE; + } + + } +} //***************************************************************************** void PacketsHandler::Quit() { - quit_.store(true); + quit_.store(true); } //***************************************************************************** void PacketsHandler::Serialize_Mavlink_Message(const mavros_msgs::Mavlink::ConstPtr& - mavlink_msg, std::shared_ptr serialized_packet) + 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); - } + 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'); + std::shared_ptr fragment, const uint8_t packet_ID, + const uint8_t fragment_ID, const uint16_t offset) // TO DO change this function +{ + if (!single_fragment) // TO DO delete + { + 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 // TO DO delete + fragment->push_back('S'); // TO DO delete } //***************************************************************************** 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 = {}; - } +{ + 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_); - } - } + std::size_t deque_size = out_std_messages_.Get_Size(); + + if (deque_size > 0) + { + std::string frame; + std::shared_ptr out_message; + if(deque_size > MAX_WAITING_OUT_MSG){deque_full_ = true;} + else{deque_full_ = false;} + + + 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); + + for (auto& it:packet_loss_map_) + { + it.second.sent_packets_ = it.second.sent_packets_ + 1; + } + + 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(); + std::size_t deque_size = out_packets_.Get_Size(); - Send_Packet(out_packet); - } - } + 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); + 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); + + // 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()); + usleep(500 * 1000); + // TO DO sleep after ping + } + + 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_); + 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_); } @@ -511,264 +598,502 @@ bool PacketsHandler::Load_Database_Addresses() //***************************************************************************** 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; - } + 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; + local_64_bits_address_ = local_64_bits_address; + 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; + 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) +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()); - } + 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(); + std::lock_guard guard(mutex_); + + for (auto index: fragments_indexes_to_transmit_) + { + serial_device_->Send_Frame(frames.at(index)); + for (auto& it:packet_loss_map_) + { + it.second.sent_packets_ = it.second.sent_packets_ + 1; + } + + 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; + 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); + 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) + 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); - } -} + 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 = get64BitsAddress(bytes, i); + + mavlink_msg->payload64.push_back(current_int64); + } + + packet_loss_it_ = packet_loss_map_.find(mavlink_msg->sysid); + + if (packet_loss_it_ != packet_loss_map_.end()) + { + packet_loss_it_->second.received_packets_ = packet_loss_it_->second.received_packets_ + 1; + } + else + { + database_addresses_inv_it_ = database_addresses_inv_.find(mavlink_msg->sysid); + if (database_addresses_inv_it_ != database_addresses_inv_.end()) + { + uint64_t long_address = database_addresses_inv_it_->second; + packet_loss_map_.insert(std::pair(mavlink_msg->sysid, + {long_address, 0.0, 0.0, 0, 0, 0, 0})); + } + } +} //***************************************************************************** -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); +uint8_t PacketsHandler::getDeviceId(){ + return device_address_; } - //***************************************************************************** -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); +uint8_t PacketsHandler::getDequeFull(){ + if(deque_full_) {return 1;} + else {return 0;} } - //***************************************************************************** -inline void PacketsHandler::Calculate_and_Append_Checksum( - std::string* frame) +float PacketsHandler::getSignalStrength() { - 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); + return rssi_float_; } - //***************************************************************************** -inline void PacketsHandler::Convert_HEX_To_Bytes( - const std::string& HEX_data, std::string* converted_data) +float PacketsHandler::getAPISignalStrength(uint8_t short_node_id) { - 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); - } + float result = 0; + int nb_node = 0; + if(short_node_id == ALL_IDS) + { + for (auto& it:rssi_result_map_) + { + if(it.second.status == NEW_VALUE && + it.second.result == 0x00) + { + nb_node++; + result += static_cast(it.second.avg_rssi); + it.second.status = OLD_VALUE; + } + } + if(nb_node != 0) + { + result = static_cast(result / nb_node); + } + } + else + { + rssi_result_map_it_ = rssi_result_map_.find(short_node_id); + if(rssi_result_map_it_ != rssi_result_map_.end()) + { + if(rssi_result_map_it_->second.status == NEW_VALUE && + rssi_result_map_it_->second.result == 0x00) + { + result = static_cast(rssi_result_map_it_->second.avg_rssi); + rssi_result_map_it_->second.status = OLD_VALUE; + } + } + } + return result; } - //***************************************************************************** -void PacketsHandler::Generate_AT_Command(const char* command, - std::string* frame, - const unsigned char frame_ID) +float PacketsHandler::getRawPacketLoss(uint8_t short_node_id) { - const unsigned char FAME_TYPE = static_cast(0x09);/* AT Command Frame */ - std::string temporary_parameter_value; + if(short_node_id == ALL_IDS) + { + int count = 0; + float result = 0; + for(const auto& it:packet_loss_map_) + { + result += it.second.packet_loss_raw_; + count++; + } + if(count != 0) + { + return result / static_cast(count); + } + else {return PACKET_LOSS_UNAVAILABLE;} + } + else + { + packet_loss_it_ = packet_loss_map_.find(short_node_id); - frame->push_back(FAME_TYPE); - frame->push_back(frame_ID); - frame->append(command); - - Calculate_and_Append_Checksum(frame); - Add_Length_and_Start_Delimiter(frame); + if (packet_loss_it_ != packet_loss_map_.end()) + { + return packet_loss_map_[short_node_id].packet_loss_raw_; + } + else {return PACKET_LOSS_UNAVAILABLE;} + } } -uint8_t PacketsHandler::get_device_id(){ -return device_address_; +//***************************************************************************** +float PacketsHandler::getPacketLoss(uint8_t short_node_id) +{ + if(short_node_id == ALL_IDS) + { + int count = 0; + float result = 0; + for(const auto& it:packet_loss_map_) + { + result += it.second.packet_loss_filtered_; + count++; + } + if(count != 0) + { + return result / static_cast(count); + } + else {return PACKET_LOSS_UNAVAILABLE;} + } + else + { + packet_loss_it_ = packet_loss_map_.find(short_node_id); + + if (packet_loss_it_ != packet_loss_map_.end()) + { + return packet_loss_map_[short_node_id].packet_loss_filtered_; + } + else {return PACKET_LOSS_UNAVAILABLE;} + } } +//***************************************************************************** +void PacketsHandler::triggerRssiUpdate() +/* Description: function sending the AT commad DB to the Xbee module. + * + ------------------------------------------------------------------ */ +{ + std::string frame; + Generate_AT_Command(RSSI_COMMAND.c_str(), &frame); + serial_device_->Send_Frame(frame); +} + +//***************************************************************************** +uint8_t PacketsHandler::triggerAPIRssiUpdate(uint16_t rssi_payload_size, + uint16_t rssi_iterations, + uint8_t target_id) +/* Description: Function triggering the link testing function described + * pages 29-30 in Ressources/XbeeModule_Datasheet.pdf + * The packet_loss_map_ is used to know which nodes are connected + * instead of the connected_network_nodes_ since the latest is only + * updated when a Ping is sent. + ------------------------------------------------------------------ */ +{ + uint8_t sent_request = 0; + + if(target_id == ALL_IDS) + { + for (const auto& it:packet_loss_map_) + { + std::string frame; + generateLinkTestingFrame(&frame, rssi_payload_size, + rssi_iterations, + device_64_bits_address_, + database_addresses_inv_[it.first]); + serial_device_->Send_Frame(frame); + sent_request++; + + rssi_result_map_it_ = rssi_result_map_.find(it.first); + if(rssi_result_map_it_ == rssi_result_map_.end()) + { + RSSI_Result result ={rssi_payload_size, rssi_iterations, 0, 0, 0, + 0, 0, 0, 0, TRIGGERED}; + rssi_result_map_[database_addresses_it_->second] = result; + } + else + { + rssi_result_map_it_->second.payload_size = rssi_payload_size; + rssi_result_map_it_->second.iterations = rssi_iterations; + rssi_result_map_it_->second.avg_rssi = TRIGGERED; + } + + } + } + else + { + packet_loss_it_ = packet_loss_map_.find(target_id); + if(packet_loss_it_ != packet_loss_map_.end()) + { + std::string frame; + generateLinkTestingFrame(&frame, rssi_payload_size, + rssi_iterations, + device_64_bits_address_, + database_addresses_inv_[packet_loss_it_->first]); + serial_device_->Send_Frame(frame); + sent_request++; + + rssi_result_map_it_ = rssi_result_map_.find(target_id); + if(rssi_result_map_it_ == rssi_result_map_.end()) + { + RSSI_Result result ={rssi_payload_size, rssi_iterations, 0, 0, 0, + 0, 0, 0, 0, TRIGGERED}; + rssi_result_map_[database_addresses_it_->second] = result; + } + else + { + rssi_result_map_it_->second.payload_size = rssi_payload_size; + rssi_result_map_it_->second.iterations = rssi_iterations; + rssi_result_map_it_->second.avg_rssi = TRIGGERED; + } + } + } + + return sent_request; +} + +//***************************************************************************** +float PacketsHandler::computePercentage(const int16_t numerator, const int16_t denumerator) const +{ + return static_cast((numerator * 100.0) / (denumerator * 1.0)); +} + +//***************************************************************************** +float PacketsHandler::filterIIR(const float new_val, const float old_val, const float gain) const +{ + return (new_val * (1.0 - gain)) + (old_val * gain); +} + +//***************************************************************************** +void PacketsHandler::processPacketLoss(const char* packet_loss) +{ + uint8_t source_ID = static_cast(packet_loss[1]); + uint16_t received_packet_from_me = static_cast( + static_cast(static_cast(packet_loss[2])) << 8 | + static_cast(static_cast(packet_loss[3]))); + uint8_t packet_loss_msg_id = static_cast(packet_loss[4]); + + packet_loss_it_ = packet_loss_map_.find(source_ID); + + if (packet_loss_it_ != packet_loss_map_.end()) + { + if (packet_loss_msg_id == packet_loss_it_->second.packet_loss_received_id_) + { + packet_loss_it_->second.packet_loss_received_id_ = (packet_loss_it_->second.packet_loss_received_id_ + 1) % MAX_PACKET_LOSS_MSG_ID; + updatePacketLoss(packet_loss_it_->second, received_packet_from_me); + } + else + { + // Missed a packet(s) loss msg, thus we resynchronize the id + packet_loss_it_->second.packet_loss_received_id_ = (packet_loss_msg_id + 1) % MAX_PACKET_LOSS_MSG_ID; + printf("Resynchronized %i\n", packet_loss_it_->second.packet_loss_received_id_); + } + } + else + { + database_addresses_inv_it_ = database_addresses_inv_.find(source_ID); + if (database_addresses_inv_it_ != database_addresses_inv_.end()) + { + uint64_t long_address = database_addresses_inv_it_->second; + packet_loss_map_.insert(std::pair(source_ID, + {long_address, 0.0, 0.0, 0, 0, 0, 0})); + } + } +} + +//***************************************************************************** +void PacketsHandler::updatePacketLoss(On_Line_Node_S& node, const uint16_t received_packet) +/* Description: Update the packet loss of the node: node. + * The information received is condered valid only if: + * - The number of sent packets is different from 0 + * - The number of received packets is inferior to sent_packets + 2 + * + * The + 2 allows the system to take into consideration the messages sent after + * the packet loss information messages was sent. + ------------------------------------------------------------------ */ +{ + if(node.sent_packets_ != 0) // Division by 0 + { + if(node.sent_packets_ + 2 >= received_packet) + { + node.packet_loss_raw_ = computePercentage((static_cast(node.sent_packets_) - static_cast(received_packet)), + node.sent_packets_); + node.packet_loss_filtered_ = filterIIR(node.packet_loss_raw_, + node.packet_loss_filtered_, + PACKET_LOSS_FILTER_GAIN); + } + else + { + printf("Impossible received packet number: %i for %i sent (coming from %lu)", + static_cast(received_packet), + static_cast(node.sent_packets_), + static_cast(node.device_64_bits_address_)); + } + node.sent_packets_ = 0; + + } + else + { + // Do nothing + } +} + +//***************************************************************************** +void PacketsHandler::sendPacketLoss() +{ + for (auto& it:packet_loss_map_) + { + std::string packet_loss_message = PACKET_LOSS_IDENTIFIER; + std::string packet_loss_frame; + + packet_loss_message.push_back(device_address_); + packet_loss_message.push_back((it.second.received_packets_ & 0xFF00) >> 8); + packet_loss_message.push_back(it.second.received_packets_ & 0x00FF); + packet_loss_message.push_back(it.second.packet_loss_sent_id_); + + it.second.received_packets_ = 0; + it.second.packet_loss_sent_id_ = (it.second.packet_loss_sent_id_ + 1) % MAX_PACKET_LOSS_MSG_ID; + + // MSG format L source id received packets packet loss msg id + // 1 byte 8 bits 16 bits 8 bits + Generate_Transmit_Request_Frame(packet_loss_message.c_str(), + &packet_loss_frame, + packet_loss_message.size(), + static_cast(0x01), + int_to_hex(it.second.device_64_bits_address_, 16), + int_to_hex(static_cast(it.first), 4)); + + serial_device_->Send_Frame(packet_loss_frame); + + } +} + +uint16_t PacketsHandler::ucharToUint16(unsigned char msb, unsigned char lsb) +/* Description: return the numerical value of the corresponding + * binary number msb lsb + ------------------------------------------------------------------ */ + { + return (static_cast(msb)<<8) + static_cast(lsb); + } + + inline uint64_t PacketsHandler::get64BitsAddress(const char* bytes, + const int offset) + { + return ( + static_cast(static_cast(bytes[offset])) << 56 | + static_cast(static_cast(bytes[offset + 1])) << 48 | + static_cast(static_cast(bytes[offset + 2])) << 40 | + static_cast(static_cast(bytes[offset + 3])) << 32 | + static_cast(static_cast(bytes[offset + 4])) << 24 | + static_cast(static_cast(bytes[offset + 5])) << 16 | + static_cast(static_cast(bytes[offset + 6])) << 8 | + static_cast(static_cast(bytes[offset + 7]))); + } + } diff --git a/src/SerialDevice.cpp b/src/SerialDevice.cpp index 5b00d0e..6f312bc 100644 --- a/src/SerialDevice.cpp +++ b/src/SerialDevice.cpp @@ -18,7 +18,7 @@ namespace Xbee //***************************************************************************** SerialDevice::SerialDevice(): - serial_port_(io_service_) + serial_port_(io_service_) { } @@ -31,256 +31,280 @@ SerialDevice::~SerialDevice() //***************************************************************************** bool SerialDevice::Init( - const std::string & device, const std::size_t baud_rate) + const std::string & device, const std::size_t baud_rate) { - serial_port_.open(device); - - if (serial_port_.is_open()) - { - Set_Port_Options(baud_rate); - Init_Frame_Type_Keys(); - return true; - } - else - { - std::cout << "Port Failed To Open." << std::endl; - return false; - } + serial_port_.open(device); + + if (serial_port_.is_open()) + { + Set_Port_Options(baud_rate); + Init_Frame_Type_Keys(); + return true; + } + else + { + std::cout << "Port Failed To Open." << std::endl; + return false; + } } //***************************************************************************** void SerialDevice::Set_Port_Options(const std::size_t baud_rate) { - serial_port_.set_option(boost::asio::serial_port::baud_rate(baud_rate)); - serial_port_.set_option(boost::asio::serial_port::character_size(8)); - serial_port_.set_option(boost::asio::serial_port::parity( - boost::asio::serial_port::serial_port_base::parity::none)); - serial_port_.set_option(boost::asio::serial_port::stop_bits( - boost::asio::serial_port::serial_port_base::stop_bits::one)); - serial_port_.set_option(boost::asio::serial_port::flow_control( - boost::asio::serial_port::serial_port_base::flow_control::none)); + serial_port_.set_option(boost::asio::serial_port::baud_rate(baud_rate)); + serial_port_.set_option(boost::asio::serial_port::character_size(8)); + serial_port_.set_option(boost::asio::serial_port::parity( + boost::asio::serial_port::serial_port_base::parity::none)); + serial_port_.set_option(boost::asio::serial_port::stop_bits( + boost::asio::serial_port::serial_port_base::stop_bits::one)); + serial_port_.set_option(boost::asio::serial_port::flow_control( + boost::asio::serial_port::serial_port_base::flow_control::none)); } //***************************************************************************** void SerialDevice::Send_Frame(const std::string& frame) { - io_service_.post( - [this, frame]() - { - bool write_in_progress = !out_messages_.empty(); - out_messages_.push_back(frame); - - if (!write_in_progress) - Write_Frame(); - }); + io_service_.post( + [this, frame]() + { + bool write_in_progress = !out_messages_.empty(); + out_messages_.push_back(frame); + + if (!write_in_progress) + Write_Frame(); + }); } //***************************************************************************** void SerialDevice::Run_Service() { - Read_Frame_Header(); - io_service_.run(); + Read_Frame_Header(); + io_service_.run(); } //***************************************************************************** void SerialDevice::Stop_Service() { - io_service_.post([this]() {io_service_.stop(); }); + io_service_.post([this]() {io_service_.stop(); }); } //***************************************************************************** void SerialDevice::Close_Serial_Port() { - io_service_.post([this]() {serial_port_.close(); }); + io_service_.post([this]() {serial_port_.close(); }); } //***************************************************************************** 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) + Thread_Safe_Deque* in_fragments, + Thread_Safe_Deque* in_Acks_and_Pings, + Thread_Safe_Deque* command_responses, + Thread_Safe_Deque* in_packet_loss) { - in_std_messages_ = in_std_messages; - in_fragments_ = in_fragments; - in_Acks_and_Pings_ = in_Acks_and_Pings; - command_responses_ = command_responses; + in_std_messages_ = in_std_messages; + in_fragments_ = in_fragments; + in_Acks_and_Pings_ = in_Acks_and_Pings; + command_responses_ = command_responses; + in_packet_loss_ = in_packet_loss; } //***************************************************************************** void SerialDevice::Init_Frame_Type_Keys() { - sscanf("08", "%02X", &FRAME_TYPE_KEYS[AT_COMMAND]); - sscanf("09", "%02X", &FRAME_TYPE_KEYS[AT_COMMAND_QUEUE_REGISTER_VALUE]); - sscanf("10", "%02X", &FRAME_TYPE_KEYS[TRANSMIT_REQUEST]); - sscanf("11", "%02X", - &FRAME_TYPE_KEYS[EXPLICIT_ADDRESSING_COMMAND_FRAME]); - sscanf("17", "%02X", &FRAME_TYPE_KEYS[REMOTE_AT_COMMAND]); - sscanf("88", "%02X", &FRAME_TYPE_KEYS[AT_COMMAND_RESPONSE]); - sscanf("8A", "%02X", &FRAME_TYPE_KEYS[MODEM_STATUS]); - sscanf("8B", "%02X", &FRAME_TYPE_KEYS[TRANSMIT_STATUS]); - sscanf("8D", "%02X", &FRAME_TYPE_KEYS[ROUTE_INFORMATION_PACKET]); - sscanf("8E", "%02X", &FRAME_TYPE_KEYS[AGGREGATE_ADDRESSING_UPDATE]); - sscanf("90", "%02X", &FRAME_TYPE_KEYS[RECEIVE_PACKET]); - sscanf("91", "%02X", &FRAME_TYPE_KEYS[EXPLICIT_RX_INDICATOR]); - sscanf("92", "%02X", &FRAME_TYPE_KEYS[IO_DATA_SAMPLE_RX_INDICATOR]); - sscanf("95", "%02X", &FRAME_TYPE_KEYS[NODE_IDENTIFICATION_INDICATOR]); - sscanf("97", "%02X", &FRAME_TYPE_KEYS[REMOTE_AT_COMMAND_RESPONSE]); + sscanf("08", "%02X", &FRAME_TYPE_KEYS[AT_COMMAND]); + sscanf("09", "%02X", &FRAME_TYPE_KEYS[AT_COMMAND_QUEUE_REGISTER_VALUE]); + sscanf("10", "%02X", &FRAME_TYPE_KEYS[TRANSMIT_REQUEST]); + sscanf("11", "%02X", + &FRAME_TYPE_KEYS[EXPLICIT_ADDRESSING_COMMAND_FRAME]); + sscanf("17", "%02X", &FRAME_TYPE_KEYS[REMOTE_AT_COMMAND]); + sscanf("88", "%02X", &FRAME_TYPE_KEYS[AT_COMMAND_RESPONSE]); + sscanf("8A", "%02X", &FRAME_TYPE_KEYS[MODEM_STATUS]); + sscanf("8B", "%02X", &FRAME_TYPE_KEYS[TRANSMIT_STATUS]); + sscanf("8D", "%02X", &FRAME_TYPE_KEYS[ROUTE_INFORMATION_PACKET]); + sscanf("8E", "%02X", &FRAME_TYPE_KEYS[AGGREGATE_ADDRESSING_UPDATE]); + sscanf("90", "%02X", &FRAME_TYPE_KEYS[RECEIVE_PACKET]); + sscanf("91", "%02X", &FRAME_TYPE_KEYS[EXPLICIT_RX_INDICATOR]); + sscanf("92", "%02X", &FRAME_TYPE_KEYS[IO_DATA_SAMPLE_RX_INDICATOR]); + sscanf("95", "%02X", &FRAME_TYPE_KEYS[NODE_IDENTIFICATION_INDICATOR]); + sscanf("97", "%02X", &FRAME_TYPE_KEYS[REMOTE_AT_COMMAND_RESPONSE]); } //***************************************************************************** void SerialDevice::Read_Frame_Header() { - boost::asio::async_read(serial_port_, - boost::asio::buffer(current_frame_.Get_Frame_Data(), - Xbee::Frame::FRAME_HEADER_LENGTH), - [this](boost::system::error_code error, std::size_t) - { - if (!error) - { - int start_delimiter_position = - current_frame_.Get_Start_Delimiter_Position(); - - if (start_delimiter_position >= 0) - { - if (0 == start_delimiter_position) - current_frame_.Decode_Frame_Header(); - else - { - /* The header is corrupted. */ - boost::asio::async_read(serial_port_, - boost::asio::buffer(current_frame_.Get_Frame_Data(), - start_delimiter_position), - [this](boost::system::error_code error, std::size_t) - { - current_frame_.Rearrange_Corrupted_Header(); - current_frame_.Decode_Frame_Header(); - }); - } + boost::asio::async_read(serial_port_, + boost::asio::buffer(current_frame_.Get_Frame_Data(), + Xbee::Frame::FRAME_HEADER_LENGTH), + [this](boost::system::error_code error, std::size_t) + { + if (!error) + { + int start_delimiter_position = + current_frame_.Get_Start_Delimiter_Position(); - Read_Frame_Body(); - } - else - { - /* The header is totally corrupted, read another header. */ - Read_Frame_Header(); - } - } - else - { - std::cout << "Error Occured:" << std::endl; - std::cout << error << std::endl; - std::cout << "Communication With XBee is Lost." << std::endl; - serial_port_.close(); - } - }); + if (start_delimiter_position >= 0) + { + if (0 == start_delimiter_position) + current_frame_.Decode_Frame_Header(); + else + { + /* The header is corrupted. */ + boost::asio::async_read(serial_port_, + boost::asio::buffer(current_frame_.Get_Frame_Data(), + start_delimiter_position), + [this](boost::system::error_code error, std::size_t) + { + current_frame_.Rearrange_Corrupted_Header(); + current_frame_.Decode_Frame_Header(); + }); + } + + Read_Frame_Body(); + } + else + { + /* The header is totally corrupted, read another header. */ + Read_Frame_Header(); + } + } + else + { + std::cout << "Error Occured:" << std::endl; + std::cout << error << std::endl; + std::cout << "Communication With XBee is Lost." << std::endl; + serial_port_.close(); + } + }); } //***************************************************************************** void SerialDevice::Read_Frame_Body() { - boost::asio::async_read(serial_port_, - boost::asio::buffer(current_frame_.Get_Frame_Body(), - current_frame_.Get_Frame_Body_Length()), - [this](boost::system::error_code error, std::size_t) - { - if (!error) - { - if (current_frame_.Get_Frame_Type() == - FRAME_TYPE_KEYS[RECEIVE_PACKET]) - { - 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); - } + boost::asio::async_read(serial_port_, + boost::asio::buffer(current_frame_.Get_Frame_Body(), + current_frame_.Get_Frame_Body_Length()), + [this](boost::system::error_code error, std::size_t) + { + if (!error) + { + if (current_frame_.Get_Frame_Type() == FRAME_TYPE_KEYS[RECEIVE_PACKET]) + { + char msg_type = current_frame_.Get_Message_Type(); + std::shared_ptr in_message = + std::make_shared(); - Read_Frame_Header(); - } - else - { - std::cout << "Error Occured:" << std::endl; - std::cout << error << std::endl; - std::cout << "Communication With XBee is Lost." << std::endl; - serial_port_.close(); - } - }); + if (msg_type == FRAGMENT_MSG_ID) + { + 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 == ACKNOWLEDGEMENT_MSG_ID || msg_type == PING_MSG_ID) + { + 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 == STANDARD_MSG_ID) + { + 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 (msg_type == PACKET_LOSS_MSG_ID) + { + in_message->append(current_frame_.Get_Frame_Body() + 11, + current_frame_.Get_Frame_Body_Length() - 12); + in_packet_loss_->Push_Back(in_message); + } + + else + { + // nothing to do + } + } + 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); + } + else if (current_frame_.Get_Frame_Type() == + FRAME_TYPE_KEYS[EXPLICIT_RX_INDICATOR]) + { + std::shared_ptr in_message = + std::make_shared(); + + in_message->append(current_frame_.Get_Frame_Body() + 17, + current_frame_.Get_Frame_Body_Length() - 18); + in_message->insert(in_message->begin(), 'S'); + in_message->insert(in_message->begin(), 'R'); + command_responses_->Push_Back(in_message); + } + + Read_Frame_Header(); + } + else + { + std::cout << "Error Occured:" << std::endl; + std::cout << error << std::endl; + std::cout << "Communication With XBee is Lost." << std::endl; + serial_port_.close(); + } + }); } //***************************************************************************** void SerialDevice::Write_Frame() { - boost::asio::async_write(serial_port_, - boost::asio::buffer(out_messages_.front().data(), - out_messages_.front().size()), - [this](boost::system::error_code error, - std::size_t transferred_bytes) - { - if (!error) - { - out_messages_.pop_front(); - if (!out_messages_.empty()) - Write_Frame(); - } - }); + boost::asio::async_write(serial_port_, + boost::asio::buffer(out_messages_.front().data(), + out_messages_.front().size()), + [this](boost::system::error_code error, + std::size_t transferred_bytes) + { + if (!error) + { + out_messages_.pop_front(); + if (!out_messages_.empty()) + Write_Frame(); + } + }); } //***************************************************************************** bool SerialDevice::Is_IO_Service_Stopped() { - return io_service_.stopped(); + return io_service_.stopped(); } //***************************************************************************** void SerialDevice::Reset_IO_Service() { - io_service_.reset(); -} - - + io_service_.reset(); +} + } diff --git a/src/TestBuzz.cpp b/src/TestBuzz.cpp index 3bcffb9..a846510 100644 --- a/src/TestBuzz.cpp +++ b/src/TestBuzz.cpp @@ -3,84 +3,99 @@ /* September 20, 2016 -- @Copyright Aymen Soussia. All rights reserved. */ /* (aymen.soussia@gmail.com) */ - #include -#include -#include +#include +#include #include +#include "mavros_msgs/ParamGet.h" #include - - //***************************************************************************** -void Receive_Payload_Callback(const mavros_msgs::Mavlink::ConstPtr& mavlink_msg) -{ - std::cout << "Received Mavlink Message:" << std::endl; - for (unsigned short i = 0; i < mavlink_msg->payload64.size(); i++) - std::cout << mavlink_msg->payload64.at(i) << std::endl; - std::cout << std::endl; +void Receive_Payload_Callback( + const mavros_msgs::Mavlink::ConstPtr &mavlink_msg) { + std::cout << "Received Mavlink Message:" << std::endl; + for (unsigned short i = 0; i < mavlink_msg->payload64.size(); i++) + std::cout << mavlink_msg->payload64.at(i) << std::endl; + std::cout << std::endl; } - //***************************************************************************** -unsigned int Get_Random_Size(unsigned int min, unsigned int max) -{ - return rand() % (max - min) + min; +unsigned int Get_Random_Size(unsigned int min, unsigned int max) { + return rand() % (max - min) + min; } - //***************************************************************************** -unsigned long long Get_Random_Int64(unsigned long long min, unsigned long long max) -{ - return rand() % (max - min) + min; +unsigned long long Get_Random_Int64(unsigned long long min, + unsigned long long max) { + return rand() % (max - min) + min; } +//***************************************************************************** +void Init_Random_Seed() { srand(time(NULL)); } //***************************************************************************** -void Init_Random_Seed() -{ - srand (time(NULL)); -} +int main(int argc, char **argv) { + const unsigned int MIN_PAYLOAD_SIZE = 1; + const unsigned int MAX_PAYLOAD_SIZE = 5; + const int DEFAULT_RATE = 10; + int rate = DEFAULT_RATE; - -//***************************************************************************** -int main(int argc, char **argv) -{ - const unsigned int MIN_PAYLOAD_SIZE = 750; - const unsigned int MAX_PAYLOAD_SIZE = 752; - 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::Publisher mavlink_pub = + node_handle.advertise("outMavlink", 1000); + ros::Subscriber mavlink_sub = + node_handle.subscribe("inMavlink", 1000, Receive_Payload_Callback); + + mavros_msgs::Mavlink mavlink_msg_; + mavlink_msg_.msgid = 1; + if (argc > 1) { + try + { + rate = atoi(argv[1]); + } + catch (...) + { + rate = DEFAULT_RATE; + } + } + + ros::ServiceClient xbeestatus_srv; + xbeestatus_srv = node_handle.serviceClient("/network_status"); + mavros_msgs::ParamGet::Request robot_id_srv_request; robot_id_srv_request.param_id="id"; + mavros_msgs::ParamGet::Response robot_id_srv_response; + while(!xbeestatus_srv.call(robot_id_srv_request,robot_id_srv_response)){ + ros::Duration(0.1).sleep(); + ROS_WARN("Waiting for Xbee to respond to get device ID"); + } + mavlink_msg_.sysid=robot_id_srv_response.value.integer; + + ros::Rate loop_rate(rate); 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; - unsigned int payload_size = Get_Random_Size(MIN_PAYLOAD_SIZE, MAX_PAYLOAD_SIZE); + unsigned int payload_size = + Get_Random_Size(MIN_PAYLOAD_SIZE, MAX_PAYLOAD_SIZE); + mavlink_msg_.payload64.clear(); - for (unsigned int i = 0; i < payload_size; i++) - { + for (unsigned int i = 0; i < payload_size; i++) { mavlink_msg_.payload64.push_back(Get_Random_Int64(1, ULLONG_MAX)); } std::cout << "Sent Mavlink Message " << count << std::endl; - for (unsigned int i = 0; i < payload_size; i++) - { + for (unsigned int i = 0; i < payload_size; i++) { std::cout << mavlink_msg_.payload64.at(i) << std::endl; } @@ -89,6 +104,7 @@ int main(int argc, char **argv) mavlink_pub.publish(mavlink_msg_); ros::spinOnce(); + loop_rate.sleep(); count++; std::cout << "Press Enter to Send New Mavlink Message..." << std::endl; diff --git a/src/TestBuzzCyclic.cpp b/src/TestBuzzCyclic.cpp new file mode 100644 index 0000000..d808e76 --- /dev/null +++ b/src/TestBuzzCyclic.cpp @@ -0,0 +1,114 @@ +/* RosBuzz.cpp -- Basic ROS Buzz node -- */ +/* ------------------------------------------------------------------------- */ +/* September 20, 2016 -- @Copyright Aymen Soussia. All rights reserved. */ +/* (aymen.soussia@gmail.com) */ + +#include +#include +#include + +#include +#include "mavros_msgs/ParamGet.h" +#include + +//***************************************************************************** +void Receive_Payload_Callback( + const mavros_msgs::Mavlink::ConstPtr &mavlink_msg) { + std::cout << "Received Mavlink Message:" << std::endl; + for (unsigned short i = 0; i < mavlink_msg->payload64.size(); i++) + std::cout << mavlink_msg->payload64.at(i) << std::endl; + std::cout << std::endl; +} + +//***************************************************************************** +unsigned int Get_Random_Size(unsigned int min, unsigned int max) { + return rand() % (max - min) + min; +} + +//***************************************************************************** +unsigned long long Get_Random_Int64(unsigned long long min, + unsigned long long max) { + return rand() % (max - min) + min; +} + +//***************************************************************************** +void Init_Random_Seed() { srand(time(NULL)); } + +//***************************************************************************** +int main(int argc, char **argv) { + const unsigned int MIN_PAYLOAD_SIZE = 10; + const unsigned int MAX_PAYLOAD_SIZE = 32; + const int DEFAULT_RATE = 10; + int rate = DEFAULT_RATE; + + 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); + + mavros_msgs::Mavlink mavlink_msg_; + mavlink_msg_.msgid = 1; + if (argc > 1) { + try + { + rate = atoi(argv[1]); + } + catch (...) + { + rate = DEFAULT_RATE; + } + } + + ros::ServiceClient xbeestatus_srv; + xbeestatus_srv = node_handle.serviceClient("network_status"); + mavros_msgs::ParamGet::Request robot_id_srv_request; robot_id_srv_request.param_id="id"; + mavros_msgs::ParamGet::Response robot_id_srv_response; + while(!xbeestatus_srv.call(robot_id_srv_request,robot_id_srv_response)){ + ros::Duration(0.1).sleep(); + ROS_WARN("Waiting for Xbee to respond to get device ID"); + } + mavlink_msg_.sysid=robot_id_srv_response.value.integer; + + ros::Rate loop_rate(rate); + + 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)) + { + + unsigned int payload_size = + Get_Random_Size(MIN_PAYLOAD_SIZE, MAX_PAYLOAD_SIZE); + mavlink_msg_.payload64.clear(); + + for (unsigned int i = 0; i < payload_size; i++) { + mavlink_msg_.payload64.push_back(Get_Random_Int64(1, ULLONG_MAX)); + } + + std::cout << "Sent Mavlink Message " << count << std::endl; + + /*for (unsigned int i = 0; i < payload_size; i++) { + std::cout << mavlink_msg_.payload64.at(i) << std::endl; + }*/ + + std::cout << std::endl; + + mavlink_pub.publish(mavlink_msg_); + + ros::spinOnce(); + loop_rate.sleep(); + + count++; + // std::cout << "Press Enter to Send New Mavlink Message..." << std::endl; + } + + return 0; +} diff --git a/src/XBeeSetup.cpp b/src/XBeeSetup.cpp new file mode 100644 index 0000000..f19479b --- /dev/null +++ b/src/XBeeSetup.cpp @@ -0,0 +1,70 @@ +/* ---------------------------------------------------------------- + * File: XBeeSetup.cpp + * Date: 02/06/2017 + * Author: Pierre-Yves Breches + * Description: Implementation of the xbeeSetup function to + * configure the xbee module + * Copyright Humanitas Solutions. All rights reserved. + ------------------------------------------------------------------ */ + +#include "XBeeSetup.h" + +bool setupXBee(const std::string &device_port, const unsigned int baud_rate) { + XBeeModule xbee_module; + XMLConfigParser config_parser; + + if (xbee_module.Init_Port(device_port, baud_rate)) { + std::thread th_service(&XBeeModule::Run_Service, &xbee_module); + + while (!xbee_module.Is_Connected() && + !xbee_module.Check_Time_Out_Exceeded()) { + continue; + } + + if (xbee_module.Is_Connected()) { + std::cout << "Connected to XBee." << std::endl; + std::cout << "Loading Config File..." << std::endl; + + if (config_parser.Load_Config()) { + std::cout << "Config Loaded Successfully." << std::endl; + std::cout << "Transferring Data..." << std::endl; + std::vector *config_parameters = + config_parser.Get_Loaded_Parameters(); + + for (std::size_t i = 0; i < config_parameters->size(); i++) { + std::string current_command; + xbee_module.Format_AT_Command(config_parameters->at(i), + ¤t_command); + xbee_module.Send_Data(current_command); + } + + std::string write_command = "ATWR \r"; + xbee_module.Send_Data(write_command); + } + } + + th_service.join(); + + std::cout << "Exiting AT Command Mode..." << std::endl; + + if (xbee_module.Is_Connected()) + { + xbee_module.Exit_AT_Command_Mode(); + + if (config_parser.Is_Config_Loaded_Successfully()) + { + std::cout << "XBee Configured Successfully." << std::endl; + return true; + } + else + { + std::cout << "XBee Configuration Failed." << std::endl; + } + } + else + { + std::cout << "XBee Configuration Failed." << std::endl; + } + } + return false; +} diff --git a/src/Xbee.cpp b/src/Xbee.cpp index 53b2669..51448a5 100644 --- a/src/Xbee.cpp +++ b/src/Xbee.cpp @@ -3,62 +3,73 @@ /* September 20, 2016 -- @Copyright Aymen Soussia. All rights reserved. */ /* (aymen.soussia@gmail.com) */ - +#include "XBeeSetup.h" #include "CommunicationManager.h" - +#include //***************************************************************************** -int main(int argc, char* argv[]) -{ +int main(int argc, char *argv[]) { - try - { - ros::init(argc, argv, "xbee"); - ros::NodeHandle node_handle; + try { + ros::init(argc, argv, "xbee"); - Mist::Xbee::CommunicationManager communication_manager; - 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 = - Mist::Xbee::CommunicationManager::RUNNING_MODE::SOLO; + Mist::Xbee::CommunicationManager communication_manager; + 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 = + Mist::Xbee::CommunicationManager::RUNNING_MODE::SOLO; - if (argc > 1) - { - if (!strcmp(argv[1], "master")) - drone_type = Mist::Xbee::CommunicationManager::DRONE_TYPE::MASTER; + if (argc > 1) { + if (!strcmp(argv[1], "master")) + drone_type = Mist::Xbee::CommunicationManager::DRONE_TYPE::MASTER; - if (argc > 2) - { - if (!strcmp(argv[2], "swarm")) - 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 (argc > 2) { + if (!strcmp(argv[2], "swarm")) + running_mode = Mist::Xbee::CommunicationManager::RUNNING_MODE::SWARM; + } + } - 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); + ros::NodeHandle node_handle; + + 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 (!node_handle.getParam("Baud_rate", baud_rate)) + { + std::cout << "Failed to Get Topic Name: param 'Baud_rate' Not Found." << std::endl; + return EXIT_FAILURE; + } + + //setupXBee(device, static_cast(baud_rate)); + + std::cout << "Going to init !!!!" << std::endl; + + if (communication_manager.Init(device, static_cast(baud_rate))) + { + communication_manager.Run(drone_type, running_mode); + } } - catch (const std::exception& e) + catch (const std::exception &e) { - std::cout << "Error Occured:" << std::endl; - std::cout << e.what() << std::endl; + std::cout << "Error: " << e.what() << std::endl; + std::cout << "Please Try One of The Following Options:" << std::endl; + std::cout << " 1) Change The Baud Rate. Make Sure It Matches The Current Baud Rate Used by The XBee (By default BD = 9600 bps)." << std::endl; + std::cout << " 2) Change The Specified Device (e.g. COM1 for Windows or /dev/tty/USB0 for Linux)." << std::endl; + std::cout << " 3) Disconnect and Connect The XBee." << std::endl; + std::cout << " 4) Press The Reset Button on The XBee." << std::endl; + std::cout << " 5) Update The Firmware With XCTU." << std::endl; + std::cout << "XBee Configuration Failed." << std::endl; } catch (...) { - std::cout << "Unexpected Fatal Error." << std::endl; - std::cout << "Communication With XBee is Lost." << std::endl; - return EXIT_FAILURE; + std::cout << "Unexpected Fatal Error." << std::endl; + std::cout << "Communication With XBee is Lost." << std::endl; + return EXIT_FAILURE; } std::cout << std::endl; diff --git a/src/frame_generators.cpp b/src/frame_generators.cpp new file mode 100644 index 0000000..6661a96 --- /dev/null +++ b/src/frame_generators.cpp @@ -0,0 +1,169 @@ +/* ---------------------------------------------------------------- + * File: frame_generators.cpp + * Created on: 21/06/2017 + * Author: Pierre-Yves Breches + * Description: This file contains functions used for the the creation of xbee + * frame + * Copyright Humanitas Solutions. All rights reserved. + ------------------------------------------------------------------ */ + +#include "frame_generators.h" + +namespace Mist +{ + +namespace Xbee +{ + void 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) + /* Description: Generate a link testing frame as presented page 73-74 + * in Ressources/XbeeModule_Datasheet.pdf + ------------------------------------------------------------------ */ + { + const unsigned char FRAME_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(FRAME_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); + } + + void generateLinkTestingFrame(std::string* frame, + uint16_t rssi_payload_size, + uint16_t rssi_iterations, + std::string device_64_bits_address, + uint64_t target_64_bits_address) + /* Description: Generate a link testing frame as presented page 29-30 + * in Ressources/XbeeModule_Datasheet.pdf + ------------------------------------------------------------------ */ + { + static const unsigned char FRAME_TYPE = static_cast(0x11); + static const unsigned char FRAME_ID = static_cast(0x01); + static const std::string link_testing_params = "FFFEE6E60014C1050000"; + + std::string frame_parameters; + std::string bytes_frame_parameters; + + frame_parameters = link_testing_params; + frame_parameters += int_to_hex(target_64_bits_address, 16); + frame_parameters += int_to_hex(rssi_payload_size, 4); // packet size for testing + frame_parameters += int_to_hex(rssi_iterations, 4); // number of iterations + + Convert_HEX_To_Bytes(frame_parameters, &bytes_frame_parameters); + + frame->push_back(FRAME_TYPE); + frame->push_back(FRAME_ID); + frame->append(device_64_bits_address); + frame->append(bytes_frame_parameters); + + Calculate_and_Append_Checksum(frame); + Add_Length_and_Start_Delimiter(frame); + } + + + void Generate_AT_Command(const char* command, + std::string* frame, + const unsigned char frame_ID) + /* Description: Generate an AT command frame as decribed page 73 + * in Ressources/XbeeModule_Datasheet.pdf + ------------------------------------------------------------------ */ + { + const unsigned char FRAME_TYPE = static_cast(0x09);/* AT Command Frame */ + std::string temporary_parameter_value; + + frame->push_back(FRAME_TYPE); + frame->push_back(frame_ID); + frame->append(command); + + Calculate_and_Append_Checksum(frame); + Add_Length_and_Start_Delimiter(frame); + } + + void Convert_HEX_To_Bytes(const std::string& HEX_data, + std::string* converted_data) + /* Description: + * + ------------------------------------------------------------------ */ + { + 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 Calculate_and_Append_Checksum(std::string* frame) + /* Description: + * + ------------------------------------------------------------------ */ + { + 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); + } + + + void Add_Length_and_Start_Delimiter(std::string* frame) + /* Description: + * + ------------------------------------------------------------------ */ + { + const unsigned short FIVE_BYTES = 5; + const unsigned char START_DLIMITER = static_cast(0x7E); + 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); + } + +} + +} diff --git a/src/main.cpp b/src/main.cpp index 97bb2de..fc6a1b1 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -3,112 +3,49 @@ /* November 30, 2016 -- @Copyright Aymen Soussia. All rights reserved. */ /* (aymen.soussia@gmail.com) */ - -#include - -#include"XBeeModule.h" -#include"XMLConfigParser.h" - - -//***************************************************************************** -void Setup_XBee(const std::string& device_port, const unsigned int baud_rate) -{ - XBeeModule xbee_module; - XMLConfigParser config_parser; - - if (xbee_module.Init_Port(device_port, baud_rate)) - { - std::thread th_service(&XBeeModule::Run_Service, &xbee_module); - - while (!xbee_module.Is_Connected() && !xbee_module.Check_Time_Out_Exceeded()) - { - continue; - } - - if (xbee_module.Is_Connected()) - { - std::cout << "Connected to XBee." << std::endl; - std::cout << "Loading Config File..." << std::endl; - - if (config_parser.Load_Config()) - { - std::cout << "Config Loaded Successfully." << std::endl; - std::cout << "Transferring Data..." << std::endl; - std::vector* config_parameters = config_parser.Get_Loaded_Parameters(); - - for (std::size_t i = 0; i < config_parameters->size(); i++) - { - std::string current_command; - xbee_module.Format_AT_Command(config_parameters->at(i), ¤t_command); - xbee_module.Send_Data(current_command); - } - - std::string write_command = "ATWR \r"; - xbee_module.Send_Data(write_command); - } - } - - th_service.join(); - - std::cout << "Exiting AT Command Mode..." << std::endl; - - if (xbee_module.Is_Connected()) - { - xbee_module.Exit_AT_Command_Mode(); - - if (config_parser.Is_Config_Loaded_Successfully()) - std::cout << "XBee Configured Successfully." << std::endl; - else - std::cout << "XBee Configuration Failed." << std::endl; - } - else - { - std::cout << "XBee Configuration Failed." << std::endl; - } - } -} +#include "XBeeSetup.h" //***************************************************************************** int main(int argc, char*argv[]) { - try - { - std::string device_port; - unsigned int baud_rate = 0; - const unsigned int DEFAULT_BAUD_RATE = 9600; - const std::string DEFAULT_DEVICE_PORT = "/dev/ttyUSB0"; + try + { + std::string device_port; + unsigned int baud_rate = 0; + const unsigned int DEFAULT_BAUD_RATE = 9600; + const std::string DEFAULT_DEVICE_PORT = "/dev/ttyUSB0"; - if (argc < 1) - device_port = DEFAULT_DEVICE_PORT; - else - device_port.append(argv[1]); + if (argc < 1) + device_port = DEFAULT_DEVICE_PORT; + else + device_port.append(argv[1]); - if (argc < 2) - baud_rate = DEFAULT_BAUD_RATE; - else - sscanf(argv[2], "%u", &baud_rate); + if (argc < 2) + baud_rate = DEFAULT_BAUD_RATE; + else + sscanf(argv[2], "%u", &baud_rate); - Setup_XBee(device_port, baud_rate); - } - catch (const std::exception& e) - { - std::cout << "Error: " << e.what() << std::endl; - std::cout << "Please Try One of The Following Options:" << std::endl; - std::cout << " 1) Change The Baud Rate. Make Sure It Matches The Current Baud Rate Used by The XBee (By default BD = 9600 bps)." << std::endl; - std::cout << " 2) Change The Specified Device (e.g. COM1 for Windows or /dev/tty/USB0 for Linux)." << std::endl; - std::cout << " 3) Disconnect and Connect The XBee." << std::endl; - std::cout << " 4) Press The Reset Button on The XBee." << std::endl; - std::cout << " 5) Update The Firmware With XCTU." << std::endl; - std::cout << "XBee Configuration Failed." << std::endl; + //setupXBee(device_port, baud_rate); + } + catch (const std::exception& e) + { + std::cout << "Error: " << e.what() << std::endl; + std::cout << "Please Try One of The Following Options:" << std::endl; + std::cout << " 1) Change The Baud Rate. Make Sure It Matches The Current Baud Rate Used by The XBee (By default BD = 9600 bps)." << std::endl; + std::cout << " 2) Change The Specified Device (e.g. COM1 for Windows or /dev/tty/USB0 for Linux)." << std::endl; + std::cout << " 3) Disconnect and Connect The XBee." << std::endl; + std::cout << " 4) Press The Reset Button on The XBee." << std::endl; + std::cout << " 5) Update The Firmware With XCTU." << std::endl; + std::cout << "XBee Configuration Failed." << std::endl; - } - catch (...) - { - std::cout << " Unexpected Error." << std::endl; - std::cout << "XBee Configuration Failed." << std::endl; - return EXIT_FAILURE; - } + } + catch (...) + { + std::cout << " Unexpected Error." << std::endl; + std::cout << "XBee Configuration Failed." << std::endl; + return EXIT_FAILURE; + } - return EXIT_SUCCESS; + return EXIT_SUCCESS; }