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

first commit

This commit is contained in:
AymenMist 2017-01-10 12:14:41 -05:00
commit 5eb4ef78b4
20 changed files with 2191 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

81
CMakeLists.txt Normal file
View File

@ -0,0 +1,81 @@
cmake_minimum_required(VERSION 2.8.3)
project(xbee_ros_node)
if(UNIX)
SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -std=gnu++11")
endif()
set (BOOST_INCLUDEDIR "/usr/include")
## Find catkin macros and libraries
find_package(catkin REQUIRED COMPONENTS
roscpp
std_msgs
)
## System dependencies are found with CMake's conventions
find_package(Boost REQUIRED COMPONENTS
system)
################################################
## Declare ROS messages, services and actions ##
################################################
################################################
## Declare ROS dynamic reconfigure parameters ##
################################################
###################################
## catkin specific configuration ##
###################################
catkin_package(
INCLUDE_DIRS include
# LIBRARIES xbee_ros_node
CATKIN_DEPENDS roscpp std_msgs
# DEPENDS system_lib
)
###########
## Build ##
###########
## Specify additional locations of header files
include_directories(
include ${xbee_ros_node_INCLUDE_DIRS}
${catkin_INCLUDE_DIRS}
)
add_executable(xbee_mav src/Xbee.cpp src/XBeeFrame.cpp src/SerialDevice.cpp src/CommunicationManager.cpp)
target_link_libraries(xbee_mav ${catkin_LIBRARIES})
#add_executable(config src/main.cpp src/XBeeModule.cpp src/XMLConfigParser.cpp)
#target_link_libraries(config ${catkin_LIBRARIES})
#add_executable(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})
#############
## Install ##
#############
#############
## Testing ##
#############

68
Resources/XBee_Config.xml Normal file
View File

@ -0,0 +1,68 @@
<?xml version="1.0" encoding="UTF-8"?>
<XBeeConfig>
<Settings>
<Parameter Command="CM" Description="Channel Mask">00FFFFFFFFFFFF7FFF</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>
<Parameter Command="PL" Description="TX Power Level">4</Parameter>
<Parameter Command="RR" Description="Unicast Retries">A</Parameter>
<Parameter Command="CE" Description="Routing/Messaging Mode">0</Parameter>
<Parameter Command="BH" Description="Broadcast Hops">0</Parameter>
<Parameter Command="NH" Description="Network Hops">7</Parameter>
<Parameter Command="MR" Description="Mesh Unicast Retries">1</Parameter>
<Parameter Command="NN" Description="Network Delay Slots">3</Parameter>
<Parameter Command="DH" Description="Destination Address High">0</Parameter>
<Parameter Command="DL" Description="Destination Address Low">FFFF</Parameter>
<Parameter Command="TO" Description="Transmit Options">C0</Parameter>
<Parameter Command="NI" Description="Node Identifier">'Node 1'</Parameter>
<Parameter Command="NT" Description="Network Dsicovery Back-Off">82</Parameter>
<Parameter Command="NO" Description="Network Dsicovery Options">0</Parameter>
<Parameter Command="CI" Description="Cluster ID">11</Parameter>
<Parameter Command="EE" Description="Encryption Enbale">0</Parameter>
<Parameter Command="KY" Description="Encryption Key"></Parameter>
<Parameter Command="BD" Description="Baud Rate">8</Parameter>
<Parameter Command="NB" Description="Parity">0</Parameter>
<Parameter Command="SB" Description="Stop Bits">0</Parameter>
<Parameter Command="RO" Description="Packetization Timeout">3</Parameter>
<Parameter Command="FT" Description="Flow Control Threshold">13F</Parameter>
<Parameter Command="AP" Description="API Enable">1</Parameter>
<Parameter Command="AO" Description="API Options">0</Parameter>
<Parameter Command="D0" Description="DIO0/AD0">1</Parameter>
<Parameter Command="D1" Description="DIO1/AD1/SPI_ATTN">0</Parameter>
<Parameter Command="D2" Description="DIO2/AD2/SPI_SCLK">0</Parameter>
<Parameter Command="D3" Description="DIO3/AD3/SPI_SSEL">0</Parameter>
<Parameter Command="D4" Description="DIO4">0</Parameter>
<Parameter Command="D5" Description="DIO5/ASSOCIATED_INDICATOR">1</Parameter>
<Parameter Command="D6" Description="DIO6/RTS">0</Parameter>
<Parameter Command="D7" Description="DIO7/CTS">1</Parameter>
<Parameter Command="D8" Description="DIO8/SLEEP_REQUEST">1</Parameter>
<Parameter Command="D9" Description="DIO9/ON_SLEEP">1</Parameter>
<Parameter Command="P0" Description="DIO10/RSSI/PWM0">1</Parameter>
<Parameter Command="P1" Description="DIO11/PWM1">0</Parameter>
<Parameter Command="P2" Description="DIO12">0</Parameter>
<Parameter Command="P3" Description="DIO13/DOUT">1</Parameter>
<Parameter Command="P4" Description="DIO14/DIN">1</Parameter>
<Parameter Command="PD" Description="Pull Direction">7FFF</Parameter>
<Parameter Command="PR" Description="Pull-Up/Down Resistor">7FFF</Parameter>
<Parameter Command="M0" Description=" PWM0 Duty Cycle">0</Parameter>
<Parameter Command="M1" Description="PWM1 Duty Cycle">0</Parameter>
<Parameter Command="LT" Description="Associate LED Blink Time">0</Parameter>
<Parameter Command="RP" Description="RSSI PWM Timer">28</Parameter>
<Parameter Command="AV" Description="Analog Voltage Reference">1</Parameter>
<Parameter Command="IC" Description="DIO Change Detect">0</Parameter>
<Parameter Command="IF" Description="Sleep Sample Rate">1</Parameter>
<Parameter Command="IR" Description="Sample Rate">0</Parameter>
<Parameter Command="SM" Description="Sleep Mode">0</Parameter>
<Parameter Command="SO" Description="Sleep Options">2</Parameter>
<Parameter Command="SN" Description="Nu,ber of Cycles between On/Sleep">1</Parameter>
<Parameter Command="SP" Description="Sleep Time">12C</Parameter>
<Parameter Command="ST" Description="Wake Time">BB8</Parameter>
<Parameter Command="WH" Description="Wake Host Delay">0</Parameter>
<Parameter Command="CC" Description="Commande Sequence Character">2B</Parameter>
<Parameter Command="CT" Description="Command Mode Time Out">60</Parameter>
<Parameter Command="GT" Description="Guard Time">3E8</Parameter>
<Parameter Command="DD" Description="Device Type Identifier">B0000</Parameter>
</Settings>
</XBeeConfig>

97
include/CommunicationManager.h Normal file
View File

