135 lines
4.3 KiB
Lua
135 lines
4.3 KiB
Lua
--------------------------------------------------------------------
|
|
-- lib/pilotage.lua : regulation et mixage.
|
|
--
|
|
-- base = PID d'altitude (double regime montee / descente)
|
|
-- assiette = PID tangage + PID roulis vers des angles cibles
|
|
-- (0 sauf en mode drone ou le joystick les commande)
|
|
-- helice(coin) = base + signeBF*corrTangage + signeLR*corrRoulis
|
|
-- propulseurs = mixage differentiel avance / virage
|
|
--
|
|
-- La consigne d'altitude passe par une rampe (VITESSE_RAMPE) pour
|
|
-- garantir des transitions douces et sans depassement.
|
|
--------------------------------------------------------------------
|
|
local Pilotage = {}
|
|
|
|
local SIGNES = {
|
|
lb = { bf = -1, lr = 1 },
|
|
rb = { bf = -1, lr = -1 },
|
|
lf = { bf = 1, lr = 1 },
|
|
rf = { bf = 1, lr = -1 },
|
|
}
|
|
|
|
-- materiel.lireAssiette() est NORMALISE par la calibration gimbal:
|
|
-- tangage negatif = nez bas, roulis negatif = penche a gauche.
|
|
-- Une helice AVANT pousse plus quand le tangage est negatif, une
|
|
-- helice GAUCHE pousse plus quand le roulis est negatif => signes -1
|
|
local SIGNE_TANGAGE, SIGNE_ROULIS = -1, -1
|
|
|
|
function Pilotage.nouveau(conf, etat, materiel, Pid, journal)
|
|
local p = {}
|
|
local VMAX = conf.VITESSE_RSC_MAX
|
|
local gains = etat.pid or conf.PID
|
|
|
|
local pidMontee = Pid.nouveau(gains.montee, 0, VMAX)
|
|
local pidDescente = Pid.nouveau(gains.descente, 0, VMAX)
|
|
local pidTangage = Pid.nouveau(gains.tangage, -VMAX / 2, VMAX / 2)
|
|
local pidRoulis = Pid.nouveau(gains.roulis, -VMAX / 2, VMAX / 2)
|
|
|
|
local consigneRampe = nil
|
|
local regimePrec = nil
|
|
local facteurProp = 1.0 -- reduit en cas de surcharge du moteur
|
|
|
|
function p.gains()
|
|
return gains
|
|
end
|
|
|
|
-- 0..1 : applique aux propulseurs (priorite de delestage en
|
|
-- cas de surcharge du reseau de stress)
|
|
function p.reglerFacteurProp(f)
|
|
facteurProp = math.max(0, math.min(1, f))
|
|
end
|
|
|
|
function p.reglerGains(nouveaux)
|
|
gains = nouveaux
|
|
etat.pid = nouveaux
|
|
pidMontee:reglerGains(gains.montee)
|
|
pidDescente:reglerGains(gains.descente)
|
|
pidTangage:reglerGains(gains.tangage)
|
|
pidRoulis:reglerGains(gains.roulis)
|
|
end
|
|
|
|
function p.raz()
|
|
pidMontee:raz()
|
|
pidDescente:raz()
|
|
pidTangage:raz()
|
|
pidRoulis:raz()
|
|
consigneRampe = nil
|
|
regimePrec = nil
|
|
end
|
|
|
|
-- Un pas de regulation en vol.
|
|
-- consigneY : altitude cible
|
|
-- angles : { tangage = deg, roulis = deg } cibles
|
|
-- avance, virage : commande propulseurs dans -1..1
|
|
function p.reguler(consigneY, angles, avance, virage, dt)
|
|
local altitude = materiel.lireAltitude()
|
|
local tangage, roulis = materiel.lireAssiette()
|
|
|
|
-- rampe de consigne
|
|
if consigneRampe == nil then consigneRampe = altitude end
|
|
local pas = conf.VITESSE_RAMPE * dt
|
|
local ecart = consigneY - consigneRampe
|
|
if ecart > pas then ecart = pas elseif ecart < -pas then ecart = -pas end
|
|
consigneRampe = consigneRampe + ecart
|
|
|
|
-- double regime : montee ou descente
|
|
local montee = (consigneRampe - altitude) >= 0
|
|
local pid = montee and pidMontee or pidDescente
|
|
if regimePrec ~= nil and regimePrec ~= montee then
|
|
pid:raz()
|
|
end
|
|
regimePrec = montee
|
|
local base = pid:calculer(consigneRampe, altitude, dt)
|
|
|
|
-- assiette
|
|
local corrTangage = SIGNE_TANGAGE
|
|
* pidTangage:calculer(angles.tangage or 0, tangage, dt)
|
|
local corrRoulis = SIGNE_ROULIS
|
|
* pidRoulis:calculer(angles.roulis or 0, roulis, dt)
|
|
|
|
for role, s in pairs(SIGNES) do
|
|
local v = base + s.bf * corrTangage + s.lr * corrRoulis
|
|
if v < 0 then v = 0 elseif v > VMAX then v = VMAX end
|
|
materiel.reglerRsc(role, v)
|
|
end
|
|
|
|
-- propulseurs (differentiel)
|
|
local function borner(v)
|
|
if v > 1 then return 1 elseif v < -1 then return -1 end
|
|
return v
|
|
end
|
|
materiel.reglerRsc("prop_r", borner(avance + virage) * VMAX * facteurProp)
|
|
materiel.reglerRsc("prop_l", borner(avance - virage) * VMAX * facteurProp)
|
|
|
|
return altitude, consigneRampe
|
|
end
|
|
|
|
-- Stationnement: helices a pleine puissance inversee (plaquage),
|
|
-- propulseurs a 0
|
|
function p.plaquer()
|
|
for role in pairs(SIGNES) do
|
|
materiel.reglerRsc(role, -VMAX)
|
|
end
|
|
materiel.reglerRsc("prop_l", 0)
|
|
materiel.reglerRsc("prop_r", 0)
|
|
end
|
|
|
|
function p.arreter()
|
|
materiel.toutArreter()
|
|
p.raz()
|
|
end
|
|
|
|
return p
|
|
end
|
|
|
|
return Pilotage
|