Spiri
/
ROSBuzz_MISTLab
12
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

fix gohome manual

This commit is contained in:
dave 2018-11-12 18:04:56 -05:00
parent 33e19e16ae
commit 277842c3ef
2 changed files with 11 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
buzz_scripts/include/act/naviguation.bzz
View File

@ -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)
}

16
buzz_scripts/include/act/states.bzz
View File

@ -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)
}
}
}