@ -0,0 +1,97 @@
/* CommunicationManager.h -- Communication Manager class for XBee:
Handles all communications with other ROS nodes
and the serial port -- */
/* ------------------------------------------------------------------------- */
/* September 20, 2016 -- @Copyright Aymen Soussia. All rights reserved. */
/* (aymen.soussia@gmail.com) */
#pragma once
#include <inttypes.h>
#include<thread>
#include<mavros_msgs/CommandCode.h>
#include<mavros_msgs/CommandInt.h>
#include<mavros_msgs/Mavlink.h>
#include <ros/ros.h>
#include"SerialDevice.h"
namespace Mist
{
namespace Xbee
{
//*****************************************************************************
struct Waypoint_S
{
unsigned int latitude;
unsigned int longitude;
double altitude;
unsigned int staytime;
unsigned int heading;
};
//*****************************************************************************
class CommunicationManager
{
public:
CommunicationManager();
~CommunicationManager();
enum class DRONE_TYPE {MASTER, SLAVE};
enum class RUNNING_MODE {SWARM, SOLO};
bool Init(const std::string& device, const std::size_t baud_rate);
void Run(DRONE_TYPE drone_type, RUNNING_MODE running_mode);
private:
const unsigned char START_DLIMITER;
const std::size_t LOOP_RATE;
void Run_In_Solo_Mode(DRONE_TYPE drone_type);
void Run_In_Swarm_Mode();
void Generate_Transmit_Request_Frame(
const char* const message,
std::string* frame,
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 Check_In_Messages_and_Transfer_To_Topics();
void Display_Init_Communication_Failure();
void Convert_HEX_To_Bytes(const std::string& HEX_data,
std::string* converted_data);
void Calculate_and_Append_Checksum(std::string* frame);
void Add_Length_and_Start_Delimiter(std::string* frame);
void Send_Mavlink_Message_Callback(
const mavros_msgs::Mavlink::ConstPtr& mavlink_msg);
void Display_Drone_Type_and_Running_Mode(DRONE_TYPE drone_type,
RUNNING_MODE running_mode);
bool Serve_Flight_Controller(mavros_msgs::CommandInt::
Request& request, mavros_msgs::CommandInt::Response& response);
void Check_In_Messages_and_Transfer_To_Server();
Mist::Xbee::SerialDevice serial_device_;
Thread_Safe_Deque* in_messages_;
ros::NodeHandle node_handle_;
ros::Subscriber mavlink_subscriber_;
ros::Publisher mavlink_publisher_;
ros::ServiceClient mav_dji_client_;
ros::ServiceServer mav_dji_server_;
};
}
}

73
include/MultithreadingDeque.hpp Normal file
View File

@ -0,0 +1,73 @@
/* MultithreadingDeque.hpp -- Safe Multithreading Deque class -- */
/* ------------------------------------------------------------------------- */
/* September 20, 2016 -- @Copyright Aymen Soussia. All rights reserved. */
/* (aymen.soussia@gmail.com) */
#pragma once
#include<deque>
#include<mutex>
namespace Mist
{
namespace Xbee
{
//*****************************************************************************
template<typename _T>
class MultithreadingDeque
{
public:
//****************************************************************************
MultithreadingDeque()
{
}
//****************************************************************************
~MultithreadingDeque()
{
}
//****************************************************************************
void Push_Pack(const _T& new_data)
{
std::lock_guard<std::mutex> guard(mutex_);
deque_.push_back(new_data);
}
//****************************************************************************
_T Pop_Front()
{
std::lock_guard<std::mutex> guard(mutex_);
_T value = deque_.front();
deque_.pop_front();
return value;
}
//****************************************************************************
unsigned int Get_Size()
{
std::lock_guard<std::mutex> guard(mutex_);
return deque_.size();
}
private:
std::mutex mutex_;
std::deque<_T> deque_;
};
}
}

83
include/SerialDevice.h Normal file
View File

@ -0,0 +1,83 @@
/* SerialDevice.h -- Serial Device class to handle serial communications
with XBee -- */
/* ------------------------------------------------------------------------- */
/* September 20, 2016 -- @Copyright Aymen Soussia. All rights reserved. */
/* (aymen.soussia@gmail.com) */
#pragma once
#include<deque>
#include<iostream>
#include<memory>
#include<boost/asio.hpp>
#include"MultithreadingDeque.hpp"
#include"XBeeFrame.h"
namespace Mist
{
namespace Xbee
{
typedef MultithreadingDeque<std::shared_ptr<std::string>> Thread_Safe_Deque;
//*****************************************************************************
class SerialDevice
{
public:
SerialDevice();
~SerialDevice();
bool Init(const std::string& device, const std::size_t baud_rate);
void Send_Frame(const std::string& frame);
void Run_Service();
void Stop_Service();
Thread_Safe_Deque* Get_In_Messages_Pointer();
private:
enum FRAME_TYPE
{
AT_COMMAND = 0,
AT_COMMAND_QUEUE_REGISTER_VALUE = 1,
TRANSMIT_REQUEST = 2,
EXPLICIT_ADDRESSING_COMMAND_FRAME = 3,
REMOTE_AT_COMMAND = 4,
AT_COMMAND_RESPONSE = 5,
MODEM_STATUS = 6,
TRANSMIT_STATUS = 7,
ROUTE_INFORMATION_PACKET = 8,
AGGREGATE_ADDRESSING_UPDATE = 9,
RECEIVE_PACKET = 10,
EXPLICIT_RX_INDICATOR = 11,
IO_DATA_SAMPLE_RX_INDICATOR = 12,
NODE_IDENTIFICATION_INDICATOR = 13,
REMOTE_AT_COMMAND_RESPONSE = 14
};
void Init_Frame_Type_Keys();
void Set_Port_Options(const std::size_t baud_rate);
void Read_Frame_Header();
void Read_Frame_Body();
void Write_Frame();
boost::asio::io_service io_service_;
boost::asio::serial_port serial_port_;
std::deque<std::string> out_messages_;
Thread_Safe_Deque in_messages_;
Mist::Xbee::Frame current_frame_;
unsigned int FRAME_TYPE_KEYS[REMOTE_AT_COMMAND_RESPONSE + 1];
};
}
}

56
include/XBeeFrame.h Normal file
View File

@ -0,0 +1,56 @@
/* XBeeFrame.cpp -- XBee Frame class -- */
/* ------------------------------------------------------------------------- */
/* September 20, 2016 -- @Copyright Aymen Soussia. All rights reserved. */
/* (aymen.soussia@gmail.com) */
#pragma once
#include<cstdio>
#include<cstdlib>
#include<cstring>
namespace Mist
{
namespace Xbee
{
//*****************************************************************************
class Frame
{
public:
Frame();
~Frame();
enum {FRAME_HEADER_LENGTH = 4};
enum {MAX_FRAME_BODY_LENGTH = 280}; // TO DO check value
const char* Get_Frame_Data() const;
char* Get_Frame_Data();
std::size_t Get_Frame_Length() const;
const char* Get_Frame_Body() const;
char* Get_Frame_Body();
std::size_t Get_Frame_Body_Length() const;
bool Decode_Frame_Header();
std::size_t Get_Frame_Type() const;
int Get_Start_Delimiter_Position();
void Rearrange_Corrupted_Header();
private:
char data_buffer_[FRAME_HEADER_LENGTH + MAX_FRAME_BODY_LENGTH];
std::size_t frame_body_length_;
unsigned int frame_type_;
int start_delimiter_position_;
};
}
}

65
include/XBeeModule.h Normal file
View File

@ -0,0 +1,65 @@
/* XBeeModule.h -- Xbee Module class provides functions to communicate
with Xbee -- */
/* ------------------------------------------------------------------------- */
/* November 30, 2016 -- @Copyright Aymen Soussia. All rights reserved. */
/* (aymen.soussia@gmail.com) */
#pragma once
#include<deque>
#include<iostream>
#include<unistd.h>
#include<boost/asio.hpp>
#include<boost/bind.hpp>
#include"XBeeParameter.h"
//*****************************************************************************
using namespace boost;
//*****************************************************************************
typedef std::deque < std::string > commands_queue;
//*****************************************************************************
class XBeeModule
{
public:
XBeeModule();
~XBeeModule();
enum {MAX_MSG_LENGTH = 8};
bool Init_Port(const std::string& device_port, const unsigned int baud_rate);
void Run_Service();
void Send_Data(const std::string& command);
void Exit_AT_Command_Mode();
bool Is_Connected();
bool Check_Time_Out_Exceeded();
void Format_AT_Command(const XBee_Parameter_S& parameter, std::string* command);
private:
const unsigned short TIME_OUT;
bool Open_Port(const std::string& device_port);
void Start_Receive();
void Handle_Receive(const system::error_code& error, size_t bytes_transferred);
void Set_AT_Command_Mode();
void Check_Time_Out(const system::error_code& error);
void Do_Write();
void Handle_Write(const system::error_code& error);
asio::io_service io_service_;
asio::serial_port serial_port_;
asio::deadline_timer timer_;
char receive_buffer_[MAX_MSG_LENGTH];
bool connected_to_XBee_;
bool time_out_exceeded_;
commands_queue commands_sequence_;
};

17
include/XBeeParameter.h Normal file
View File

@ -0,0 +1,17 @@
/* XBee_Parameter.h -- Xbee command struct -- */
/* ------------------------------------------------------------------------- */
/* November 30, 2016 -- @Copyright Aymen Soussia. All rights reserved. */
/* (aymen.soussia@gmail.com) */
#pragma once
#include<string>
//*****************************************************************************
struct XBee_Parameter_S
{
std::string command_;
std::string value_;
};

41
include/XMLConfigParser.h Normal file
View File

@ -0,0 +1,41 @@
/* XMLConfigParser.h -- XML Config Parser class -- */
/* ------------------------------------------------------------------------- */
/* November 30, 2016 -- @Copyright Aymen Soussia. All rights reserved. */
/* (aymen.soussia@gmail.com) */
#pragma once
#include<iostream>
#include<boost/property_tree/ptree.hpp>
#include<boost/property_tree/xml_parser.hpp>
#include<boost/foreach.hpp>
#include<boost/filesystem.hpp>
#include"XBeeParameter.h"
//*****************************************************************************
using boost::property_tree::ptree;
//*****************************************************************************
class XMLConfigParser
{
public:
XMLConfigParser();
~XMLConfigParser();
bool Load_Config();
std::vector<XBee_Parameter_S>* Get_Loaded_Parameters();
bool Is_Config_Loaded_Successfully();
private:
bool Check_Config_File_Exists(const std::string& file_name);
std::vector<XBee_Parameter_S> xbee_parameters_;
bool config_loaded_successfully_;
};

12
launch/xbeemav.launch Normal file
View File

@ -0,0 +1,12 @@
<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="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" />
</launch>

19
package.xml Normal file
View File

@ -0,0 +1,19 @@
<?xml version="1.0"?>
<package>
<name>xbee_ros_node</name>
<version>1.0.2</version>
<description>The xbee_ros_node package</description>
<maintainer email="aymen.soussia@gmail.com">aymen</maintainer>
<license>Boost Software License</license>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roscpp</build_depend>
<build_depend>std_msgs</build_depend>
<run_depend>roscpp</run_depend>
<run_depend>std_msgs</run_depend>
<export>
</export>
</package>

442
src/CommunicationManager.cpp Normal file
View File

@ -0,0 +1,442 @@
/* CommunicationManager.cpp -- Communication Manager class for XBee:
Handles all communications with other ROS nodes
and the serial port -- */
/* ------------------------------------------------------------------------- */
/* September 20, 2016 -- @Copyright Aymen Soussia. All rights reserved. */
/* (aymen.soussia@gmail.com) */
#include "CommunicationManager.h"
namespace Mist
{
namespace Xbee
{
//*****************************************************************************
CommunicationManager::CommunicationManager():
START_DLIMITER(static_cast<unsigned char>(0x7E)),
LOOP_RATE(10) /* 10 fps */
{
}
//*****************************************************************************
CommunicationManager::~CommunicationManager()
{
}
//*****************************************************************************
bool CommunicationManager::Init(
const std::string& device, const std::size_t baud_rate)
{
if (serial_device_.Init(device, baud_rate))
{
in_messages_ = serial_device_.Get_In_Messages_Pointer();
return true;
}
else
{
Display_Init_Communication_Failure();
return false;
}
}
//*****************************************************************************
void CommunicationManager::Run(DRONE_TYPE drone_type,
RUNNING_MODE running_mode)
{
std::thread thread_service(&SerialDevice::Run_Service, &serial_device_);
Display_Drone_Type_and_Running_Mode(drone_type, running_mode);
if (RUNNING_MODE::SWARM == running_mode)
Run_In_Swarm_Mode();
else
Run_In_Solo_Mode(drone_type);
serial_device_.Stop_Service();
thread_service.join();
}
//*****************************************************************************
void CommunicationManager::Run_In_Solo_Mode(DRONE_TYPE drone_type)
{
std::string service_name;
bool success = false;
if (DRONE_TYPE::MASTER == drone_type)
{
if (node_handle_.getParam("Xbee_In_From_Controller", service_name))
{
mav_dji_server_ = node_handle_.advertiseService(service_name.c_str(), &CommunicationManager::Serve_Flight_Controller, this);
success = true;
}
else
std::cout << "Failed to Get Service Name: param 'Xbee_In_From_Controller' Not Found." << std::endl;
}
else
{
if (node_handle_.getParam("Xbee_Out_To_Controller", service_name))
{
mav_dji_client_ = node_handle_.serviceClient<mavros_msgs::CommandInt>(service_name.c_str());
success = true;
}
else
std::cout << "Failed to Get Service Name: param 'Xbee_Out_To_Controller' Not Found." << std::endl;
}
if (success)
{
ros::Rate loop_rate(LOOP_RATE);
while (ros::ok())
{
Check_In_Messages_and_Transfer_To_Server();
ros::spinOnce();
loop_rate.sleep();
}
}
}
//*****************************************************************************
void CommunicationManager::Run_In_Swarm_Mode()
{
std::string out_messages_topic;
std::string in_messages_topic;
bool success_1 = false;
bool success_2 = false;
if (node_handle_.getParam("Xbee_In_From_Buzz", out_messages_topic))
{
mavlink_subscriber_ = node_handle_.subscribe(out_messages_topic.c_str(), 1000,
&CommunicationManager::Send_Mavlink_Message_Callback, this);
success_1 = true;
}
else
std::cout << "Failed to Get Topic Name: param 'Xbee_In_From_Buzz' Not Found." << std::endl;
if (node_handle_.getParam("Xbee_Out_To_Buzz", in_messages_topic))
{
mavlink_publisher_ = node_handle_.advertise<mavros_msgs::Mavlink>(
in_messages_topic.c_str(), 1000);
success_2 = true;
}
else
std::cout << "Failed to Get Topic Name: param 'Xbee_Out_To_Buzz' Not Found." << std::endl;
if (success_1 && success_2)
{
ros::Rate loop_rate(LOOP_RATE);
while (ros::ok())
{
Check_In_Messages_and_Transfer_To_Topics();
ros::spinOnce();
loop_rate.sleep();
}
}
}
//*****************************************************************************
inline bool CommunicationManager::Serve_Flight_Controller(mavros_msgs::CommandInt::
Request& request, mavros_msgs::CommandInt::Response& response)
{
const std::size_t MAX_BUFFER_SIZE = 255;
unsigned int command = 0;
char temporary_buffer[MAX_BUFFER_SIZE];
std::string frame;
switch(request.command)
{
case mavros_msgs::CommandCode::NAV_TAKEOFF:
{
command = static_cast<unsigned int>(mavros_msgs::CommandCode::NAV_TAKEOFF);
sprintf(temporary_buffer, "%u ", command);
response.success = true;
break;
}
case mavros_msgs::CommandCode::NAV_LAND:
{
command = static_cast<unsigned int>(mavros_msgs::CommandCode::NAV_LAND);
sprintf(temporary_buffer, "%u ", command);
response.success = true;
break;
}
case mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH:
{
command = static_cast<unsigned int>(mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH);
sprintf(temporary_buffer, "%u ", command);
response.success = true;
break;
}
case mavros_msgs::CommandCode::NAV_WAYPOINT:
{
command = static_cast<unsigned int>(mavros_msgs::CommandCode::NAV_WAYPOINT);
sprintf(temporary_buffer, "%u %u %u %lf %u %u ",
command, request.x, request.y, request.z, 0, 1); // TO DO change 0 and 1
response.success = true;
break;
}
case mavros_msgs::CommandCode::CMD_MISSION_START:
{
command = static_cast<unsigned int>(mavros_msgs::CommandCode::CMD_MISSION_START);
sprintf(temporary_buffer, "%u ", command);
response.success = true;
break;
}
default:
response.success = false;
}
if (command != 0)
{
Generate_Transmit_Request_Frame(temporary_buffer, &frame);
serial_device_.Send_Frame(frame);
}
return true;
}
//*****************************************************************************
void CommunicationManager::Display_Init_Communication_Failure()
{
std::cout << "Failed to Init Communication With XBee." << std::endl;
std::cout << "Please Check The Following Parameters:" << std::endl;
std::cout << " 1) Device (e.g. /dev/ttyUSB0 for Linux. " << std::endl;
std::cout << " 2) Baud Rate." << std::endl;
std::cout << " 3) Press Reset Button on The XBee." << std::endl;
std::cout << " 4) Connect and Disconnect The XBee." << std::endl;
std::cout << " 5) Update The XBee Firmware." << std::endl;
std::cout << " 6) Reinstall The FTDI Driver." << std::endl;
}
//*****************************************************************************
inline void CommunicationManager::Generate_Transmit_Request_Frame(
const char* const message,
std::string * frame,
const unsigned char frame_ID,
const std::string& destination_adssress,
const std::string& short_destination_adress,
const std::string& broadcast_radius,
const std::string& options)
{
const unsigned char FAME_TYPE = static_cast<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, std::strlen(message));
Calculate_and_Append_Checksum(frame);
Add_Length_and_Start_Delimiter(frame);
}
//*****************************************************************************
inline void CommunicationManager::Convert_HEX_To_Bytes(
const std::string& HEX_data, std::string* converted_data)
{
const unsigned short TWO_BYTES = 2;
char temporary_buffer[TWO_BYTES];
unsigned char temporary_char;
int temporary_HEX;
for (unsigned short i = 0; i <= HEX_data.size() - TWO_BYTES;
i += TWO_BYTES)
{
memcpy(temporary_buffer, HEX_data.c_str() + i, TWO_BYTES);
sscanf(temporary_buffer, "%02X", &temporary_HEX);
temporary_char = static_cast<unsigned char>(temporary_HEX);
converted_data->push_back(temporary_char);
}
}
//*****************************************************************************
inline void CommunicationManager::Calculate_and_Append_Checksum(
std::string* frame)
{
unsigned short bytes_sum = 0;
unsigned lowest_8_bits = 0;
unsigned short checksum = 0;
unsigned char checksum_byte;
for (unsigned short i = 0; i < frame->size(); i++)
bytes_sum += static_cast<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 CommunicationManager::Add_Length_and_Start_Delimiter(
std::string* frame)
{
const unsigned short FIVE_BYTES = 5;
char temporary_buffer[FIVE_BYTES];
unsigned char temporary_char;
int Hex_frame_length_1;
int Hex_frame_length_2;
std::string header;
int frame_length = frame->size() - 1; /* frame_length = frame_size - checksum byte */
header.push_back(START_DLIMITER);
sprintf(temporary_buffer, "%04X", frame_length);
sscanf(temporary_buffer, "%02X%02X", &Hex_frame_length_1,
&Hex_frame_length_2);
temporary_char = static_cast<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 CommunicationManager::Check_In_Messages_and_Transfer_To_Topics()
{
std::size_t size_in_messages = in_messages_->Get_Size();
if (size_in_messages > 0)
{
uint64_t current_int64 = 0;
for (std::size_t j = 0; j < size_in_messages; j++)
{
std::shared_ptr<std::string> in_message =
in_messages_->Pop_Front();
mavros_msgs::Mavlink mavlink_msg;
for (std::size_t i = 0; i < in_message->size(); i++)
{
if (' ' == in_message->at(i) || 0 == i)
{
sscanf(in_message->c_str() + i, "%" PRIu64 " ",
&current_int64);
mavlink_msg.payload64.push_back(current_int64);
}
}
mavlink_publisher_.publish(mavlink_msg);
}
}
}
//*****************************************************************************
inline void CommunicationManager::Check_In_Messages_and_Transfer_To_Server()
{
std::size_t size_in_messages = in_messages_->Get_Size();
if (size_in_messages > 0)
{
for (std::size_t j = 0; j < size_in_messages; j++)
{
std::shared_ptr<std::string> in_message =
in_messages_->Pop_Front();
mavros_msgs:: CommandInt command_msg;
unsigned int command = 0;
sscanf(in_message->c_str(), "%u", &command);
if (static_cast<unsigned int>(mavros_msgs::CommandCode::NAV_WAYPOINT) == command)
{
Waypoint_S new_waypoint;
sscanf(in_message->c_str(), "%u %u %u %lf %u %u ",
&command, &new_waypoint.latitude,
&new_waypoint.longitude, &new_waypoint.altitude,
&new_waypoint.staytime, &new_waypoint.heading);
command_msg.request.x = new_waypoint.latitude;
command_msg.request.y = new_waypoint.longitude;
command_msg.request.z = new_waypoint.altitude;
// TO DO add staytime and heading;
}
command_msg.request.command = command;
if (mav_dji_client_.call(command_msg))
ROS_INFO("XBeeMav: Command Successfully Tranferred to Dji Mav.");
else
ROS_INFO("XBeeMav: Faild to Tranfer Command.");
}
}
}
//*****************************************************************************
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);
}
}
//*****************************************************************************
void CommunicationManager::Display_Drone_Type_and_Running_Mode(
DRONE_TYPE drone_type, RUNNING_MODE running_mode)
{
if (DRONE_TYPE::MASTER == drone_type)
std::cout << "Drone: MASTER" << std::endl;
else
std::cout << "Drone: SLAVE" << std::endl;
if (RUNNING_MODE::SOLO == running_mode)
std::cout << "XBeeMav Running in SOLO Mode..." << std::endl;
else
std::cout << "XBeeMav Running in SWARM Mode..." << std::endl;
}
}
}

