set home only once

launch/rosbuzz-testing-sitl.launch

@@ -33,6 +33,8 @@
 		<param name="in_payload" value="/inMavlink"/>
 		<param name="out_payload" value="/outMavlink"/>
 		<param name="xbee_status_srv" value="/xbee_status"/>
+		<param name="xbee_plugged" value="false"/>
+		<param name="name" value="solos1"/>
 		<param name="stand_by" value="/home/ivan/catkin_ws/src/rosbuzz/src/stand_by.bo"/>
 	</node>
 	

src/roscontroller.cpp

@@ -74,6 +74,7 @@ namespace rosbzz_node{
                         //////////////////////////////////////////////////////
 			while (ros::ok() && !buzz_utility::buzz_script_done())
   			{
+				std::cout<<std::setprecision(17)<<"Home: " << home[0] << home[1] << std::endl;
       				/*Update neighbors position inside Buzz*/
      				buzz_utility::neighbour_pos_callback(neighbours_pos_map);
 				/*Neighbours of the robot published with id in respective topic*/
@@ -425,12 +426,19 @@ namespace rosbzz_node{
 				break;
 			case mavros_msgs::CommandCode::NAV_TAKEOFF:
 				if(!armstate){
+
 					SetMode("LOITER", 0);
 					armstate = 1;
 					Arm();
 					ros::Duration(0.5).sleep();
 					// Registering HOME POINT.
+					if(home[0] == 0){
+						//test #1: set home only once -- ok
 						home[0] = cur_pos[0]; home[1] = cur_pos[1]; home[2] = cur_pos[2];
+						//test #2: set home mavros -- nope
+						//SetMavHomePosition(cur_pos[0], cur_pos[1], cur_pos[2]);
+
+					}
 				}
 				if(current_mode != "GUIDED")
 					SetMode("GUIDED", 2000); // for real solo, just add 2000ms delay (it should always be in loiter after arm/disarm)
@@ -781,6 +789,7 @@ namespace rosbzz_node{
 		}
 	}
 
+
 	void roscontroller::SetStreamRate(int id, int rate, int on_off){
 		mavros_msgs::StreamRate message;
 		message.request.stream_id = id;

src/testflockfev.bzz

@@ -49,8 +49,12 @@ function hexagon() {
   	timeW =0
   	statef=land
   	} else {
+  		if(timeW >= (WAIT_TIMEOUT / 2)){
+  			uav_moveto(5.0,0.0)
+  		} else {
+  			uav_moveto(0.0,5.0)
+  		}
   		timeW = timeW+1
-  	uav_moveto(0.2,0.0)
   	}
 }
  
@@ -86,7 +90,7 @@ function barrier_ready() {
 #
 # Executes the barrier
 #
-WAIT_TIMEOUT = 200
+WAIT_TIMEOUT = 300
 timeW=0
 function barrier_wait(threshold, transf) {
   barrier.get(id)