384 lines
14 KiB
Lua
384 lines
14 KiB
Lua
--------------------------------------------------------------------
|
|
-- lib/regulation.lua : stabilisation et mixage.
|
|
--
|
|
-- STRUCTURE (concue contre les oscillations constatees en V1):
|
|
-- ALTITUDE en cascade: erreur -> vitesse verticale cible (P,
|
|
-- bornee) -> PID vitesse -> delta rpm autour du rpm de
|
|
-- sustentation MESURE (calib altitude). La vitesse verticale
|
|
-- vient du velocity sensor vertical auto-detecte a la calib
|
|
-- (repli: derivee filtree de l'altitude).
|
|
-- ASSIETTE: PID par axe a derivee filtree, autorite bornee a
|
|
-- ASSIETTE_PLAFOND * rpm de sustentation, zone morte reglable.
|
|
-- SUPERVISEUR D'OSCILLATION: si une boucle oscille (passages a
|
|
-- zero repetes au-dela d'un seuil d'amplitude), ses gains sont
|
|
-- reduits automatiquement de 20%% et l'evenement journalise:
|
|
-- le systeme converge vers la stabilite meme mal regle.
|
|
-- MOTEUR: mode on/off (pleine puissance) ou adaptatif (niveau
|
|
-- regle AVANT l'ecriture des RSC en montee, apres en descente).
|
|
--------------------------------------------------------------------
|
|
local Regulation = {}
|
|
|
|
-- coefficients de mixage par coin
|
|
local SIGNES = {
|
|
lb = { bf = -1, lr = 1 },
|
|
rb = { bf = -1, lr = -1 },
|
|
lf = { bf = 1, lr = 1 },
|
|
rf = { bf = 1, lr = -1 },
|
|
}
|
|
|
|
-- Algebre du signe (demontree en V1): nez bas => tangage mesure < 0
|
|
-- => erreur (0 - mesure) > 0 => sortie PID > 0, a APPLIQUER en plus
|
|
-- a l'AVANT (bf = +1) pour relever le nez. Signes +1. Un -1 ici
|
|
-- AMPLIFIE l'inclinaison au lieu de la corriger.
|
|
local SIGNE_TANGAGE, SIGNE_ROULIS = 1, 1
|
|
|
|
function Regulation.nouveau(conf, etat, materiel, Pid, journal)
|
|
local r = {}
|
|
local VMAX = conf.VMAX
|
|
|
|
-- borne HELICE courante: 256 RSC * rapport de boite
|
|
local function vmaxHelice()
|
|
return VMAX * materiel.rapportBoite()
|
|
end
|
|
|
|
local function borner(v, mini, maxi)
|
|
if v < mini then return mini elseif v > maxi then return maxi end
|
|
return v
|
|
end
|
|
|
|
-- gains: conf.PID est la table VIVE (surcharges appliquees au
|
|
-- demarrage, page PARAM et calibrations la modifient en place)
|
|
local pidVitesse = Pid.nouveau(conf.PID.vitesse, -VMAX, VMAX, 30)
|
|
-- assiette: derivee filtree FORT (gimbal bruite + pas de 0.1 s)
|
|
local pidTangage = Pid.nouveau(conf.PID.tangage, -VMAX, VMAX, 15,
|
|
0.3)
|
|
local pidRoulis = Pid.nouveau(conf.PID.roulis, -VMAX, VMAX, 15,
|
|
0.3)
|
|
|
|
local consigneRampe = nil
|
|
local ffAdapt = 0 -- adaptation lente (charge variable)
|
|
local altPrec, vFiltre = nil, 0
|
|
local vCapteurF = nil -- vitesse capteur filtree
|
|
local basePrec = nil -- limiteur de pente de la base
|
|
local injection = {} -- relais de calibration par axe
|
|
local gainsModifies = false -- superviseur: a persister
|
|
|
|
------------------------------------------------------------------
|
|
-- SUSTENTATION: rpm mesure par la calib altitude, corrige par la
|
|
-- pression (la poussee diminue avec l'altitude)
|
|
------------------------------------------------------------------
|
|
-- rpm HELICE de sustentation (etat.rpmHover est en unites
|
|
-- helice: invariant au rapport de boite)
|
|
function r.rpmHover()
|
|
local base = etat.rpmHover or VMAX * 0.3
|
|
if etat.pressionHover then
|
|
local facteur = etat.pressionHover
|
|
/ math.max(materiel.lirePression(), 0.05)
|
|
base = base * facteur
|
|
end
|
|
return borner(base + ffAdapt, 0, vmaxHelice())
|
|
end
|
|
|
|
------------------------------------------------------------------
|
|
-- VITESSE VERTICALE: capteur auto-detecte, repli derivee filtree
|
|
------------------------------------------------------------------
|
|
-- vitesse verticale ASSAINIE: filtrage + rejet des valeurs
|
|
-- aberrantes (le capteur lit dans le repere du vaisseau et se
|
|
-- pollue quand l'assiette bouge)
|
|
function r.vitesseVerticale(dt)
|
|
local vv = etat.veloVertical
|
|
if vv then
|
|
local brute = materiel.lireVitesseCapteur(vv.nom) * vv.signe
|
|
if math.abs(brute) > conf.REGUL.V_ABERRANT then
|
|
return vCapteurF or 0
|
|
end
|
|
vCapteurF = (vCapteurF or brute)
|
|
+ conf.REGUL.FILTRE_V * (brute - (vCapteurF or brute))
|
|
return vCapteurF
|
|
end
|
|
local altitude = materiel.lireAltitude()
|
|
if altPrec and dt and dt > 0 then
|
|
local brute = (altitude - altPrec) / dt
|
|
if math.abs(brute) <= conf.REGUL.V_ABERRANT then
|
|
vFiltre = vFiltre + 0.3 * (brute - vFiltre)
|
|
end
|
|
end
|
|
altPrec = altitude
|
|
return vFiltre
|
|
end
|
|
|
|
------------------------------------------------------------------
|
|
-- SUPERVISEUR D'OSCILLATION
|
|
-- Une boucle qui traverse zero >= 8 fois en 6 s avec une
|
|
-- amplitude significative oscille: ses gains sont reduits de 20%%
|
|
------------------------------------------------------------------
|
|
local surveillance = {
|
|
vitesse = { seuil = 0.4, gains = conf.PID.vitesse },
|
|
tangage = { seuil = 0.4, gains = conf.PID.tangage },
|
|
roulis = { seuil = 0.4, gains = conf.PID.roulis },
|
|
}
|
|
for _, s in pairs(surveillance) do
|
|
s.croisements, s.signePrec, s.arme = {}, nil, false
|
|
end
|
|
|
|
local function surveiller(nom, valeur)
|
|
local s = surveillance[nom]
|
|
local maintenant = os.clock()
|
|
if math.abs(valeur) > s.seuil then s.arme = true end
|
|
local signe = valeur >= 0
|
|
if s.signePrec ~= nil and signe ~= s.signePrec and s.arme then
|
|
table.insert(s.croisements, maintenant)
|
|
s.arme = false
|
|
end
|
|
s.signePrec = signe
|
|
while s.croisements[1]
|
|
and maintenant - s.croisements[1] > 6 do
|
|
table.remove(s.croisements, 1)
|
|
end
|
|
if #s.croisements >= 8 then
|
|
s.croisements = {}
|
|
s.gains.kp = s.gains.kp * 0.8
|
|
s.gains.ki = s.gains.ki * 0.8
|
|
s.gains.kd = s.gains.kd * 0.85
|
|
gainsModifies = true
|
|
journal.alerte(("oscillation %s: gains reduits (kp=%.2f)")
|
|
:format(nom, s.gains.kp))
|
|
end
|
|
end
|
|
|
|
-- le superviseur a-t-il modifie des gains (a persister) ?
|
|
function r.gainsModifies()
|
|
if gainsModifies then gainsModifies = false return true end
|
|
return false
|
|
end
|
|
|
|
------------------------------------------------------------------
|
|
-- MOTEUR: besoin prevu et niveau anticipe
|
|
------------------------------------------------------------------
|
|
-- cibles: helices en rpm HELICE, propulseurs en rpm RSC; les
|
|
-- coefficients (mesures a rapport 1/1) sont en su par rpm helice
|
|
local function besoinPrevu(cibles)
|
|
if not etat.coeffRsc then return nil end
|
|
local besoin = 0
|
|
for role, v in pairs(cibles) do
|
|
local nom = materiel.nomParRole[role]
|
|
local coeff = nom and etat.coeffRsc[nom]
|
|
if coeff then besoin = besoin + coeff * math.abs(v) end
|
|
end
|
|
return besoin
|
|
end
|
|
|
|
------------------------------------------------------------------
|
|
-- BOITE AUTOMATIQUE: maintient la commande RSC des helices dans
|
|
-- la bande [RSC_BAS, RSC_HAUT] en changeant le rapport d'UN cran,
|
|
-- uniquement en quasi-stationnaire et a plat, avec temps mort.
|
|
-- Le regulateur (unites helice) n'y voit RIEN: gains invariants.
|
|
------------------------------------------------------------------
|
|
local prochainChangement = 0
|
|
local boiteModifiee = false
|
|
local boiteGelee = false
|
|
|
|
-- gel de la boite (calibrations: un changement de rapport en
|
|
-- pleine mesure la polluerait)
|
|
function r.gelerBoite(gel)
|
|
boiteGelee = gel and true or false
|
|
end
|
|
|
|
local function gererBoite(cibles, calme)
|
|
local b = etat.boite
|
|
if not b or not calme or boiteGelee then return end
|
|
if injection.vitesse or injection.tangage
|
|
or injection.roulis then return end
|
|
if os.clock() < prochainChangement then return end
|
|
local base = (cibles.lb + cibles.rb + cibles.lf + cibles.rf) / 4
|
|
local rsc = base / b.rapport
|
|
local k = math.floor(b.rapport * 16 + 0.5)
|
|
local nouveauK = nil
|
|
if rsc > conf.BOITE.RSC_HAUT and k < 16 then
|
|
nouveauK = (k == 14) and 16 or (k + 1)
|
|
elseif rsc < conf.BOITE.RSC_BAS and k > 1 then
|
|
nouveauK = (k == 16) and 14 or (k - 1)
|
|
end
|
|
if nouveauK then
|
|
local rpmHelices = { lb = cibles.lb, rb = cibles.rb,
|
|
lf = cibles.lf, rf = cibles.rf }
|
|
if materiel.changerRapport(nouveauK, rpmHelices) then
|
|
prochainChangement = os.clock() + conf.BOITE.ATTENTE
|
|
boiteModifiee = true
|
|
journal.info(("boite: rapport %d/16 (RSC sustentation "
|
|
.. "~%.0f)"):format(nouveauK, base * 16 / nouveauK))
|
|
end
|
|
end
|
|
end
|
|
|
|
-- le rapport a-t-il change (etat a persister) ?
|
|
function r.boiteModifiee()
|
|
if boiteModifiee then boiteModifiee = false return true end
|
|
return false
|
|
end
|
|
|
|
local function niveauPour(besoin)
|
|
local capacites = etat.capaciteParNiveau
|
|
if not capacites then return 15 end
|
|
local vise = besoin * (1 + conf.MOTEUR.MARGE)
|
|
for n = 1, 15 do
|
|
if (capacites[n] or 0) >= vise then return n end
|
|
end
|
|
return 15
|
|
end
|
|
|
|
------------------------------------------------------------------
|
|
-- APPLICATION DES COMMANDES (moteur anticipe puis RSC)
|
|
------------------------------------------------------------------
|
|
local function appliquer(cibles)
|
|
if etat.modeMoteur == "adaptatif" then
|
|
local besoin = besoinPrevu(cibles)
|
|
if besoin then
|
|
-- garde de stress: jamais plus que la capacite maximale
|
|
local capacites = etat.capaciteParNiveau
|
|
local capaMax = capacites and capacites[15]
|
|
if capaMax and capaMax > 0
|
|
and besoin > capaMax * conf.MOTEUR.SECURITE then
|
|
local facteur = capaMax * conf.MOTEUR.SECURITE / besoin
|
|
for role, v in pairs(cibles) do
|
|
cibles[role] = v * facteur
|
|
end
|
|
besoin = besoin * facteur
|
|
end
|
|
local niveau = niveauPour(besoin)
|
|
if niveau > materiel.niveauMoteur() then
|
|
-- MONTEE de regime AVANT les RSC (anticipation)
|
|
materiel.reglerMoteur(niveau)
|
|
for role, v in pairs(cibles) do
|
|
materiel.reglerRsc(role, v)
|
|
end
|
|
return
|
|
else
|
|
-- descente: RSC d'abord, moteur ensuite
|
|
for role, v in pairs(cibles) do
|
|
materiel.reglerRsc(role, v)
|
|
end
|
|
materiel.reglerMoteur(niveau)
|
|
return
|
|
end
|
|
end
|
|
end
|
|
for role, v in pairs(cibles) do
|
|
materiel.reglerRsc(role, v)
|
|
end
|
|
end
|
|
|
|
------------------------------------------------------------------
|
|
-- UN PAS DE REGULATION EN VOL
|
|
-- avance / virage: joystick (-1..1), propulseurs differentiels
|
|
------------------------------------------------------------------
|
|
function r.reguler(consigneY, avance, virage, dt)
|
|
local altitude = materiel.lireAltitude()
|
|
local tangage, roulis = materiel.lireAssiette()
|
|
local hover = r.rpmHover()
|
|
|
|
-- rampe de consigne
|
|
if consigneRampe == nil then consigneRampe = altitude end
|
|
local pas = conf.RAMPE * dt
|
|
consigneRampe = consigneRampe
|
|
+ borner(consigneY - consigneRampe, -pas, pas)
|
|
|
|
-- cascade: erreur -> v cible -> delta rpm
|
|
local vCible = borner(conf.PID.alt.kp * (consigneRampe - altitude),
|
|
-conf.V_DESCENTE_MAX, conf.V_MONTEE_MAX)
|
|
local vMesuree = r.vitesseVerticale(dt)
|
|
-- ANTI-CLAQUAGE: la boucle de vitesse ne commande jamais plus
|
|
-- que +- DELTA_MAX * sustentation; l'integrale est dimensionnee
|
|
-- pour <= 30% de la sustentation
|
|
local deltaMax = math.max(4, conf.REGUL.DELTA_MAX * hover)
|
|
pidVitesse.borneMin, pidVitesse.borneMax = -deltaMax, deltaMax
|
|
pidVitesse.iMax = 0.3 * hover
|
|
/ math.max(conf.PID.vitesse.ki, 0.01)
|
|
local deltaRpm = injection.vitesse
|
|
or pidVitesse:calculer(vCible, vMesuree, dt)
|
|
surveiller("vitesse", vCible - vMesuree)
|
|
|
|
-- adaptation lente: en quasi-stationnaire le residu du PID est
|
|
-- transfere vers la sustentation (charge variable)
|
|
local calme = math.abs(vCible) < 0.2 and math.abs(vMesuree) < 0.3
|
|
if not injection.vitesse and calme then
|
|
ffAdapt = borner(ffAdapt + 0.05 * dt * deltaRpm,
|
|
-vmaxHelice() / 2, vmaxHelice() / 2)
|
|
end
|
|
|
|
-- assiette: autorite proportionnelle a la sustentation
|
|
local plafond = math.max(2, conf.ASSIETTE_PLAFOND * hover)
|
|
pidTangage.borneMin, pidTangage.borneMax = -plafond, plafond
|
|
pidRoulis.borneMin, pidRoulis.borneMax = -plafond, plafond
|
|
local corrTangage = injection.tangage or (SIGNE_TANGAGE
|
|
* pidTangage:calculer(0, tangage, dt))
|
|
local corrRoulis = injection.roulis or (SIGNE_ROULIS
|
|
* pidRoulis:calculer(0, roulis, dt))
|
|
surveiller("tangage", tangage)
|
|
surveiller("roulis", roulis)
|
|
|
|
-- mixage (helices en rpm HELICE), base limitee en PENTE (les
|
|
-- helices ont un temps de transition: des echelons plus raides
|
|
-- n'excitent que l'oscillation)
|
|
local base = borner(hover + deltaRpm, 0, vmaxHelice())
|
|
if basePrec then
|
|
local pasMax = conf.REGUL.PENTE * hover * dt
|
|
base = basePrec + borner(base - basePrec, -pasMax, pasMax)
|
|
end
|
|
basePrec = base
|
|
local cibles = {}
|
|
for role, s in pairs(SIGNES) do
|
|
cibles[role] = borner(base + s.bf * corrTangage
|
|
+ s.lr * corrRoulis, 0, vmaxHelice())
|
|
end
|
|
cibles.prop_r = borner(avance + virage, -1, 1) * VMAX
|
|
cibles.prop_l = borner(avance - virage, -1, 1) * VMAX
|
|
appliquer(cibles)
|
|
|
|
-- boite automatique (apres application: bascule sur des cibles
|
|
-- fraiches, uniquement au calme et a plat)
|
|
gererBoite(cibles, calme
|
|
and math.abs(tangage) < conf.ANGLE_MAX / 2
|
|
and math.abs(roulis) < conf.ANGLE_MAX / 2)
|
|
|
|
return altitude, consigneRampe
|
|
end
|
|
|
|
------------------------------------------------------------------
|
|
-- MODES SPECIAUX
|
|
------------------------------------------------------------------
|
|
-- stationnement: helices a pleine puissance INVERSEE (plaquage)
|
|
function r.plaquer()
|
|
local v = vmaxHelice()
|
|
appliquer({ lb = -v, rb = -v, lf = -v, rf = -v,
|
|
prop_l = 0, prop_r = 0 })
|
|
end
|
|
|
|
function r.arreter()
|
|
materiel.toutArreter()
|
|
r.raz()
|
|
end
|
|
|
|
function r.raz()
|
|
pidVitesse:raz()
|
|
pidTangage:raz()
|
|
pidRoulis:raz()
|
|
consigneRampe = nil
|
|
altPrec, vFiltre = nil, 0
|
|
vCapteurF, basePrec = nil, nil
|
|
end
|
|
|
|
function r.razAdaptation()
|
|
ffAdapt = 0
|
|
end
|
|
|
|
-- calibrations: forcer une boucle ("vitesse", "tangage",
|
|
-- "roulis") a `valeur` (nil = rendre la main au PID)
|
|
function r.reglerInjection(boucle, valeur)
|
|
injection[boucle] = valeur
|
|
end
|
|
|
|
return r
|
|
end
|
|
|
|
return Regulation
|