232
src/SerialDevice.cpp Normal file
View File

@ -0,0 +1,232 @@
/* SerialDevice.cpp -- Serial Device class to handle serial communications
with XBee -- */
/* ------------------------------------------------------------------------- */
/* September 20, 2016 -- @Copyright Aymen Soussia. All rights reserved. */
/* (aymen.soussia@gmail.com) */
#include "SerialDevice.h"
namespace Mist
{
namespace Xbee
{
//*****************************************************************************
SerialDevice::SerialDevice():
serial_port_(io_service_)
{
}
//*****************************************************************************
SerialDevice::~SerialDevice()
{
}
//*****************************************************************************
bool SerialDevice::Init(
const std::string & device, const std::size_t baud_rate)
{
serial_port_.open(device);
if (serial_port_.is_open())
{
Set_Port_Options(baud_rate);
Init_Frame_Type_Keys();
Read_Frame_Header();
return true;
}
else
{
std::cout << "Port Failed To Open." << std::endl;
return false;
}
}
//*****************************************************************************
void SerialDevice::Set_Port_Options(const std::size_t baud_rate)
{
serial_port_.set_option(boost::asio::serial_port::baud_rate(baud_rate));
serial_port_.set_option(boost::asio::serial_port::character_size(8));
serial_port_.set_option(boost::asio::serial_port::parity(
boost::asio::serial_port::serial_port_base::parity::none));
serial_port_.set_option(boost::asio::serial_port::stop_bits(
boost::asio::serial_port::serial_port_base::stop_bits::one));
serial_port_.set_option(boost::asio::serial_port::flow_control(
boost::asio::serial_port::serial_port_base::flow_control::none));
}
//*****************************************************************************
void SerialDevice::Send_Frame(const std::string& frame)
{
io_service_.post(
[this, frame]()
{
bool write_in_progress = !out_messages_.empty();
out_messages_.push_back(frame);
if (!write_in_progress)
Write_Frame();
});
}
//*****************************************************************************
void SerialDevice::Run_Service()
{
io_service_.run();
}
//*****************************************************************************
void SerialDevice::Stop_Service()
{
io_service_.post([this]() {io_service_.stop(); });
io_service_.post([this]() {serial_port_.close(); });
}
//*****************************************************************************
Thread_Safe_Deque * SerialDevice::Get_In_Messages_Pointer()
{
return &in_messages_;
}
//*****************************************************************************
void SerialDevice::Init_Frame_Type_Keys()
{
sscanf("08", "%02X", &FRAME_TYPE_KEYS[AT_COMMAND]);
sscanf("09", "%02X", &FRAME_TYPE_KEYS[AT_COMMAND_QUEUE_REGISTER_VALUE]);
sscanf("10", "%02X", &FRAME_TYPE_KEYS[TRANSMIT_REQUEST]);
sscanf("11", "%02X",
&FRAME_TYPE_KEYS[EXPLICIT_ADDRESSING_COMMAND_FRAME]);
sscanf("17", "%02X", &FRAME_TYPE_KEYS[REMOTE_AT_COMMAND]);
sscanf("88", "%02X", &FRAME_TYPE_KEYS[AT_COMMAND_RESPONSE]);
sscanf("8A", "%02X", &FRAME_TYPE_KEYS[MODEM_STATUS]);
sscanf("8B", "%02X", &FRAME_TYPE_KEYS[TRANSMIT_STATUS]);
sscanf("8D", "%02X", &FRAME_TYPE_KEYS[ROUTE_INFORMATION_PACKET]);
sscanf("8E", "%02X", &FRAME_TYPE_KEYS[AGGREGATE_ADDRESSING_UPDATE]);
sscanf("90", "%02X", &FRAME_TYPE_KEYS[RECEIVE_PACKET]);
sscanf("91", "%02X", &FRAME_TYPE_KEYS[EXPLICIT_RX_INDICATOR]);
sscanf("92", "%02X", &FRAME_TYPE_KEYS[IO_DATA_SAMPLE_RX_INDICATOR]);
sscanf("95", "%02X", &FRAME_TYPE_KEYS[NODE_IDENTIFICATION_INDICATOR]);
sscanf("97", "%02X", &FRAME_TYPE_KEYS[REMOTE_AT_COMMAND_RESPONSE]);
}
//*****************************************************************************
void SerialDevice::Read_Frame_Header()
{
boost::asio::async_read(serial_port_,
boost::asio::buffer(current_frame_.Get_Frame_Data(),
Xbee::Frame::FRAME_HEADER_LENGTH),
[this](boost::system::error_code error, std::size_t)
{
if (!error)
{
int start_delimiter_position =
current_frame_.Get_Start_Delimiter_Position();
if (start_delimiter_position >= 0)
{
if (0 == start_delimiter_position)
current_frame_.Decode_Frame_Header();
else
{
/* The header is corrupted. */
boost::asio::async_read(serial_port_,
boost::asio::buffer(current_frame_.Get_Frame_Data(),
start_delimiter_position),
[this](boost::system::error_code error, std::size_t)
{
current_frame_.Rearrange_Corrupted_Header();
current_frame_.Decode_Frame_Header();
});
}
Read_Frame_Body();
}
else
/* The header is totally corrupted, read another header. */
Read_Frame_Header();
}
else
{
std::cout << "Error Occured:" << std::endl;
std::cout << error << std::endl;
std::cout << "Communication With XBee is Lost." << std::endl;
serial_port_.close();
}
});
}
//*****************************************************************************
void SerialDevice::Read_Frame_Body()
{
boost::asio::async_read(serial_port_,
boost::asio::buffer(current_frame_.Get_Frame_Body(),
current_frame_.Get_Frame_Body_Length()),
[this](boost::system::error_code error, std::size_t)
{
if (!error)
{
if (current_frame_.Get_Frame_Type() ==
FRAME_TYPE_KEYS[RECEIVE_PACKET])
{
const unsigned short ELEVEN_BYTES = 11;
const unsigned short TWELVE_BYTES = 12;
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);
}
Read_Frame_Header();
}
else
{
std::cout << "Error Occured:" << std::endl;
std::cout << error << std::endl;
std::cout << "Communication With XBee is Lost." << std::endl;
serial_port_.close();
}
});
}
//*****************************************************************************
void SerialDevice::Write_Frame()
{
boost::asio::async_write(serial_port_,
boost::asio::buffer(out_messages_.front().data(),
out_messages_.front().size()),
[this](boost::system::error_code error,
std::size_t transferred_bytes)
{
if (!error)
{
out_messages_.pop_front();
if (!out_messages_.empty())
Write_Frame();
}
});
}
}
}

