diff --git a/README.md b/README.md index f1abcea..b4dcb7f 100644 --- a/README.md +++ b/README.md @@ -19,6 +19,13 @@ la V1. Installation: copier drone.lua puis `drone maj ` 3. BRUIT DE MESURE: derivee FILTREE dans le PID; vitesse verticale par velocity sensor AUTO-DETECTE a la calib altitude (repli: derivee filtree de l'altitude). +3b. ANTI-CLAQUAGE: la boucle de vitesse est bornee a +-DELTA_MAX + (60%) du rpm de sustentation, son integrale a 30%, et la base + est limitee en PENTE (2x la sustentation par seconde: les + helices ont un temps de transition reel, mesure et journalise + par la calib altitude). Le passage brutal de 0 a pleine + puissance est structurellement impossible; l'autorite d'assiette + n'est jamais detruite par un ecretage a zero. 4. FILET DE SECURITE: un SUPERVISEUR detecte l'oscillation de chaque boucle en vol (passages a zero repetes au-dela d'un seuil d'amplitude) et reduit ses gains de 20% automatiquement, avec diff --git a/drone.lua b/drone.lua index 7473813..1c860d0 100644 --- a/drone.lua +++ b/drone.lua @@ -89,6 +89,10 @@ local PARAMS = { mini = 0 }, { chemin = "MOTEUR.MARGE", label = "marge moteur", pas = 0.05, mini = 0.05, maxi = 0.5 }, + { chemin = "REGUL.DELTA_MAX", label = "autorite vitesse", + pas = 0.05, mini = 0.2, maxi = 1 }, + { chemin = "REGUL.PENTE", label = "pente base (x sust/s)", + pas = 0.25, mini = 0.5, maxi = 6 }, } local function lireChemin(chemin) @@ -158,6 +162,7 @@ local carburantPct, autonomieMin = nil, nil local alerteMoteurA = 0 local reidentificationApres = 0 local DT = 0.1 +local DT_REEL = 0.1 local function sauverEtat() etat.mode = mode @@ -314,11 +319,11 @@ local function pasControle() regulation.plaquer() elseif mode == "vol" then local avance, virage = materiel.lireJoystick() - regulation.reguler(consigneY, avance, virage, DT) + regulation.reguler(consigneY, avance, virage, DT_REEL) surveillerBoites() elseif mode == "atterrissage" then - consigneY = consigneY - conf.VITESSE_ATTERRISSAGE * DT - regulation.reguler(consigneY, 0, 0, DT) + consigneY = consigneY - conf.VITESSE_ATTERRISSAGE * DT_REEL + regulation.reguler(consigneY, 0, 0, DT_REEL) if materiel.auSol() then changerMode("stationnement", "sol atteint") end @@ -342,10 +347,17 @@ local function pasControle() end local function tacheControle() + local precedent = os.clock() while true do if not enCalibration then + -- dt REEL (le pas peut deriver du DT nominal): borne 0.05-0.3 + local maintenant = os.clock() + DT_REEL = math.max(0.05, math.min(0.3, maintenant - precedent)) + precedent = maintenant local ok, err = pcall(pasControle) if not ok then journal.erreur("controle: " .. tostring(err)) end + else + precedent = os.clock() end sleep(DT) end diff --git a/lib/calib.lua b/lib/calib.lua index 387d70e..26436d4 100644 --- a/lib/calib.lua +++ b/lib/calib.lua @@ -595,13 +595,52 @@ function Calib.altitude(conf, etat, materiel, regulation, journal, if not etat.calibInclinaison then for _, axe in ipairs({ "tangage", "roulis" }) do local g = conf.PID[axe] - g.kp = math.max(0.5, 0.08 * rpm) - g.ki = math.max(0.05, 0.008 * rpm) - g.kd = math.max(0.5, 0.15 * rpm) + 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") + .. "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 + if etat.speedoParRole and etat.speedoParRole.lf then + ui.progres(0.28, "mesure du retard d'actionnement") + local stabiliser = os.clock() + 2 + local altTenue = materiel.lireAltitude() + while os.clock() < stabiliser do + regulation.reguler(altTenue, 0, 0, dt) + sleep(dt) + end + local v0 = math.abs(materiel.lireVitesseHelice("lf") or 0) + local cibleRpm = v0 * 1.2 + regulation.reglerInjection("vitesse", 0.2 * rpm) + local t0, t63 = os.clock(), nil + while os.clock() - t0 < 3 do + regulation.reguler(altTenue, 0, 0, dt) + local v = math.abs(materiel.lireVitesseHelice("lf") or 0) + if not t63 and v >= v0 + 0.632 * (cibleRpm - v0) then + t63 = os.clock() - t0 + end + sleep(dt) + end + regulation.reglerInjection("vitesse", nil) + if t63 then + journal.info(("calib altitude: retard d'actionnement ~%.2f s " + .. "(constante 63%%)"):format(t63)) + else + journal.alerte("calib altitude: retard d'actionnement > 3 s " + .. "(transition d'helice tres lente ?)") + end + -- redescendre la base tranquillement + local calmer = os.clock() + 2 + while os.clock() < calmer do + regulation.reguler(altTenue, 0, 0, dt) + sleep(dt) + end end -- Phase 2: montee reguliere de 3 blocs pour identifier le @@ -696,6 +735,22 @@ function Calib.inclinaison(conf, etat, materiel, regulation, journal, sleep(dt) end + -- PRE-VERIFICATION: le relais exige un fond CALME. 3 s d'assiette + -- tenue sous ANGLE_MAX, sinon abandon avec la marche a suivre. + ui.progres(0.02, "verification du calme") + local debut = os.clock() + while os.clock() - debut < 3 do + local t, r2 = materiel.lireAssiette() + if math.max(math.abs(t), math.abs(r2)) > conf.ANGLE_MAX then + journal.erreur("calib inclinaison: assiette instable AVANT le " + .. "relais; reduire tangage/roulis kp (page PARAM) ou " + .. "verifier la calib altitude, puis relancer") + ui.progres(0, "ECHEC: stabiliser d'abord (voir journal)") + return false + end + pas() + end + for iAxe, axe in ipairs({ "tangage", "roulis" }) do local base01 = (iAxe - 1) * 0.5 ui.progres(base01, axe) diff --git a/lib/conf.lua b/lib/conf.lua index 6801408..a6e0649 100644 --- a/lib/conf.lua +++ b/lib/conf.lua @@ -25,9 +25,20 @@ local DEFAUT = [[ -- remplacent (persistes en surcharges, retouchables page PARAM) PID = { alt = { kp = 1.0 }, -- err alt -> v cible (1/s) - vitesse = { kp = 8, ki = 12, kd = 1 }, -- v -> delta rpm - tangage = { kp = 2, ki = 0.2, kd = 3 }, - roulis = { kp = 2, ki = 0.2, kd = 3 }, + vitesse = { kp = 4, ki = 4, kd = 1 }, -- v -> delta rpm + tangage = { kp = 1.5, ki = 0.1, kd = 2 }, + roulis = { kp = 1.5, ki = 0.1, kd = 2 }, + }, + + -- garde-fous de la regulation (anti claquage 0 <-> max) + REGUL = { + DELTA_MAX = 0.6, -- sortie de la boucle de vitesse bornee a + -- +- fraction du rpm de sustentation + PENTE = 2.0, -- pente max de la base: fraction de la + -- sustentation PAR SECONDE (les helices ont + -- un temps de transition reel) + FILTRE_V = 0.5, -- lissage de la vitesse verticale (1 = brut) + V_ABERRANT = 15, -- |v| au-dela: mesure rejetee (b/s) }, MOTEUR = { diff --git a/lib/regulation.lua b/lib/regulation.lua index ce29809..d2b4318 100644 --- a/lib/regulation.lua +++ b/lib/regulation.lua @@ -49,12 +49,17 @@ function Regulation.nouveau(conf, etat, materiel, Pid, journal) -- 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) - local pidTangage = Pid.nouveau(conf.PID.tangage, -VMAX, VMAX, 15) - local pidRoulis = Pid.nouveau(conf.PID.roulis, -VMAX, VMAX, 15) + -- 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 @@ -77,15 +82,26 @@ function Regulation.nouveau(conf, etat, materiel, Pid, journal) ------------------------------------------------------------------ -- 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 - return materiel.lireVitesseCapteur(vv.nom) * vv.signe + 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 - vFiltre = vFiltre + 0.3 * (brute - vFiltre) + if math.abs(brute) <= conf.REGUL.V_ABERRANT then + vFiltre = vFiltre + 0.3 * (brute - vFiltre) + end end altPrec = altitude return vFiltre @@ -262,8 +278,13 @@ function Regulation.nouveau(conf, etat, materiel, Pid, journal) local vCible = borner(conf.PID.alt.kp * (consigneRampe - altitude), -conf.V_DESCENTE_MAX, conf.V_MONTEE_MAX) local vMesuree = r.vitesseVerticale(dt) - pidVitesse.borneMin = -vmaxHelice() - pidVitesse.borneMax = vmaxHelice() + -- 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) @@ -287,8 +308,15 @@ function Regulation.nouveau(conf, etat, materiel, Pid, journal) surveiller("tangage", tangage) surveiller("roulis", roulis) - -- mixage (helices en rpm HELICE) + -- 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 @@ -328,6 +356,7 @@ function Regulation.nouveau(conf, etat, materiel, Pid, journal) pidRoulis:raz() consigneRampe = nil altPrec, vFiltre = nil, 0 + vCapteurF, basePrec = nil, nil end function r.razAdaptation()