diff --git a/buzz_scripts/include/act/naviguation.bzz b/buzz_scripts/include/act/naviguation.bzz index 7cd6879..072d312 100644 --- a/buzz_scripts/include/act/naviguation.bzz +++ b/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) diff --git a/buzz_scripts/include/act/neighborcomm.bzz b/buzz_scripts/include/act/neighborcomm.bzz index 1761039..7ed6c07 100644 --- a/buzz_scripts/include/act/neighborcomm.bzz +++ b/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) - } - }) -} diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz index 01fda7f..7245ce0 100644 --- a/buzz_scripts/include/act/states.bzz +++ b/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) diff --git a/buzz_scripts/main.bzz b/buzz_scripts/main.bzz index 787ed84..d07fc97 100644 --- a/buzz_scripts/main.bzz +++ b/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: diff --git a/include/buzzuav_closures.h b/include/buzzuav_closures.h index 46ef816..f8ae4d5 100644 --- a/include/buzzuav_closures.h +++ b/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 */ diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp index 4611452..d9a80ca 100644 --- a/src/buzz_utility.cpp +++ b/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; } diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index b76aa21..a18a093 100644 --- a/src/buzzuav_closures.cpp +++ b/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.