96
src/TestBuzz.cpp Normal file
View File

@ -0,0 +1,96 @@
/* RosBuzz.cpp -- Basic ROS Buzz node -- */
/* ------------------------------------------------------------------------- */
/* September 20, 2016 -- @Copyright Aymen Soussia. All rights reserved. */
/* (aymen.soussia@gmail.com) */
#include <climits>
#include <stdlib.h>
#include <time.h>
#include<mavros_msgs/Mavlink.h>
#include <ros/ros.h>
//*****************************************************************************
void Receive_Payload_Callback(const mavros_msgs::Mavlink::ConstPtr& mavlink_msg)
{
std::cout << "Received Mavlink Message:" << std::endl;
for (unsigned short i = 0; i < mavlink_msg->payload64.size(); i++)
std::cout << mavlink_msg->payload64.at(i) << std::endl;
std::cout << std::endl;
}
//*****************************************************************************
unsigned int Get_Random_Size(unsigned int min, unsigned int max)
{
return rand() % (max - min) + min;
}
//*****************************************************************************
unsigned long long Get_Random_Int64(unsigned long long min, unsigned long long max)
{
return rand() % (max - min) + min;
}
//*****************************************************************************
void Init_Random_Seed()
{
srand (time(NULL));
}
//*****************************************************************************
int main(int argc, char **argv)
{
const unsigned int MIN_PAYLOAD_SIZE = 1;
const unsigned int MAX_PAYLOAD_SIZE = 10;
ros::init(argc, argv, "flight_controller");
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;
while (ros::ok())
{
mavros_msgs::Mavlink mavlink_msg_;
unsigned int payload_size = Get_Random_Size(MIN_PAYLOAD_SIZE, MAX_PAYLOAD_SIZE);
for (unsigned int i = 0; i < payload_size; i++)
{
mavlink_msg_.payload64.push_back(Get_Random_Int64(1, ULLONG_MAX));
}
std::cout << "Sent Mavlink Message " << count << std::endl;
for (unsigned int i = 0; i < payload_size; i++)
{
std::cout << mavlink_msg_.payload64.at(i) << std::endl;
}
std::cout << std::endl;
mavlink_pub.publish(mavlink_msg_);
ros::spinOnce();
loop_rate.sleep();
count++;
}
return 0;
}

