enhanced individual waypoint control stability

buzz_scripts/include/act/naviguation.bzz

@@ -10,7 +10,6 @@ function goto_gps(transf) {
  	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
-		if(transf!=nil)
 			transf()
 	} else {
 		m_navigation = LimitSpeed(m_navigation, 1.0)

buzz_scripts/include/act/neighborcomm.bzz

@@ -110,20 +110,3 @@ 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

@@ -95,25 +95,46 @@ function stop() {
 	}
 }
 
-# State function for individual waypoint control
+# State functions for individual waypoint control
 firsttimeinwp = 1
+wpreached = 1
 function indiWP() {
 	if(firsttimeinwp) {
-		nei_goal_listen()
+		v_wp = stigmergy.create(8)
 		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) {
+			wpreached = 0
 			storegoal(rc_goto.latitude, rc_goto.longitude, pose.position.altitude)
-		} else 
-			bd_goal()
+			return
+		} else {
+			v_wp.put(rc_goto.id,{.lat=rc_goto.latitude, .lon=rc_goto.longitude, .pro=0})
+			reset_rc()
+		}
 	}
 
-	goto_gps(nil)
+  wp = v_wp.get(id)
+	if(wp!=nil) {
+		#log(wp.pro,wpreached)
+		if(wp.pro == 0) {
+				wpreached = 0
+        storegoal(wp.lat, wp.lon, pose.position.altitude)
+				v_wp.put(id,{.lat=wp.lat, .lon=wp.lon, .pro=1})
+				return
+			}      
+	}
 
+	if(wpreached!=1)
+		goto_gps(indiWP_done)
+
+}
+
+function indiWP_done() {
+	BVMSTATE = "INDIWP"
+	wpreached = 1
 }
 
 # State function to take a picture from the camera server (developed by HS)

buzz_scripts/main.bzz

@@ -11,13 +11,13 @@ include "utils/takeoff_heights.bzz"
 
 #State launched after takeoff
 
-AUTO_LAUNCH_STATE = "BIDDING"
+AUTO_LAUNCH_STATE = "INDIWP"
 TARGET     = 9.0
 EPSILON    = 30.0
 ROOT_ID = 3
 graph_id = 3
 graph_loop = 0
-LAND_AFTER_BARRIER_EXPIRE = 0 # if set to be 1 , the robots will land after barrier expire; if set to be 0, the robots will carry on to AUTO_LAUNCH_STATE.
+LAND_AFTER_BARRIER_EXPIRE = 1 # if set to be 1 , the robots will land after barrier expire; if set to be 0, the robots will carry on to AUTO_LAUNCH_STATE.
 
 #####
 # Vehicule type:

include/buzzuav_closures.h

@@ -46,6 +46,10 @@ int buzz_exportmap(buzzvm_t vm);
  * closure to take a picture
  */
 int buzzuav_takepicture(buzzvm_t vm);
+/*
+ * closure to reset RC input
+ */
+int buzzuav_resetrc(buzzvm_t vm);
 /*
  * Returns the current command from local variable
  */

src/buzz_utility.cpp

@@ -237,6 +237,9 @@ static int buzz_register_hooks()
   buzzvm_pushs(VM, buzzvm_string_register(VM, "export_map", 1));
   buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzz_exportmap));
   buzzvm_gstore(VM);
+  buzzvm_pushs(VM, buzzvm_string_register(VM, "reset_rc", 1));
+  buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_resetrc));
+  buzzvm_gstore(VM);
 
   return VM->state;
 }

src/buzzuav_closures.cpp

@@ -333,6 +333,15 @@ mavros_msgs::Mavlink get_status()
   return payload_out;
 }
 
+int buzzuav_resetrc(buzzvm_t vm)
+/*
+/ Buzz closure to reset the RC input.
+/----------------------------------------*/
+{
+  rc_id = -1;
+  return buzzvm_ret0(vm);
+}
+
 int buzzuav_takepicture(buzzvm_t vm)
 /*
 / Buzz closure to take a picture here.