77 lines
2.4 KiB
Lua
77 lines
2.4 KiB
Lua
--------------------------------------------------------------------
|
|
-- lib/navigation.lua : position GPS, estimation du cap, ralliement.
|
|
--
|
|
-- Le cap n'est pas mesurable directement (pas de boussole): il est
|
|
-- estime a partir du deplacement GPS quand la vitesse est
|
|
-- suffisante. Le ralliement avance donc "en aveugle" au depart puis
|
|
-- corrige le cap des que le drone bouge.
|
|
--------------------------------------------------------------------
|
|
local Navigation = {}
|
|
|
|
function Navigation.nouveau(conf, materiel, journal)
|
|
local n = {}
|
|
local posPrec, tPrec = nil, nil
|
|
local cap = nil -- radians, atan2(dx, dz) du deplacement
|
|
|
|
function n.position()
|
|
return materiel.lirePosition()
|
|
end
|
|
|
|
-- A appeler periodiquement: met a jour l'estimation du cap
|
|
function n.actualiser()
|
|
local x, y, z = materiel.lirePosition()
|
|
if not x then return nil end
|
|
local t = os.clock()
|
|
if posPrec and t > tPrec then
|
|
local dx, dz = x - posPrec.x, z - posPrec.z
|
|
local dist = math.sqrt(dx * dx + dz * dz)
|
|
local vitesse = dist / (t - tPrec)
|
|
if vitesse >= conf.NAV.VITESSE_GPS_MIN then
|
|
cap = math.atan2(dx, dz)
|
|
end
|
|
end
|
|
posPrec, tPrec = { x = x, y = y, z = z }, t
|
|
return x, y, z
|
|
end
|
|
|
|
-- Commandes propulseurs pour rallier (cibleX, cibleZ).
|
|
-- Retourne avance, virage, distance (ou nil si pas de GPS).
|
|
function n.rallier(cibleX, cibleZ)
|
|
local x, y, z = n.actualiser()
|
|
if not x then return nil end
|
|
|
|
local dx, dz = cibleX - x, cibleZ - z
|
|
local distance = math.sqrt(dx * dx + dz * dz)
|
|
if distance <= conf.NAV.SEUIL_ARRIVEE then
|
|
return 0, 0, distance
|
|
end
|
|
|
|
local capCible = math.atan2(dx, dz)
|
|
local virage = 0
|
|
if cap then
|
|
local ecart = capCible - cap
|
|
while ecart > math.pi do ecart = ecart - 2 * math.pi end
|
|
while ecart < -math.pi do ecart = ecart + 2 * math.pi end
|
|
-- proportionnel borne: plein virage au dela de 45 degres
|
|
virage = math.max(-1, math.min(1, ecart / (math.pi / 4)))
|
|
end
|
|
|
|
-- avance reduite tant que le cap est incertain ou tres desaxe
|
|
local avance = 0.8
|
|
if cap == nil then
|
|
avance = 0.4 -- demarrage: avancer pour estimer le cap
|
|
elseif math.abs(virage) > 0.7 then
|
|
avance = 0.2 -- virage serre: privilegier la rotation
|
|
end
|
|
|
|
return avance, virage, distance
|
|
end
|
|
|
|
function n.razCap()
|
|
cap, posPrec, tPrec = nil, nil, nil
|
|
end
|
|
|
|
return n
|
|
end
|
|
|
|
return Navigation
|