212
src/TestController.cpp Normal file
View File

@ -0,0 +1,212 @@
/* FlightController.cpp -- Basic Flight Controller ROS node -- */
/* ------------------------------------------------------------------------- */
/* September 20, 2016 -- @Copyright Aymen Soussia. All rights reserved. */
/* (aymen.soussia@gmail.com) */
#include<climits>
#include<cstdlib>
#include<ctime>
#include<iostream>
#include<mavros_msgs/CommandCode.h>
#include<mavros_msgs/CommandInt.h>
#include<mavros_msgs/Mavlink.h>
#include <ros/ros.h>
enum DRONE_TYPE {MASTER, SLAVE};
const std::size_t LOOP_RATE = 10; /* (10 fps) */
//*****************************************************************************
float Get_Random_Float(float min, float max)
{
return min + static_cast <float> (rand()) /( static_cast <float> (RAND_MAX/(max - min)));
}
//*****************************************************************************
unsigned long int Get_Random_Unsigned_Int(unsigned long int min, unsigned long int max)
{
return rand() % (max - min) + min;
}
//*****************************************************************************
bool Fill_Command_Message(mavros_msgs::CommandInt* cmd_msg, const unsigned int command_ID)
{
switch (command_ID)
{
case 7:
cmd_msg->request.command = mavros_msgs::CommandCode::NAV_TAKEOFF;
return true;
case 8:
cmd_msg->request.command = mavros_msgs::CommandCode::NAV_LAND;
return true;
case 9:
cmd_msg->request.command = mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH;
return true;
case 21:
cmd_msg->request.command = mavros_msgs::CommandCode::NAV_WAYPOINT;
cmd_msg->request.x = Get_Random_Unsigned_Int(0, ULONG_MAX);
cmd_msg->request.y = Get_Random_Unsigned_Int(0, ULONG_MAX);
cmd_msg->request.z = Get_Random_Float(0, FLT_MAX);
return true;
case 22:
cmd_msg->request.command = mavros_msgs::CommandCode::CMD_MISSION_START;
return true;
case 23:
cmd_msg->request.command = mavros_msgs::CommandCode::CMD_MISSION_START;
cmd_msg->request.param1 = 666;
return true;
default:
return false;
}
}
//*****************************************************************************
void Init_Random_Seed()
{
srand (time(NULL));
}
//*****************************************************************************
bool Serve_XBee(mavros_msgs::CommandInt::Request& request,
mavros_msgs::CommandInt::Response& response)
{
switch(request.command)
{
case mavros_msgs::CommandCode::NAV_TAKEOFF:
response.success = true;
std::cout << "Received TakeOff Command" << std::endl;
return true;
case mavros_msgs::CommandCode::NAV_LAND:
response.success = true;
std::cout << "Received Land Command" << std::endl;
return true;
case mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH:
response.success = true;
std::cout << "Received Return To Launch Command" << std::endl;
return true;
case mavros_msgs::CommandCode::NAV_WAYPOINT:
std::cout << "Received New Waypoint Command" << std::endl;
std::cout << " Latitude " << request.x << std::endl;
std::cout << " Longitude " << request.y << std::endl;
std::cout << " Altitude " << request.z << std::endl;
std::cout << " Staytime " << std::endl;
std::cout << " Heading " << std::endl;
response.success = true;
return true;
case mavros_msgs::CommandCode::CMD_MISSION_START:
std::cout << "Received Mission Start Command" << std::endl;
response.success = true;
return true;
default:
std::cout << "Unknown Command" << std::endl;
response.success = false;
return false;
}
}
//*****************************************************************************
void Run_As_Master_Drone()
{
Init_Random_Seed();
const std::size_t MAX_BUFFER_SIZE = 64;
char line[MAX_BUFFER_SIZE];
unsigned int command_ID = 0;
const unsigned int MIN_COMMAND_ID = 1;
const unsigned int MAX_COMMAND_ID = 32;
ros::NodeHandle node_handle;
ros::ServiceClient mav_dji_client_ = node_handle.serviceClient<mavros_msgs::CommandInt>("xbee_cmd");
std::cout << "Enter a Command ID Between 1 and 32" << std::endl;
ros::Rate loop_rate(LOOP_RATE);
while (ros::ok() && std::cin.getline(line, MAX_BUFFER_SIZE))
{
sscanf(line, "%u", &command_ID);
if (command_ID >= MIN_COMMAND_ID && command_ID <= MAX_COMMAND_ID)
{
mavros_msgs::CommandInt new_command;
Fill_Command_Message(&new_command, command_ID);
if (mav_dji_client_.call(new_command))
std::cout << "Sent Command." << std::endl;
else
std::cout << "Failed to Send Command." << std::endl;
}
else
{
if (command_ID < MIN_COMMAND_ID)
std::cout << "Introduced Command is Smaller Than 1" << std::endl;
else if (command_ID > MAX_COMMAND_ID)
std::cout << "Introduced Command is Greater Than 32" << std::endl;
}
ros::spinOnce();
loop_rate.sleep();
std::cout << "Enter a Command ID Between 1 and 32" << std::endl;
}
}
//*****************************************************************************
void Run_As_Slave_Drone()
{
ros::NodeHandle node_handle;
ros::ServiceServer service = node_handle.advertiseService("mav_dji_cmd", Serve_XBee);
ros::Rate loop_rate(LOOP_RATE);
while (ros::ok())
{
ros::spinOnce();
loop_rate.sleep();
}
}
//*****************************************************************************
int main(int argc, char** argv)
{
DRONE_TYPE drone_type = SLAVE;
if (argc > 1)
{
if (!strcmp(argv[1], "master"))
drone_type = MASTER;
}
if (SLAVE == drone_type)
std::cout << "Drone: Slave" << std::endl;
else
std::cout << "Drone: Master" << std::endl;
std::cout << "Flight Controller Running in SOLO Mode..." << std::endl;
ros::init(argc, argv, "flight_controller");
if (MASTER == drone_type)
Run_As_Master_Drone();
else
Run_As_Slave_Drone();
std::cout << "Flight Controller Exited." << std::endl;
return 0;
}

