45 lines
2.0 KiB
Plaintext
45 lines
2.0 KiB
Plaintext
GOTO_MAXVEL = 1.5 # m/steps
|
|
GOTO_MAXDIST = 150 # m.
|
|
GOTODIST_TOL = 0.4 # m.
|
|
GOTOANG_TOL = 0.1 # rad.
|
|
GPSlimit = {.1={.lat=45.510386, .lng=-73.610400},
|
|
.2={.lat=45.509839, .lng=-73.610047},
|
|
.3={.lat=45.510859, .lng=-73.608714},
|
|
.4={.lat=45.510327, .lng=-73.608393}}
|
|
|
|
# Core naviguation function to travel to a GPS target location.
|
|
function goto_gps(transf) {
|
|
if(Geofence()) {
|
|
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)
|
|
#m_navigation = LCA(m_navigation)
|
|
goto_abs(m_navigation.x, m_navigation.y, cur_goal.altitude - pose.position.altitude, 0.0)
|
|
}
|
|
} else
|
|
log("Geofencing prevents from going to that location!")
|
|
}
|
|
|
|
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
|
|
}
|
|
|
|
function Geofence(){ #TODO: rotate the fence box to really fit the coordinates
|
|
if(cur_goal.latitude > GPSlimit[1].lat and cur_goal.latitude > GPSlimit[2].lat and cur_goal.latitude > GPSlimit[3].lat and cur_goal.latitude > GPSlimit[4].lat)
|
|
return 0;
|
|
if(cur_goal.latitude < GPSlimit[1].lat and cur_goal.latitude < GPSlimit[2].lat and cur_goal.latitude < GPSlimit[3].lat and cur_goal.latitude < GPSlimit[4].lat)
|
|
return 0;
|
|
if(cur_goal.longitude > GPSlimit[1].lng and cur_goal.longitude > GPSlimit[2].lng and cur_goal.longitude > GPSlimit[3].lng and cur_goal.longitude > GPSlimit[4].lng)
|
|
return 0;
|
|
if(cur_goal.longitude < GPSlimit[1].lng and cur_goal.longitude < GPSlimit[2].lng and cur_goal.longitude < GPSlimit[3].lng and cur_goal.longitude < GPSlimit[4].lng)
|
|
return 0;
|
|
|
|
return 1
|
|
} |