diff --git a/CMakeLists.txt b/CMakeLists.txt
index a54cab7..962e5a5 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -56,8 +56,8 @@ include_directories(
 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(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})
diff --git a/Resources/XBee_Config.xml b/Resources/XBee_Config.xml
index f7f4b2d..e8a0b56 100644
--- a/Resources/XBee_Config.xml
+++ b/Resources/XBee_Config.xml
@@ -22,7 +22,7 @@
 		<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="BD" Description="Baud Rate">4</Parameter>
 		<Parameter Command="NB" Description="Parity">0</Parameter>
 		<Parameter Command="SB" Description="Stop Bits">0</Parameter>
 		<Parameter Command="RO" Description="Packetization Timeout">3</Parameter>
diff --git a/src/CommunicationManager.cpp b/src/CommunicationManager.cpp
index 651ec3d..100627f 100644
--- a/src/CommunicationManager.cpp
+++ b/src/CommunicationManager.cpp
@@ -44,7 +44,7 @@ namespace Xbee
 //*****************************************************************************
 CommunicationManager::CommunicationManager():
 	START_DLIMITER(static_cast<unsigned char>(0x7E)),
-	LOOP_RATE(10) /* 10 fps */
+	LOOP_RATE(30) /* 10 fps */
 {
 }
 
@@ -604,7 +604,7 @@ 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 = 25;
+	const unsigned short MAX_NBR_OF_INT64 = 24;
 	char temporary_buffer[MAX_BUFFER_SIZE];
 	std::string frame;
 	int converted_bytes = 0;
diff --git a/src/Xbee.cpp b/src/Xbee.cpp
index d71e3a6..60c823c 100644
--- a/src/Xbee.cpp
+++ b/src/Xbee.cpp
@@ -17,7 +17,7 @@ int main(int argc, char* argv[])
 
 	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.
+	const std::size_t baud_rate = 19200; // 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 =