147
src/XBeeFrame.cpp Normal file
View File

@ -0,0 +1,147 @@
/* XBeeFrame.cpp -- XBee Frame class -- */
/* ------------------------------------------------------------------------- */
/* September 20, 2016 -- @Copyright Aymen Soussia. All rights reserved. */
/* (aymen.soussia@gmail.com) */
#include "XBeeFrame.h"
namespace Mist
{
namespace Xbee
{
//*****************************************************************************
Frame::Frame():
frame_body_length_(0),
frame_type_(0),
start_delimiter_position_(0)
{
}
//*****************************************************************************
Frame::~Frame()
{
}
//*****************************************************************************
const char * Frame::Get_Frame_Data() const
{
return data_buffer_;
}
//*****************************************************************************
char * Frame::Get_Frame_Data()
{
return data_buffer_;
}
//*****************************************************************************
std::size_t Frame::Get_Frame_Length() const
{
return FRAME_HEADER_LENGTH + frame_body_length_;
}
//*****************************************************************************
const char * Frame::Get_Frame_Body() const
{
return data_buffer_ + FRAME_HEADER_LENGTH;
}
//*****************************************************************************
char * Frame::Get_Frame_Body()
{
return data_buffer_ + FRAME_HEADER_LENGTH;
}
//*****************************************************************************
std::size_t Frame::Get_Frame_Body_Length() const
{
return frame_body_length_;
}
//*****************************************************************************
bool Frame::Decode_Frame_Header()
{
const unsigned short THREE_BYTES = 3;
const unsigned short EIGHT_BYTES = 8;
const unsigned short FRAME_LENGTH_OFFSET = 1;
int frame_length_1 = 0;
int frame_length_2 = 0;
unsigned int frame_length = 0;
unsigned char temporary_buffer[THREE_BYTES];
char header_buffer[EIGHT_BYTES];
memcpy(temporary_buffer, data_buffer_ + FRAME_LENGTH_OFFSET, THREE_BYTES);
frame_length_1 = static_cast<int>(temporary_buffer[0]);
frame_length_2 = static_cast<int>(temporary_buffer[1]);
frame_type_ = static_cast<int>(temporary_buffer[2]);
sprintf(header_buffer, "%02X%02X%02X", frame_length_1,
frame_length_2, frame_type_);
sscanf(header_buffer, "%04X%02X", &frame_length, &frame_type_);
frame_body_length_ = frame_length;
if (frame_body_length_ > MAX_FRAME_BODY_LENGTH)
frame_body_length_ = 0; /* The message header is corrupted. Ignore the total frame */
return true;
}
//*****************************************************************************
std::size_t Frame::Get_Frame_Type() const
{
return frame_type_;
}
//*****************************************************************************
int Frame::Get_Start_Delimiter_Position()
{
const unsigned char START_DELIMITER = static_cast<unsigned char>(0x7E);
for (int i = 0; i < FRAME_HEADER_LENGTH; i++)
{
if (START_DELIMITER == data_buffer_[i])
{
start_delimiter_position_ = i;
return i;
}
}
return -1;
}
//*****************************************************************************
void Frame::Rearrange_Corrupted_Header()
{
char temporary_char;
for (unsigned short i = 0; i < start_delimiter_position_; i++)
{
temporary_char = data_buffer_[i];
data_buffer_[i] = data_buffer_[start_delimiter_position_ + i];
data_buffer_[start_delimiter_position_ + i] = temporary_char;
}
}
}
}

