diff --git a/include/roscontroller.h b/include/roscontroller.h
index 0568521..dc9d6aa 100644
--- a/include/roscontroller.h
+++ b/include/roscontroller.h
@@ -66,6 +66,9 @@ private:
 	/*Commands for flight controller*/
   	mavros_msgs::CommandInt cmd_srv;	
   	
+  	std::vector<std::string> m_sMySubscriptions;
+  	std::map<std::string, std::string> m_smTopic_infos;
+
 
 	void Initialize_pub_sub(ros::NodeHandle n_c);
 
@@ -110,8 +113,11 @@ private:
 	/*battery status callback*/ 
 	void battery(const mavros_msgs::BatteryStatus::ConstPtr& msg);
 	
+	/*flight extended status callback*/
+	void flight_extended_status_update(const mavros_msgs::ExtendedState::ConstPtr& msg);
+
 	/*flight status callback*/
-	void flight_status_update(const mavros_msgs::ExtendedState::ConstPtr& msg);
+	void flight_status_update(const mavros_msgs::State::ConstPtr& msg);
 	
 	/*current position callback*/
 	void current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg);
@@ -125,6 +131,9 @@ private:
 
 	void obstacle_dist(const sensor_msgs::LaserScan::ConstPtr& msg);
 
+	void GetSubscriptionParameters(ros::NodeHandle node_handle);
+
+	void Subscribe(ros::NodeHandle n_c);
 };
 
 }
diff --git a/launch/rosbuzz.launch b/launch/rosbuzz.launch
index 31df5c1..2bb907a 100644
--- a/launch/rosbuzz.launch
+++ b/launch/rosbuzz.launch
@@ -1,19 +1,19 @@
 <!-- Launch file for ROSBuzz -->
+
 <launch>
-	<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" >
-		<rosparam file="$(env ROS_WS)/src/ROSBuzz/launch/robot_config/$(env ROBOT).yaml"/>
-		<param name="program_mode" value="production "/>
-		<!-- Move this to yaml -->
-		<param name="bzzfile_name" value="/home/ivan/catkin_ws/src/ROSBuzz/src/test1.bzz" />
-	    <param name="rcclient" value="true" /> 
+		<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" >	
+		<rosparam file="$(env ROS_WS)/src/ROSBuzz/launch/launch_config/$(env ROBOT).yaml"/>
+		<param name="bzzfile_name" value="$(env ROS_WS)/src/ROSBuzz/src/test1.bzz" />
+        <param name="rcclient" value="true" /> 
 		<param name="rcservice_name" value="/buzzcmd" />
-		<!-- Move this to yaml -->
-		
-		<param name="fcclient_name" value="/mavros/cmd/command_int" />
+		<!-- <param name="fcclient_name" value="/mavros/cmd/command_int" /> -->
+		<param name="fcclient_name" value="/dji_mavcmd" />
 		
 		<param name="in_payload" value="/inMavlink"/>
 		<param name="out_payload" value="/outMavlink"/>
 		<param name="robot_id" value="3"/>
-		<param name="No_of_Robots" value="2"/>
-	</node>
+		<param name="No_of_Robots" value="3"/>
+		<param name="stand_by" value="$(env ROS_WS)/src/ROSBuzz/src/stand_by.bo"/>
+		</node>
+		
 </launch>
diff --git a/launch/rosbuzzm100.launch b/launch/rosbuzzm100.launch
index 74de01b..e30a10b 100644
--- a/launch/rosbuzzm100.launch
+++ b/launch/rosbuzzm100.launch
@@ -2,7 +2,7 @@
 
 <launch>
 		<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" >	
-		<param name="bzzfile_name" value="../ROS_WS/src/ROSBuzz/src/test1.bzz" />
+		<param name="bzzfile_name" value="$(env ROS_WS)/src/ROSBuzz/src/test1.bzz" />
                 <param name="rcclient" value="true" /> 
 		<param name="rcservice_name" value="/buzzcmd" />
 		<param name="fcclient_name" value="/dji_mavcmd" />
@@ -10,7 +10,7 @@
 		<param name="out_payload" value="/outMavlink"/>
 		<param name="robot_id" value="3"/>
 		<param name="No_of_Robots" value="3"/>
-		<param name="stand_by" value="../ROS_WS/src/ROSBuzz/src/stand_by.bo"/>
+		<param name="stand_by" value="$(env ROS_WS)/src/ROSBuzz/src/stand_by.bo"/>
 		</node>
 		
 		
diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp
index aaab1c6..12656f4 100644
--- a/src/roscontroller.cpp
+++ b/src/roscontroller.cpp
@@ -139,16 +139,42 @@ namespace rosbzz_node{
 		/*Obtain standby script to run during update*/
 		n_c.getParam("stand_by", stand_by);
 		
+		GetSubscriptionParameters(n_c);
+		// initialize topics to null?
 
 	}
 
