From 277842c3effb03f19e2eb02f1e322c9b87500cd4 Mon Sep 17 00:00:00 2001 From: dave Date: Mon, 12 Nov 2018 18:04:56 -0500 Subject: [PATCH] fix gohome manual --- buzz_scripts/include/act/naviguation.bzz | 4 ++-- buzz_scripts/include/act/states.bzz | 16 +++++++++------- 2 files changed, 11 insertions(+), 9 deletions(-) diff --git a/buzz_scripts/include/act/naviguation.bzz b/buzz_scripts/include/act/naviguation.bzz index 5aec163..1425145 100644 --- a/buzz_scripts/include/act/naviguation.bzz +++ b/buzz_scripts/include/act/naviguation.bzz @@ -2,7 +2,7 @@ GOTO_MAXVEL = 1.5 # m/steps GOTO_MAXDIST = 150 # m. GOTODIST_TOL = 0.4 # m. GOTOANG_TOL = 0.1 # rad. -GPSlimit = {.1={.lat=45.510523, .lng=-73.611118},#{.lat=45.510400, .lng=-73.610421}, +GPSlimit = {.1={.lat=45.510400, .lng=-73.610421}, .2={.lat=45.510896, .lng=-73.608731}, .3={.lat=45.510355, .lng=-73.608404}, .4={.lat=45.509840, .lng=-73.610072}} @@ -18,7 +18,7 @@ function goto_gps(transf) { } else { m_navigation = LimitSpeed(m_navigation, 1.0) gf = {.0=m_navigation, .1=vec_from_gps(GPSlimit[1].lat, GPSlimit[1].lng, 0), .2=vec_from_gps(GPSlimit[2].lat, GPSlimit[2].lng, 0), .3=vec_from_gps(GPSlimit[3].lat, GPSlimit[3].lng, 0), .4=vec_from_gps(GPSlimit[4].lat, GPSlimit[4].lng, 0)} - #geofence(gf) + geofence(gf) #m_navigation = LCA(m_navigation) goto_abs(m_navigation.x, m_navigation.y, cur_goal.altitude - pose.position.altitude, 0.0) } diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz index a0b1903..e9b9988 100644 --- a/buzz_scripts/include/act/states.bzz +++ b/buzz_scripts/include/act/states.bzz @@ -74,13 +74,15 @@ gohomeT=100 function goinghome() { BVMSTATE = "GOHOME" if(V_TYPE == 0 or V_TYPE == 1) { # flying vehicle so TAKE_OFF - storegoal(homegps.lat, homegps.lng, pose.position.altitude) - goto_gps(AUTO_LAUNCH_STATE) - #if(gohomeT > 0) { # TODO: Make a real check if home is reached - # gohome() - # gohomeT = gohomeT - 1 - #} else - # BVMSTATE = AUTO_LAUNCH_STATE + m_navigation = vec_from_gps(homegps.latitude, homegps.longitude, 0) + #print(" has to move ", math.vec2.length(m_navigation), math.vec2.angle(m_navigation)) + if(math.vec2.length(m_navigation) < GOTODIST_TOL){ # reached destination + BVMSTATE = AUTO_LAUNCH_STATE + } else { + m_navigation = LimitSpeed(m_navigation, 3.0) + #m_navigation = LCA(m_navigation) + goto_abs(m_navigation.x, m_navigation.y, cur_goal.altitude - pose.position.altitude, 0.0) + } } }