individual waypoint ctl, map-based waypoint in webctl in formation

buzz_scripts/include/act/naviguation.bzz

@@ -1,15 +1,21 @@
+GOTO_MAXVEL = 1.5 # m/steps
+GOTO_MAXDIST = 150 # m.
+GOTODIST_TOL = 0.4  # m.
+GOTOANG_TOL = 0.1 # rad.
+
 # Core naviguation function to travel to a GPS target location.
 function goto_gps(transf) {
-	m_navigation = vec_from_gps(rc_goto.latitude, rc_goto.longitude, 0)
+	m_navigation = vec_from_gps(cur_goal.latitude, cur_goal.longitude, 0)
 	#print(" has to move ", math.vec2.length(m_navigation), math.vec2.angle(m_navigation))
  	if(math.vec2.length(m_navigation)>GOTO_MAXDIST)
 		log("Sorry this is too far (", math.vec2.length(m_navigation), " / ", GOTO_MAXDIST, " )")
-	else if(math.vec2.length(m_navigation) < GOTODIST_TOL and math.vec2.angle(m_navigation) < GOTOANG_TOL) # reached destination
+	else if(math.vec2.length(m_navigation) < GOTODIST_TOL and math.vec2.angle(m_navigation) < GOTOANG_TOL){ # reached destination
+		if(transf!=nil)
 			transf()
-	else {
+	} else {
 		m_navigation = LimitSpeed(m_navigation, 1.0)
 		#m_navigation = LCA(m_navigation)
-		goto_abs(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude, 0.0)
+		goto_abs(m_navigation.x, m_navigation.y, cur_goal.altitude - pose.position.altitude, 0.0)
 	}
 }
 

buzz_scripts/include/act/neighborcomm.bzz

@@ -18,9 +18,9 @@ function rc_cmd_listen() {
 		barrier_set(ROBOTS, "GOHOME", BVMSTATE, 20)
 		barrier_ready(20)
 		neighbors.broadcast("cmd", 20)
-	} else if(flight.rc_cmd==16) {
+#	} else if(flight.rc_cmd==16) {
-		flight.rc_cmd=0
+#		flight.rc_cmd=0
-		BVMSTATE = "PATHPLAN"
+#		BVMSTATE = "PATHPLAN"
 	} else if(flight.rc_cmd==400) {
 		flight.rc_cmd=0
 		arm()
@@ -110,3 +110,20 @@ function nei_cmd_listen() {
 				#}
     })
 }
+
+# broadcast GPS goals
+function bd_goal() {
+    neighbors.broadcast("goal", {.id=rc_goto.id, .la=rc_goto.latitude, .lo=rc_goto.longitude, .al=rc_goto.altitude})
+}
+
+# listens to neighbors broadcasting gps goals
+function nei_goal_listen() {
+    neighbors.listen("goal",
+			function(vid, value, rid) {
+                print("Got (", vid, ",", value.id, value.la, value.lo, ") #", rid)
+				if(value.id==id) {
+                    print("Got (", vid, ",", value, ") #", rid)
+                    storegoal(value.la, value.lo, pose.position.altitude)
+                }
+    })
+}

buzz_scripts/include/act/states.bzz

@@ -14,10 +14,6 @@ TARGET_ALTITUDE = 15.0 # m.
 BVMSTATE = "TURNEDOFF"
 PICTURE_WAIT = 20 # steps
 
-GOTO_MAXVEL = 1.5 # m/steps
-GOTO_MAXDIST = 150 # m.
-GOTODIST_TOL = 0.4  # m.
-GOTOANG_TOL = 0.1 # rad.
 path_it = 0
 pic_time = 0
 g_it = 0
@@ -100,8 +96,23 @@ function stop() {
 }
 
 # State function for individual waypoint control
+firsttimeinwp = 1
 function indiWP() {
+	if(firsttimeinwp) {
+		nei_goal_listen()
+		storegoal(pose.position.latitude, pose.position.longitude, pose.position.altitude)
+		firsttimeinwp = 0
+	}
 	BVMSTATE = "INDIWP"
+	if(rc_goto.id != -1) {
+		log(rc_goto.id)
+		if(rc_goto.id == id) {
+			storegoal(rc_goto.latitude, rc_goto.longitude, pose.position.altitude)
+		} else 
+			bd_goal()
+	}
+
+	goto_gps(nil)
 
 }
 