206
src/XBeeModule.cpp Normal file
View File

@ -0,0 +1,206 @@
/* XBeeModule.cpp -- Xbee Module class provides functions to communicate
with Xbee -- */
/* ------------------------------------------------------------------------- */
/* November 30, 2016 -- @Copyright Aymen Soussia. All rights reserved. */
/* (aymen.soussia@gmail.com) */
#include "XBeeModule.h"
//*****************************************************************************
XBeeModule::XBeeModule():
TIME_OUT(7),
serial_port_(io_service_),
timer_(serial_port_.get_io_service()),
connected_to_XBee_(false),
time_out_exceeded_(false)
{
}
//*****************************************************************************
XBeeModule::~XBeeModule()
{
}
//*****************************************************************************
bool XBeeModule::Init_Port(const std::string& device_port, const unsigned int baud_rate)
{
if (Open_Port(device_port))
{
serial_port_.set_option(boost::asio::serial_port::baud_rate(baud_rate));
Start_Receive();
Set_AT_Command_Mode();
timer_.expires_from_now(posix_time::seconds(TIME_OUT));
timer_.async_wait(bind(&XBeeModule::Check_Time_Out, this,
boost::asio::placeholders::error));
return true;
}
else
return false;
}
//*****************************************************************************
bool XBeeModule::Open_Port(const std::string& device_port)
{
serial_port_.open(device_port);
if (serial_port_.is_open())
{
std::cout << "Serial Port Open..." << std::endl;
return true;
}
else
{
std::cout << "Failed to Open The Serial Port." << std::endl;
std::cout << "Please Check The Introduced Serial Port is Correct." << std::endl;
return false;
}
}
//*****************************************************************************
void XBeeModule::Start_Receive()
{
const std::size_t THREE_BYTES = 3;
boost::asio::async_read(serial_port_,
boost::asio::buffer(receive_buffer_, THREE_BYTES),
bind(&XBeeModule::Handle_Receive, this,
asio::placeholders::error,
asio::placeholders::bytes_transferred));
}
//*****************************************************************************
void XBeeModule::Handle_Receive(const boost::system::error_code& error, size_t bytes_transferred)
{
if (error)
{
std::cout << "An Error Occured: " << error << std::endl;
}
if (0 == strncmp(receive_buffer_, "OK\r", bytes_transferred) && !connected_to_XBee_)
{
connected_to_XBee_ = true;
}
}
//*****************************************************************************
void XBeeModule::Set_AT_Command_Mode()
{
const useconds_t ONE_SECOND = 1*1000*1000; /* 1s = 1 * 10⁶ microseconds. */
char AT_command_sequence[] = "+++";
const unsigned short THREE_BYTES = 3;
usleep(ONE_SECOND);
serial_port_.write_some(asio::buffer(AT_command_sequence, THREE_BYTES));
usleep(ONE_SECOND);
}
//*****************************************************************************
void XBeeModule::Check_Time_Out(const system::error_code& error)
{
if (error)
return;
else
{
if (!connected_to_XBee_)
{
std::cout << "Time Out: The XBee Module is Not Responding." << std::endl;
std::cout << "Please Try One of The Following Options:" << std::endl;
std::cout << " 1) Change The Baud Rate. Make Sure It Matches The Baud Rate Used by The XBee." << std::endl;
std::cout << " 2) Disconnect and Connect The XBee." << std::endl;
std::cout << " 3) Press The Reset Button on The XBee." << std::endl;
std::cout << " 4) Update The Firmware With XCTU." << std::endl;
serial_port_.close();
time_out_exceeded_ = true;
}
}
}
//*****************************************************************************
void XBeeModule::Run_Service()
{
io_service_.run();
}
//*****************************************************************************
void XBeeModule::Send_Data(const std::string& command)
{
io_service_.post(
[this, command]()
{
bool write_in_progress = !commands_sequence_.empty();
commands_sequence_.push_back(command);
if (!write_in_progress)
Do_Write();
});
}
//*****************************************************************************
void XBeeModule::Exit_AT_Command_Mode()
{
const unsigned short FIVE_BYTES = 5;
serial_port_.write_some(boost::asio::buffer("ATCN\r", FIVE_BYTES));
io_service_.post([this]() {io_service_.stop(); });
io_service_.post([this]() {serial_port_.close(); });
}
//*****************************************************************************
bool XBeeModule::Is_Connected()
{
return connected_to_XBee_;
}
//*****************************************************************************
bool XBeeModule::Check_Time_Out_Exceeded()
{
return time_out_exceeded_;
}
//*****************************************************************************
void XBeeModule::Do_Write()
{
boost::asio::async_write(serial_port_,
boost::asio::buffer(commands_sequence_.front().data(), commands_sequence_.front().size()),
boost::bind(&XBeeModule::Handle_Write, this,
asio::placeholders::error));
}
//*****************************************************************************
void XBeeModule::Handle_Write(const system::error_code& error)
{
if (!error)
{
commands_sequence_.pop_front();
if (!commands_sequence_.empty())
Do_Write();
}
}
//*****************************************************************************
void XBeeModule::Format_AT_Command(const XBee_Parameter_S& parameter, std::string* command)
{
std::string current_command = "AT";
current_command.append(parameter.command_);
current_command.append(" ");
current_command.append(parameter.value_);
current_command.append("\r");
command->append(current_command);
}

