change stabilization strategy
This commit is contained in:
parent
2f7d6e2847
commit
0acd8e3571
5 changed files with 533 additions and 61 deletions
125
drone.lua
125
drone.lua
|
|
@ -74,6 +74,63 @@ if etat.pid and etat.empreintePid ~= empreinte then
|
|||
etat.pid = nil
|
||||
end
|
||||
|
||||
--------------------------------------------------------------------
|
||||
-- PARAMETRES EDITABLES (page PARAM du moniteur)
|
||||
-- Les surcharges sont persistees dans drone.etat et PRIMENT sur
|
||||
-- drone.conf (sans l'ecraser).
|
||||
--------------------------------------------------------------------
|
||||
local PARAMS = {
|
||||
{ chemin = "CALIB.TOL_DEPASSEMENT", label = "depassement tolere",
|
||||
pas = 0.1, mini = 0 },
|
||||
{ chemin = "ANGLE_MAX", label = "angle max (deg)", pas = 0.25,
|
||||
mini = 0.25 },
|
||||
{ chemin = "ANGLE_DRONE_MAX", label = "angle mode drone (deg)",
|
||||
pas = 1, mini = 1 },
|
||||
{ chemin = "POIDS", label = "poids A VIDE (pN)", pas = 500,
|
||||
mini = 0 },
|
||||
{ chemin = "POUSSEE_HELICE_MAX", label = "poussee helice (pN)",
|
||||
pas = 250, mini = 1 },
|
||||
{ chemin = "POUSSEE_EXPOSANT", label = "exposant poussee",
|
||||
pas = 1, mini = 1, maxi = 2 },
|
||||
{ chemin = "PRESSION_REF", label = "pression de reference",
|
||||
pas = 0.01, mini = 0.1, maxi = 1.5 },
|
||||
{ chemin = "RATIO_CHARGE_MAX", label = "ratio charge max",
|
||||
pas = 0.05, mini = 0.1, maxi = 1 },
|
||||
{ chemin = "ADAPTATION.SEUIL_DESEQUILIBRE",
|
||||
label = "seuil desequilibre (rpm)", pas = 5, mini = 5 },
|
||||
{ chemin = "V_MONTEE_MAX", label = "v montee max (b/s)",
|
||||
pas = 0.5, mini = 0.5 },
|
||||
{ chemin = "V_DESCENTE_MAX", label = "v descente max (b/s)",
|
||||
pas = 0.5, mini = 0.5 },
|
||||
{ chemin = "VITESSE_RAMPE", label = "rampe consigne (b/s)",
|
||||
pas = 0.5, mini = 0.5 },
|
||||
{ chemin = "Y_VOL", label = "altitude croisiere", pas = 5 },
|
||||
{ chemin = "Y_MINI", label = "altitude plancher", pas = 5 },
|
||||
}
|
||||
|
||||
local function lireChemin(chemin)
|
||||
local noeud = conf
|
||||
for partie in chemin:gmatch("[^%.]+") do
|
||||
if type(noeud) ~= "table" then return nil end
|
||||
noeud = noeud[partie]
|
||||
end
|
||||
return noeud
|
||||
end
|
||||
|
||||
local function ecrireChemin(chemin, valeur)
|
||||
local noeud, precedente, cle = conf, nil, nil
|
||||
for partie in chemin:gmatch("[^%.]+") do
|
||||
precedente, cle = noeud, partie
|
||||
noeud = noeud[partie]
|
||||
end
|
||||
precedente[cle] = valeur
|
||||
end
|
||||
|
||||
etat.surcharges = etat.surcharges or {}
|
||||
for chemin, valeur in pairs(etat.surcharges) do
|
||||
ecrireChemin(chemin, valeur)
|
||||
end
|
||||
|
||||
local pilotage = charger("lib/pilotage.lua")
|
||||
.nouveau(conf, etat, materiel, Pid, journal)
|
||||
local navigation = charger("lib/navigation.lua")
|
||||
|
|
@ -101,6 +158,7 @@ local calibEnCours = nil
|
|||
local calibProgres, calibTexte = nil, nil
|
||||
local rscViolet = nil
|
||||
local alerteMoteurA = 0
|
||||
local alerteChargeA = 0
|
||||
local boostMoteurFin = 0
|
||||
|
||||
local function sauverEtat()
|
||||
|
|
@ -143,6 +201,7 @@ end
|
|||
--------------------------------------------------------------------
|
||||
local function changerMode(nouveau, raison)
|
||||
if nouveau == mode then return end
|
||||
local modePrec = mode
|
||||
|
||||
local verrou = verrouDe(nouveau)
|
||||
if verrou then
|
||||
|
|
@ -166,10 +225,19 @@ local function changerMode(nouveau, raison)
|
|||
materiel.reglerMoteur(15)
|
||||
end
|
||||
|
||||
local etaitAuSol = { off = true, inactif = true, calibrage = true,
|
||||
stationnement = true }
|
||||
mode = nouveau
|
||||
pilotage.raz()
|
||||
navigation.razCap()
|
||||
|
||||
if etaitAuSol[modePrec or ""] and (mode == "vol" or mode == "drone"
|
||||
or mode == "auto" or mode == "atterrissage") then
|
||||
-- nouveau vol = nouvelle cargaison: on repart de zero
|
||||
pilotage.razAdaptation()
|
||||
journal.info("adaptation remise a zero (nouveau vol)")
|
||||
end
|
||||
|
||||
if mode == "vol" or mode == "drone" then
|
||||
consigneY = materiel.lireAltitude()
|
||||
elseif mode == "auto" then
|
||||
|
|
@ -214,6 +282,27 @@ local function pasControle()
|
|||
end
|
||||
end
|
||||
|
||||
-- surveillance de la charge (poids estime, equilibrage)
|
||||
local enVol = mode == "vol" or mode == "drone" or mode == "auto"
|
||||
or mode == "atterrissage"
|
||||
if enVol and os.clock() - alerteChargeA > 8 then
|
||||
if pilotage.poidsEstime() > pilotage.poidsMax() then
|
||||
alerteChargeA = os.clock()
|
||||
journal.alerte(("SURCHARGE: poids estime %.0f > max %.0f pN")
|
||||
:format(pilotage.poidsEstime(), pilotage.poidsMax()))
|
||||
ihm.message("SURCHARGE POIDS", 6)
|
||||
else
|
||||
local tT, tR = pilotage.trim()
|
||||
if math.max(math.abs(tT), math.abs(tR))
|
||||
> conf.ADAPTATION.SEUIL_DESEQUILIBRE then
|
||||
alerteChargeA = os.clock()
|
||||
journal.alerte(("chargement desequilibre (trim %.0f/%.0f rpm)")
|
||||
:format(tT, tR))
|
||||
ihm.message("CHARGEMENT DESEQUILIBRE", 6)
|
||||
end
|
||||
end
|
||||
end
|
||||
|
||||
if mode == "off" then
|
||||
pilotage.arreter()
|
||||
|
||||
|
|
@ -348,13 +437,16 @@ local function lancerCalibration(quoi)
|
|||
Calibration.rsc(conf, etat, materiel, journal, Etat, ui)
|
||||
elseif quoi == "rscman" then
|
||||
Calibration.rscManuel(conf, etat, materiel, journal, Etat, ui)
|
||||
elseif quoi == "capteurs" or quoi == "pid" then
|
||||
elseif quoi == "capteurs" or quoi == "pid" or quoi == "pidmath" then
|
||||
if mode ~= "vol" then
|
||||
journal.alerte("calib " .. quoi .. ": passer en mode vol d'abord")
|
||||
ihm.message("passer en mode vol")
|
||||
elseif quoi == "pid" then
|
||||
Calibration.pid(conf, etat, materiel, pilotage, journal, Etat,
|
||||
empreinte, ui)
|
||||
elseif quoi == "pidmath" then
|
||||
Calibration.mathematique(conf, etat, materiel, pilotage,
|
||||
journal, Etat, empreinte, ui)
|
||||
else
|
||||
local function tenir(duree, angles, avance)
|
||||
local cible = materiel.lireAltitude()
|
||||
|
|
@ -399,6 +491,7 @@ local function calibStatuts()
|
|||
and #materiel.propulseursManquants() == 0,
|
||||
capteurs = #materiel.axesManquants() == 0,
|
||||
pid = etat.pid ~= nil,
|
||||
pidmath = etat.pid ~= nil,
|
||||
}
|
||||
local statuts = {}
|
||||
for nom, fait in pairs(faits) do
|
||||
|
|
@ -618,9 +711,37 @@ local function contexteIhm()
|
|||
or ("%d %d"):format(cibleNav.x, cibleNav.z)) or nil,
|
||||
distanceCible = distanceCible,
|
||||
saisieCible = saisieCible,
|
||||
params = listeParams(),
|
||||
poidsEstime = pilotage.poidsEstime(),
|
||||
poidsMax = pilotage.poidsMax(),
|
||||
trimTangage = select(1, pilotage.trim()),
|
||||
trimRoulis = select(2, pilotage.trim()),
|
||||
}
|
||||
end
|
||||
|
||||
local function listeParams()
|
||||
local liste = {}
|
||||
for _, p in ipairs(PARAMS) do
|
||||
table.insert(liste, { label = p.label, valeur = lireChemin(p.chemin) })
|
||||
end
|
||||
return liste
|
||||
end
|
||||
|
||||
local function ajusterParam(idx, sens)
|
||||
local p = PARAMS[idx]
|
||||
if not p then return end
|
||||
local valeur = (lireChemin(p.chemin) or 0) + sens * p.pas
|
||||
if p.mini and valeur < p.mini then valeur = p.mini end
|
||||
if p.maxi and valeur > p.maxi then valeur = p.maxi end
|
||||
ecrireChemin(p.chemin, valeur)
|
||||
etat.surcharges[p.chemin] = valeur
|
||||
Etat.sauver(etat)
|
||||
journal.info(("param %s = %s"):format(p.chemin, tostring(valeur)))
|
||||
if etat.pid then
|
||||
ihm.message("param modifie: recalibrer pid conseille")
|
||||
end
|
||||
end
|
||||
|
||||
-- lance la cible saisie: "x z" ou "x y z"
|
||||
local function lancerCible(texte)
|
||||
local nombres = {}
|
||||
|
|
@ -674,6 +795,8 @@ local function tacheIhm()
|
|||
end
|
||||
elseif action.type == "annuler" then
|
||||
changerMode("vol", "annulation auto")
|
||||
elseif action.type == "param" then
|
||||
ajusterParam(action.idx, action.sens)
|
||||
elseif action.type == "pave" then
|
||||
if action.valeur == "eff" then
|
||||
saisieCible = saisieCible:sub(1, -2)
|
||||
|
|
|
|||
|
|
@ -415,7 +415,180 @@ function Calibration.capteurs(conf, etat, materiel, journal, Etat, ui,
|
|||
end
|
||||
|
||||
--------------------------------------------------------------------
|
||||
-- PID D'ALTITUDE (en vol stationnaire, marge d'altitude requise)
|
||||
-- PID MATHEMATIQUE (en vol stationnaire, marge d'altitude requise)
|
||||
-- 1. Identification en boucle ouverte autour du feedforward: un
|
||||
-- echelon de +deltaRpm est applique, la reponse en vitesse
|
||||
-- verticale donne le modele FOPDT (gain K, constante de temps
|
||||
-- tau, retard L).
|
||||
-- 2. Gains candidats calcules par COHEN-COON et ZIEGLER-NICHOLS
|
||||
-- (reponse indicielle), pour la montee puis la descente
|
||||
-- (echelon negatif).
|
||||
-- 3. Validation en boucle fermee de chaque candidat (memes criteres
|
||||
-- que l'auto-tune: zero depassement, assiette bornee): le
|
||||
-- meilleur temps de reponse VALIDE est conserve.
|
||||
--------------------------------------------------------------------
|
||||
local function identifierFOPDT(pilotage, deltaRpm, dt, duree, journal)
|
||||
-- stabiliser au feedforward pur
|
||||
for _ = 1, math.floor(1.5 / dt) do
|
||||
pilotage.pasOuvert(0, dt)
|
||||
sleep(dt)
|
||||
end
|
||||
-- echelon et enregistrement
|
||||
local temps, vitesses = {}, {}
|
||||
local debut = os.clock()
|
||||
while os.clock() - debut < duree do
|
||||
local v = pilotage.pasOuvert(deltaRpm, dt)
|
||||
table.insert(temps, os.clock() - debut)
|
||||
table.insert(vitesses, v)
|
||||
sleep(dt)
|
||||
end
|
||||
pilotage.pasOuvert(0, dt)
|
||||
|
||||
-- valeur finale: moyenne du dernier quart
|
||||
local somme, n = 0, 0
|
||||
for i = math.floor(#vitesses * 0.75), #vitesses do
|
||||
somme, n = somme + vitesses[i], n + 1
|
||||
end
|
||||
local vFinale = somme / math.max(n, 1)
|
||||
if math.abs(vFinale) < 0.05 then
|
||||
journal.alerte("pidmath: reponse trop faible, augmenter l'echelon")
|
||||
return nil
|
||||
end
|
||||
|
||||
local K = vFinale / deltaRpm
|
||||
-- retard L: premier passage a 5% de vFinale; tau: 63.2% - L
|
||||
local L, t63 = nil, nil
|
||||
for i, v in ipairs(vitesses) do
|
||||
local ratio = v / vFinale
|
||||
if not L and ratio >= 0.05 then L = temps[i] end
|
||||
if not t63 and ratio >= 0.632 then t63 = temps[i] end
|
||||
end
|
||||
if not L or not t63 or t63 <= L then
|
||||
journal.alerte("pidmath: identification impossible (bruit ?)")
|
||||
return nil
|
||||
end
|
||||
local tau = t63 - L
|
||||
L = math.max(L, dt) -- retard minimal: une periode
|
||||
journal.info(("pidmath: K=%.4f tau=%.2f L=%.2f"):format(K, tau, L))
|
||||
return K, tau, L
|
||||
end
|
||||
|
||||
local function gainsCohenCoon(K, tau, L)
|
||||
local r = L / tau
|
||||
local kp = (1 / K) * (tau / L) * (4 / 3 + r / 4)
|
||||
local Ti = L * (32 + 6 * r) / (13 + 8 * r)
|
||||
local Td = L * 4 / (11 + 2 * r)
|
||||
return { kp = kp, ki = kp / Ti, kd = kp * Td }
|
||||
end
|
||||
|
||||
local function gainsZieglerNichols(K, tau, L)
|
||||
local kp = 1.2 * tau / (K * L)
|
||||
return { kp = kp, ki = kp / (2 * L), kd = kp * 0.5 * L }
|
||||
end
|
||||
|
||||
function Calibration.mathematique(conf, etat, materiel, pilotage,
|
||||
journal, Etat, empreinte, ui)
|
||||
local C = conf.CALIB
|
||||
local dt = 0.1
|
||||
local base = materiel.lireAltitude()
|
||||
local gains = pilotage.gains()
|
||||
local deltaRpm = math.max(8, conf.VITESSE_RSC_MAX * 0.05)
|
||||
|
||||
-- meme critere de validation que l'auto-tune
|
||||
local function valider(cible, direction)
|
||||
local debut = os.clock()
|
||||
local extremum = materiel.lireAltitude()
|
||||
local assietteMax = 0
|
||||
local dansBande, atteint, tempsReponse = nil, false, nil
|
||||
while os.clock() - debut < C.TIMEOUT do
|
||||
local alt = pilotage.reguler(cible, { tangage = 0, roulis = 0 },
|
||||
0, 0, dt)
|
||||
local t, r = materiel.lireAssiette()
|
||||
assietteMax = math.max(assietteMax, math.abs(t), math.abs(r))
|
||||
if direction > 0 then extremum = math.max(extremum, alt)
|
||||
else extremum = math.min(extremum, alt) end
|
||||
if math.abs(alt - cible) <= C.BANDE then
|
||||
dansBande = dansBande or os.clock()
|
||||
if os.clock() - dansBande >= C.DELAI_STABLE then
|
||||
atteint, tempsReponse = true, dansBande - debut
|
||||
break
|
||||
end
|
||||
else
|
||||
dansBande = nil
|
||||
end
|
||||
sleep(dt)
|
||||
end
|
||||
local depassement = (direction > 0)
|
||||
and math.max(0, extremum - cible)
|
||||
or math.max(0, cible - extremum)
|
||||
local valide = atteint and depassement <= C.TOL_DEPASSEMENT
|
||||
and assietteMax <= conf.ANGLE_MAX
|
||||
return valide, tempsReponse or math.huge, depassement
|
||||
end
|
||||
|
||||
for iDir, dir in ipairs({
|
||||
{ nom = "montee", sens = 1 },
|
||||
{ nom = "descente", sens = -1 },
|
||||
}) do
|
||||
ui.progres((iDir - 1) / 2, dir.nom .. ": identification")
|
||||
journal.info("pidmath: identification " .. dir.nom)
|
||||
local K, tau, L = identifierFOPDT(pilotage, dir.sens * deltaRpm,
|
||||
dt, 8, journal)
|
||||
if K then
|
||||
local candidats = {
|
||||
{ nom = "cohen-coon", g = gainsCohenCoon(math.abs(K), tau, L) },
|
||||
{ nom = "ziegler-nichols",
|
||||
g = gainsZieglerNichols(math.abs(K), tau, L) },
|
||||
}
|
||||
local meilleur, meilleurTemps = nil, math.huge
|
||||
for iC, c in ipairs(candidats) do
|
||||
ui.progres((iDir - 1) / 2 + iC * 0.2, dir.nom .. ": " .. c.nom)
|
||||
journal.info(("pidmath %s %s: kp=%.1f ki=%.1f kd=%.1f")
|
||||
:format(dir.nom, c.nom, c.g.kp, c.g.ki, c.g.kd))
|
||||
gains[dir.nom] = c.g
|
||||
pilotage.reglerGains(gains)
|
||||
-- retour a la base puis echelon de validation
|
||||
valider(base, -dir.sens)
|
||||
local valide, temps, dep = valider(
|
||||
base + dir.sens * C.AMPLITUDE, dir.sens)
|
||||
valider(base, -dir.sens)
|
||||
journal.info((" %s t=%.1f dep=%.2f"):format(
|
||||
valide and "VALIDE" or "rejete",
|
||||
temps == math.huge and -1 or temps, dep))
|
||||
if valide and temps < meilleurTemps then
|
||||
meilleur, meilleurTemps = c, temps
|
||||
end
|
||||
end
|
||||
if meilleur then
|
||||
gains[dir.nom] = meilleur.g
|
||||
journal.info(("pidmath %s: retenu %s"):format(dir.nom,
|
||||
meilleur.nom))
|
||||
else
|
||||
-- aucun candidat brut valide: version adoucie de cohen-coon
|
||||
local g = gainsCohenCoon(math.abs(K), tau, L)
|
||||
g.kp, g.ki, g.kd = g.kp * 0.6, g.ki * 0.6, g.kd * 0.6
|
||||
gains[dir.nom] = g
|
||||
journal.alerte("pidmath " .. dir.nom
|
||||
.. ": candidats rejetes, version adoucie appliquee")
|
||||
end
|
||||
pilotage.reglerGains(gains)
|
||||
end
|
||||
end
|
||||
|
||||
etat.empreintePid = empreinte
|
||||
etat.poidsCalibration = pilotage.poidsEstime()
|
||||
journal.info(("pidmath: poids de calibration %.0f pN")
|
||||
:format(etat.poidsCalibration))
|
||||
Etat.sauver(etat)
|
||||
valider(base, 1)
|
||||
ui.progres(1, "terminee")
|
||||
journal.info("pidmath: terminee")
|
||||
return true
|
||||
end
|
||||
|
||||
--------------------------------------------------------------------
|
||||
-- PID AUTO-TUNE (en vol stationnaire, marge d'altitude requise)
|
||||
-- Recherche iterative sur la boucle de vitesse verticale.
|
||||
--------------------------------------------------------------------
|
||||
function Calibration.pid(conf, etat, materiel, pilotage, journal, Etat,
|
||||
empreinte, ui)
|
||||
|
|
@ -514,6 +687,9 @@ function Calibration.pid(conf, etat, materiel, pilotage, journal, Etat,
|
|||
|
||||
pilotage.reglerGains(gains)
|
||||
etat.empreintePid = empreinte
|
||||
etat.poidsCalibration = pilotage.poidsEstime()
|
||||
journal.info(("calib pid: poids de calibration %.0f pN")
|
||||
:format(etat.poidsCalibration))
|
||||
Etat.sauver(etat)
|
||||
|
||||
local fin = os.clock() + C.TIMEOUT
|
||||
|
|
|
|||
38
lib/conf.lua
38
lib/conf.lua
|
|
@ -48,13 +48,40 @@ return {
|
|||
OFFSET_Z = 0,
|
||||
|
||||
------------------------------------------------------------------
|
||||
-- Regulation
|
||||
-- Regulation (cascade altitude -> vitesse verticale -> rpm)
|
||||
------------------------------------------------------------------
|
||||
VITESSE_RAMPE = 3.0, -- rampe de consigne d'altitude (blocs/s)
|
||||
VITESSE_RSC_MAX = 256, -- borne des rotation speed controllers
|
||||
V_MONTEE_MAX = 3.0, -- vitesse verticale max en montee (b/s)
|
||||
V_DESCENTE_MAX = 2.0, -- vitesse verticale max en descente (b/s)
|
||||
|
||||
-- Feedforward de sustentation: le rpm d'equilibre est calcule
|
||||
-- depuis le poids et la poussee, le PID ne corrige qu'autour.
|
||||
-- Transport a charge variable: POIDS est le poids A VIDE; la
|
||||
-- charge est estimee en vol (adaptation) et la poussee est
|
||||
-- corrigee par la pression mesuree (elle diminue avec l'altitude).
|
||||
POIDS = 32000, -- poids A VIDE du drone (pixelNewton)
|
||||
POUSSEE_HELICE_MAX = 10000, -- poussee d'UNE helice a
|
||||
-- VITESSE_RSC_MAX et PRESSION_REF (pN)
|
||||
POUSSEE_EXPOSANT = 1, -- poussee ~ rpm^exp (1 lineaire, 2 carre)
|
||||
PRESSION_REF = 1.0, -- pression a laquelle POUSSEE_HELICE_MAX
|
||||
-- est mesuree (getAirPressure)
|
||||
RATIO_CHARGE_MAX = 0.60, -- poids total max = ratio * 4 * poussee
|
||||
-- de reference (alerte SURCHARGE au dela)
|
||||
|
||||
-- Adaptation en vol (le drone se pese et se trime en stationnaire)
|
||||
ADAPTATION = {
|
||||
TAUX_POIDS = 0.05, -- vitesse de transfert vers le
|
||||
-- feedforward (1/s); petit = stable
|
||||
TAUX_TRIM = 0.05, -- idem pour le trim d'assiette
|
||||
SEUIL_DESEQUILIBRE = 15, -- |trim| (rpm) declenchant l'alerte
|
||||
-- "chargement desequilibre"
|
||||
},
|
||||
|
||||
PID = { -- gains par defaut (remplaces par calib)
|
||||
montee = { kp = 12, ki = 2.0, kd = 18 },
|
||||
descente = { kp = 10, ki = 1.5, kd = 22 },
|
||||
alt = { kp = 1.2 }, -- erreur alt -> v cible (1/s)
|
||||
montee = { kp = 40, ki = 60, kd = 5 }, -- v -> delta rpm
|
||||
descente = { kp = 40, ki = 60, kd = 5 },
|
||||
tangage = { kp = 4, ki = 0.5, kd = 8 },
|
||||
roulis = { kp = 4, ki = 0.5, kd = 8 },
|
||||
},
|
||||
|
|
@ -150,6 +177,11 @@ function Conf.empreintePid(conf)
|
|||
vmax = conf.VITESSE_RSC_MAX,
|
||||
rampe = conf.VITESSE_RAMPE,
|
||||
angle = conf.ANGLE_MAX,
|
||||
poids = conf.POIDS,
|
||||
poussee = conf.POUSSEE_HELICE_MAX,
|
||||
exposant = conf.POUSSEE_EXPOSANT,
|
||||
vmont = conf.V_MONTEE_MAX,
|
||||
vdesc = conf.V_DESCENTE_MAX,
|
||||
})
|
||||
end
|
||||
|
||||
|
|
|
|||
36
lib/ihm.lua
36
lib/ihm.lua
|
|
@ -222,6 +222,15 @@ function Ihm.nouveau(conf, materiel, journal)
|
|||
materiel.vitesseHorizontale(), materiel.lireAltitude(),
|
||||
ctx.consigneY or 0))
|
||||
y = y + 1
|
||||
local surcharge = ctx.poidsEstime
|
||||
and ctx.poidsMax and ctx.poidsEstime > ctx.poidsMax
|
||||
texte(1, y, ("poids est. %s / max %s trim %s/%s"):format(
|
||||
ctx.poidsEstime and ("%.0f"):format(ctx.poidsEstime) or "?",
|
||||
ctx.poidsMax and ("%.0f"):format(ctx.poidsMax) or "?",
|
||||
ctx.trimTangage and ("%+.0f"):format(ctx.trimTangage) or "?",
|
||||
ctx.trimRoulis and ("%+.0f"):format(ctx.trimRoulis) or "?"),
|
||||
surcharge and colors.red or colors.white)
|
||||
y = y + 1
|
||||
bouton(1, y, "-10", { type = "delta", valeur = -10 })
|
||||
bouton(7, y, "-1", { type = "delta", valeur = -1 })
|
||||
bouton(12, y, "+1", { type = "delta", valeur = 1 })
|
||||
|
|
@ -276,7 +285,7 @@ function Ihm.nouveau(conf, materiel, journal)
|
|||
colors.lightGray)
|
||||
local x, y = 1, 3
|
||||
for _, c in ipairs({ "gimbal", "joystick", "typew", "rsc",
|
||||
"rscman", "capteurs", "pid" }) do
|
||||
"rscman", "capteurs", "pid", "pidmath" }) do
|
||||
local statut = ctx.calibStatuts[c]
|
||||
local couleur = colors.gray
|
||||
if statut == "encours" then couleur = colors.yellow
|
||||
|
|
@ -310,7 +319,24 @@ function Ihm.nouveau(conf, materiel, journal)
|
|||
texte(1, H - 1,
|
||||
"sol: gimbal joystick typew rsc rscman (mode calibrage)",
|
||||
colors.lightGray)
|
||||
texte(1, H, "vol: capteurs pid (mode vol)", colors.lightGray)
|
||||
texte(1, H, "vol: capteurs pid pidmath (mode vol)",
|
||||
colors.lightGray)
|
||||
end
|
||||
|
||||
------------------------------------------------------------------
|
||||
-- PAGE PARAMS (edition tactile; surcharges dans drone.etat)
|
||||
------------------------------------------------------------------
|
||||
local function dessinerPageParams(ctx)
|
||||
texte(1, 2, "surcharges sauvees dans drone.etat", colors.lightGray)
|
||||
local y = 3
|
||||
for i2, p in ipairs(ctx.params or {}) do
|
||||
texte(1, y, ("%-24s"):format(p.label))
|
||||
texte(26, y, tostring(p.valeur), colors.yellow)
|
||||
bouton(L - 9, y, "-", { type = "param", idx = i2, sens = -1 })
|
||||
bouton(L - 4, y, "+", { type = "param", idx = i2, sens = 1 })
|
||||
y = y + 1
|
||||
if y > H then break end
|
||||
end
|
||||
end
|
||||
|
||||
------------------------------------------------------------------
|
||||
|
|
@ -334,9 +360,10 @@ function Ihm.nouveau(conf, materiel, journal)
|
|||
moniteur.clear()
|
||||
|
||||
-- barre d'onglets
|
||||
local onglets = { mode = "MODE", calib = "CALIB", journal = "JOURNAL" }
|
||||
local onglets = { mode = "MODE", calib = "CALIB",
|
||||
params = "PARAM", journal = "JOURNAL" }
|
||||
local x = 1
|
||||
for _, id in ipairs({ "mode", "calib", "journal" }) do
|
||||
for _, id in ipairs({ "mode", "calib", "params", "journal" }) do
|
||||
bouton(x, 1, onglets[id], { type = "page", valeur = id },
|
||||
page == id and colors.green or colors.gray)
|
||||
x = x + #onglets[id] + 3
|
||||
|
|
@ -345,6 +372,7 @@ function Ihm.nouveau(conf, materiel, journal)
|
|||
|
||||
if page == "mode" then dessinerPageMode(ctx)
|
||||
elseif page == "calib" then dessinerPageCalib(ctx)
|
||||
elseif page == "params" then dessinerPageParams(ctx)
|
||||
else dessinerPageJournal() end
|
||||
end
|
||||
|
||||
|
|
|
|||
217
lib/pilotage.lua
217
lib/pilotage.lua
|
|
@ -1,14 +1,17 @@
|
|||
--------------------------------------------------------------------
|
||||
-- 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.
|
||||
-- ASSERVISSEMENT EN CASCADE + FEEDFORWARD :
|
||||
-- 1. feedforward : rpm d'equilibre (sustentation) calcule depuis
|
||||
-- POIDS et POUSSEE_HELICE_MAX; le PID ne corrige qu'autour de
|
||||
-- ce point au lieu de devoir le trouver par integrale
|
||||
-- 2. boucle externe : erreur d'altitude -> consigne de vitesse
|
||||
-- verticale (P simple, bornee V_MONTEE_MAX / V_DESCENTE_MAX)
|
||||
-- 3. boucle interne : PID sur la vitesse verticale MESUREE
|
||||
-- (velocity sensor vertical calibre; repli: derivee filtree de
|
||||
-- l'altitude) -> delta rpm autour du feedforward
|
||||
-- 4. assiette : PID tangage + roulis, corrections signees par coin
|
||||
-- 5. propulseurs : mixage differentiel avance / virage
|
||||
--------------------------------------------------------------------
|
||||
local Pilotage = {}
|
||||
|
||||
|
|
@ -19,10 +22,9 @@ local SIGNES = {
|
|||
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
|
||||
-- assiette normalisee: une helice AVANT pousse plus quand le tangage
|
||||
-- est negatif (nez bas), une helice GAUCHE quand le roulis est
|
||||
-- negatif (penche a gauche)
|
||||
local SIGNE_TANGAGE, SIGNE_ROULIS = -1, -1
|
||||
|
||||
function Pilotage.nouveau(conf, etat, materiel, Pid, journal)
|
||||
|
|
@ -30,25 +32,80 @@ function Pilotage.nouveau(conf, etat, materiel, Pid, journal)
|
|||
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 pidMontee = Pid.nouveau(gains.montee, -VMAX, VMAX, 20)
|
||||
local pidDescente = Pid.nouveau(gains.descente, -VMAX, VMAX, 20)
|
||||
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
|
||||
local facteurProp = 1.0
|
||||
local altPrec, vFiltre = nil, 0 -- repli de mesure de vitesse
|
||||
|
||||
-- ADAPTATION (transport a charge variable, remise a zero au
|
||||
-- decollage via razAdaptation)
|
||||
local rpmAdapt = 0 -- correction adaptative du ff
|
||||
local trimTangage, trimRoulis = 0, 0 -- trim d'assiette adaptatif
|
||||
|
||||
------------------------------------------------------------------
|
||||
-- FEEDFORWARD : rpm d'equilibre d'UNE helice
|
||||
-- poussee(rpm) = pousseeEffective * (rpm/VMAX)^exp, avec la
|
||||
-- poussee CORRIGEE PAR LA PRESSION (elle diminue avec l'altitude):
|
||||
-- pousseeEffective = POUSSEE_HELICE_MAX * pression / PRESSION_REF
|
||||
-- equilibre: 4 * poussee(rpm) = POIDS (a vide) + charge estimee
|
||||
------------------------------------------------------------------
|
||||
local function pousseeEffective()
|
||||
local facteur = materiel.lirePression() / conf.PRESSION_REF
|
||||
return conf.POUSSEE_HELICE_MAX * math.max(facteur, 0.05)
|
||||
end
|
||||
|
||||
function p.rpmSustentation()
|
||||
local ratio = (conf.POIDS / 4) / pousseeEffective()
|
||||
if ratio <= 0 then return 0 end
|
||||
if ratio >= 1 then return VMAX end
|
||||
return VMAX * ratio ^ (1 / conf.POUSSEE_EXPOSANT)
|
||||
end
|
||||
|
||||
-- poids total estime (a vide + charge), inverse de la courbe de
|
||||
-- poussee au rpm de sustentation adapte, a la pression courante
|
||||
function p.poidsEstime()
|
||||
local rpm = math.max(0, math.min(VMAX, p.rpmSustentation() + rpmAdapt))
|
||||
return 4 * pousseeEffective() * (rpm / VMAX) ^ conf.POUSSEE_EXPOSANT
|
||||
end
|
||||
|
||||
-- plafond de poids total (regle des 60% a la pression de reference)
|
||||
function p.poidsMax()
|
||||
return conf.RATIO_CHARGE_MAX * 4 * conf.POUSSEE_HELICE_MAX
|
||||
end
|
||||
|
||||
function p.trim()
|
||||
return trimTangage, trimRoulis
|
||||
end
|
||||
|
||||
function p.razAdaptation()
|
||||
rpmAdapt, trimTangage, trimRoulis = 0, 0, 0
|
||||
end
|
||||
|
||||
------------------------------------------------------------------
|
||||
-- MESURE DE VITESSE VERTICALE
|
||||
-- capteur calibre si present, sinon derivee filtree de l'altitude
|
||||
------------------------------------------------------------------
|
||||
function p.vitesseVerticale(altitude, dt)
|
||||
if materiel.veloParAxe["vertical"] then
|
||||
return materiel.lireVitesseAxe("vertical")
|
||||
end
|
||||
if altPrec and dt > 0 then
|
||||
local brute = (altitude - altPrec) / dt
|
||||
vFiltre = vFiltre + 0.3 * (brute - vFiltre)
|
||||
end
|
||||
altPrec = altitude
|
||||
return vFiltre
|
||||
end
|
||||
|
||||
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
|
||||
|
|
@ -58,6 +115,10 @@ function Pilotage.nouveau(conf, etat, materiel, Pid, journal)
|
|||
pidRoulis:reglerGains(gains.roulis)
|
||||
end
|
||||
|
||||
function p.reglerFacteurProp(f)
|
||||
facteurProp = math.max(0, math.min(1, f))
|
||||
end
|
||||
|
||||
function p.raz()
|
||||
pidMontee:raz()
|
||||
pidDescente:raz()
|
||||
|
|
@ -65,12 +126,39 @@ function Pilotage.nouveau(conf, etat, materiel, Pid, journal)
|
|||
pidRoulis:raz()
|
||||
consigneRampe = nil
|
||||
regimePrec = nil
|
||||
altPrec, vFiltre = nil, 0
|
||||
end
|
||||
|
||||
-- Un pas de regulation en vol.
|
||||
-- consigneY : altitude cible
|
||||
-- angles : { tangage = deg, roulis = deg } cibles
|
||||
-- avance, virage : commande propulseurs dans -1..1
|
||||
local function borner(v, mini, maxi)
|
||||
if v < mini then return mini elseif v > maxi then return maxi end
|
||||
return v
|
||||
end
|
||||
|
||||
-- applique base + trim + corrections d'assiette aux 4 helices.
|
||||
-- `adapter` (booleen): en quasi-stationnaire, le trim absorbe
|
||||
-- lentement la composante statique des corrections (CG decale par
|
||||
-- la cargaison); le PID reste centre sur la dynamique.
|
||||
local function appliquerHelices(base, tangage, roulis, dt,
|
||||
anglesCibles, adapter)
|
||||
local corrTangage = SIGNE_TANGAGE
|
||||
* pidTangage:calculer(anglesCibles.tangage or 0, tangage, dt)
|
||||
local corrRoulis = SIGNE_ROULIS
|
||||
* pidRoulis:calculer(anglesCibles.roulis or 0, roulis, dt)
|
||||
if adapter then
|
||||
local taux = conf.ADAPTATION.TAUX_TRIM * dt
|
||||
trimTangage = trimTangage + taux * corrTangage
|
||||
trimRoulis = trimRoulis + taux * corrRoulis
|
||||
end
|
||||
for role, s in pairs(SIGNES) do
|
||||
materiel.reglerRsc(role, borner(base
|
||||
+ s.bf * (trimTangage + corrTangage)
|
||||
+ s.lr * (trimRoulis + corrRoulis), 0, VMAX))
|
||||
end
|
||||
end
|
||||
|
||||
------------------------------------------------------------------
|
||||
-- UN PAS DE REGULATION EN VOL
|
||||
------------------------------------------------------------------
|
||||
function p.reguler(consigneY, angles, avance, virage, dt)
|
||||
local altitude = materiel.lireAltitude()
|
||||
local tangage, roulis = materiel.lireAssiette()
|
||||
|
|
@ -78,44 +166,69 @@ function Pilotage.nouveau(conf, etat, materiel, Pid, journal)
|
|||
-- 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
|
||||
consigneRampe = consigneRampe
|
||||
+ borner(consigneY - consigneRampe, -pas, pas)
|
||||
|
||||
-- double regime : montee ou descente
|
||||
local montee = (consigneRampe - altitude) >= 0
|
||||
-- boucle externe: erreur alt -> vitesse verticale cible
|
||||
local vCible = borner(gains.alt.kp * (consigneRampe - altitude),
|
||||
-conf.V_DESCENTE_MAX, conf.V_MONTEE_MAX)
|
||||
|
||||
-- boucle interne: vitesse verticale -> delta rpm (double regime)
|
||||
local vMesuree = p.vitesseVerticale(altitude, dt)
|
||||
local montee = vCible >= 0
|
||||
local pid = montee and pidMontee or pidDescente
|
||||
if regimePrec ~= nil and regimePrec ~= montee then
|
||||
pid:raz()
|
||||
end
|
||||
if regimePrec ~= nil and regimePrec ~= montee then pid:raz() end
|
||||
regimePrec = montee
|
||||
local base = pid:calculer(consigneRampe, altitude, dt)
|
||||
local deltaRpm = pid:calculer(vCible, vMesuree, 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)
|
||||
-- GAIN SCHEDULING: la boucle de vitesse est mise a l'echelle du
|
||||
-- poids (delta de poussee requis proportionnel a la masse)
|
||||
if etat.poidsCalibration and etat.poidsCalibration > 0 then
|
||||
deltaRpm = deltaRpm
|
||||
* borner(p.poidsEstime() / etat.poidsCalibration, 0.5, 2.0)
|
||||
end
|
||||
|
||||
-- propulseurs (differentiel)
|
||||
local function borner(v)
|
||||
if v > 1 then return 1 elseif v < -1 then return -1 end
|
||||
return v
|
||||
-- ADAPTATION DE POIDS: en quasi-stationnaire, le residu du PID
|
||||
-- est transfere lentement vers le feedforward (le drone se pese)
|
||||
local stationnaire = math.abs(vCible) < 0.2
|
||||
and math.abs(vMesuree) < 0.3
|
||||
if stationnaire then
|
||||
rpmAdapt = borner(
|
||||
rpmAdapt + conf.ADAPTATION.TAUX_POIDS * dt * deltaRpm,
|
||||
-VMAX / 2, VMAX / 2)
|
||||
end
|
||||
materiel.reglerRsc("prop_r", borner(avance + virage) * VMAX * facteurProp)
|
||||
materiel.reglerRsc("prop_l", borner(avance - virage) * VMAX * facteurProp)
|
||||
|
||||
local base = borner(p.rpmSustentation() + rpmAdapt + deltaRpm,
|
||||
0, VMAX)
|
||||
appliquerHelices(base, tangage, roulis, dt, angles, stationnaire
|
||||
and math.abs(tangage) < conf.ANGLE_MAX
|
||||
and math.abs(roulis) < conf.ANGLE_MAX)
|
||||
|
||||
-- propulseurs (differentiel, delestables en surcharge)
|
||||
local function b1(v) return borner(v, -1, 1) end
|
||||
materiel.reglerRsc("prop_r", b1(avance + virage) * VMAX * facteurProp)
|
||||
materiel.reglerRsc("prop_l", b1(avance - virage) * VMAX * facteurProp)
|
||||
|
||||
return altitude, consigneRampe
|
||||
end
|
||||
|
||||
-- Stationnement: helices a pleine puissance inversee (plaquage),
|
||||
-- propulseurs a 0
|
||||
------------------------------------------------------------------
|
||||
-- PAS EN BOUCLE OUVERTE (identification pour pidmath)
|
||||
-- applique rpmSustentation + deltaRpm (assiette toujours asservie)
|
||||
-- et retourne la vitesse verticale mesuree
|
||||
------------------------------------------------------------------
|
||||
function p.pasOuvert(deltaRpm, dt)
|
||||
local altitude = materiel.lireAltitude()
|
||||
local tangage, roulis = materiel.lireAssiette()
|
||||
local base = borner(p.rpmSustentation() + rpmAdapt + deltaRpm,
|
||||
0, VMAX)
|
||||
appliquerHelices(base, tangage, roulis, dt,
|
||||
{ tangage = 0, roulis = 0 }, false)
|
||||
materiel.reglerRsc("prop_l", 0)
|
||||
materiel.reglerRsc("prop_r", 0)
|
||||
return p.vitesseVerticale(altitude, dt), altitude
|
||||
end
|
||||
|
||||
function p.plaquer()
|
||||
for role in pairs(SIGNES) do
|
||||
materiel.reglerRsc(role, -VMAX)
|
||||
|
|
|
|||
Loading…
Reference in a new issue