From f410475bdf1d8d668e776036f97dad7f17cae01d Mon Sep 17 00:00:00 2001 From: Ploush Date: Sun, 19 Jul 2026 23:41:27 +0200 Subject: [PATCH] fix oscillation --- lib/calib.lua | 34 ++++++++++++++++++----------- lib/regulation.lua | 53 ++++++++++++++++++++++++++++++++++++++++++---- 2 files changed, 71 insertions(+), 16 deletions(-) diff --git a/lib/calib.lua b/lib/calib.lua index d9c5c8b..115379f 100644 --- a/lib/calib.lua +++ b/lib/calib.lua @@ -592,18 +592,6 @@ function Calib.altitude(conf, etat, materiel, regulation, journal, -- gains d'assiette par defaut DIMENSIONNES sur ce rpm (si la -- calib inclinaison n'a pas encore ete faite) - if not etat.calibInclinaison then - for _, axe in ipairs({ "tangage", "roulis" }) do - local g = conf.PID[axe] - g.kp = math.max(0.4, 0.05 * rpm) - g.ki = math.max(0.03, 0.004 * rpm) - g.kd = math.max(0.4, 0.08 * rpm) - persisterGains(etat, Etat, axe, g) - end - journal.info("calib altitude: gains d'assiette par defaut " - .. "dimensionnes sur la sustentation (doux)") - end - -- Phase 1c: RETARD D'ACTIONNEMENT des helices (diagnostic cle): -- echelon de +20%% sur lf, le speedometer donne la constante de -- temps (63%%) entre commande et vitesse reelle @@ -629,9 +617,13 @@ function Calib.altitude(conf, etat, materiel, regulation, journal, end regulation.reglerInjection("vitesse", nil) if t63 then + etat.retardActionnement = t63 + Etat.sauver(etat) journal.info(("calib altitude: retard d'actionnement ~%.2f s " .. "(constante 63%%)"):format(t63)) else + etat.retardActionnement = 3.0 + Etat.sauver(etat) journal.alerte("calib altitude: retard d'actionnement > 3 s " .. "(transition d'helice tres lente ?)") end @@ -643,6 +635,24 @@ function Calib.altitude(conf, etat, materiel, regulation, journal, end end + -- gains d'assiette par defaut: dimensionnes sur la sustentation + -- ET sur le retard d'actionnement (mesure en phase 1c ou par une + -- calib precedente): avec un actionneur lent, kp doit etre bas et + -- kd ~ kp * retard (avance de phase calee sur le retard, pas plus) + local retard = etat.retardActionnement or 0.5 + if not etat.calibInclinaison then + for _, axe in ipairs({ "tangage", "roulis" }) do + local g = conf.PID[axe] + g.kp = math.max(0.3, 0.03 * rpm / math.max(retard * 2, 1)) + g.ki = math.max(0.02, 0.05 * g.kp) + g.kd = g.kp * math.min(retard, 1.5) + persisterGains(etat, Etat, axe, g) + end + journal.info(("calib altitude: gains d'assiette par defaut " + .. "cales sur retard %.2f s (kp=%.2f kd=%.2f)"):format( + retard, conf.PID.tangage.kp, conf.PID.tangage.kd)) + end + -- Phase 2: montee reguliere de 3 blocs pour identifier le -- velocity sensor vertical (celui qui suit la montee) ui.progres(0.3, "detection capteur vertical") diff --git a/lib/regulation.lua b/lib/regulation.lua index b20c737..6bf3096 100644 --- a/lib/regulation.lua +++ b/lib/regulation.lua @@ -51,9 +51,9 @@ function Regulation.nouveau(conf, etat, materiel, Pid, journal) 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) + 0.2) local pidRoulis = Pid.nouveau(conf.PID.roulis, -VMAX, VMAX, 15, - 0.3) + 0.2) local consigneRampe = nil local ffAdapt = 0 -- adaptation lente (charge variable) @@ -114,8 +114,8 @@ function Regulation.nouveau(conf, etat, materiel, Pid, journal) ------------------------------------------------------------------ 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 }, + tangage = { seuil = 0.3, gains = conf.PID.tangage }, + roulis = { seuil = 0.3, gains = conf.PID.roulis }, } for _, s in pairs(surveillance) do s.croisements, s.signePrec, s.arme = {}, nil, false @@ -152,6 +152,45 @@ function Regulation.nouveau(conf, etat, materiel, Pid, journal) return false end + ------------------------------------------------------------------ + -- DETECTEUR DE BATTEMENT: correction d'assiette en BUTEE + -- (+-plafond) en alternance repetee = kd trop fort pour le retard + -- d'actionnement, MEME si l'angle reste petit (invisible au + -- superviseur d'angle). Reduction ciblee de kd. + ------------------------------------------------------------------ + local battement = { + tangage = { evenements = {}, sensPrec = nil, + gains = conf.PID.tangage }, + roulis = { evenements = {}, sensPrec = nil, + gains = conf.PID.roulis }, + } + + local function surveillerButee(nom, corr, plafond) + local b = battement[nom] + local maintenant = os.clock() + local sens = nil + if corr >= plafond * 0.95 then sens = 1 + elseif corr <= -plafond * 0.95 then sens = -1 end + if sens and b.sensPrec and sens ~= b.sensPrec then + table.insert(b.evenements, maintenant) + end + if sens then b.sensPrec = sens end + -- fenetre LONGUE: avec un actionneur lent le battement peut + -- etre a ~1 alternance/s seulement + while b.evenements[1] + and maintenant - b.evenements[1] > 10 do + table.remove(b.evenements, 1) + end + if #b.evenements >= 6 then + b.evenements = {} + b.gains.kd = b.gains.kd * 0.7 + b.gains.kp = b.gains.kp * 0.9 + gainsModifies = true + journal.alerte(("battement %s: kd reduit (kd=%.2f kp=%.2f)") + :format(nom, b.gains.kd, b.gains.kp)) + end + end + ------------------------------------------------------------------ -- MOTEUR: besoin prevu et niveau anticipe ------------------------------------------------------------------ @@ -316,6 +355,12 @@ function Regulation.nouveau(conf, etat, materiel, Pid, journal) * pidRoulis:calculer(0, roulis, dt)) surveiller("tangage", tangage) surveiller("roulis", roulis) + if not injection.tangage then + surveillerButee("tangage", corrTangage, plafond) + end + if not injection.roulis then + surveillerButee("roulis", corrRoulis, plafond) + end -- mixage (helices en rpm HELICE), base limitee en PENTE (les -- helices ont un temps de transition: des echelons plus raides