60 lines
2.7 KiB
Plaintext
60 lines
2.7 KiB
Plaintext
GOTO_MAXVEL = 12.5 # m/steps
|
|
GOTO_MAXDIST = 250 # m.
|
|
GOTODIST_TOL = 0.4 # m.
|
|
GOTOANG_TOL = 0.1 # rad.
|
|
# Geofence polygon
|
|
# CEPSUM field
|
|
#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}}
|
|
# Pangeae final site
|
|
#GPSlimit = {.1={.lat=29.067746, .lng=-13.663315},
|
|
# .2={.lat=29.068724, .lng=-13.662634},
|
|
# .3={.lat=29.068113, .lng=-13.661427},
|
|
# .4={.lat=29.067014, .lng=-13.661564}}
|
|
# Pangeae first site
|
|
GPSlimit = {.1={.lat=29.021055, .lng=-13.715155},
|
|
.2={.lat=29.021055, .lng=-13.714132},
|
|
.3={.lat=29.019470, .lng=-13.714132},
|
|
.4={.lat=29.019470, .lng=-13.715240}}
|
|
|
|
# Core naviguation function to travel to a GPS target location.
|
|
function goto_gps(transf) {
|
|
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
|
|
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)
|
|
#m_navigation = LCA(m_navigation)
|
|
goto_abs(m_navigation.x, m_navigation.y, cur_goal.altitude - pose.position.altitude, 0.0)
|
|
}
|
|
}
|
|
|
|
function LimitSpeed(vel_vec, factor){
|
|
if(math.vec2.length(vel_vec)>GOTO_MAXVEL*factor)
|
|
vel_vec = math.vec2.scale(vel_vec, GOTO_MAXVEL*factor/math.vec2.length(vel_vec))
|
|
return vel_vec
|
|
}
|
|
|
|
# Core naviguation function to travel to provided GPS target location.
|
|
function goto_gps_in(transf, gps_to_go) {
|
|
m_navigation = vec_from_gps(gps_to_go.latitude, gps_to_go.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
|
|
;
|
|
# transf()
|
|
} else {
|
|
#log("move to x", m_navigation.x," y ", m_navigation.y)
|
|
m_navigation = LimitSpeed(m_navigation, 1.0)
|
|
#log("after limit speed move to x", m_navigation.x," y ", m_navigation.y)
|
|
goto_abs(m_navigation.x, m_navigation.y, gps_to_go.altitude - pose.position.altitude, 0.0)
|
|
}
|
|
} |