73
src/XMLConfigParser.cpp Normal file
View File

@ -0,0 +1,73 @@
/* XMLConfigParser.cpp -- XML Config Parser class -- */
/* ------------------------------------------------------------------------- */
/* November 30, 2016 -- @Copyright Aymen Soussia. All rights reserved. */
/* (aymen.soussia@gmail.com) */
#include "XMLConfigParser.h"
//*****************************************************************************
XMLConfigParser::XMLConfigParser():
config_loaded_successfully_(false)
{
}
//*****************************************************************************
XMLConfigParser::~XMLConfigParser()
{
}
//*****************************************************************************
bool XMLConfigParser::Load_Config()
{
const std::string FILE_NAME = "/home/mistlab/catkin_ws/src/xbee_ros_node/Resources/XBee_Config.xml";
if (Check_Config_File_Exists(FILE_NAME))
{
ptree pt;
boost::property_tree::read_xml(FILE_NAME, pt);
config_loaded_successfully_ = true;
BOOST_FOREACH(ptree::value_type const&v, pt.get_child("XBeeConfig.Settings"))
{
if (v.first == "Parameter")
{
XBee_Parameter_S new_xbee_parameter;
new_xbee_parameter.command_ = v.second.get<std::string>("<xmlattr>.Command");
new_xbee_parameter.value_ = v.second.data();
xbee_parameters_.push_back(new_xbee_parameter);
}
}
return true;
}
else
{
std::cout << "Error: Config File Not Found." << std::endl;
return false;
}
}
//*****************************************************************************
std::vector<XBee_Parameter_S>* XMLConfigParser::Get_Loaded_Parameters()
{
return &xbee_parameters_;
}
//*****************************************************************************
bool XMLConfigParser::Check_Config_File_Exists(const std::string& file_name)
{
return boost::filesystem::exists(file_name);
}
bool XMLConfigParser::Is_Config_Loaded_Successfully()
{
return config_loaded_successfully_;
}

57
src/Xbee.cpp Normal file
View File

@ -0,0 +1,57 @@
/* xbee.cpp -- XBeeMav ROS node main -- */
/* ------------------------------------------------------------------------- */
/* September 20, 2016 -- @Copyright Aymen Soussia. All rights reserved. */
/* (aymen.soussia@gmail.com) */
#include "CommunicationManager.h"
//*****************************************************************************
int main(int argc, char* argv[])
{
try
{
ros::init(argc, argv, "xbee");
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.
Mist::Xbee::CommunicationManager::DRONE_TYPE drone_type =
Mist::Xbee::CommunicationManager::DRONE_TYPE::SLAVE;
Mist::Xbee::CommunicationManager::RUNNING_MODE running_mode =
Mist::Xbee::CommunicationManager::RUNNING_MODE::SOLO;
if (argc > 1)
{
if (!strcmp(argv[1], "master"))
drone_type = Mist::Xbee::CommunicationManager::DRONE_TYPE::MASTER;
if (argc > 2)
{
if (!strcmp(argv[2], "swarm"))
running_mode = Mist::Xbee::CommunicationManager::RUNNING_MODE::SWARM;
}
}
if (communication_manager.Init(device, baud_rate))
communication_manager.Run(drone_type, running_mode);
}
catch (const std::exception& e)
{
std::cout << "Error Occured:" << std::endl;
std::cout << e.what() << std::endl;
}
catch (...)
{
std::cout << "Unexpected Fatal Error." << std::endl;
std::cout << "Communication With XBee is Lost." << std::endl;
return EXIT_FAILURE;
}
std::cout << std::endl;
std::cout << "XBeeMav Exited." << std::endl;
return EXIT_SUCCESS;
}

114
src/main.cpp Normal file
View File

@ -0,0 +1,114 @@
/* main.cpp -- */
/* ------------------------------------------------------------------------- */
/* November 30, 2016 -- @Copyright Aymen Soussia. All rights reserved. */
/* (aymen.soussia@gmail.com) */
#include<thread>
#include"XBeeModule.h"
#include"XMLConfigParser.h"
//*****************************************************************************
void Setup_XBee(const std::string& device_port, const unsigned int baud_rate)
{
XBeeModule xbee_module;
XMLConfigParser config_parser;
if (xbee_module.Init_Port(device_port, baud_rate))
{
std::thread th_service(&XBeeModule::Run_Service, &xbee_module);
while (!xbee_module.Is_Connected() && !xbee_module.Check_Time_Out_Exceeded())
{
continue;
}
if (xbee_module.Is_Connected())
{
std::cout << "Connected to XBee." << std::endl;
std::cout << "Loading Config File..." << std::endl;
if (config_parser.Load_Config())
{
std::cout << "Config Loaded Successfully." << std::endl;
std::cout << "Transferring Data..." << std::endl;
std::vector<XBee_Parameter_S>* config_parameters = config_parser.Get_Loaded_Parameters();
for (std::size_t i = 0; i < config_parameters->size(); i++)
{
std::string current_command;
xbee_module.Format_AT_Command(config_parameters->at(i), &current_command);
xbee_module.Send_Data(current_command);
}
std::string write_command = "ATWR \r";
xbee_module.Send_Data(write_command);
}
}
th_service.join();
std::cout << "Exiting AT Command Mode..." << std::endl;
if (xbee_module.Is_Connected())
{
xbee_module.Exit_AT_Command_Mode();
if (config_parser.Is_Config_Loaded_Successfully())
std::cout << "XBee Configured Successfully." << std::endl;
else
std::cout << "XBee Configuration Failed." << std::endl;
}
else
{
std::cout << "XBee Configuration Failed." << std::endl;
}
}
}
//*****************************************************************************
int main(int argc, char*argv[])
{
try
{
std::string device_port;
unsigned int baud_rate = 0;
const unsigned int DEFAULT_BAUD_RATE = 9600;
const std::string DEFAULT_DEVICE_PORT = "/dev/ttyUSB0";
if (argc < 1)
device_port = DEFAULT_DEVICE_PORT;
else
device_port.append(argv[1]);
if (argc < 2)
baud_rate = DEFAULT_BAUD_RATE;
else
sscanf(argv[2], "%u", &baud_rate);
Setup_XBee(device_port, baud_rate);
}
catch (const std::exception& e)
{
std::cout << "Error: " << e.what() << std::endl;
std::cout << "Please Try One of The Following Options:" << std::endl;
std::cout << " 1) Change The Baud Rate. Make Sure It Matches The Current Baud Rate Used by The XBee (By default BD = 9600 bps)." << std::endl;
std::cout << " 2) Change The Specified Device (e.g. COM1 for Windows or /dev/tty/USB0 for Linux)." << std::endl;
std::cout << " 3) Disconnect and Connect The XBee." << std::endl;
std::cout << " 4) Press The Reset Button on The XBee." << std::endl;
std::cout << " 5) Update The Firmware With XCTU." << std::endl;
std::cout << "XBee Configuration Failed." << std::endl;
}
catch (...)
{
std::cout << " Unexpected Error." << std::endl;
std::cout << "XBee Configuration Failed." << std::endl;
return EXIT_FAILURE;
}
return EXIT_SUCCESS;
}