buzz_scripts/include/utils/takeoff_heights.bzz

@@ -2,10 +2,11 @@ takeoff_heights ={
     .1 = 21.0,
     .2 = 18.0,
     .3 = 12.0,
-    .5 = 24.0,
+    .5 = 12.0,
     .6 = 3.0,
     .9 = 15.0,
     .11 = 6.0,
+    .14 = 18.0,
     .16 = 9.0,
     .17 = 3.0,
     .18 = 6.0,

include/buzzuav_closures.h

@@ -50,6 +50,10 @@ int buzzuav_takepicture(buzzvm_t vm);
  * Returns the current command from local variable
  */
 int getcmd();
+/*
+ * update GPS goal value
+ */
+void set_gpsgoal(double goal[3]);
 /*
  * Sets goto position from rc client
  */

src/buzzuav_closures.cpp

@@ -14,23 +14,29 @@ namespace buzzuav_closures
 // TODO: Minimize the required global variables and put them in the header
 // static const rosbzz_node::roscontroller* roscontroller_ptr;
 static double goto_pos[4];
-static double rc_goto_pos[3];
+static double goto_gpsgoal[3];
-static float rc_gimbal[4];
-static float batt[3];
-static float obst[5] = { 0, 0, 0, 0, 0 };
 static double cur_pos[4];
 static double cur_NEDpos[2];
-static uint8_t status;
+
-static int cur_cmd = 0;
-static int rc_cmd = 0;
 static int rc_id = -1;
+static int rc_cmd = 0;
+static double rc_gpsgoal[3];
+static float rc_gimbal[4];
+
+static float batt[3];
+static float obst[5] = { 0, 0, 0, 0, 0 };
+static uint8_t status;
+
+static int cur_cmd = 0;
 static int buzz_cmd = 0;
 static float height = 0;
+
 static bool deque_full = false;
 static int rssi = 0;
 static float raw_packet_loss = 0.0;
 static int filtered_packet_loss = 0;
 static float api_rssi = 0.0;
+
 string WPlistname = "";
 
 std::map<int, buzz_utility::RB_struct> targets_map;
@@ -231,7 +237,7 @@ int buzzuav_moveto(buzzvm_t vm)
   //  DEBUG
   // ROS_WARN("[%i] Buzz requested Move To: x: %.7f , y: %.7f, z: %.7f", (int)buzz_utility::get_robotid(), goto_pos[0],
   // goto_pos[1], goto_pos[2]);
-  buzz_cmd = NAV_SPLINE_WAYPOINT;  // TODO: standard mavros?
+  buzz_cmd = NAV_SPLINE_WAYPOINT;
   return buzzvm_ret0(vm);
 }
 
@@ -386,14 +392,24 @@ int buzzuav_storegoal(buzzvm_t vm)
     wplist_map.erase(wplist_map.begin()->first);
   }
 
-  double rb[3];
+  set_gpsgoal(goal);
+  // prevent an overwrite
+  rc_id = -1;
 
+  return buzzvm_ret0(vm);
+}
+
+void set_gpsgoal(double goal[3])
+/*
+/ update GPS goal value
+-----------------------------------*/
+{
+  double rb[3];
   rb_from_gps(goal, rb, cur_pos);
   if (fabs(rb[0]) < 150.0) {
-    rc_set_goto((int)buzz_utility::get_robotid(), goal[0], goal[1], goal[2]);
+    goto_gpsgoal[0] = goal[0];goto_gpsgoal[1] = goal[1];goto_gpsgoal[2] = goal[2];
-    ROS_INFO("Set RC_GOTO ---- %f %f %f (%f %f, %f %f)", goal[0], goal[1], goal[2], cur_pos[0], cur_pos[1], rb[0], rb[1]);
+    ROS_INFO("Set GPS GOAL TO ---- %f %f %f (%f %f, %f %f)", goal[0], goal[1], goal[2], cur_pos[0], cur_pos[1], rb[0], rb[1]);
   }
-  return buzzvm_ret0(vm);
 }
 
 int buzzuav_arm(buzzvm_t vm)
