merge with dev
This commit is contained in:
CIM Husky
commit
7f675930a9
|
|
@ -11,6 +11,7 @@ set (BOOST_INCLUDEDIR "/usr/include")
|
|||
find_package(catkin REQUIRED COMPONENTS
|
||||
roscpp
|
||||
std_msgs
|
||||
mavros_msgs
|
||||
)
|
||||
|
||||
## System dependencies are found with CMake's conventions
|
||||
|
|
@ -37,14 +38,18 @@ find_package(catkin REQUIRED COMPONENTS
|
|||
catkin_package(
|
||||
INCLUDE_DIRS include
|
||||
# LIBRARIES xbee_ros_node
|
||||
CATKIN_DEPENDS roscpp std_msgs
|
||||
CATKIN_DEPENDS roscpp std_msgs mavros_msgs
|
||||
# DEPENDS system_lib
|
||||
)
|
||||
|
||||
###########
|
||||
## Build ##
|
||||
###########
|
||||
|
||||
## Add definitions
|
||||
add_definitions(
|
||||
-DDATABASE_PATH="${CMAKE_CURRENT_SOURCE_DIR}/Resources/database.xml"
|
||||
-DXBEE_CONFIG_PATH="${CMAKE_CURRENT_SOURCE_DIR}/Resources/XBee_Config.xml"
|
||||
)
|
||||
|
||||
## Specify additional locations of header files
|
||||
include_directories(
|
||||
|
|
@ -53,19 +58,19 @@ include_directories(
|
|||
)
|
||||
|
||||
|
||||
add_executable(xbee_mav src/Xbee.cpp src/XBeeFrame.cpp src/SerialDevice.cpp src/CommunicationManager.cpp)
|
||||
|
||||
add_executable(xbee_mav src/Xbee.cpp src/XBeeFrame.cpp src/SerialDevice.cpp src/CommunicationManager.cpp src/PacketsHandler)
|
||||
target_link_libraries(xbee_mav ${catkin_LIBRARIES})
|
||||
|
||||
add_executable(config src/main.cpp src/XBeeModule.cpp src/XMLConfigParser.cpp)
|
||||
target_link_libraries(config ${catkin_LIBRARIES})
|
||||
add_executable(xbee_config src/main.cpp src/XBeeModule.cpp src/XMLConfigParser.cpp)
|
||||
target_link_libraries(xbee_config ${catkin_LIBRARIES})
|
||||
|
||||
#add_executable(test_controller src/TestController.cpp)
|
||||
#target_link_libraries(test_controller ${catkin_LIBRARIES})
|
||||
|
||||
#add_executable(test_buzz src/TestBuzz.cpp)
|
||||
#target_link_libraries(test_buzz ${catkin_LIBRARIES})
|
||||
|
||||
|
||||
add_executable(test_buzz src/TestBuzz.cpp)
|
||||
target_link_libraries(test_buzz ${catkin_LIBRARIES})
|
||||
|
||||
#############
|
||||
## Install ##
|
||||
|
|
@ -77,5 +82,3 @@ target_link_libraries(config ${catkin_LIBRARIES})
|
|||
## Testing ##
|
||||
#############
|
||||
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -2,7 +2,7 @@
|
|||
|
||||
<XBeeConfig>
|
||||
<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="ID" Description="Network ID">5FFF</Parameter>
|
||||
<Parameter Command="MT" Description="Broadcast Multi-Transmits">3</Parameter>
|
||||
|
|
|
|||
|
|
@ -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>
|
||||
|
|
@ -1,6 +1,6 @@
|
|||
/* CommunicationManager.h -- Communication Manager class for XBee:
|
||||
Handles all communications with other ROS nodes
|
||||
and the serial port -- */
|
||||
and the serial port -- */
|
||||
/* ------------------------------------------------------------------------- */
|
||||
/* September 20, 2016 -- @Copyright Aymen Soussia. All rights reserved. */
|
||||
/* (aymen.soussia@gmail.com) */
|
||||
|
|
@ -10,12 +10,14 @@
|
|||
|
||||
#include <inttypes.h>
|
||||
#include<thread>
|
||||
|
||||
#include <std_msgs/UInt8.h>
|
||||
#include<mavros_msgs/CommandCode.h>
|
||||
#include<mavros_msgs/CommandInt.h>
|
||||
#include<mavros_msgs/Mavlink.h>
|
||||
#include <ros/ros.h>
|
||||
|
||||
#include <mavros_msgs/ParamGet.h>
|
||||
#include <mavros_msgs/ParamValue.h>
|
||||
#include"PacketsHandler.h"
|
||||
#include"SerialDevice.h"
|
||||
|
||||
|
||||
|
|
@ -58,7 +60,7 @@ private:
|
|||
|
||||
void Run_In_Solo_Mode(DRONE_TYPE drone_type);
|
||||
void Run_In_Swarm_Mode();
|
||||
void Generate_Transmit_Request_Frame(
|
||||
/*void Generate_Transmit_Request_Frame(
|
||||
const char* const message,
|
||||
std::string* frame,
|
||||
const unsigned char frame_ID =
|
||||
|
|
@ -66,13 +68,13 @@ private:
|
|||
const std::string& destination_adssress = "000000000000FFFF",
|
||||
const std::string& short_destination_adress = "FFFF",
|
||||
const std::string& broadcast_radius = "00",
|
||||
const std::string& options = "00");
|
||||
void Check_In_Messages_and_Transfer_To_Topics();
|
||||
const std::string& options = "00");*/
|
||||
//void Check_In_Messages_and_Transfer_To_Topics();
|
||||
void Display_Init_Communication_Failure();
|
||||
void Convert_HEX_To_Bytes(const std::string& HEX_data,
|
||||
std::string* converted_data);
|
||||
void Calculate_and_Append_Checksum(std::string* frame);
|
||||
void Add_Length_and_Start_Delimiter(std::string* frame);
|
||||
//void Convert_HEX_To_Bytes(const std::string& HEX_data,
|
||||
//std::string* converted_data);
|
||||
//void Calculate_and_Append_Checksum(std::string* frame);
|
||||
//void Add_Length_and_Start_Delimiter(std::string* frame);
|
||||
void Send_Mavlink_Message_Callback(
|
||||
const mavros_msgs::Mavlink::ConstPtr& mavlink_msg);
|
||||
void Display_Drone_Type_and_Running_Mode(DRONE_TYPE drone_type,
|
||||
|
|
@ -80,14 +82,28 @@ private:
|
|||
bool Serve_Flight_Controller(mavros_msgs::CommandInt::
|
||||
Request& request, mavros_msgs::CommandInt::Response& response);
|
||||
void Check_In_Messages_and_Transfer_To_Server();
|
||||
void Process_In_Standard_Messages();
|
||||
void Process_In_Acks_and_Pings();
|
||||
void Process_In_Fragments();
|
||||
void Process_In_Packets();
|
||||
void Process_Command_Responses();
|
||||
bool Get_Param(mavros_msgs::ParamGet::Request& req, mavros_msgs::ParamGet::Response& res);
|
||||
|
||||
Mist::Xbee::SerialDevice serial_device_;
|
||||
Thread_Safe_Deque* in_messages_;
|
||||
Mist::Xbee::PacketsHandler packets_handler_;
|
||||
Thread_Safe_Deque in_std_messages_;
|
||||
Thread_Safe_Deque in_fragments_;
|
||||
Thread_Safe_Deque in_Acks_and_Pings_;
|
||||
Thread_Safe_Deque command_responses_;
|
||||
Thread_Safe_Deque in_packets_;
|
||||
ros::NodeHandle node_handle_;
|
||||
ros::Subscriber mavlink_subscriber_;
|
||||
ros::Publisher mavlink_publisher_;
|
||||
ros::ServiceClient mav_dji_client_;
|
||||
ros::ServiceServer mav_dji_server_;
|
||||
ros::ServiceServer StatusSrv_;
|
||||
std_msgs::UInt8 device_id_out;
|
||||
std::shared_ptr<std::thread> service_thread_; // TO DO delete !?
|
||||
};
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -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_);
|
||||
deque_.push_back(new_data);
|
||||
|
|
|
|||
|
|
@ -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_;
|
||||
};
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
|
@ -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<Out_Packet_S> Thread_Safe_Deque_Of_Vectors;
|
||||
|
||||
|
||||
//*****************************************************************************
|
||||
|
|
@ -39,7 +46,13 @@ public:
|
|||
void Send_Frame(const std::string& frame);
|
||||
void Run_Service();
|
||||
void Stop_Service();
|
||||
Thread_Safe_Deque* Get_In_Messages_Pointer();
|
||||
void Set_In_Messages_Pointers(Thread_Safe_Deque* in_std_messages,
|
||||
Thread_Safe_Deque* in_fragments,
|
||||
Thread_Safe_Deque* in_Acks_and_Pings,
|
||||
Thread_Safe_Deque* command_responses);
|
||||
bool Is_IO_Service_Stopped(); // TO DO delete this function
|
||||
void Reset_IO_Service(); // TO DO delete this function
|
||||
void Close_Serial_Port();
|
||||
|
||||
private:
|
||||
|
||||
|
|
@ -71,7 +84,10 @@ private:
|
|||
boost::asio::io_service io_service_;
|
||||
boost::asio::serial_port serial_port_;
|
||||
std::deque<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_;
|
||||
unsigned int FRAME_TYPE_KEYS[REMOTE_AT_COMMAND_RESPONSE + 1];
|
||||
};
|
||||
|
|
|
|||
|
|
@ -27,7 +27,7 @@ public:
|
|||
~Frame();
|
||||
|
||||
enum {FRAME_HEADER_LENGTH = 4};
|
||||
enum {MAX_FRAME_BODY_LENGTH = 280}; // TO DO check value
|
||||
enum {MAX_FRAME_BODY_LENGTH = 270}; // TO DO check value
|
||||
|
||||
const char* Get_Frame_Data() const;
|
||||
char* Get_Frame_Data();
|
||||
|
|
@ -39,6 +39,7 @@ public:
|
|||
std::size_t Get_Frame_Type() const;
|
||||
int Get_Start_Delimiter_Position();
|
||||
void Rearrange_Corrupted_Header();
|
||||
char Get_Message_Type(); // TO DO const !?
|
||||
|
||||
|
||||
private:
|
||||
|
|
|
|||
|
|
@ -1,12 +1,14 @@
|
|||
<launch>
|
||||
<!-- 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 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="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 swarm" output="screen" -->
|
||||
<!-- xmee_mav Topics and Services Names -->
|
||||
<param name="Xbee_In_From_Buzz" type="str" value="outMavlink" />
|
||||
<param name="Xbee_Out_To_Buzz" type="str" value="inMavlink" />
|
||||
<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="USB_port" type="str" value="/dev/ttyUSB0" />
|
||||
<param name="Baud_rate" type="double" value="230400" />
|
||||
</launch>
|
||||
|
|
|
|||
|
|
@ -13,6 +13,9 @@
|
|||
<build_depend>std_msgs</build_depend>
|
||||
<run_depend>roscpp</run_depend>
|
||||
<run_depend>std_msgs</run_depend>
|
||||
|
||||
<build_depend>mavros_msgs</build_depend>
|
||||
<run_depend>mavros_msgs</run_depend>
|
||||
|
||||
<export>
|
||||
</export>
|
||||
|
|
|
|||
|
|
@ -37,14 +37,38 @@ bool CommunicationManager::Init(
|
|||
{
|
||||
if (serial_device_.Init(device, baud_rate))
|
||||
{
|
||||
in_messages_ = serial_device_.Get_In_Messages_Pointer();
|
||||
return true;
|
||||
serial_device_.Set_In_Messages_Pointers(&in_std_messages_, &in_fragments_,
|
||||
&in_Acks_and_Pings_, &command_responses_);
|
||||
|
||||
service_thread_ = std::make_shared<std::thread>(std::thread(&SerialDevice::Run_Service, &serial_device_));
|
||||
|
||||
if (!packets_handler_.Init(&serial_device_, &in_packets_))
|
||||
return false;
|
||||
|
||||
std::clock_t elapsed_time = std::clock();
|
||||
bool device_ID_loaded = false;
|
||||
|
||||
while (std::clock() - elapsed_time <= 300000)
|
||||
{
|
||||
Process_Command_Responses();
|
||||
|
||||
if (packets_handler_.Init_Device_ID())
|
||||
{
|
||||
device_ID_loaded = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (!device_ID_loaded)
|
||||
return false;
|
||||
}
|
||||
else
|
||||
{
|
||||
Display_Init_Communication_Failure();
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -53,7 +77,7 @@ bool CommunicationManager::Init(
|
|||
void CommunicationManager::Run(DRONE_TYPE drone_type,
|
||||
RUNNING_MODE running_mode)
|
||||
{
|
||||
std::thread thread_service(&SerialDevice::Run_Service, &serial_device_);
|
||||
std::thread thread_packets_handler(&PacketsHandler::Run, &packets_handler_);
|
||||
|
||||
Display_Drone_Type_and_Running_Mode(drone_type, running_mode);
|
||||
|
||||
|
|
@ -63,7 +87,10 @@ void CommunicationManager::Run(DRONE_TYPE drone_type,
|
|||
Run_In_Solo_Mode(drone_type);
|
||||
|
||||
serial_device_.Stop_Service();
|
||||
thread_service.join();
|
||||
serial_device_.Close_Serial_Port();
|
||||
packets_handler_.Quit();
|
||||
service_thread_->join();
|
||||
thread_packets_handler.join();
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -77,7 +104,7 @@ void CommunicationManager::Run_In_Solo_Mode(DRONE_TYPE drone_type)
|
|||
{
|
||||
if (node_handle_.getParam("Xbee_In_From_Controller", service_name))
|
||||
{
|
||||
mav_dji_server_ = node_handle_.advertiseService(service_name.c_str(), &CommunicationManager::Serve_Flight_Controller, this);
|
||||
mav_dji_server_ = node_handle_.advertiseService(service_name.c_str(), &CommunicationManager::Serve_Flight_Controller, this);
|
||||
success = true;
|
||||
}
|
||||
else
|
||||
|
|
@ -116,7 +143,7 @@ void CommunicationManager::Run_In_Swarm_Mode()
|
|||
std::string in_messages_topic;
|
||||
bool success_1 = false;
|
||||
bool success_2 = false;
|
||||
|
||||
StatusSrv_ = node_handle_.advertiseService("/xbee_status", &CommunicationManager::Get_Param, this);
|
||||
if (node_handle_.getParam("Xbee_In_From_Buzz", out_messages_topic))
|
||||
{
|
||||
mavlink_subscriber_ = node_handle_.subscribe(out_messages_topic.c_str(), 1000,
|
||||
|
|
@ -137,14 +164,17 @@ void CommunicationManager::Run_In_Swarm_Mode()
|
|||
|
||||
if (success_1 && success_2)
|
||||
{
|
||||
|
||||
ros::Rate loop_rate(LOOP_RATE);
|
||||
|
||||
ros::Rate loop_rate(LOOP_RATE);
|
||||
while (ros::ok())
|
||||
{
|
||||
Check_In_Messages_and_Transfer_To_Topics();
|
||||
Process_In_Standard_Messages();
|
||||
Process_In_Fragments();
|
||||
Process_In_Acks_and_Pings();
|
||||
Process_In_Packets();
|
||||
packets_handler_.Delete_Packets_With_Time_Out();
|
||||
ros::spinOnce();
|
||||
loop_rate.sleep();
|
||||
loop_rate.sleep();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -154,12 +184,12 @@ void CommunicationManager::Run_In_Swarm_Mode()
|
|||
inline bool CommunicationManager::Serve_Flight_Controller(mavros_msgs::CommandInt::
|
||||
Request& request, mavros_msgs::CommandInt::Response& response)
|
||||
{
|
||||
const std::size_t MAX_BUFFER_SIZE = 255;
|
||||
const std::size_t MAX_BUFFER_SIZE = 256;
|
||||
unsigned int command = 0;
|
||||
char temporary_buffer[MAX_BUFFER_SIZE];
|
||||
std::string frame;
|
||||
|
||||
switch(request.command)
|
||||
/*switch(request.command)
|
||||
{
|
||||
case mavros_msgs::CommandCode::NAV_TAKEOFF:
|
||||
{
|
||||
|
|
@ -205,7 +235,7 @@ inline bool CommunicationManager::Serve_Flight_Controller(mavros_msgs::CommandIn
|
|||
{
|
||||
Generate_Transmit_Request_Frame(temporary_buffer, &frame);
|
||||
serial_device_.Send_Frame(frame);
|
||||
}
|
||||
}*/
|
||||
|
||||
return true;
|
||||
}
|
||||
|
|
@ -226,7 +256,7 @@ void CommunicationManager::Display_Init_Communication_Failure()
|
|||
|
||||
|
||||
//*****************************************************************************
|
||||
inline void CommunicationManager::Generate_Transmit_Request_Frame(
|
||||
/*inline void CommunicationManager::Generate_Transmit_Request_Frame(
|
||||
const char* const message,
|
||||
std::string * frame,
|
||||
const unsigned char frame_ID,
|
||||
|
|
@ -236,7 +266,7 @@ inline void CommunicationManager::Generate_Transmit_Request_Frame(
|
|||
const std::string& options)
|
||||
{
|
||||
const unsigned char FAME_TYPE = static_cast<unsigned char>(0x10); /* Transmit Request Frame */
|
||||
std::string frame_parameters;
|
||||
/*std::string frame_parameters;
|
||||
std::string bytes_frame_parameters;
|
||||
|
||||
frame_parameters.append(destination_adssress);
|
||||
|
|
@ -253,11 +283,11 @@ inline void CommunicationManager::Generate_Transmit_Request_Frame(
|
|||
|
||||
Calculate_and_Append_Checksum(frame);
|
||||
Add_Length_and_Start_Delimiter(frame);
|
||||
}
|
||||
}*/
|
||||
|
||||
|
||||
//*****************************************************************************
|
||||
inline void CommunicationManager::Convert_HEX_To_Bytes(
|
||||
/*inline void CommunicationManager::Convert_HEX_To_Bytes(
|
||||
const std::string& HEX_data, std::string* converted_data)
|
||||
{
|
||||
const unsigned short TWO_BYTES = 2;
|
||||
|
|
@ -273,11 +303,11 @@ inline void CommunicationManager::Convert_HEX_To_Bytes(
|
|||
temporary_char = static_cast<unsigned char>(temporary_HEX);
|
||||
converted_data->push_back(temporary_char);
|
||||
}
|
||||
}
|
||||
}*/
|
||||
|
||||
|
||||
//*****************************************************************************
|
||||
inline void CommunicationManager::Calculate_and_Append_Checksum(
|
||||
/*inline void CommunicationManager::Calculate_and_Append_Checksum(
|
||||
std::string* frame)
|
||||
{
|
||||
unsigned short bytes_sum = 0;
|
||||
|
|
@ -292,11 +322,11 @@ inline void CommunicationManager::Calculate_and_Append_Checksum(
|
|||
checksum = 0xFF - lowest_8_bits;
|
||||
checksum_byte = static_cast<unsigned char>(checksum);
|
||||
frame->push_back(checksum_byte);
|
||||
}
|
||||
}*/
|
||||
|
||||
|
||||
//*****************************************************************************
|
||||
inline void CommunicationManager::Add_Length_and_Start_Delimiter(
|
||||
/*inline void CommunicationManager::Add_Length_and_Start_Delimiter(
|
||||
std::string* frame)
|
||||
{
|
||||
const unsigned short FIVE_BYTES = 5;
|
||||
|
|
@ -307,7 +337,7 @@ inline void CommunicationManager::Add_Length_and_Start_Delimiter(
|
|||
std::string header;
|
||||
int frame_length = frame->size() - 1; /* frame_length = frame_size - checksum byte */
|
||||
|
||||
header.push_back(START_DLIMITER);
|
||||
/*header.push_back(START_DLIMITER);
|
||||
sprintf(temporary_buffer, "%04X", frame_length);
|
||||
sscanf(temporary_buffer, "%02X%02X", &Hex_frame_length_1,
|
||||
&Hex_frame_length_2);
|
||||
|
|
@ -316,11 +346,11 @@ inline void CommunicationManager::Add_Length_and_Start_Delimiter(
|
|||
temporary_char = static_cast<unsigned char>(Hex_frame_length_2);
|
||||
header.push_back(temporary_char);
|
||||
frame->insert(0, header);
|
||||
}
|
||||
}*/
|
||||
|
||||
|
||||
//*****************************************************************************
|
||||
inline void CommunicationManager::Check_In_Messages_and_Transfer_To_Topics()
|
||||
/*inline void CommunicationManager::Check_In_Messages_and_Transfer_To_Topics() // TO DO delete
|
||||
{
|
||||
std::size_t size_in_messages = in_messages_->Get_Size();
|
||||
|
||||
|
|
@ -349,15 +379,15 @@ inline void CommunicationManager::Check_In_Messages_and_Transfer_To_Topics()
|
|||
}
|
||||
|
||||
}
|
||||
}
|
||||
}*/
|
||||
|
||||
|
||||
//*****************************************************************************
|
||||
inline void CommunicationManager::Check_In_Messages_and_Transfer_To_Server()
|
||||
inline void CommunicationManager::Check_In_Messages_and_Transfer_To_Server() // TO DO change it
|
||||
{
|
||||
std::size_t size_in_messages = in_messages_->Get_Size();
|
||||
//std::size_t size_in_messages = in_messages_->Get_Size();
|
||||
|
||||
if (size_in_messages > 0)
|
||||
/*if (size_in_messages > 0)
|
||||
{
|
||||
for (std::size_t j = 0; j < size_in_messages; j++)
|
||||
{
|
||||
|
|
@ -389,7 +419,7 @@ inline void CommunicationManager::Check_In_Messages_and_Transfer_To_Server()
|
|||
ROS_INFO("XBeeMav: Faild to Tranfer Command.");
|
||||
|
||||
}
|
||||
}
|
||||
}*/
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -397,26 +427,8 @@ inline void CommunicationManager::Check_In_Messages_and_Transfer_To_Server()
|
|||
inline void CommunicationManager::Send_Mavlink_Message_Callback(
|
||||
const mavros_msgs::Mavlink::ConstPtr& mavlink_msg)
|
||||
{
|
||||
const unsigned short MAX_BUFFER_SIZE = 211; /* 20 (length(uint64_t)) * 10 (max int number) + 10 (spaces) + 1 */
|
||||
const unsigned short MAX_NBR_OF_INT64 = 10;
|
||||
char temporary_buffer[MAX_BUFFER_SIZE];
|
||||
std::string frame;
|
||||
int converted_bytes = 0;
|
||||
|
||||
/* Check the payload is not too long. Otherwise ignore it. */
|
||||
if (mavlink_msg->payload64.size() <= MAX_NBR_OF_INT64)
|
||||
{
|
||||
for (unsigned short i = 0; i < mavlink_msg->payload64.size(); i++)
|
||||
{
|
||||
converted_bytes += sprintf(
|
||||
temporary_buffer + converted_bytes, "%" PRIu64 " ",
|
||||
(uint64_t)mavlink_msg->payload64.at(i));
|
||||
|
||||
}
|
||||
|
||||
Generate_Transmit_Request_Frame(temporary_buffer, &frame);
|
||||
serial_device_.Send_Frame(frame);
|
||||
}
|
||||
//std::cout << "Received Message From Buzz" << std::endl; // TO DO delete
|
||||
packets_handler_.Handle_Mavlink_Message(mavlink_msg);
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -436,6 +448,113 @@ void CommunicationManager::Display_Drone_Type_and_Running_Mode(
|
|||
}
|
||||
|
||||
|
||||
//*****************************************************************************
|
||||
void CommunicationManager::Process_In_Standard_Messages()
|
||||
{
|
||||
std::size_t in_messages_size = in_std_messages_.Get_Size();
|
||||
|
||||
if (in_messages_size > 0)
|
||||
{
|
||||
for (std::size_t j = 0; j < in_messages_size; j++)
|
||||
{
|
||||
std::shared_ptr<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;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -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_;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
|
@ -39,7 +39,6 @@ bool SerialDevice::Init(
|
|||
{
|
||||
Set_Port_Options(baud_rate);
|
||||
Init_Frame_Type_Keys();
|
||||
Read_Frame_Header();
|
||||
return true;
|
||||
}
|
||||
else
|
||||
|
|
@ -82,6 +81,7 @@ void SerialDevice::Send_Frame(const std::string& frame)
|
|||
//*****************************************************************************
|
||||
void SerialDevice::Run_Service()
|
||||
{
|
||||
Read_Frame_Header();
|
||||
io_service_.run();
|
||||
}
|
||||
|
||||
|
|
@ -90,14 +90,26 @@ void SerialDevice::Run_Service()
|
|||
void SerialDevice::Stop_Service()
|
||||
{
|
||||
io_service_.post([this]() {io_service_.stop(); });
|
||||
}
|
||||
|
||||
|
||||
//*****************************************************************************
|
||||
void SerialDevice::Close_Serial_Port()
|
||||
{
|
||||
io_service_.post([this]() {serial_port_.close(); });
|
||||
}
|
||||
|
||||
|
||||
//*****************************************************************************
|
||||
Thread_Safe_Deque * SerialDevice::Get_In_Messages_Pointer()
|
||||
void SerialDevice::Set_In_Messages_Pointers(Thread_Safe_Deque* in_std_messages,
|
||||
Thread_Safe_Deque* in_fragments,
|
||||
Thread_Safe_Deque* in_Acks_and_Pings,
|
||||
Thread_Safe_Deque* command_responses)
|
||||
{
|
||||
return &in_messages_;
|
||||
in_std_messages_ = in_std_messages;
|
||||
in_fragments_ = in_fragments;
|
||||
in_Acks_and_Pings_ = in_Acks_and_Pings;
|
||||
command_responses_ = command_responses;
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -156,8 +168,10 @@ void SerialDevice::Read_Frame_Header()
|
|||
Read_Frame_Body();
|
||||
}
|
||||
else
|
||||
{
|
||||
/* The header is totally corrupted, read another header. */
|
||||
Read_Frame_Header();
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
|
|
@ -183,15 +197,42 @@ void SerialDevice::Read_Frame_Body()
|
|||
if (current_frame_.Get_Frame_Type() ==
|
||||
FRAME_TYPE_KEYS[RECEIVE_PACKET])
|
||||
{
|
||||
const unsigned short ELEVEN_BYTES = 11;
|
||||
const unsigned short TWELVE_BYTES = 12;
|
||||
|
||||
char msg_type = current_frame_.Get_Message_Type();
|
||||
std::shared_ptr<std::string> in_message =
|
||||
std::make_shared<std::string>();
|
||||
in_message->append(current_frame_.Get_Frame_Body()
|
||||
+ ELEVEN_BYTES,
|
||||
current_frame_.Get_Frame_Body_Length() - TWELVE_BYTES);
|
||||
in_messages_.Push_Pack(in_message);
|
||||
|
||||
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<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();
|
||||
|
|
@ -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();
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -8,7 +8,7 @@
|
|||
#include <stdlib.h>
|
||||
#include <time.h>
|
||||
|
||||
#include<mavros_msgs/Mavlink.h>
|
||||
#include <mavros_msgs/Mavlink.h>
|
||||
#include <ros/ros.h>
|
||||
|
||||
|
||||
|
|
@ -47,25 +47,28 @@ void Init_Random_Seed()
|
|||
//*****************************************************************************
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
const unsigned int MIN_PAYLOAD_SIZE = 1;
|
||||
const unsigned int MAX_PAYLOAD_SIZE = 10;
|
||||
const unsigned int MIN_PAYLOAD_SIZE = 750;
|
||||
const unsigned int MAX_PAYLOAD_SIZE = 752;
|
||||
|
||||
ros::init(argc, argv, "flight_controller");
|
||||
ros::init(argc, argv, "test_buzz");
|
||||
|
||||
ros::NodeHandle node_handle;
|
||||
ros::Publisher mavlink_pub = node_handle.advertise<mavros_msgs::Mavlink>("outMavlink", 1000);
|
||||
ros::Subscriber mavlink_sub = node_handle.subscribe("inMavlink", 1000, Receive_Payload_Callback);
|
||||
|
||||
ros::Rate loop_rate(0.2);
|
||||
|
||||
Init_Random_Seed();
|
||||
|
||||
int count = 0;
|
||||
const std::size_t MAX_BUFFER_SIZE = 64;
|
||||
char line[MAX_BUFFER_SIZE];
|
||||
|
||||
|
||||
while (ros::ok())
|
||||
std::cout << "Press Enter to Send New Mavlink Message..." << std::endl;
|
||||
|
||||
while (ros::ok() && std::cin.getline(line, MAX_BUFFER_SIZE))
|
||||
{
|
||||
mavros_msgs::Mavlink mavlink_msg_;
|
||||
mavlink_msg_.sysid = 2;
|
||||
mavlink_msg_.msgid = 1;
|
||||
|
||||
unsigned int payload_size = Get_Random_Size(MIN_PAYLOAD_SIZE, MAX_PAYLOAD_SIZE);
|
||||
|
||||
|
|
@ -87,9 +90,8 @@ int main(int argc, char **argv)
|
|||
|
||||
ros::spinOnce();
|
||||
|
||||
loop_rate.sleep();
|
||||
|
||||
count++;
|
||||
std::cout << "Press Enter to Send New Mavlink Message..." << std::endl;
|
||||
}
|
||||
|
||||
return 0;
|
||||
|
|
|
|||
|
|
@ -141,6 +141,13 @@ void Frame::Rearrange_Corrupted_Header()
|
|||
}
|
||||
|
||||
|
||||
//*****************************************************************************
|
||||
char Frame::Get_Message_Type()
|
||||
{
|
||||
return data_buffer_[15];
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -7,6 +7,7 @@
|
|||
#include "XMLConfigParser.h"
|
||||
|
||||
|
||||
|
||||
//*****************************************************************************
|
||||
XMLConfigParser::XMLConfigParser():
|
||||
config_loaded_successfully_(false)
|
||||
|
|
@ -23,7 +24,7 @@ XMLConfigParser::~XMLConfigParser()
|
|||
//*****************************************************************************
|
||||
bool XMLConfigParser::Load_Config()
|
||||
{
|
||||
const std::string FILE_NAME = "/home/ubuntu/ROS_WS/src/xbeemav/Resources/XBee_Config.xml";
|
||||
const std::string FILE_NAME = XBEE_CONFIG_PATH;
|
||||
|
||||
if (Check_Config_File_Exists(FILE_NAME))
|
||||
{
|
||||
|
|
|
|||
18
src/Xbee.cpp
18
src/Xbee.cpp
|
|
@ -14,10 +14,11 @@ int main(int argc, char* argv[])
|
|||
try
|
||||
{
|
||||
ros::init(argc, argv, "xbee");
|
||||
ros::NodeHandle node_handle;
|
||||
|
||||
Mist::Xbee::CommunicationManager communication_manager;
|
||||
const std::string& device = "/dev/ttyUSB0"; // TO DO can be introduced as command.
|
||||
const std::size_t baud_rate = 230400; // TO DO Can be introduced as command.
|
||||
std::string device;
|
||||
double baud_rate;
|
||||
Mist::Xbee::CommunicationManager::DRONE_TYPE drone_type =
|
||||
Mist::Xbee::CommunicationManager::DRONE_TYPE::SLAVE;
|
||||
Mist::Xbee::CommunicationManager::RUNNING_MODE running_mode =
|
||||
|
|
@ -34,9 +35,18 @@ int main(int argc, char* argv[])
|
|||
running_mode = Mist::Xbee::CommunicationManager::RUNNING_MODE::SWARM;
|
||||
}
|
||||
}
|
||||
|
||||
if (!node_handle.getParam("USB_port", device))
|
||||
{
|
||||
std::cout << "Failed to Get Topic Name: param 'USB_port' Not Found." << std::endl;
|
||||
return EXIT_FAILURE;
|
||||
}
|
||||
|
||||
if (communication_manager.Init(device, baud_rate))
|
||||
if (!node_handle.getParam("Baud_rate", baud_rate))
|
||||
{
|
||||
std::cout << "Failed to Get Topic Name: param 'Baud_rate' Not Found." << std::endl;
|
||||
return EXIT_FAILURE;
|
||||
}
|
||||
if (communication_manager.Init(device, static_cast<std::size_t>(baud_rate)))
|
||||
communication_manager.Run(drone_type, running_mode);
|
||||
}
|
||||
catch (const std::exception& e)
|
||||
|
|
|
|||
Reference in New Issue