+	void roscontroller::GetSubscriptionParameters(ros::NodeHandle node_handle){
+		//todo: make it as an array in yaml?
+		m_sMySubscriptions.clear();
+		std::string gps_topic, gps_type;
+		node_handle.getParam("topics/gps", gps_topic);
+		node_handle.getParam("type/gps", gps_type);
+		m_smTopic_infos.insert(pair <std::string, std::string>(gps_topic, gps_type));
+
+		std::string battery_topic, battery_type;
+		node_handle.getParam("topics/battery", battery_topic);
+		node_handle.getParam("type/battery", battery_type);
+		m_smTopic_infos.insert(pair <std::string, std::string>(battery_topic, battery_type));
+
+
+		std::string status_topic, status_type;
+		node_handle.getParam("topics/status", status_topic);
+		node_handle.getParam("type/status", status_type);
+		m_smTopic_infos.insert(pair <std::string, std::string>(status_topic, status_type));
+	}
+
 	void roscontroller::Initialize_pub_sub(ros::NodeHandle n_c){
 		/*subscribers*/
-  		current_position_sub = n_c.subscribe("/global_position", 1000, &roscontroller::current_pos,this);
-  		battery_sub = n_c.subscribe("/power_status", 1000, &roscontroller::battery,this);
+
+		Subscribe(n_c);
+
+  		//current_position_sub = n_c.subscribe("/global_position", 1000, &roscontroller::current_pos,this);
+  		//battery_sub = n_c.subscribe("/power_status", 1000, &roscontroller::battery,this);
   		payload_sub = n_c.subscribe(in_payload, 1000, &roscontroller::payload_obt,this);
-		flight_status_sub =n_c.subscribe("/flight_status",100, &roscontroller::flight_status_update,this);
-		obstacle_sub= n_c.subscribe("/guidance/obstacle_distance",100, &roscontroller::obstacle_dist,this);
+		//flight_status_sub =n_c.subscribe("/flight_status",100, &roscontroller::flight_extended_status_update,this);
+
+  		obstacle_sub= n_c.subscribe("/guidance/obstacle_distance",100, &roscontroller::obstacle_dist,this);
   		/*publishers*/
 		payload_pub = n_c.advertise<mavros_msgs::Mavlink>(out_payload, 1000);
 		neigh_pos_pub = n_c.advertise<rosbuzz::neigh_pos>("/neighbours_pos",1000);	
@@ -157,6 +183,26 @@ namespace rosbzz_node{
 		multi_msg=true;
 	}
 	
+	void roscontroller::Subscribe(ros::NodeHandle n_c){
+
+  		for(std::map<std::string, std::string>::iterator it = m_smTopic_infos.begin(); it != m_smTopic_infos.end(); ++it){
+  			if(it->second == "mavros_msgs/ExtendedState"){
+  				flight_status_sub = n_c.subscribe(it->first, 100, &roscontroller::flight_extended_status_update, this);
+  			}
+  			else if(it->second == "mavros_msgs/State"){
+  				flight_status_sub = n_c.subscribe(it->first, 100, &roscontroller::flight_status_update, this);
+  			}
+  			else if(it->second == "mavros_msgs/BatteryStatus"){
+  		  		battery_sub = n_c.subscribe(it->first, 1000, &roscontroller::battery, this);
+  			}
+  			else if(it->second == "sensor_msgs/NavSatFix"){
+  		  		current_position_sub = n_c.subscribe(it->first, 1000, &roscontroller::current_pos, this);
+ 			}
+
+	  		std::cout << "Subscribed to: " << it->first << endl;
+  		}
+	}
+
 	void roscontroller::Compile_bzz(){
 		/*Compile the buzz code .bzz to .bo*/
 		stringstream bzzfile_in_compile;
@@ -485,8 +531,21 @@ namespace rosbzz_node{
 		buzzuav_closures::set_battery(msg->voltage,msg->current,msg->remaining);
 		//ROS_INFO("voltage : %d  current : %d  remaining : %d",msg->voltage, msg->current, msg ->remaining);
 	}
-	/*flight status update*/
-	void roscontroller::flight_status_update(const mavros_msgs::ExtendedState::ConstPtr& msg){
+
+	/*flight extended status update*/
+	void roscontroller::flight_status_update(const mavros_msgs::State::ConstPtr& msg){
+		// http://wiki.ros.org/mavros/CustomModes
+		std::cout << "Message: " << msg->mode << std::endl;
+		if(msg->mode == "GUIDED")
+			buzzuav_closures::flight_status_update(1);
+		else if (msg->mode == "LAND")
+			buzzuav_closures::flight_status_update(4);
+		else // ground standby = LOITER?
+			buzzuav_closures::flight_status_update(5);//?
+	}
+
+	/*flight extended status update*/
+	void roscontroller::flight_extended_status_update(const mavros_msgs::ExtendedState::ConstPtr& msg){
 		buzzuav_closures::flight_status_update(msg->landed_state);
 	}
 	/*current position callback*/
diff --git a/src/test1.bzz b/src/test1.bzz
index caa76ba..719e929 100644
--- a/src/test1.bzz
+++ b/src/test1.bzz
@@ -1,7 +1,7 @@
 
 # We need this for 2D vectors
 # Make sure you pass the correct include path to "bzzc -I <path1:path2> ..."
-include "/home/ubuntu/buzz/src/include/vec2.bzz"
+include "/home/ivan/dev/buzz/src/include/vec2.bzz"
 ####################################################################################################
 # Updater related
 # This should be here for the updater to work, changing position of code will crash the updater