@@ -504,9 +520,9 @@ void rc_set_goto(int id, double latitude, double longitude, double altitude)
 -----------------------------------*/
 {
   rc_id = id;
-  rc_goto_pos[0] = latitude;
+  rc_gpsgoal[0] = latitude;
-  rc_goto_pos[1] = longitude;
+  rc_gpsgoal[1] = longitude;
-  rc_goto_pos[2] = altitude;
+  rc_gpsgoal[2] = altitude;
 }
 
 void rc_set_gimbal(int id, float yaw, float roll, float pitch, float t)
@@ -773,15 +789,30 @@ int buzzuav_update_flight_status(buzzvm_t vm)
   buzzvm_tput(vm);
   buzzvm_dup(vm);
   buzzvm_pushs(vm, buzzvm_string_register(vm, "latitude", 1));
-  buzzvm_pushf(vm, rc_goto_pos[0]);
+  buzzvm_pushf(vm, rc_gpsgoal[0]);
   buzzvm_tput(vm);
   buzzvm_dup(vm);
   buzzvm_pushs(vm, buzzvm_string_register(vm, "longitude", 1));
-  buzzvm_pushf(vm, rc_goto_pos[1]);
+  buzzvm_pushf(vm, rc_gpsgoal[1]);
   buzzvm_tput(vm);
   buzzvm_dup(vm);
   buzzvm_pushs(vm, buzzvm_string_register(vm, "altitude", 1));
-  buzzvm_pushf(vm, rc_goto_pos[2]);
+  buzzvm_pushf(vm, rc_gpsgoal[2]);
+  buzzvm_tput(vm);
+  buzzvm_gstore(vm);
+  buzzvm_pushs(vm, buzzvm_string_register(vm, "cur_goal", 1));
+  buzzvm_pusht(vm);
+  buzzvm_dup(vm);
+  buzzvm_pushs(vm, buzzvm_string_register(vm, "latitude", 1));
+  buzzvm_pushf(vm, goto_gpsgoal[0]);
+  buzzvm_tput(vm);
+  buzzvm_dup(vm);
+  buzzvm_pushs(vm, buzzvm_string_register(vm, "longitude", 1));
+  buzzvm_pushf(vm, goto_gpsgoal[1]);
+  buzzvm_tput(vm);
+  buzzvm_dup(vm);
+  buzzvm_pushs(vm, buzzvm_string_register(vm, "altitude", 1));
+  buzzvm_pushf(vm, goto_gpsgoal[2]);
   buzzvm_tput(vm);
   buzzvm_gstore(vm);
   return vm->state;

src/roscontroller.cpp

@@ -762,6 +762,9 @@ script
         Arm();
         // Registering HOME POINT.
         home = cur_pos;
+        // Initialize GPS goal for safety.
+        double gpspgoal[3] = {cur_pos.latitude,cur_pos.longitude,cur_pos.altitude};
+        buzzuav_closures::set_gpsgoal(gpspgoal);
 	      BClpose = true;
       }
       if (current_mode != "GUIDED" && setmode)
@@ -944,7 +947,7 @@ void roscontroller::gps_rb(POSE nei_pos, double out[])
 
 void roscontroller::gps_ned_cur(float& ned_x, float& ned_y, POSE t)
 /*
-/ Get GPS from NED and a reference GPS point (struct input)
+/ Get NED coordinates from a reference GPS point (struct input)
 ----------------------------------------------------------- */
 {
   gps_convert_ned(ned_x, ned_y, t.longitude, t.latitude, cur_pos.longitude, cur_pos.latitude);
@@ -953,7 +956,7 @@ void roscontroller::gps_ned_cur(float& ned_x, float& ned_y, POSE t)
 void roscontroller::gps_convert_ned(float& ned_x, float& ned_y, double gps_t_lon, double gps_t_lat, double gps_r_lon,
                                     double gps_r_lat)
 /*
-/ Get GPS from NED and a reference GPS point
+/ Get NED coordinates from a reference GPS point and current GPS location
 ----------------------------------------------------------- */
 {
   double d_lon = gps_t_lon - gps_r_lon;