diff --git a/CMakeLists.txt b/CMakeLists.txt
index 1d20408..247e8a5 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,11 +1,6 @@
 cmake_minimum_required(VERSION 2.8.3)
 project(rosbuzz)
-
-
-
 ## Find catkin macros and libraries
-## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
-## is used, also find other catkin packages
 find_package(catkin REQUIRED COMPONENTS
   roscpp
   std_msgs
@@ -13,186 +8,33 @@ find_package(catkin REQUIRED COMPONENTS
   sensor_msgs
 )
 
-
-## System dependencies are found with CMake's conventions
-# find_package(Boost REQUIRED COMPONENTS system)
-
-
-################################################
-## Declare ROS messages, services and actions ##
-################################################
-
-## To declare and build messages, services or actions from within this
-## package, follow these steps:
-## * Let MSG_DEP_SET be the set of packages whose message types you use in
-##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
-## * In the file package.xml:
-##   * add a build_depend tag for "message_generation"
-##   * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
-##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
-##     but can be declared for certainty nonetheless:
-##     * add a run_depend tag for "message_runtime"
-## * In this file (CMakeLists.txt):
-##   * add "message_generation" and every package in MSG_DEP_SET to
-##     find_package(catkin REQUIRED COMPONENTS ...)
-##   * add "message_runtime" and every package in MSG_DEP_SET to
-##     catkin_package(CATKIN_DEPENDS ...)
-##   * uncomment the add_*_files sections below as needed
-##     and list every .msg/.srv/.action file to be processed
-##   * uncomment the generate_messages entry below
-##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
-
-## Generate messages in the 'msg' folder
-# add_message_files(
-#   DIRECTORY msg
-#   FILES
-#   neighbour_pos.msg
-#   Message2.msg
-# )
-
-## Generate services in the 'srv' folder
-# add_service_files(
-#   FILES
-#   Service1.srv
-#   Service2.srv
-# )
-
-## Generate actions in the 'action' folder
-# add_action_files(
-#   FILES
-#   Action1.action
-#   Action2.action
-# )
-
-## Generate added messages and services with any dependencies listed here
-# generate_messages(
-#   DEPENDENCIES
-#   sensor_msgs
-# )
-
-################################################
-## Declare ROS dynamic reconfigure parameters ##
-################################################
-
-## To declare and build dynamic reconfigure parameters within this
-## package, follow these steps:
-## * In the file package.xml:
-##   * add a build_depend and a run_depend tag for "dynamic_reconfigure"
-## * In this file (CMakeLists.txt):
-##   * add "dynamic_reconfigure" to
-##     find_package(catkin REQUIRED COMPONENTS ...)
-##   * uncomment the "generate_dynamic_reconfigure_options" section below
-##     and list every .cfg file to be processed
-
-## Generate dynamic reconfigure parameters in the 'cfg' folder
-# generate_dynamic_reconfigure_options(
-#   cfg/DynReconf1.cfg
-#   cfg/DynReconf2.cfg
-# )
-
 ###################################
 ## catkin specific configuration ##
 ###################################
-## The catkin_package macro generates cmake config files for your package
-## Declare things to be passed to dependent projects
-## INCLUDE_DIRS: uncomment this if you package contains header files
-## LIBRARIES: libraries you create in this project that dependent projects also need
-## CATKIN_DEPENDS: catkin_packages dependent projects also need
-## DEPENDS: system dependencies of this project that dependent projects also need
+
 catkin_package(
 #CATKIN_DEPENDS message_runtime
-#  INCLUDE_DIRS include
-#  LIBRARIES buzzros
-#  CATKIN_DEPENDS Buzz roscpp st_msgs
-#  DEPENDS system_lib
 )
-
 ###########
 ## Build ##
 ###########
 
-## Specify additional locations of header files
-## Your package locations should be listed before other locations
-# include_directories(include)
 include_directories(
   ${catkin_INCLUDE_DIRS}
 )
 
-## Declare a C++ library
-# add_library(buzzros
-#   src/${PROJECT_NAME}/buzzros.cpp
-# )
-
-## Add cmake target dependencies of the library
-## as an example, code may need to be generated before libraries
-## either from message generation or dynamic reconfigure
-# add_dependencies(buzzros ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
-## Declare a C++ executable
-# add_executable(buzzros_node src/buzzros_node.cpp)
-
-## Add cmake target dependencies of the executable
-## same as for the library above
-# add_dependencies(buzzros_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
-## Specify libraries to link a library or executable target against
-# target_link_libraries(buzzros_node
-#   ${catkin_LIBRARIES}
-# )
-
+# Executables 
 add_executable(rosbuzz_node src/rosbuzz.cpp
   src/buzz_utility.cpp
   src/buzzuav_closures.cpp
   src/uav_utility.cpp)
 target_link_libraries(rosbuzz_node ${catkin_LIBRARIES} /usr/local/lib/libbuzz.so /usr/local/lib/libbuzzdbg.so -lpthread)
-#add_dependencies(rosbuzz_node rosbuzz_generate_messages_cpp)
-#############
-## Install ##
-#############
 
-# all install targets should use catkin DESTINATION variables
-# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
-
-## Mark executable scripts (Python etc.) for installation
-## in contrast to setup.py, you can choose the destination
-# install(PROGRAMS
-#   scripts/my_python_script
-#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
-
-## Mark executables and/or libraries for installation
+# Executables and libraries for installation to do
 install(TARGETS rosbuzz_node
    ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
    LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
    RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )
 
-## Mark cpp header files for installation
-# install(DIRECTORY include/${PROJECT_NAME}/
-#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
-#   FILES_MATCHING PATTERN "*.h"
-#   PATTERN ".svn" EXCLUDE
-# )
-
-## Mark other files for installation (e.g. launch and bag files, etc.)
-# install(FILES
-#   # myfile1
-#   # myfile2
-#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-# )
-
-#############
-## Testing ##
-#############
-
-## Add gtest based cpp test target and link libraries
-# catkin_add_gtest(${PROJECT_NAME}-test test/test_buzzros.cpp)
-# if(TARGET ${PROJECT_NAME}-test)
-#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
-# endif()
-
-## Add folders to be run by python nosetests
-# catkin_add_nosetests(test)
-
-
 
diff --git a/package.xml b/package.xml
index 6e779ce..3f9b78d 100644
--- a/package.xml
+++ b/package.xml
@@ -1,19 +1,19 @@
 <?xml version="1.0"?>
 <package>
   <name>rosbuzz</name>
-  <version>0.0.0</version>
+  <version>0.1.0</version>
   <description>The rosbuzz package</description>
 
   <!-- One maintainer tag required, multiple allowed, one person per tag --> 
   <!-- Example:  -->
   <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
-  <maintainer email="vivek@todo.todo">vivek</maintainer>
+  <maintainer email="vivek-shankar.varadharajan@polymtl.ca">vivek</maintainer>
 
 
   <!-- One license tag required, multiple allowed, one license per tag -->
   <!-- Commonly used license strings: -->
   <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
-  <license>TODO</license>
+  <license>MIT</license>
 
 
   <!-- Url tags are optional, but mutiple are allowed, one per tag -->
@@ -25,7 +25,7 @@
   <!-- Author tags are optional, mutiple are allowed, one per tag -->
   <!-- Authors do not have to be maintianers, but could be -->
   <!-- Example: -->
-  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+   <author email="vivek-shankar.varadharajan@polymtl.ca">vivek</author> 
 
 
   <!-- The *_depend tags are used to specify dependencies -->
diff --git a/readme.md b/readme.md
new file mode 100644
index 0000000..5b71774
--- /dev/null
+++ b/readme.md
@@ -0,0 +1,85 @@
+							ROS Implemenation of Buzz
+							=========================
+
+What is Buzz?
+=============
+
+Buzz is a novel programming language for heterogeneous robots swarms.
+
+Buzz advocates a compositional approach, by offering primitives to define swarm behaviors both in a bottom-up and in a top-down fashion.
+
+Bottom-up primitives include robot-wise commands and manipulation of neighborhood data through mapping/reducing/filtering operations.
+
+Top-down primitives allow for the dynamic management of robot teams, and for sharing information globally across the swarm.
+
+Self-organization results from the fact that the Buzz run-time platform is purely distributed.
+
+The language can be extended to add new primitives (thus supporting heterogeneous robot swarms) and can be laid on top of other frameworks, such as ROS.
+
+More information is available at http://the.swarming.buzz/wiki/doku.php?id=start.
+
+Downloading ROS Package
+=======================
+
+    $ git clone
+
+Requirements
+------------
+* Buzz : You can download the development sources through git:
+
+    $ git clone https://github.com/MISTLab/Buzz.git buzz
+
+* ROS binary distribution (Indigo or higher) with catkin
+
+You need the following package:
+
+*  mavros_msgs : You can install using apt-get:
+
+    $ sudo apt-get install ros-"distro"-mavros ros-"distro"-mavros-extras
+
+Compilation
+-----------
+
+To compile the ros package, execute the following:
+
+    $ cd catkin_ws
+    $ catkin_make
+    $ source devel/setup.bash
+    
+Run
+===
+
+    $ rosrun rosbuzz rosbuzz_node
+
+Publisher
+----------
+
+* Messages from Buzz (BVM):
+The package publishes mavros_msgs/Mavlink message with a topic name of "outMavlink". 
+
+Subscribers
+-----------
+
+* Current position of the Robot:
+The package subscribes' to sensor_msgs/NavSatFix message with a topic name of "current_pos".
+
+* Messages to Buzz (BVM):
+The package subscribes' to mavros_msgs/Mavlink message with a topic name of "inMavlink".
+
+* Battery status:
+The package subscribes' to mavros_msgs/BatteryStatus message with a topic name of "battery_state".
+
+Service
+-------
+
+* Remote Controller command:
+The package offers service using mavros_msgs/CommandInt service with name "djicmd_rc".
+
+Client
+------
+
+* Flight controller client:
+This package is a client of mavros_msgs/CommandInt service with name "djicmd".
+ 
+
+
diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp
index d725137..f521feb 100644
--- a/src/buzz_utility.cpp
+++ b/src/buzz_utility.cpp
@@ -1,23 +1,19 @@
+/** @file      buzz_utility.cpp
+ *  @version   1.0 
+ *  @date      27.09.2016
+ *  @brief     Buzz Implementation as a node in ROS for Dji M100 Drone. 
+ *  @author    Vivek Shankar Varadharajan
+ *  @copyright 2016 MistLab. All rights reserved.
+ */
 #define _GNU_SOURCE
 #include <stdio.h>
-
 #include "buzz_utility.h"
 #include "buzzuav_closures.h"
 #include <buzz/buzzdebug.h>
-
 #include <stdlib.h>
 #include <string.h>
 #include <unistd.h>
-
-#include <arpa/inet.h>
-#include <errno.h>
-#include <netdb.h>
-#include <sys/types.h>
-#include <sys/socket.h>
-#include <pthread.h>
-#include <vector>
 #include <iostream>
-#include <bitset>
 #include <map>
 using namespace std;
 /****************************************/
@@ -33,15 +29,13 @@ static int         TCP_COMM_STREAM = -1;
 static uint8_t*    STREAM_SEND_BUF = NULL;
 
 
-
-
 /****************************************/
-
+/*adds neighbours position*/
 void neighbour_pos_callback(std::map< int,  Pos_struct> neighbours_pos_map){
- /* Reset neighbor information */
-   buzzneighbors_reset(VM);
+  /* Reset neighbor information */
+    buzzneighbors_reset(VM);
   /* Get robot id and update neighbor information */
-map< int, Pos_struct >::iterator it;
+    map< int, Pos_struct >::iterator it;
     for (it=neighbours_pos_map.begin(); it!=neighbours_pos_map.end(); ++it){
     buzzneighbors_add(VM,
                         it->first,
@@ -49,10 +43,10 @@ map< int, Pos_struct >::iterator it;
                         (it->second).y,
                         (it->second).z);
     }
-
-
 }
-
+/***************************************/
+/*Reinterprets  uint64_t into 4 uint16_t*/
+/***************************************/
 uint16_t* u64_cvt_u16(uint64_t u64){
    uint16_t* out = new uint16_t[4];
    uint32_t int32_1 = u64 & 0xFFFFFFFF;
@@ -63,9 +57,12 @@ uint16_t* u64_cvt_u16(uint64_t u64){
     out[3] = (int32_2 & (0xFFFF0000) ) >> 16;
    //cout << " values " <<out[0] <<"  "<<out[1] <<"  "<<out[2] <<"  "<<out[3] <<"  ";
 return out;
-
 } 
 
+/***************************************************/
+/*Appends obtained messages to buzz in message Queue*/
+/***************************************************/
+
 void in_msg_process(uint64_t* payload){
 
    /* Go through messages and add them to the FIFO */
@@ -74,10 +71,10 @@ void in_msg_process(uint64_t* payload){
    delete[] data;
    uint8_t* pl =(uint8_t*)malloc(size);
    memset(pl, 0,size);
-      /* Copy packet into temporary buffer */
+   /* Copy packet into temporary buffer */
    memcpy(pl, payload ,size);
 
-    size_t tot = sizeof(uint32_t);
+   size_t tot = sizeof(uint32_t);
       
       /* Go through the messages until there's nothing else to read */
       uint16_t unMsgSize;
@@ -96,15 +93,18 @@ void in_msg_process(uint64_t* payload){
    /* Process messages */
 buzzvm_process_inmsgs(VM);
 }
+/***************************************************/
+/*Obtains messages from buzz out message Queue*/
+/***************************************************/
 
 uint64_t* out_msg_process(){
    buzzvm_process_outmsgs(VM);
    uint8_t* buff_send =(uint8_t*)malloc(MSG_SIZE);
    memset(buff_send, 0, MSG_SIZE);
    ssize_t tot = sizeof(uint16_t);
-    /* Send robot id */
+   /* Send robot id */
    *(uint16_t*)(buff_send+tot) = (uint16_t) VM->robot;
-    tot += sizeof(uint16_t);
+   tot += sizeof(uint16_t);
    /* Send messages from FIFO */
    do {
       /* Are there more messages? */
@@ -135,16 +135,18 @@ uint64_t* out_msg_process(){
    int total_size =(ceil((float)tot/(float)sizeof(uint64_t))); 
    *(uint16_t*)buff_send = (uint16_t) total_size;   
   
-
-  uint64_t* payload_64 = new uint64_t[total_size];
+   
+   uint64_t* payload_64 = new uint64_t[total_size];
  
- memcpy((void*)payload_64, (void*)buff_send, total_size*sizeof(uint64_t));
-
+   memcpy((void*)payload_64, (void*)buff_send, total_size*sizeof(uint64_t));
+   /*for(int i=0;i<total_size;i++){
+   cout<<" payload from out msg  "<<*(payload_64+i)<<endl;
+   }*/
    /* Send message */
 return payload_64;
 }
 /****************************************/
-
+/*Buzz script not able to load*/
 /****************************************/
 
 static const char* buzz_error_info() {
@@ -170,6 +172,7 @@ static const char* buzz_error_info() {
 }
 
 /****************************************/
+/*Buzz hooks that can be used inside .bzz file*/
 /****************************************/
 
 static int buzz_register_hooks() {
@@ -192,7 +195,7 @@ static int buzz_register_hooks() {
 }
 
 /****************************************/
-
+/*Sets the .bzz and .bdbg file*/
 /****************************************/
 
 int buzz_script_set(const char* bo_filename,
@@ -202,7 +205,7 @@ int buzz_script_set(const char* bo_filename,
    gethostname(hstnm, 30);
    /* Make numeric id from hostname */
    /* NOTE: here we assume that the hostname is in the format Knn */
-   int id =0; // strtol(hstnm + 1, NULL, 10);
+   int id = strtol(hstnm + 1, NULL, 10);
    cout << " Robot ID" << id<< endl;
    /* Reset the Buzz VM */
    if(VM) buzzvm_destroy(&VM);
@@ -260,6 +263,7 @@ int buzz_script_set(const char* bo_filename,
 }
 
 /****************************************/
+/*Swarm struct*/
 /****************************************/
 
 struct buzzswarm_elem_s {
@@ -297,11 +301,9 @@ void check_swarm_members(const void* key, void* data, void* params) {
       }
    }
 }
+/*Step through the buzz script*/
 
 void buzz_script_step() {
-
-   
-   //buzzvm_process_inmsgs(VM);
    /*
     * Update sensors
     */
@@ -319,7 +321,7 @@ void buzz_script_step() {
    /* Print swarm */
    buzzswarm_members_print(stdout, VM->swarmmembers, VM->robot);
    /* Check swarm state */
- /*  int status = 1;
+   /*  int status = 1;
    buzzdict_foreach(VM->swarmmembers, check_swarm_members, &status);
    if(status == 1 &&
       buzzdict_size(VM->swarmmembers) < 9)
@@ -330,13 +332,10 @@ void buzz_script_step() {
 }
 
 /****************************************/
+/*Destroy the bvm and other resorces*/
 /****************************************/
 
 void buzz_script_destroy() {
-   
-   /* Get rid of stream buffer */
-   //free(STREAM_SEND_BUF);
-   /* Get rid of virtual machine */
    if(VM) {
       if(VM->state != BUZZVM_STATE_READY) {
          fprintf(stderr, "%s: execution terminated abnormally: %s\n\n",
@@ -353,6 +352,7 @@ void buzz_script_destroy() {
 }
 
 /****************************************/
+/*Execution completed*/
 /****************************************/
 
 int buzz_script_done() {
diff --git a/src/buzz_utility.h b/src/buzz_utility.h
index 54ac04f..2bd0102 100644
--- a/src/buzz_utility.h
+++ b/src/buzz_utility.h
@@ -1,6 +1,5 @@
 #ifndef BUZZ_UTILITY_H
 #define BUZZ_UTILITY_H
-#include <vector>
 #include <stdint.h>
 #include <map>
 struct pos_struct
@@ -13,19 +12,21 @@ typedef struct pos_struct Pos_struct ;
 
 uint16_t* u64_cvt_u16(uint64_t u64);
 
-extern int buzz_listen(const char* type,
+int buzz_listen(const char* type,
                        int msg_size);
+
 void neighbour_pos_callback(std::map< int,  Pos_struct> neighbours_pos_map);
+
 void in_msg_process(uint64_t* payload);
 
 uint64_t* out_msg_process();
 
-extern int buzz_script_set(const char* bo_filename,
+int buzz_script_set(const char* bo_filename,
                            const char* bdbg_filename);
-extern void buzz_script_step();
+void buzz_script_step();
 
-extern void buzz_script_destroy();
+void buzz_script_destroy();
 
-extern int buzz_script_done();
+int buzz_script_done();
 
 #endif
diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp
index 9b28a94..7cf051f 100644
--- a/src/buzzuav_closures.cpp
+++ b/src/buzzuav_closures.cpp
@@ -1,4 +1,11 @@
-#define _GNU_SOURCE
+/** @file      buzzuav_closures.cpp
+ *  @version   1.0 
+ *  @date      27.09.2016
+ *  @brief     Buzz Implementation as a node in ROS for Dji M100 Drone. 
+ *  @author    Vivek Shankar Varadharajan
+ *  @copyright 2016 MistLab. All rights reserved.
+ */
+//#define _GNU_SOURCE
 #include <stdio.h>
 #include "buzzuav_closures.h"
 #include "uav_utility.h"
@@ -56,16 +63,17 @@ int buzzros_print(buzzvm_t vm) {
 
 int buzzuav_goto(buzzvm_t vm) {
    buzzvm_lnum_assert(vm, 3);
-   buzzvm_lload(vm, 1); /* Lattitude */
+   buzzvm_lload(vm, 1); /* Altitude */
    buzzvm_lload(vm, 2); /* Longitude */
-   buzzvm_lload(vm, 3); /* Altitude */
+   buzzvm_lload(vm, 3); /* Latitude */
    buzzvm_type_assert(vm, 3, BUZZTYPE_FLOAT);
    buzzvm_type_assert(vm, 2, BUZZTYPE_FLOAT);
    buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT);
-goto_pos[2]=buzzvm_stack_at(vm, 1)->f.value * 10.0f;
-goto_pos[1]=buzzvm_stack_at(vm, 2)->f.value * 10.0f;
-goto_pos[0]=buzzvm_stack_at(vm, 3)->f.value * 10.0f;
-cur_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT;
+   goto_pos[2]=buzzvm_stack_at(vm, 1)->f.value; 
+   goto_pos[1]=buzzvm_stack_at(vm, 2)->f.value; 
+   goto_pos[0]=buzzvm_stack_at(vm, 3)->f.value; 
+   cur_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT;
+   printf(" Buzz requested Go To, to Latitude: %15f , Longitude: %15f, Altitude: %15f  \n",goto_pos[0],goto_pos[1],goto_pos[2]);
    return buzzvm_ret0(vm);
 }
 
@@ -95,24 +103,27 @@ cur_cmd=rc_cmd;
 
 int buzzuav_takeoff(buzzvm_t vm) {
    cur_cmd=mavros_msgs::CommandCode::NAV_TAKEOFF;
+   printf(" Buzz requested Take off !!! \n");
    return buzzvm_ret0(vm);
 }
 
 int buzzuav_land(buzzvm_t vm) {
    cur_cmd=mavros_msgs::CommandCode::NAV_LAND;
+   printf(" Buzz requested Land !!! \n");
    return buzzvm_ret0(vm);
 }
 
 int buzzuav_gohome(buzzvm_t vm) {
    cur_cmd=mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH;
+   printf(" Buzz requested gohome !!! \n");
    return buzzvm_ret0(vm);
 }
 
 /****************************************/
 void set_battery(float voltage,float current,float remaining){
-batt[0]=voltage;
-batt[1]=current;
-batt[2]=remaining;
+ batt[0]=voltage;
+ batt[1]=current;
+ batt[2]=remaining;
 }
 /****************************************/
 
@@ -137,10 +148,11 @@ int buzzuav_update_battery(buzzvm_t vm) {
 }
 
 /****************************************/
+/*To do !!!!!*/
 /****************************************/
 
 int buzzuav_update_prox(buzzvm_t vm) {
-   static char PROXIMITY_BUF[256];
+ /*  static char PROXIMITY_BUF[256];
    int i;
    //kh4_proximity_ir(PROXIMITY_BUF, DSPIC);
    buzzvm_pushs(vm, buzzvm_string_register(vm, "proximity_ir", 1));
@@ -161,7 +173,7 @@ int buzzuav_update_prox(buzzvm_t vm) {
       buzzvm_tput(vm);
    }
    buzzvm_gstore(vm);
-   return vm->state;
+   return vm->state;*/
 }
 
 /****************************************/
diff --git a/src/buzzuav_closures.h b/src/buzzuav_closures.h
index 1759230..33671db 100644
--- a/src/buzzuav_closures.h
+++ b/src/buzzuav_closures.h
@@ -17,15 +17,13 @@ int buzzros_print(buzzvm_t vm);
 int buzzuav_goto(buzzvm_t vm);
 /* Returns the current command from local variable*/
 int getcmd();
-
+/*Sets goto position could be used for bypassing*/
 void set_goto(double pos[]);
-
+/*sets rc requested command */
 void rc_call(int rc_cmd);
-
-/* sets the battery state to the local variable
- */
+/* sets the battery state */
 void set_battery(float voltage,float current,float remaining);
-/*retuns the current go to position from local variable*/
+/*retuns the current go to position */
 double* getgoto();
 /*
  * Commands the UAV to takeoff
@@ -47,7 +45,7 @@ int buzzuav_update_battery(buzzvm_t vm);
 
 /*
  * Updates IR information in Buzz
- * Proximity and ground sensors
+ * Proximity and ground sensors to do !!!!
  */
 int buzzuav_update_prox(buzzvm_t vm);
 
diff --git a/src/out.basm b/src/out.basm
index 6223250..4cb2833 100644
--- a/src/out.basm
+++ b/src/out.basm
@@ -1,13 +1,11 @@
-!23
+!24
 'init
 'i
-'v
-'stigmergy
-'create
-'step
 's
 'swarm
+'create
 'join
+'step
 'neighbors
 'foreach
 'print
@@ -20,20 +18,23 @@
 'azimuth
 'elevation = 
 'elevation
+'exec
+'uav_takeoff
+'uav_goto
 'reset
 'destroy
 
 	pushs 0
 	pushcn @__label_1
 	gstore
-	pushs 5
+	pushs 6
 	pushcn @__label_2
 	gstore
-	pushs 21
-	pushcn @__label_4
-	gstore
 	pushs 22
-	pushcn @__label_5
+	pushcn @__label_8
+	gstore
+	pushs 23
+	pushcn @__label_9
 	gstore
 	nop
 
@@ -47,67 +48,103 @@
 	pushi 0	|3,2,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
 	gstore	|3,3,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
 	pushs 2	|4,2,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
-	pushs 3	|4,13,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
-	gload	|4,13,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
-	pushs 4	|4,14,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
-	tget	|4,20,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
-	pushi 1	|4,21,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
-	pushi 1	|4,23,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
-	callc	|4,23,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
-	gstore	|4,23,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
-	ret0	|6,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	pushs 3	|4,9,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	gload	|4,9,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	pushs 4	|4,10,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	tget	|4,16,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	pushi 10	|4,17,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	pushi 1	|4,20,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	callc	|4,20,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	gstore	|4,20,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	pushs 2	|5,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	gload	|5,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	pushs 5	|5,2,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	tget	|5,6,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	pushi 0	|5,8,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	callc	|5,8,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	ret0	|7,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
 
 @__label_2
-	pushs 6	|10,2,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
-	pushs 7	|10,9,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
-	gload	|10,9,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
-	pushs 4	|10,10,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
-	tget	|10,16,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
-	pushi 10	|10,17,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
-	pushi 1	|10,20,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
-	callc	|10,20,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
-	gstore	|10,20,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
-	pushs 6	|11,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
-	gload	|11,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
-	pushs 8	|11,2,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
-	tget	|11,6,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
-	pushi 0	|11,8,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
-	callc	|11,8,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
-	pushs 9	|13,9,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
-	gload	|13,9,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
-	pushs 10	|13,10,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
-	tget	|13,17,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
-	pushl @__label_3	|14,2,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
-	pushi 1	|18,42,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
-	callc	|18,42,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
-	ret0	|30,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	pushs 7	|12,9,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	gload	|12,9,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	pushs 8	|12,10,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	tget	|12,17,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	pushl @__label_3	|13,2,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	pushi 1	|17,42,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	callc	|17,42,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	pushs 1	|18,4,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	gload	|18,4,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	pushi 1	|18,6,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	eq	|18,7,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	jumpz @__label_4	|18,8,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	pushs 2	|19,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	gload	|19,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	pushs 19	|19,2,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	tget	|19,6,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	pushl @__label_6	|19,7,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	pushi 1	|19,34,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	callc	|19,34,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	pushs 1	|20,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	pushi 0	|20,2,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	gstore	|20,3,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	jump @__label_5	|22,4,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+@__label_4	|22,4,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	pushs 2	|23,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	gload	|23,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	pushs 19	|23,2,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	tget	|23,6,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	pushl @__label_7	|23,7,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	pushi 1	|23,51,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	callc	|23,51,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	pushs 1	|24,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	pushi 1	|24,2,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	gstore	|24,3,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+@__label_5	|25,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	ret0	|35,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
 
 @__label_3
-	pushs 11	|15,9,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
-	gload	|15,9,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
-	pushs 12	|15,10,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
-	lload 1	|15,23,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
-	pushs 13	|15,25,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
-	pushs 14	|16,8,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	pushs 9	|14,9,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	gload	|14,9,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	pushs 10	|14,10,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	lload 1	|14,23,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	pushs 11	|14,25,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	pushs 12	|15,8,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	lload 2	|15,28,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	pushs 13	|15,29,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	tget	|15,37,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	pushs 14	|15,39,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	pushs 15	|16,8,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
 	lload 2	|16,28,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
-	pushs 15	|16,29,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
-	tget	|16,37,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
-	pushs 16	|16,39,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	pushs 16	|16,29,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	tget	|16,36,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	pushs 14	|16,38,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
 	pushs 17	|17,8,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
 	lload 2	|17,28,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
 	pushs 18	|17,29,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
-	tget	|17,36,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
-	pushs 16	|17,38,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
-	pushs 19	|18,8,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
-	lload 2	|18,28,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
-	pushs 20	|18,29,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
-	tget	|18,38,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
-	pushi 11	|18,40,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
-	callc	|18,40,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
-	ret0	|18,41,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	tget	|17,38,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	pushi 11	|17,40,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	callc	|17,40,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	ret0	|17,41,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
 
-@__label_4
-	ret0	|34,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+@__label_6
+	pushs 20	|19,30,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	gload	|19,30,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	pushi 0	|19,32,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	callc	|19,32,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	ret0	|19,33,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
 
-@__label_5
-	ret0	|38,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+@__label_7
+	pushs 21	|23,27,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	gload	|23,27,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	pushf 1.1234	|23,28,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	pushf 2.2345	|23,35,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	pushf 3.3456	|23,42,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	pushi 3	|23,49,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	callc	|23,49,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	ret0	|23,50,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+
+@__label_8
+	ret0	|39,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+
+@__label_9
+	ret0	|43,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
diff --git a/src/out.bdbg b/src/out.bdbg
index 34fafc5..53ec6f7 100644
Binary files a/src/out.bdbg and b/src/out.bdbg differ
diff --git a/src/out.bo b/src/out.bo
index 15e6a6d..8a95520 100644
Binary files a/src/out.bo and b/src/out.bo differ
diff --git a/src/rosbuzz.cpp b/src/rosbuzz.cpp
index 0fdc304..2be3aa9 100644
--- a/src/rosbuzz.cpp
+++ b/src/rosbuzz.cpp
@@ -1,7 +1,8 @@
 /** @file      rosbuzz.cpp
- *  @version   1 
+ *  @version   1.0 
  *  @date      27.09.2016
- *  @brief     Buzz Implementation as a node in ROS. 
+ *  @brief     Buzz Implementation as a node in ROS for Dji M100 Drone. 
+ *  @author    Vivek Shankar Varadharajan
  *  @copyright 2016 MistLab. All rights reserved.
  */
 
@@ -27,21 +28,28 @@
 #include <map>
 using namespace std;
 
+
+/**
+ * This program implements Buzz node in ros using mavros_msgs, Developed for Dji M100.
+ */
+
+
+
 static int done = 0;
 static double cur_pos[3];
 static uint64_t payload;
 static std::map< int,  Pos_struct> neighbours_pos_map;
 static int timer_step=0;
-/**
- * This program implements Buzz node in ros.
- */
+
+/*Refresh neighbours Position for every ten step*/
 void maintain_pos(int tim_step){
 if(timer_step >=10){
 neighbours_pos_map.clear();
 timer_step=0;
 }
-
 }
+
+/*Maintain neighbours position*/
 void neighbours_pos_maintain(int id, Pos_struct pos_arr ){
 map< int, Pos_struct >::iterator it = neighbours_pos_map.find(id);
 if(it!=neighbours_pos_map.end())
@@ -49,22 +57,15 @@ neighbours_pos_map.erase(it);
 neighbours_pos_map.insert(make_pair(id, pos_arr));
 }
 
-/*usage*/
+/*print usage information not needed at the moment*/
 void usage(const char* path, int status) {
-   fprintf(stderr, "Usage:\n");
-   fprintf(stderr, "\t%s <stream> <msg_size> <file.bo> <file.bdb>\n\n", path);
-   fprintf(stderr, "== Options ==\n\n");
-   fprintf(stderr, "  stream        The stream type: tcp or bt\n");
-   fprintf(stderr, "  msg_size      The message size in bytes\n");
-   fprintf(stderr, "  file.bo       The Buzz bytecode file\n");
-   fprintf(stderr, "  file.bdbg     The Buzz debug file\n\n");
-   exit(status);
+  
 }
+
 /*Set the current position of the robot callback*/
 void set_cur_pos(double latitude,
 		 double longitude,
 		 double altitude){
-/* set the current position of the robot*/
 cur_pos [0] =latitude;
 cur_pos [1] =longitude;
 cur_pos [2] =altitude;
@@ -73,8 +74,8 @@ cur_pos [2] =altitude;
 
 /*convert from catresian to spherical coordinate system callback */
 double* cvt_spherical_coordinates(double neighbours_pos_payload []){
+
 double latitude,longitude,altitude;
-//for(int i=0;i<pos_vect.size();i++){
 latitude=neighbours_pos_payload[0];
 longitude = neighbours_pos_payload[1];
 altitude=neighbours_pos_payload[2];
@@ -99,39 +100,35 @@ set_cur_pos(msg->latitude,msg->longitude,msg->altitude);
 /*payload callback callback*/
 void payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg)
 {
-uint64_t message_obt[(msg->payload64.end())-(msg->payload64.begin())];
+uint64_t message_obt[msg->payload64.size()];
 int i = 0;
 
-	// print all the remaining numbers
+	/* Go throught the obtained payload*/
 	for(std::vector<long unsigned int>::const_iterator it = msg->payload64.begin(); it != msg->payload64.end(); ++it)
 	{
 		message_obt[i] =(uint64_t) *it;
-                cout<<" obtaind message "<<message_obt[i]<<endl;
+                //cout<<"[Debug:] obtaind message "<<message_obt[i]<<endl;
 		i++;
         }
+/* Extract neighbours position from payload*/
 double neighbours_pos_payload[3];
 memcpy(neighbours_pos_payload, message_obt, 3*sizeof(uint64_t));
-//cout<<"neighbours pos recived "<<" lat " << (double)neighbours_pos_payload[0]<<"  long "<<(double)neighbours_pos_payload[1] <<"  alti "<<(double)neighbours_pos_payload[2]<<endl;
-
+/*Convert obtained position to relative position*/
 for(i=0;i<3;i++){
 neighbours_pos_payload[i]=neighbours_pos_payload[i]-cur_pos[i];
 }
  double* cvt_neighbours_pos_payload = cvt_spherical_coordinates(neighbours_pos_payload);
-//cout<<" neighbours pos cvt " <<" lat " << cvt_neighbours_pos_payload[0]<<"  long "<<cvt_neighbours_pos_payload[1] <<"  alti "<<cvt_neighbours_pos_payload[2]<<endl;
-
-    uint16_t* out = u64_cvt_u16((uint64_t)*(message_obt+3));  
-//cout << " values " <<out[0] <<"  "<<(int)out[1] <<"  "<<out[2] <<"  "<<out[3] <<"  "<<endl;
-struct pos_struct n_pos;
-n_pos.x=cvt_neighbours_pos_payload[0];
-n_pos.y=cvt_neighbours_pos_payload[1];
-n_pos.z=cvt_neighbours_pos_payload[2];
+/*Extract robot id of the neighbour*/
+ uint16_t* out = u64_cvt_u16((uint64_t)*(message_obt+3));  
+/*pass neighbour position to local maintaner*/
+Pos_struct n_pos(cvt_neighbours_pos_payload[0],cvt_neighbours_pos_payload[1],cvt_neighbours_pos_payload[2]);
 neighbours_pos_maintain((int)out[1],n_pos);
 delete[] out;
 in_msg_process((message_obt+3));
    
 }
  
-
+/*Dji RC commands service */
 int oldcmdID=0;
 int rc_cmd;
 bool rc_callback(mavros_msgs::CommandInt::Request  &req,
@@ -181,7 +178,7 @@ if(req.command==oldcmdID)
 }
 
 
-/*controlc handler callback*/
+/*control c handler callback*/
 static void ctrlc_handler(int sig) {
    done = 1;
 }
@@ -227,9 +224,9 @@ int main(int argc, char **argv)
 
    
    /* The bytecode filename */
-   char* bcfname = "../catkin_ws/src/rosbuzz/src/out.bo"; //argv[1];
+   char* bcfname = (char*)"../catkin_ws/src/rosbuzz/src/out.bo"; //argv[1];
    /* The debugging information file name */
-   char* dbgfname = "../catkin_ws/src/rosbuzz/src/out.bdbg"; //argv[2];
+   char* dbgfname = (char*)"../catkin_ws/src/rosbuzz/src/out.bdbg"; //argv[2];
    /* Set CTRL-C handler */
    signal(SIGTERM, ctrlc_handler);
    signal(SIGINT, ctrlc_handler);
@@ -239,8 +236,9 @@ int main(int argc, char **argv)
    if(buzz_script_set(bcfname, dbgfname)) {
    fprintf(stdout, "Bytecode file found and set\n");
 
-  
+  /*Commands for dji flight controller*/
   mavros_msgs::CommandInt cmd_srv;
+
   int count = 0;
   while (ros::ok() && !done && !buzz_script_done())
   {
@@ -262,19 +260,27 @@ int main(int argc, char **argv)
     /* diji client call*/
     if (mav_client.call(cmd_srv)){ ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); }
     else{ ROS_ERROR("Failed to call service 'djicmd'"); }
-    /*Prepare Pay load to be sent*/  
+    /*obtain Pay load to be sent*/  
     uint64_t* payload_out_ptr= out_msg_process();
     uint64_t  position[3];
+    /*Appened current position to message*/
     memcpy(position, cur_pos, 3*sizeof(uint64_t));
     mavros_msgs::Mavlink payload_out;
     payload_out.payload64.push_back(position[0]);
     payload_out.payload64.push_back(position[1]);
     payload_out.payload64.push_back(position[2]);
-    
+    /*Append Buzz message*/
     uint16_t* out = u64_cvt_u16(payload_out_ptr[0]);  
     for(int i=0;i<out[0];i++){
     payload_out.payload64.push_back(payload_out_ptr[i]);
     }
+    int i=0;
+    for(std::vector<long unsigned int>::const_iterator it = payload_out.payload64.begin(); it != payload_out.payload64.end(); ++it)
+	{
+		//message_obt[i] =(uint64_t) *it;
+                //cout<<" [Debug:] sent message "<<*it<<endl;
+		i++;
+        }
     /*publish prepared messages in respective topic*/
     payload_pub.publish(payload_out);
     delete[] out;
@@ -286,13 +292,7 @@ int main(int argc, char **argv)
     ++count;
     timer_step+=1;
     maintain_pos(timer_step);
-   /* cout<<"time step"<<timer_step<<endl;
-    map< int, Pos_struct >::iterator it;
-    for (it=neighbours_pos_map.begin(); it!=neighbours_pos_map.end(); ++it){
-    std::cout << it->first << " => " << (it->second).x <<"   " << (it->second).y <<"   " << (it->second).z<<endl;
-	}
-   */
-
+   
   }
   /* Cleanup */
    buzz_script_destroy();
diff --git a/src/test.bzz b/src/test.bzz
index 62d128a..e481c0d 100644
--- a/src/test.bzz
+++ b/src/test.bzz
@@ -1,31 +1,36 @@
 # Executed once at init time.
 function init() {
 i=0
-v = stigmergy.create(1)
-#v.put("a", 6)
+s = swarm.create(10)
+s.join()
+
 }
  
 # Executed at each time step.
 function step() {
-s = swarm.create(10)
-s.join()
-
+# print all the neighbours position
 neighbors.foreach(
   function(rid, data) {
     print("robot ", rid, ": ",
         "distance  = ", data.distance, ", ",
         "azimuth   = ", data.azimuth, ", ",
         "elevation = ", data.elevation) })
-#if(i==1){
+if(i==1){
+s.exec(function() {uav_takeoff()})
+i=0
+}
+else{
+s.exec(function() {uav_goto(1.1234,2.2345,3.3456)})
+i=1
+}
+#if(i==5){
 #s.leave()
 #i=0
 #}
 #else{
 #i=1
 #}
-#s.select(uav_takeoff)
-#print("Distance to source:", mydist, neighbors.count())
-
+#i = i + 1
 
 }
  
diff --git a/src/uav_utility.cpp b/src/uav_utility.cpp
index 1dffbd9..b116e92 100644
--- a/src/uav_utility.cpp
+++ b/src/uav_utility.cpp
@@ -4,50 +4,20 @@
 /****************************************/
 /****************************************/
 
-//knet_dev_t* DSPIC;
-static const int ACC_INC       = 3;
-static const int ACC_DIV       = 0;
-static const int MIN_SPEED_ACC = 20;
-static const int MIN_SPEED_DEC = 1;
-static const int MAX_SPEED     = 400; /* mm/sec */
-
 /****************************************/
+/*To do !*/
 /****************************************/
 
 void uav_setup() {
-   /* Set the libkhepera debug level */
-   /*kb_set_debug_level(2);
-   // initiate libkhepera and robot access
-   if(kh4_init(0, NULL) !=0)
-   {
-      printf("\nERROR: could not initialize the khepera!\n\n");
-      return;
-   }
-   /* open robot socket and store the handle in their respective pointers 
-   DSPIC = knet_open("Khepera4:dsPic", KNET_BUS_I2C, 0, NULL);
-   /* Set speed profile 
-   kh4_SetSpeedProfile(ACC_INC,       /* Acceleration increment 
-                       ACC_DIV,       /* Acceleration divider
-                       MIN_SPEED_ACC, /* Minimum speed acc
-                       MIN_SPEED_DEC, /* Minimum speed dec 
-                       MAX_SPEED,     /* Maximum speed 
-                       DSPIC
-      );
-   kh4_SetMode(kh4RegSpeedProfile, DSPIC);
-   /* Mute ultrasonic sensor 
-   kh4_activate_us(0, DSPIC);*/
+  
 }
 
 /****************************************/
+/*To do !*/
 /****************************************/
 
 void uav_done() {
-  /* /* Stop wheels 
-   kh4_set_speed(0, 0, DSPIC);
-   /* Set motors to idle 
-   kh4_SetMode(kh4RegIdle, DSPIC);
-   /* Clear rgb leds because they consume energy 
-   kh4_SetRGBLeds(0, 0, 0, 0, 0, 0, 0, 0, 0, DSPIC); */
+ 
    fprintf(stdout, "Robot stopped.\n");
 }
 
diff --git a/src/uav_utility.h b/src/uav_utility.h
index b8aa940..5d37e73 100644
--- a/src/uav_utility.h
+++ b/src/uav_utility.h
@@ -1,12 +1,9 @@
 #ifndef UAV_UTILITY_H
 #define UAV_UTILITY_H
 
-//#include <khepera/khepera.h>
-
 extern void uav_setup();
 
 extern void uav_done();
 
-//extern knet_dev_t* DSPIC;
 
 #endif