Spiri
/
XbeeMav
Archived
13
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

merge with dev

This commit is contained in:
CIM Husky 2017-09-20 13:23:21 -04:00
parent 959caadd13 78da1ed74b
commit 7f675930a9
17 changed files with 1283 additions and 102 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
CMakeLists.txt
View File

@ -11,6 +11,7 @@ set (BOOST_INCLUDEDIR "/usr/include")
find_package(catkin REQUIRED COMPONENTS find_package(catkin REQUIRED COMPONENTS
roscpp roscpp
std_msgs std_msgs
mavros_msgs
) )
## System dependencies are found with CMake's conventions ## System dependencies are found with CMake's conventions
@ -37,14 +38,18 @@ find_package(catkin REQUIRED COMPONENTS
catkin_package( catkin_package(
INCLUDE_DIRS include INCLUDE_DIRS include
# LIBRARIES xbee_ros_node # LIBRARIES xbee_ros_node
CATKIN_DEPENDS roscpp std_msgs CATKIN_DEPENDS roscpp std_msgs mavros_msgs
# DEPENDS system_lib # DEPENDS system_lib
) )
########### ###########
## Build ## ## Build ##
########### ###########
## Add definitions
add_definitions(
-DDATABASE_PATH="${CMAKE_CURRENT_SOURCE_DIR}/Resources/database.xml"
-DXBEE_CONFIG_PATH="${CMAKE_CURRENT_SOURCE_DIR}/Resources/XBee_Config.xml"
)
## Specify additional locations of header files ## Specify additional locations of header files
include_directories( include_directories(
@ -53,19 +58,19 @@ include_directories(
) )
add_executable(xbee_mav src/Xbee.cpp src/XBeeFrame.cpp src/SerialDevice.cpp src/CommunicationManager.cpp)
add_executable(xbee_mav src/Xbee.cpp src/XBeeFrame.cpp src/SerialDevice.cpp src/CommunicationManager.cpp src/PacketsHandler)
target_link_libraries(xbee_mav ${catkin_LIBRARIES}) target_link_libraries(xbee_mav ${catkin_LIBRARIES})
add_executable(config src/main.cpp src/XBeeModule.cpp src/XMLConfigParser.cpp) add_executable(xbee_config src/main.cpp src/XBeeModule.cpp src/XMLConfigParser.cpp)
target_link_libraries(config ${catkin_LIBRARIES}) target_link_libraries(xbee_config ${catkin_LIBRARIES})
#add_executable(test_controller src/TestController.cpp) #add_executable(test_controller src/TestController.cpp)
#target_link_libraries(test_controller ${catkin_LIBRARIES}) #target_link_libraries(test_controller ${catkin_LIBRARIES})
#add_executable(test_buzz src/TestBuzz.cpp)
#target_link_libraries(test_buzz ${catkin_LIBRARIES})
add_executable(test_buzz src/TestBuzz.cpp)
target_link_libraries(test_buzz ${catkin_LIBRARIES})
############# #############
## Install ## ## Install ##
@ -77,5 +82,3 @@ target_link_libraries(config ${catkin_LIBRARIES})
## Testing ## ## Testing ##
############# #############

2
Resources/XBee_Config.xml
View File

@ -2,7 +2,7 @@
<XBeeConfig> <XBeeConfig>
<Settings> <Settings>
<Parameter Command="CM" Description="Channel Mask">00FFFFFFFFFFFF7FFF</Parameter> <Parameter Command="CM" Description="Channel Mask">00FFFFFFFFFFF7FFFF</Parameter>
<Parameter Command="HP" Description="Preamble ID">1</Parameter> <Parameter Command="HP" Description="Preamble ID">1</Parameter>
<Parameter Command="ID" Description="Network ID">5FFF</Parameter> <Parameter Command="ID" Description="Network ID">5FFF</Parameter>
<Parameter Command="MT" Description="Broadcast Multi-Transmits">3</Parameter> <Parameter Command="MT" Description="Broadcast Multi-Transmits">3</Parameter>

18
Resources/database.xml Normal file
View File

@ -0,0 +1,18 @@
<?xml version="1.0" encoding="UTF-8"?>
<Addresses>
<Device Address= "1" >0013A20040D8CA1E</Device>
<Device Address= "2" >0013A200415278B8</Device>
<Device Address= "3" >0013A2004103B363</Device>
<Device Address= "4" >0013A200415278AD</Device>
<Device Address= "5" >0013A2004103B356</Device>
<Device Address= "6" >0013A200415278B7</Device>
<Device Address= "7" >0013A2004098A7A7</Device>
<Device Address= "8" >0013A2004098A7BA</Device>
<Device Address= "9" >0013A200415A9DDD</Device>
<Device Address= "10" >0013A200415A9DE4</Device>
<Device Address= "11" >0013A200415A9DDF</Device>
<Device Address= "12" >0013A200415A9DDE</Device>
<Device Address= "13" >0013A200415A9DE8</Device>
</Addresses>

38
include/CommunicationManager.h
View File

@ -1,6 +1,6 @@
/* CommunicationManager.h -- Communication Manager class for XBee: /* CommunicationManager.h -- Communication Manager class for XBee:
Handles all communications with other ROS nodes Handles all communications with other ROS nodes
and the serial port -- */ and the serial port -- */
/* ------------------------------------------------------------------------- */ /* ------------------------------------------------------------------------- */
/* September 20, 2016 -- @Copyright Aymen Soussia. All rights reserved. */ /* September 20, 2016 -- @Copyright Aymen Soussia. All rights reserved. */
/* (aymen.soussia@gmail.com) */ /* (aymen.soussia@gmail.com) */
@ -10,12 +10,14 @@
#include <inttypes.h> #include <inttypes.h>
#include<thread> #include<thread>
#include <std_msgs/UInt8.h>
#include<mavros_msgs/CommandCode.h> #include<mavros_msgs/CommandCode.h>
#include<mavros_msgs/CommandInt.h> #include<mavros_msgs/CommandInt.h>
#include<mavros_msgs/Mavlink.h> #include<mavros_msgs/Mavlink.h>
#include <ros/ros.h> #include <ros/ros.h>
#include <mavros_msgs/ParamGet.h>
#include <mavros_msgs/ParamValue.h>
#include"PacketsHandler.h"
#include"SerialDevice.h" #include"SerialDevice.h"
@ -58,7 +60,7 @@ private:
void Run_In_Solo_Mode(DRONE_TYPE drone_type); void Run_In_Solo_Mode(DRONE_TYPE drone_type);
void Run_In_Swarm_Mode(); void Run_In_Swarm_Mode();
void Generate_Transmit_Request_Frame( /*void Generate_Transmit_Request_Frame(
const char* const message, const char* const message,
std::string* frame, std::string* frame,
const unsigned char frame_ID = const unsigned char frame_ID =
@ -66,13 +68,13 @@ private:
const std::string& destination_adssress = "000000000000FFFF", const std::string& destination_adssress = "000000000000FFFF",
const std::string& short_destination_adress = "FFFF", const std::string& short_destination_adress = "FFFF",
const std::string& broadcast_radius = "00", const std::string& broadcast_radius = "00",
const std::string& options = "00"); const std::string& options = "00");*/
void Check_In_Messages_and_Transfer_To_Topics(); //void Check_In_Messages_and_Transfer_To_Topics();
void Display_Init_Communication_Failure(); void Display_Init_Communication_Failure();
void Convert_HEX_To_Bytes(const std::string& HEX_data, //void Convert_HEX_To_Bytes(const std::string& HEX_data,
std::string* converted_data); //std::string* converted_data);
void Calculate_and_Append_Checksum(std::string* frame); //void Calculate_and_Append_Checksum(std::string* frame);
void Add_Length_and_Start_Delimiter(std::string* frame); //void Add_Length_and_Start_Delimiter(std::string* frame);
void Send_Mavlink_Message_Callback( void Send_Mavlink_Message_Callback(
const mavros_msgs::Mavlink::ConstPtr& mavlink_msg); const mavros_msgs::Mavlink::ConstPtr& mavlink_msg);
void Display_Drone_Type_and_Running_Mode(DRONE_TYPE drone_type, void Display_Drone_Type_and_Running_Mode(DRONE_TYPE drone_type,
@ -80,14 +82,28 @@ private:
bool Serve_Flight_Controller(mavros_msgs::CommandInt:: bool Serve_Flight_Controller(mavros_msgs::CommandInt::
Request& request, mavros_msgs::CommandInt::Response& response); Request& request, mavros_msgs::CommandInt::Response& response);
void Check_In_Messages_and_Transfer_To_Server(); 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::SerialDevice serial_device_;
Thread_Safe_Deque* in_messages_; Mist::Xbee::PacketsHandler packets_handler_;
Thread_Safe_Deque in_std_messages_;
Thread_Safe_Deque in_fragments_;
Thread_Safe_Deque in_Acks_and_Pings_;
Thread_Safe_Deque command_responses_;
Thread_Safe_Deque in_packets_;
ros::NodeHandle node_handle_; ros::NodeHandle node_handle_;
ros::Subscriber mavlink_subscriber_; ros::Subscriber mavlink_subscriber_;
ros::Publisher mavlink_publisher_; ros::Publisher mavlink_publisher_;
ros::ServiceClient mav_dji_client_; ros::ServiceClient mav_dji_client_;
ros::ServiceServer mav_dji_server_; ros::ServiceServer mav_dji_server_;
ros::ServiceServer StatusSrv_;
std_msgs::UInt8 device_id_out;
std::shared_ptr<std::thread> service_thread_; // TO DO delete !?
}; };

2
include/MultithreadingDeque.hpp
View File

@ -36,7 +36,7 @@ public:
//**************************************************************************** //****************************************************************************
void Push_Pack(const _T& new_data) void Push_Back(const _T& new_data)
{ {
std::lock_guard<std::mutex> guard(mutex_); std::lock_guard<std::mutex> guard(mutex_);
deque_.push_back(new_data); deque_.push_back(new_data);

153
include/PacketsHandler.h Normal file
View File

@ -0,0 +1,153 @@
/* PacketsHandler.h-- Packets Handler class for XBee:
Serialize, deserialize, fragment and reassemly mavlink
messages -- */
/* ------------------------------------------------------------------------- */
/* February 06, 2017 -- @Copyright Aymen Soussia. All rights reserved. */
/* (aymen.soussia@gmail.com) */
#pragma once
#include<atomic>
#include<inttypes.h>
#include<mutex>
#include<map>
#include<set>
#include<string>
#include<unistd.h>
#include<vector>
#include<boost/property_tree/ptree.hpp>
#include<boost/property_tree/xml_parser.hpp>
#include<boost/foreach.hpp>
#include<boost/filesystem.hpp>
#include<mavros_msgs/Mavlink.h>
#include"MultithreadingDeque.hpp"
#include"SerialDevice.h"
//*****************************************************************************
using boost::property_tree::ptree;
namespace Mist
{
namespace Xbee
{
//*****************************************************************************
class PacketsHandler
{
public:
PacketsHandler();
~PacketsHandler();
bool Init(SerialDevice* serial_device, Thread_Safe_Deque* in_packets);
bool Init_Device_ID();
void Run();
void Handle_Mavlink_Message(const mavros_msgs::Mavlink::ConstPtr& mavlink_msg);
void Process_Fragment(std::shared_ptr<std::string> fragment);
void Process_Ping_Or_Acknowledgement(std::shared_ptr<std::string> 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<uint8_t> 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<std::string> serialized_packet);
void Insert_Fragment_Header(bool single_fragment,
std::shared_ptr<std::string> 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<std::string>* frames,
std::shared_ptr<std::vector<std::shared_ptr<std::string>>> packet);
void Transmit_Fragments(const std::vector<std::string>& 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<unsigned char>(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<unsigned char>(0x01));
struct On_Line_Node_S
{
bool received_hole_packet_;
std::string address_64_bits_;
};
std::set<uint8_t> fragments_indexes_to_transmit_;
SerialDevice* serial_device_;
std::atomic<bool> quit_;
Thread_Safe_Deque out_std_messages_;
Thread_Safe_Deque_Of_Vectors out_packets_;
Thread_Safe_Deque* in_packets_;
std::map<uint8_t, bool> connected_network_nodes_;
std::map<uint8_t, bool>::iterator connected_network_nodes_it_;
std::map<uint8_t, Reassembly_Packet_S> packets_assembly_map_;
std::map<uint8_t, Reassembly_Packet_S>::iterator assembly_map_it_;
std::map<uint64_t, uint8_t> database_addresses_;
std::map<uint64_t, uint8_t>::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_;
};
}
}

20
include/SerialDevice.h
View File

@ -25,7 +25,14 @@ namespace Xbee
{ {
struct Out_Packet_S
{
uint8_t packet_ID_;
std::shared_ptr<std::vector<std::shared_ptr<std::string>>> packet_buffer_;
};
typedef MultithreadingDeque<std::shared_ptr<std::string>> Thread_Safe_Deque; typedef MultithreadingDeque<std::shared_ptr<std::string>> Thread_Safe_Deque;
typedef MultithreadingDeque<Out_Packet_S> Thread_Safe_Deque_Of_Vectors;
//***************************************************************************** //*****************************************************************************
@ -39,7 +46,13 @@ public:
void Send_Frame(const std::string& frame); void Send_Frame(const std::string& frame);
void Run_Service(); void Run_Service();
void Stop_Service(); void Stop_Service();
Thread_Safe_Deque* Get_In_Messages_Pointer(); void Set_In_Messages_Pointers(Thread_Safe_Deque* in_std_messages,
Thread_Safe_Deque* in_fragments,
Thread_Safe_Deque* in_Acks_and_Pings,
Thread_Safe_Deque* command_responses);
bool Is_IO_Service_Stopped(); // TO DO delete this function
void Reset_IO_Service(); // TO DO delete this function
void Close_Serial_Port();
private: private:
@ -71,7 +84,10 @@ private:
boost::asio::io_service io_service_; boost::asio::io_service io_service_;
boost::asio::serial_port serial_port_; boost::asio::serial_port serial_port_;
std::deque<std::string> out_messages_; std::deque<std::string> out_messages_;
Thread_Safe_Deque in_messages_; Thread_Safe_Deque* in_std_messages_;
Thread_Safe_Deque* in_fragments_;
Thread_Safe_Deque* in_Acks_and_Pings_;
Thread_Safe_Deque* command_responses_;
Mist::Xbee::Frame current_frame_; Mist::Xbee::Frame current_frame_;
unsigned int FRAME_TYPE_KEYS[REMOTE_AT_COMMAND_RESPONSE + 1]; unsigned int FRAME_TYPE_KEYS[REMOTE_AT_COMMAND_RESPONSE + 1];
}; };

3
include/XBeeFrame.h
View File

@ -27,7 +27,7 @@ public:
~Frame(); ~Frame();
enum {FRAME_HEADER_LENGTH = 4}; enum {FRAME_HEADER_LENGTH = 4};
enum {MAX_FRAME_BODY_LENGTH = 280}; // TO DO check value enum {MAX_FRAME_BODY_LENGTH = 270}; // TO DO check value
const char* Get_Frame_Data() const; const char* Get_Frame_Data() const;
char* Get_Frame_Data(); char* Get_Frame_Data();
@ -39,6 +39,7 @@ public:
std::size_t Get_Frame_Type() const; std::size_t Get_Frame_Type() const;
int Get_Start_Delimiter_Position(); int Get_Start_Delimiter_Position();
void Rearrange_Corrupted_Header(); void Rearrange_Corrupted_Header();
char Get_Message_Type(); // TO DO const !?
private: private:

6
launch/xbeemav.launch
View File

@ -1,12 +1,14 @@
<launch> <launch>
<!-- xmee_mav Drone type and Commununication Mode --> <!-- xmee_mav Drone type and Commununication Mode -->
<!-- node pkg="xbee_ros_node" type="xbee_mav" name="xbee_mav" args="master solo" output="screen" --> <!-- node pkg="xbee_ros_node" type="xbee_mav" name="xbee_mav" args="master solo" output="screen" -->
<node pkg="xbee_ros_node" type="xbee_mav" name="xbee_mav" args="master swarm" output="screen"/> <node pkg="xbee_ros_node" type="xbee_mav" name="xbee_mav" args="master swarm" output="screen" />
<!--node pkg="xbee_ros_node" type="xbee_mav" name="xbee_mav" args="slave solo" output="screen" --> <!-- node pkg="xbee_ros_node" type="xbee_mav" name="xbee_mav" args="slave solo" output="screen" -->
<!--node pkg="xbee_ros_node" type="xbee_mav" name="xbee_mav" args="slave swarm" output="screen" --> <!--node pkg="xbee_ros_node" type="xbee_mav" name="xbee_mav" args="slave swarm" output="screen" -->
<!-- xmee_mav Topics and Services Names --> <!-- xmee_mav Topics and Services Names -->
<param name="Xbee_In_From_Buzz" type="str" value="outMavlink" /> <param name="Xbee_In_From_Buzz" type="str" value="outMavlink" />
<param name="Xbee_Out_To_Buzz" type="str" value="inMavlink" /> <param name="Xbee_Out_To_Buzz" type="str" value="inMavlink" />
<param name="Xbee_In_From_Controller" type="str" value="xbee_cmd" /> <param name="Xbee_In_From_Controller" type="str" value="xbee_cmd" />
<param name="Xbee_Out_To_Controller" type="str" value="mav_dji_cmd" /> <param name="Xbee_Out_To_Controller" type="str" value="mav_dji_cmd" />
<param name="USB_port" type="str" value="/dev/ttyUSB0" />
<param name="Baud_rate" type="double" value="230400" />
</launch> </launch>

3
package.xml
View File

@ -14,6 +14,9 @@
<run_depend>roscpp</run_depend> <run_depend>roscpp</run_depend>
<run_depend>std_msgs</run_depend> <run_depend>std_msgs</run_depend>
<build_depend>mavros_msgs</build_depend>
<run_depend>mavros_msgs</run_depend>
<export> <export>
</export> </export>
</package> </package>

215
src/CommunicationManager.cpp
View File

@ -37,14 +37,38 @@ bool CommunicationManager::Init(
{ {
if (serial_device_.Init(device, baud_rate)) if (serial_device_.Init(device, baud_rate))
{ {
in_messages_ = serial_device_.Get_In_Messages_Pointer(); serial_device_.Set_In_Messages_Pointers(&in_std_messages_, &in_fragments_,
return true; &in_Acks_and_Pings_, &command_responses_);
service_thread_ = std::make_shared<std::thread>(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 else
{ {
Display_Init_Communication_Failure(); Display_Init_Communication_Failure();
return false; return false;
} }
return true;
} }
@ -53,7 +77,7 @@ bool CommunicationManager::Init(
void CommunicationManager::Run(DRONE_TYPE drone_type, void CommunicationManager::Run(DRONE_TYPE drone_type,
RUNNING_MODE running_mode) RUNNING_MODE running_mode)
{ {
std::thread thread_service(&SerialDevice::Run_Service, &serial_device_); std::thread thread_packets_handler(&PacketsHandler::Run, &packets_handler_);
Display_Drone_Type_and_Running_Mode(drone_type, running_mode); Display_Drone_Type_and_Running_Mode(drone_type, running_mode);
@ -63,7 +87,10 @@ void CommunicationManager::Run(DRONE_TYPE drone_type,
Run_In_Solo_Mode(drone_type); Run_In_Solo_Mode(drone_type);
serial_device_.Stop_Service(); serial_device_.Stop_Service();
thread_service.join(); serial_device_.Close_Serial_Port();
packets_handler_.Quit();
service_thread_->join();
thread_packets_handler.join();
} }
@ -77,7 +104,7 @@ void CommunicationManager::Run_In_Solo_Mode(DRONE_TYPE drone_type)
{ {
if (node_handle_.getParam("Xbee_In_From_Controller", service_name)) if (node_handle_.getParam("Xbee_In_From_Controller", service_name))
{ {
mav_dji_server_ = node_handle_.advertiseService(service_name.c_str(), &CommunicationManager::Serve_Flight_Controller, this); mav_dji_server_ = node_handle_.advertiseService(service_name.c_str(), &CommunicationManager::Serve_Flight_Controller, this);
success = true; success = true;
} }
else else
@ -116,7 +143,7 @@ void CommunicationManager::Run_In_Swarm_Mode()
std::string in_messages_topic; std::string in_messages_topic;
bool success_1 = false; bool success_1 = false;
bool success_2 = 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)) if (node_handle_.getParam("Xbee_In_From_Buzz", out_messages_topic))
{ {
mavlink_subscriber_ = node_handle_.subscribe(out_messages_topic.c_str(), 1000, mavlink_subscriber_ = node_handle_.subscribe(out_messages_topic.c_str(), 1000,
@ -139,12 +166,15 @@ void CommunicationManager::Run_In_Swarm_Mode()
{ {
ros::Rate loop_rate(LOOP_RATE); ros::Rate loop_rate(LOOP_RATE);
while (ros::ok()) while (ros::ok())
{ {
Check_In_Messages_and_Transfer_To_Topics(); Process_In_Standard_Messages();
Process_In_Fragments();
Process_In_Acks_and_Pings();
Process_In_Packets();
packets_handler_.Delete_Packets_With_Time_Out();
ros::spinOnce(); ros::spinOnce();
loop_rate.sleep(); loop_rate.sleep();
} }
} }
} }
@ -154,12 +184,12 @@ void CommunicationManager::Run_In_Swarm_Mode()
inline bool CommunicationManager::Serve_Flight_Controller(mavros_msgs::CommandInt:: inline bool CommunicationManager::Serve_Flight_Controller(mavros_msgs::CommandInt::
Request& request, mavros_msgs::CommandInt::Response& response) Request& request, mavros_msgs::CommandInt::Response& response)
{ {
const std::size_t MAX_BUFFER_SIZE = 255; const std::size_t MAX_BUFFER_SIZE = 256;
unsigned int command = 0; unsigned int command = 0;
char temporary_buffer[MAX_BUFFER_SIZE]; char temporary_buffer[MAX_BUFFER_SIZE];
std::string frame; std::string frame;
switch(request.command) /*switch(request.command)
{ {
case mavros_msgs::CommandCode::NAV_TAKEOFF: case mavros_msgs::CommandCode::NAV_TAKEOFF:
{ {
@ -205,7 +235,7 @@ inline bool CommunicationManager::Serve_Flight_Controller(mavros_msgs::CommandIn
{ {
Generate_Transmit_Request_Frame(temporary_buffer, &frame); Generate_Transmit_Request_Frame(temporary_buffer, &frame);
serial_device_.Send_Frame(frame); serial_device_.Send_Frame(frame);
} }*/
return true; return true;
} }
@ -226,7 +256,7 @@ void CommunicationManager::Display_Init_Communication_Failure()
//***************************************************************************** //*****************************************************************************
inline void CommunicationManager::Generate_Transmit_Request_Frame( /*inline void CommunicationManager::Generate_Transmit_Request_Frame(
const char* const message, const char* const message,
std::string * frame, std::string * frame,
const unsigned char frame_ID, const unsigned char frame_ID,
@ -236,7 +266,7 @@ inline void CommunicationManager::Generate_Transmit_Request_Frame(
const std::string& options) const std::string& options)
{ {
const unsigned char FAME_TYPE = static_cast<unsigned char>(0x10); /* Transmit Request Frame */ const unsigned char FAME_TYPE = static_cast<unsigned char>(0x10); /* Transmit Request Frame */
std::string frame_parameters; /*std::string frame_parameters;
std::string bytes_frame_parameters; std::string bytes_frame_parameters;
frame_parameters.append(destination_adssress); frame_parameters.append(destination_adssress);
@ -253,11 +283,11 @@ inline void CommunicationManager::Generate_Transmit_Request_Frame(
Calculate_and_Append_Checksum(frame); Calculate_and_Append_Checksum(frame);
Add_Length_and_Start_Delimiter(frame); Add_Length_and_Start_Delimiter(frame);
} }*/
//***************************************************************************** //*****************************************************************************
inline void CommunicationManager::Convert_HEX_To_Bytes( /*inline void CommunicationManager::Convert_HEX_To_Bytes(
const std::string& HEX_data, std::string* converted_data) const std::string& HEX_data, std::string* converted_data)
{ {
const unsigned short TWO_BYTES = 2; const unsigned short TWO_BYTES = 2;
@ -273,11 +303,11 @@ inline void CommunicationManager::Convert_HEX_To_Bytes(
temporary_char = static_cast<unsigned char>(temporary_HEX); temporary_char = static_cast<unsigned char>(temporary_HEX);
converted_data->push_back(temporary_char); converted_data->push_back(temporary_char);
} }
} }*/
//***************************************************************************** //*****************************************************************************
inline void CommunicationManager::Calculate_and_Append_Checksum( /*inline void CommunicationManager::Calculate_and_Append_Checksum(
std::string* frame) std::string* frame)
{ {
unsigned short bytes_sum = 0; unsigned short bytes_sum = 0;
@ -292,11 +322,11 @@ inline void CommunicationManager::Calculate_and_Append_Checksum(
checksum = 0xFF - lowest_8_bits; checksum = 0xFF - lowest_8_bits;
checksum_byte = static_cast<unsigned char>(checksum); checksum_byte = static_cast<unsigned char>(checksum);
frame->push_back(checksum_byte); frame->push_back(checksum_byte);
} }*/
//***************************************************************************** //*****************************************************************************
inline void CommunicationManager::Add_Length_and_Start_Delimiter( /*inline void CommunicationManager::Add_Length_and_Start_Delimiter(
std::string* frame) std::string* frame)
{ {
const unsigned short FIVE_BYTES = 5; const unsigned short FIVE_BYTES = 5;
@ -307,7 +337,7 @@ inline void CommunicationManager::Add_Length_and_Start_Delimiter(
std::string header; std::string header;
int frame_length = frame->size() - 1; /* frame_length = frame_size - checksum byte */ int frame_length = frame->size() - 1; /* frame_length = frame_size - checksum byte */
header.push_back(START_DLIMITER); /*header.push_back(START_DLIMITER);
sprintf(temporary_buffer, "%04X", frame_length); sprintf(temporary_buffer, "%04X", frame_length);
sscanf(temporary_buffer, "%02X%02X", &Hex_frame_length_1, sscanf(temporary_buffer, "%02X%02X", &Hex_frame_length_1,
&Hex_frame_length_2); &Hex_frame_length_2);
@ -316,11 +346,11 @@ inline void CommunicationManager::Add_Length_and_Start_Delimiter(
temporary_char = static_cast<unsigned char>(Hex_frame_length_2); temporary_char = static_cast<unsigned char>(Hex_frame_length_2);
header.push_back(temporary_char); header.push_back(temporary_char);
frame->insert(0, header); frame->insert(0, header);
} }*/
//***************************************************************************** //*****************************************************************************
inline void CommunicationManager::Check_In_Messages_and_Transfer_To_Topics() /*inline void CommunicationManager::Check_In_Messages_and_Transfer_To_Topics() // TO DO delete
{ {
std::size_t size_in_messages = in_messages_->Get_Size(); std::size_t size_in_messages = in_messages_->Get_Size();
@ -349,15 +379,15 @@ inline void CommunicationManager::Check_In_Messages_and_Transfer_To_Topics()
} }
} }
} }*/
//***************************************************************************** //*****************************************************************************
inline void CommunicationManager::Check_In_Messages_and_Transfer_To_Server() inline void CommunicationManager::Check_In_Messages_and_Transfer_To_Server() // TO DO change it
{ {
std::size_t size_in_messages = in_messages_->Get_Size(); //std::size_t size_in_messages = in_messages_->Get_Size();
if (size_in_messages > 0) /*if (size_in_messages > 0)
{ {
for (std::size_t j = 0; j < size_in_messages; j++) for (std::size_t j = 0; j < size_in_messages; j++)
{ {
@ -389,7 +419,7 @@ inline void CommunicationManager::Check_In_Messages_and_Transfer_To_Server()
ROS_INFO("XBeeMav: Faild to Tranfer Command."); ROS_INFO("XBeeMav: Faild to Tranfer Command.");
} }
} }*/
} }
@ -397,26 +427,8 @@ inline void CommunicationManager::Check_In_Messages_and_Transfer_To_Server()
inline void CommunicationManager::Send_Mavlink_Message_Callback( inline void CommunicationManager::Send_Mavlink_Message_Callback(
const mavros_msgs::Mavlink::ConstPtr& mavlink_msg) const mavros_msgs::Mavlink::ConstPtr& mavlink_msg)
{ {
const unsigned short MAX_BUFFER_SIZE = 211; /* 20 (length(uint64_t)) * 10 (max int number) + 10 (spaces) + 1 */ //std::cout << "Received Message From Buzz" << std::endl; // TO DO delete
const unsigned short MAX_NBR_OF_INT64 = 10; packets_handler_.Handle_Mavlink_Message(mavlink_msg);
char temporary_buffer[MAX_BUFFER_SIZE];
std::string frame;
int converted_bytes = 0;
/* Check the payload is not too long. Otherwise ignore it. */
if (mavlink_msg->payload64.size() <= MAX_NBR_OF_INT64)
{
for (unsigned short i = 0; i < mavlink_msg->payload64.size(); i++)
{
converted_bytes += sprintf(
temporary_buffer + converted_bytes, "%" PRIu64 " ",
(uint64_t)mavlink_msg->payload64.at(i));
}
Generate_Transmit_Request_Frame(temporary_buffer, &frame);
serial_device_.Send_Frame(frame);
}
} }
@ -436,6 +448,113 @@ void CommunicationManager::Display_Drone_Type_and_Running_Mode(
} }
//*****************************************************************************
void CommunicationManager::Process_In_Standard_Messages()
{
std::size_t in_messages_size = in_std_messages_.Get_Size();
if (in_messages_size > 0)
{
for (std::size_t j = 0; j < in_messages_size; j++)
{
std::shared_ptr<std::string> 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<std::string> 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<std::string> in_message =
in_fragments_.Pop_Front();
packets_handler_.Process_Fragment(in_message);
}
}
}
//*****************************************************************************
void CommunicationManager::Process_In_Packets()
{
std::size_t in_messages_size = in_packets_.Get_Size();
if (in_messages_size > 0)
{
for (std::size_t j = 0; j < in_messages_size; j++)
{
std::shared_ptr<std::string> 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<std::string> in_message =
command_responses_.Pop_Front();
packets_handler_.Process_Command_Response(in_message->c_str());
}
}
}
bool CommunicationManager::Get_Param (mavros_msgs::ParamGet::Request& req, mavros_msgs::ParamGet::Response& res){
mavros_msgs::ParamValue val;
if(req.param_id=="id"){
val.integer=packets_handler_.get_device_id();
} else if(req.param_id=="rssi"){
val.integer=-1;
} else if(req.param_id=="pl"){
val.integer=-1;
}
res.value= val;
res.success=true;
return true;
}
} }

775
src/PacketsHandler.cpp Normal file
View File

@ -0,0 +1,775 @@
/* PacketsHandler.cpp -- Packets Handler class for XBee:
Serialize, deserialize, fragment and reassemly mavlink
messages (packets and std messages) -- */
/* ------------------------------------------------------------------------- */
/* February 06, 2017 -- @Copyright Aymen Soussia. All rights reserved. */
/* (aymen.soussia@gmail.com) */
#include"PacketsHandler.h"
namespace Mist
{
namespace Xbee
{
//*****************************************************************************
PacketsHandler::PacketsHandler():
MAX_PACEKT_SIZE(64000),
XBEE_NETWORK_MTU(250),
FRAGMENT_HEADER_SIZE(6),
MAX_TIME_TO_SEND_PACKET(30000),
START_DLIMITER(static_cast<unsigned char>(0x7E)),
device_64_bits_address_("12345678"),
loaded_SL_(false),
loaded_SH_(false),
optimum_MT_NBR_(3),
delay_interframes_(100 * 1000)
{
}
//*****************************************************************************
PacketsHandler::~PacketsHandler()
{
}
//*****************************************************************************
bool PacketsHandler::Init(SerialDevice* serial_device,
Thread_Safe_Deque* in_packets)
{
serial_device_ = serial_device;
if (!Load_Database_Addresses())
return false;
Send_SL_and_SH_Commands();
in_packets_ = in_packets;
return true;
}
//*****************************************************************************
bool PacketsHandler::Init_Device_ID()
{
if (Get_Local_Address())
return true;
else
return false;
}
//*****************************************************************************
void PacketsHandler::Run()
{
quit_.store(false);
while (!quit_.load())
{
Process_Out_Standard_Messages();
Process_Out_Packets();
}
}
//*****************************************************************************
void PacketsHandler::Handle_Mavlink_Message(const mavros_msgs::Mavlink::ConstPtr& mavlink_msg)
{
std::shared_ptr<std::string> serialized_packet =
std::make_shared<std::string>();
Serialize_Mavlink_Message(mavlink_msg, serialized_packet);
if (serialized_packet->size() > XBEE_NETWORK_MTU && serialized_packet->size() <= MAX_PACEKT_SIZE)
{
std::shared_ptr<std::vector<std::shared_ptr<std::string>>> fragmented_packet =
std::make_shared<std::vector<std::shared_ptr<std::string>>>();
std::size_t offset = 0;
std::size_t NBR_of_bytes = 0;
std::size_t NBR_of_fragments = std::ceil(
static_cast<float>(serialized_packet->size()) / XBEE_NETWORK_MTU);
for (uint8_t i = 0; i < NBR_of_fragments; i++)
{
fragmented_packet->push_back(std::make_shared<std::string>());
NBR_of_bytes = std::min(XBEE_NETWORK_MTU, serialized_packet->size() - offset);
Insert_Fragment_Header(false, fragmented_packet->at(i), mavlink_msg->msgid, i, offset);
fragmented_packet->at(i)->append(serialized_packet->c_str() + offset, NBR_of_bytes);
offset += NBR_of_bytes;
}
out_packets_.Push_Back({mavlink_msg->msgid, fragmented_packet});
}
else if (serialized_packet->size() < XBEE_NETWORK_MTU)
{
serialized_packet->insert(0, 1, 'S');
out_std_messages_.Push_Back(serialized_packet);
}
}
//*****************************************************************************
void PacketsHandler::Process_Fragment(std::shared_ptr<std::string> 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<uint16_t>(
static_cast<uint16_t>(static_cast<unsigned char>(fragment->at(4))) << 8 |
static_cast<uint16_t>(static_cast<unsigned char>(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<uint8_t>::iterator it = assembly_map_it_->second.received_fragments_IDs_.find(fragment_ID);
if (it == assembly_map_it_->second.received_fragments_IDs_.end())
{
if (assembly_map_it_->second.received_fragments_IDs_.size() == 0)
assembly_map_it_->second.time_since_creation_ = std::clock();
Insert_Fragment_In_Packet_Buffer(&assembly_map_it_->second.packet_buffer_, fragment->c_str(), offset, fragment->size());
assembly_map_it_->second.received_fragments_IDs_.insert(fragment_ID);
}
}
else
{
assembly_map_it_->second = {};
assembly_map_it_->second.packet_ID_ = packet_ID;
Insert_Fragment_In_Packet_Buffer(&assembly_map_it_->second.packet_buffer_, fragment->c_str(), offset, fragment->size());
assembly_map_it_->second.received_fragments_IDs_.insert(fragment_ID);
assembly_map_it_->second.time_since_creation_ = std::clock();
}
}
else
{
Add_New_Node_To_Network(node_8_bits_address);
assembly_map_it_ = packets_assembly_map_.find(node_8_bits_address);
assembly_map_it_->second.packet_ID_ = packet_ID;
Insert_Fragment_In_Packet_Buffer(&assembly_map_it_->second.packet_buffer_, fragment->c_str(), offset, fragment->size());
assembly_map_it_->second.received_fragments_IDs_.insert(fragment_ID);
assembly_map_it_->second.time_since_creation_ = std::clock();
}
}
//*****************************************************************************
void PacketsHandler::Insert_Fragment_In_Packet_Buffer(std::string* buffer, const char* fragment, const uint16_t offset, const std::size_t length)
{
if (offset >= buffer->size())
buffer->append(fragment + FRAGMENT_HEADER_SIZE, length - FRAGMENT_HEADER_SIZE);
else
buffer->insert(offset, fragment + FRAGMENT_HEADER_SIZE, length - FRAGMENT_HEADER_SIZE);
}
//*****************************************************************************
void PacketsHandler::Process_Ping_Or_Acknowledgement(std::shared_ptr<std::string> frame)
{
uint8_t packet_ID = frame->at(12);
uint8_t node_8_bits_address = frame->at(13);
if (frame->at(11) == 'A')
{
mutex_.lock();
connected_network_nodes_it_ = connected_network_nodes_.find(node_8_bits_address);
if (connected_network_nodes_it_ == connected_network_nodes_.end())
{
mutex_.unlock();
Add_New_Node_To_Network(node_8_bits_address);
mutex_.lock();
connected_network_nodes_it_ = connected_network_nodes_.find(node_8_bits_address);
}
if (packet_ID == current_processed_packet_ID_)
{
if (frame->size() < 15)
connected_network_nodes_it_->second = true;
else
{
for (uint8_t i = 14; i < frame->size(); i++)
fragments_indexes_to_transmit_.insert(frame->at(i));
}
}
mutex_.unlock();
}
else if (frame->at(11) == 'P')
{
assembly_map_it_ = packets_assembly_map_.find(node_8_bits_address);
if (assembly_map_it_ == packets_assembly_map_.end())
{
Add_New_Node_To_Network(node_8_bits_address);
assembly_map_it_ = packets_assembly_map_.find(node_8_bits_address);
assembly_map_it_->second.packet_ID_ = packet_ID;
assembly_map_it_->second.time_since_creation_ = std::clock();
}
if (assembly_map_it_->second.packet_ID_ == packet_ID)
{
std::string Acknowledgement = "A";
Acknowledgement.push_back(packet_ID);
Acknowledgement.push_back(device_address_);
uint8_t packet_size = frame->at(14);
if (assembly_map_it_->second.received_fragments_IDs_.size() == packet_size)
{
in_packets_->Push_Back(std::make_shared<std::string>(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<uint8_t>::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<uint8_t> empty_set;
packets_assembly_map_.insert(std::pair<uint8_t, Reassembly_Packet_S>(new_node_address, {}));
std::lock_guard<std::mutex> guard(mutex_);
connected_network_nodes_.insert(std::pair<uint8_t, bool>(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<std::mutex> guard(mutex_);
if (command_response[2] == static_cast<unsigned char>(0))
{
new_node_address = static_cast<uint64_t>(
static_cast<unsigned char>(command_response[5]) << 56 |
static_cast<unsigned char>(command_response[6]) << 48 |
static_cast<unsigned char>(command_response[7]) << 40 |
static_cast<unsigned char>(command_response[8]) << 32 |
static_cast<unsigned char>(command_response[9]) << 24 |
static_cast<unsigned char>(command_response[10]) << 16 |
static_cast<unsigned char>(command_response[11]) << 8 |
static_cast<unsigned char>(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<unsigned char>(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<unsigned char>(0))
{
loaded_SL_ = true;
for (std::size_t i = 0; i < 4; i++)
device_64_bits_address_[4 + i] = command_response[3 + i];
}
}
}
//*****************************************************************************
void PacketsHandler::Quit()
{
quit_.store(true);
}
//*****************************************************************************
void PacketsHandler::Serialize_Mavlink_Message(const mavros_msgs::Mavlink::ConstPtr&
mavlink_msg, std::shared_ptr<std::string> serialized_packet)
{
serialized_packet->push_back(mavlink_msg->sysid);
serialized_packet->push_back(mavlink_msg->msgid);
std::string bytes="12345678";
for (std::size_t j = 0; j < mavlink_msg->payload64.size(); j++)
{
for (std::size_t i = 0; i < 8; i++)
bytes[7 - i] = (mavlink_msg->payload64.at(j) >> (i * 8));
serialized_packet->append(bytes);
}
}
//*****************************************************************************
void PacketsHandler::Insert_Fragment_Header(bool single_fragment,
std::shared_ptr<std::string> fragment, const uint8_t packet_ID,
const uint8_t fragment_ID, const uint16_t offset)
{
if (!single_fragment)
{
fragment->push_back('F');
fragment->push_back(packet_ID);
fragment->push_back(device_address_);
fragment->push_back(fragment_ID);
fragment->push_back(offset >> (1 * 8));
fragment->push_back(offset >> (0 * 8));
}
else
fragment->push_back('S');
}
//*****************************************************************************
void PacketsHandler::Delete_Packets_With_Time_Out()
{
for(auto& iterator: packets_assembly_map_)
{
if (std::clock_t() - iterator.second.time_since_creation_ > MAX_TIME_TO_SEND_PACKET && iterator.second.time_since_creation_ != 0)
iterator.second = {};
}
}
//*****************************************************************************
void PacketsHandler::Process_Out_Standard_Messages()
{
std::size_t deque_size = out_std_messages_.Get_Size();
if (deque_size > 0)
{
std::string frame;
std::shared_ptr<std::string> out_message;
for (std::size_t i = 0; i < deque_size; i++)
{
frame.clear();
out_message = out_std_messages_.Pop_Front();
Generate_Transmit_Request_Frame(out_message->c_str(), &frame, out_message->size());
serial_device_->Send_Frame(frame);
usleep(delay_interframes_);
}
}
}
//*****************************************************************************
void PacketsHandler::Process_Out_Packets()
{
std::size_t deque_size = out_packets_.Get_Size();
if (deque_size > 0)
{
Out_Packet_S out_packet;
for (std::size_t i = 0; i < deque_size; i++)
{
Process_Out_Standard_Messages();
out_packet = out_packets_.Pop_Front();
Send_Packet(out_packet);
}
}
}
//*****************************************************************************
void PacketsHandler::Send_Packet(const Out_Packet_S& packet)
{
std::size_t NBR_of_transmission = 0;
std::vector<std::string> frames;
Init_Network_Nodes_For_New_Transmission(packet.packet_ID_, &frames, packet.packet_buffer_);
current_processed_packet_ID_ = packet.packet_ID_;
std::clock_t start_time = std::clock();
while (std::clock() - start_time <= MAX_TIME_TO_SEND_PACKET && !Check_Packet_Transmitted_To_All_Nodes())
{
NBR_of_transmission++;
Transmit_Fragments(frames);
Send_End_Of_Packet_Ping(packet.packet_ID_, packet.packet_buffer_->size());
usleep(500 * 1000);
}
Adjust_Optimum_MT_Number(std::clock() - start_time, NBR_of_transmission);
}
//*****************************************************************************
void PacketsHandler::Send_End_Of_Packet_Ping(const uint8_t packet_ID, const uint8_t total_NBR_of_fragments)
{
std::string ping_message = "P";
std::string ping_frame;
ping_message.push_back(packet_ID);
ping_message.push_back(device_address_);
ping_message.push_back(total_NBR_of_fragments);
Generate_Transmit_Request_Frame(ping_message.c_str(), &ping_frame, ping_message.size());
serial_device_->Send_Frame(ping_frame);
usleep(delay_interframes_);
}
//*****************************************************************************
bool PacketsHandler::Load_Database_Addresses()
{
const std::string FILE_PATH = DATABASE_PATH;
if (!boost::filesystem::exists(FILE_PATH))
{
std::cout << "database.xml Not Found with path: " << FILE_PATH << std::endl;
return false;
}
ptree pt;
boost::property_tree::read_xml(FILE_PATH, pt);
std::string short_address;
std::string address_64_bits;
unsigned int short_address_int;
uint64_t address_64_bits_int;
BOOST_FOREACH(ptree::value_type const&v, pt.get_child("Addresses"))
{
if (v.first == "Device")
{
short_address = v.second.get<std::string>("<xmlattr>.Address");
address_64_bits = v.second.data();
if (sscanf(short_address.c_str(), "%3u", &short_address_int) < 0)
{
std::cout << "Short Address Error. Please Check database.xml For Possible Errors." << std::endl;
return false;
}
if (sscanf(address_64_bits.c_str(), "%" SCNx64, &address_64_bits_int) < 0)
{
std::cout << "64 bits Address Error. Please Check database.xml For Possible Errors." << std::endl;
return false;
}
database_addresses_.insert(std::pair<uint64_t, uint8_t>(address_64_bits_int, static_cast<uint8_t>(short_address_int)));
}
}
return true;
}
//*****************************************************************************
bool PacketsHandler::Get_Local_Address()
{
const useconds_t ONE_SECOND = 1*1000*1000; /* 1s = 1 * 10⁶ microseconds. */
usleep(ONE_SECOND);
if (loaded_SH_ && loaded_SL_)
{
uint64_t local_64_bits_address = (
static_cast<uint64_t>(static_cast<unsigned char>(device_64_bits_address_[0])) << 56 |
static_cast<uint64_t>(static_cast<unsigned char>(device_64_bits_address_[1])) << 48 |
static_cast<uint64_t>(static_cast<unsigned char>(device_64_bits_address_[2])) << 40 |
static_cast<uint64_t>(static_cast<unsigned char>(device_64_bits_address_[3])) << 32 |
static_cast<uint64_t>(static_cast<unsigned char>(device_64_bits_address_[4])) << 24 |
static_cast<uint64_t>(static_cast<unsigned char>(device_64_bits_address_[5])) << 16 |
static_cast<uint64_t>(static_cast<unsigned char>(device_64_bits_address_[6])) << 8 |
static_cast<uint64_t>(static_cast<unsigned char>(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<int>(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<std::mutex> 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<std::string>* frames, std::shared_ptr<std::vector<std::shared_ptr<std::string>>> packet)
{
std::lock_guard<std::mutex> 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<std::string>& frames)
{
std::lock_guard<std::mutex> guard(mutex_);
for (auto index: fragments_indexes_to_transmit_)
{
serial_device_->Send_Frame(frames.at(index));
usleep(delay_interframes_);
}
fragments_indexes_to_transmit_.clear();
}
//*****************************************************************************
void PacketsHandler::Adjust_Optimum_MT_Number(const std::clock_t elapsed_time,
const std::size_t NBR_of_transmission)
{
if (NBR_of_transmission > 1 && elapsed_time < MAX_TIME_TO_SEND_PACKET)
delay_interframes_ += 5000;
else if (NBR_of_transmission == 1 && delay_interframes_ >= 5000)
delay_interframes_ -= 5000;
}
//*****************************************************************************
void PacketsHandler::Send_SL_and_SH_Commands()
{
std::string command;
std::string frame;
command = "SL";
Generate_AT_Command(command.c_str(), &frame);
serial_device_->Send_Frame(frame);
command = "SH";
frame = "";
Generate_AT_Command(command.c_str(), &frame);
serial_device_->Send_Frame(frame);
}
//*****************************************************************************
void PacketsHandler::Deserialize_Mavlink_Message(const char * bytes,
mavros_msgs::Mavlink* mavlink_msg, const std::size_t msg_size)
{
mavlink_msg->sysid = bytes[0];
mavlink_msg->msgid = bytes[1];
uint64_t current_int64 = 0;
for (std::size_t i = 2; i < msg_size; i += 8)
{
current_int64 = (
static_cast<uint64_t>(static_cast<unsigned char>(bytes[i])) << 56 |
static_cast<uint64_t>(static_cast<unsigned char>(bytes[i + 1])) << 48 |
static_cast<uint64_t>(static_cast<unsigned char>(bytes[i + 2])) << 40 |
static_cast<uint64_t>(static_cast<unsigned char>(bytes[i + 3])) << 32 |
static_cast<uint64_t>(static_cast<unsigned char>(bytes[i + 4])) << 24 |
static_cast<uint64_t>(static_cast<unsigned char>(bytes[i + 5])) << 16 |
static_cast<uint64_t>(static_cast<unsigned char>(bytes[i + 6])) << 8 |
static_cast<uint64_t>(static_cast<unsigned char>(bytes[i + 7])));
mavlink_msg->payload64.push_back(current_int64);
}
}
//*****************************************************************************
inline void PacketsHandler::Generate_Transmit_Request_Frame(
const char* message,
std::string * frame,
const std::size_t message_size,
const unsigned char frame_ID,
const std::string& destination_adssress,
const std::string& short_destination_adress,
const std::string& broadcast_radius,
const std::string& options)
{
const unsigned char FAME_TYPE = static_cast<unsigned char>(0x10); /* Transmit Request Frame */
std::string frame_parameters;
std::string bytes_frame_parameters;
frame_parameters.append(destination_adssress);
frame_parameters.append(short_destination_adress);
frame_parameters.append(broadcast_radius);
frame_parameters.append(options);
Convert_HEX_To_Bytes(frame_parameters, &bytes_frame_parameters);
frame->push_back(FAME_TYPE);
frame->push_back(frame_ID);
frame->append(bytes_frame_parameters);
frame->append(message, message_size);
Calculate_and_Append_Checksum(frame);
Add_Length_and_Start_Delimiter(frame);
}
//*****************************************************************************
inline void PacketsHandler::Add_Length_and_Start_Delimiter(
std::string* frame)
{
const unsigned short FIVE_BYTES = 5;
char temporary_buffer[FIVE_BYTES];
unsigned char temporary_char;
int Hex_frame_length_1;
int Hex_frame_length_2;
std::string header;
int frame_length = frame->size() - 1; /* frame_length = frame_size - checksum byte */
header.push_back(START_DLIMITER);
sprintf(temporary_buffer, "%04X", frame_length);
sscanf(temporary_buffer, "%02X%02X", &Hex_frame_length_1,
&Hex_frame_length_2);
temporary_char = static_cast<unsigned char>(Hex_frame_length_1);
header.push_back(temporary_char);
temporary_char = static_cast<unsigned char>(Hex_frame_length_2);
header.push_back(temporary_char);
frame->insert(0, header);
}
//*****************************************************************************
inline void PacketsHandler::Calculate_and_Append_Checksum(
std::string* frame)
{
unsigned short bytes_sum = 0;
unsigned lowest_8_bits = 0;
unsigned short checksum = 0;
unsigned char checksum_byte;
for (unsigned short i = 0; i < frame->size(); i++)
bytes_sum += static_cast<unsigned short>(frame->at(i));
lowest_8_bits = bytes_sum & 0xFF;
checksum = 0xFF - lowest_8_bits;
checksum_byte = static_cast<unsigned char>(checksum);
frame->push_back(checksum_byte);
}
//*****************************************************************************
inline void PacketsHandler::Convert_HEX_To_Bytes(
const std::string& HEX_data, std::string* converted_data)
{
const unsigned short TWO_BYTES = 2;
char temporary_buffer[TWO_BYTES];
unsigned char temporary_char;
int temporary_HEX;
for (unsigned short i = 0; i <= HEX_data.size() - TWO_BYTES;
i += TWO_BYTES)
{
memcpy(temporary_buffer, HEX_data.c_str() + i, TWO_BYTES);
sscanf(temporary_buffer, "%02X", &temporary_HEX);
temporary_char = static_cast<unsigned char>(temporary_HEX);
converted_data->push_back(temporary_char);
}
}
//*****************************************************************************
void PacketsHandler::Generate_AT_Command(const char* command,
std::string* frame,
const unsigned char frame_ID)
{
const unsigned char FAME_TYPE = static_cast<unsigned char>(0x09);/* AT Command Frame */
std::string temporary_parameter_value;
frame->push_back(FAME_TYPE);
frame->push_back(frame_ID);
frame->append(command);
Calculate_and_Append_Checksum(frame);
Add_Length_and_Start_Delimiter(frame);
}
uint8_t PacketsHandler::get_device_id(){
return device_address_;
}
}
}

75
src/SerialDevice.cpp
View File

@ -39,7 +39,6 @@ bool SerialDevice::Init(
{ {
Set_Port_Options(baud_rate); Set_Port_Options(baud_rate);
Init_Frame_Type_Keys(); Init_Frame_Type_Keys();
Read_Frame_Header();
return true; return true;
} }
else else
@ -82,6 +81,7 @@ void SerialDevice::Send_Frame(const std::string& frame)
//***************************************************************************** //*****************************************************************************
void SerialDevice::Run_Service() void SerialDevice::Run_Service()
{ {
Read_Frame_Header();
io_service_.run(); io_service_.run();
} }
@ -90,14 +90,26 @@ void SerialDevice::Run_Service()
void SerialDevice::Stop_Service() 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(); });
} }
//***************************************************************************** //*****************************************************************************
Thread_Safe_Deque * SerialDevice::Get_In_Messages_Pointer() void SerialDevice::Set_In_Messages_Pointers(Thread_Safe_Deque* in_std_messages,
Thread_Safe_Deque* in_fragments,
Thread_Safe_Deque* in_Acks_and_Pings,
Thread_Safe_Deque* command_responses)
{ {
return &in_messages_; in_std_messages_ = in_std_messages;
in_fragments_ = in_fragments;
in_Acks_and_Pings_ = in_Acks_and_Pings;
command_responses_ = command_responses;
} }
@ -156,8 +168,10 @@ void SerialDevice::Read_Frame_Header()
Read_Frame_Body(); Read_Frame_Body();
} }
else else
{
/* The header is totally corrupted, read another header. */ /* The header is totally corrupted, read another header. */
Read_Frame_Header(); Read_Frame_Header();
}
} }
else else
{ {
@ -183,15 +197,42 @@ void SerialDevice::Read_Frame_Body()
if (current_frame_.Get_Frame_Type() == if (current_frame_.Get_Frame_Type() ==
FRAME_TYPE_KEYS[RECEIVE_PACKET]) FRAME_TYPE_KEYS[RECEIVE_PACKET])
{ {
const unsigned short ELEVEN_BYTES = 11; char msg_type = current_frame_.Get_Message_Type();
const unsigned short TWELVE_BYTES = 12;
std::shared_ptr<std::string> in_message = std::shared_ptr<std::string> in_message =
std::make_shared<std::string>(); std::make_shared<std::string>();
in_message->append(current_frame_.Get_Frame_Body()
+ ELEVEN_BYTES, if (msg_type == 'F')
current_frame_.Get_Frame_Body_Length() - TWELVE_BYTES); {
in_messages_.Push_Pack(in_message); 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<std::string> in_message =
std::make_shared<std::string>();
in_message->append(current_frame_.Get_Frame_Body() + 1,
current_frame_.Get_Frame_Body_Length() - 2);
command_responses_->Push_Back(in_message);
} }
Read_Frame_Header(); Read_Frame_Header();
@ -226,6 +267,20 @@ void SerialDevice::Write_Frame()
} }
//*****************************************************************************
bool SerialDevice::Is_IO_Service_Stopped()
{
return io_service_.stopped();
}
//*****************************************************************************
void SerialDevice::Reset_IO_Service()
{
io_service_.reset();
}
} }

20
src/TestBuzz.cpp
View File

@ -8,7 +8,7 @@
#include <stdlib.h> #include <stdlib.h>
#include <time.h> #include <time.h>
#include<mavros_msgs/Mavlink.h> #include <mavros_msgs/Mavlink.h>
#include <ros/ros.h> #include <ros/ros.h>
@ -47,25 +47,28 @@ void Init_Random_Seed()
//***************************************************************************** //*****************************************************************************
int main(int argc, char **argv) int main(int argc, char **argv)
{ {
const unsigned int MIN_PAYLOAD_SIZE = 1; const unsigned int MIN_PAYLOAD_SIZE = 750;
const unsigned int MAX_PAYLOAD_SIZE = 10; const unsigned int MAX_PAYLOAD_SIZE = 752;
ros::init(argc, argv, "flight_controller"); ros::init(argc, argv, "test_buzz");
ros::NodeHandle node_handle; ros::NodeHandle node_handle;
ros::Publisher mavlink_pub = node_handle.advertise<mavros_msgs::Mavlink>("outMavlink", 1000); ros::Publisher mavlink_pub = node_handle.advertise<mavros_msgs::Mavlink>("outMavlink", 1000);
ros::Subscriber mavlink_sub = node_handle.subscribe("inMavlink", 1000, Receive_Payload_Callback); ros::Subscriber mavlink_sub = node_handle.subscribe("inMavlink", 1000, Receive_Payload_Callback);
ros::Rate loop_rate(0.2);
Init_Random_Seed(); Init_Random_Seed();
int count = 0; 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()) while (ros::ok() && std::cin.getline(line, MAX_BUFFER_SIZE))
{ {
mavros_msgs::Mavlink mavlink_msg_; 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);
@ -87,9 +90,8 @@ int main(int argc, char **argv)
ros::spinOnce(); ros::spinOnce();
loop_rate.sleep();
count++; count++;
std::cout << "Press Enter to Send New Mavlink Message..." << std::endl;
} }
return 0; return 0;

7
src/XBeeFrame.cpp
View File

@ -141,6 +141,13 @@ void Frame::Rearrange_Corrupted_Header()
} }
//*****************************************************************************
char Frame::Get_Message_Type()
{
return data_buffer_[15];
}
} }

3
src/XMLConfigParser.cpp
View File

@ -7,6 +7,7 @@
#include "XMLConfigParser.h" #include "XMLConfigParser.h"
//***************************************************************************** //*****************************************************************************
XMLConfigParser::XMLConfigParser(): XMLConfigParser::XMLConfigParser():
config_loaded_successfully_(false) config_loaded_successfully_(false)
@ -23,7 +24,7 @@ XMLConfigParser::~XMLConfigParser()
//***************************************************************************** //*****************************************************************************
bool XMLConfigParser::Load_Config() bool XMLConfigParser::Load_Config()
{ {
const std::string FILE_NAME = "/home/ubuntu/ROS_WS/src/xbeemav/Resources/XBee_Config.xml"; const std::string FILE_NAME = XBEE_CONFIG_PATH;
if (Check_Config_File_Exists(FILE_NAME)) if (Check_Config_File_Exists(FILE_NAME))
{ {

18
src/Xbee.cpp
View File

@ -14,10 +14,11 @@ int main(int argc, char* argv[])
try try
{ {
ros::init(argc, argv, "xbee"); ros::init(argc, argv, "xbee");
ros::NodeHandle node_handle;
Mist::Xbee::CommunicationManager communication_manager; Mist::Xbee::CommunicationManager communication_manager;
const std::string& device = "/dev/ttyUSB0"; // TO DO can be introduced as command. std::string device;
const std::size_t baud_rate = 230400; // TO DO Can be introduced as command. double baud_rate;
Mist::Xbee::CommunicationManager::DRONE_TYPE drone_type = Mist::Xbee::CommunicationManager::DRONE_TYPE drone_type =
Mist::Xbee::CommunicationManager::DRONE_TYPE::SLAVE; Mist::Xbee::CommunicationManager::DRONE_TYPE::SLAVE;
Mist::Xbee::CommunicationManager::RUNNING_MODE running_mode = Mist::Xbee::CommunicationManager::RUNNING_MODE running_mode =
@ -34,9 +35,18 @@ int main(int argc, char* argv[])
running_mode = Mist::Xbee::CommunicationManager::RUNNING_MODE::SWARM; 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 (!node_handle.getParam("Baud_rate", baud_rate))
if (communication_manager.Init(device, 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<std::size_t>(baud_rate)))
communication_manager.Run(drone_type, running_mode); communication_manager.Run(drone_type, running_mode);
} }
catch (const std::exception& e) catch (const std::exception& e)