diff --git a/lib/calib.lua b/lib/calib.lua index 115379f..dc9d38d 100644 --- a/lib/calib.lua +++ b/lib/calib.lua @@ -597,20 +597,41 @@ function Calib.altitude(conf, etat, materiel, regulation, journal, -- 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 + -- MOYENNE des 4 speedometers: les corrections d'assiette + -- s'annulent par symetrie, seule la base (mode commun) reste + local function vitesseMoyenne() + local somme, n = 0, 0 + for _, role in ipairs({ "lb", "rb", "lf", "rf" }) do + local v = materiel.lireVitesseHelice(role) + if v then somme, n = somme + math.abs(v), n + 1 end + end + if n == 0 then return nil end + return somme / n + end local altTenue = materiel.lireAltitude() + local stabiliser = os.clock() + 2 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 + -- base de reference moyennee sur 1 s + local v0, n0 = 0, 0 + local fenetre = os.clock() + 1 + while os.clock() < fenetre 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 + local v = vitesseMoyenne() + if v then v0, n0 = v0 + v, n0 + 1 end + sleep(dt) + end + v0 = (n0 > 0) and (v0 / n0) or 0 + local deltaEchelon = 0.3 * rpm + local cibleRpm = v0 + deltaEchelon + regulation.reglerInjection("vitesse", deltaEchelon) + local t0, t63 = os.clock(), nil + while os.clock() - t0 < 4 do + regulation.reguler(altTenue, 0, 0, dt) + local v = vitesseMoyenne() + if not t63 and v and v >= v0 + 0.632 * deltaEchelon then t63 = os.clock() - t0 end sleep(dt) @@ -624,7 +645,7 @@ function Calib.altitude(conf, etat, materiel, regulation, journal, else etat.retardActionnement = 3.0 Etat.sauver(etat) - journal.alerte("calib altitude: retard d'actionnement > 3 s " + journal.alerte("calib altitude: retard d'actionnement > 4 s " .. "(transition d'helice tres lente ?)") end -- redescendre la base tranquillement @@ -644,7 +665,7 @@ function Calib.altitude(conf, etat, materiel, regulation, journal, 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.ki = 0.02 * g.kp -- quasi nul: integrale + retard = derive g.kd = g.kp * math.min(retard, 1.5) persisterGains(etat, Etat, axe, g) end @@ -655,35 +676,45 @@ function Calib.altitude(conf, etat, materiel, regulation, journal, -- Phase 2: montee reguliere de 3 blocs pour identifier le -- velocity sensor vertical (celui qui suit la montee) + -- le capteur vertical est celui dont la mesure CORRELE avec la + -- derivee de l'altitude barometrique pendant la montee (bien plus + -- discriminant qu'une somme brute: le tangage pollue les + -- capteurs horizontaux) ui.progres(0.3, "detection capteur vertical") local altDepart = materiel.lireAltitude() local consigne = altDepart + 3 local debut = os.clock() - while os.clock() - debut < 8 + local altPrec = materiel.lireAltitude() + while os.clock() - debut < 10 and materiel.lireAltitude() < consigne - 0.3 do regulation.reguler(consigne, 0, 0, dt) + local alt = materiel.lireAltitude() + local vAlti = (alt - altPrec) / dt + altPrec = alt for nom in pairs(materiel.velocite) do releves[nom] = (releves[nom] or 0) - + materiel.lireVitesseCapteur(nom) + + materiel.lireVitesseCapteur(nom) * vAlti end sleep(dt) end - local meilleur, somme, second = nil, 0.5, 0 + local meilleur, correlation, seconde = nil, 0, 0 for nom, s in pairs(releves) do - if math.abs(s) > math.abs(somme) then - second = math.abs(somme) - meilleur, somme = nom, s - elseif math.abs(s) > second then - second = math.abs(s) + if math.abs(s) > math.abs(correlation) then + seconde = math.abs(correlation) + meilleur, correlation = nom, s + elseif math.abs(s) > seconde then + seconde = math.abs(s) end end - if meilleur then - etat.veloVertical = { nom = meilleur, signe = (somme > 0) and 1 or -1 } - journal.info("calib altitude: capteur vertical = " .. meilleur) - if second > 0.5 * math.abs(somme) then + if meilleur and math.abs(correlation) > 0.5 then + etat.veloVertical = { nom = meilleur, + signe = (correlation > 0) and 1 or -1 } + journal.info(("calib altitude: capteur vertical = %s " + .. "(correlation %.0f, seconde %.0f)"):format(meilleur, + math.abs(correlation), seconde)) + if seconde > 0.5 * math.abs(correlation) then journal.alerte("calib altitude: detection du capteur vertical " - .. "AMBIGUE (un autre capteur bouge presque autant): " - .. "refaire la calib drone bien immobile") + .. "AMBIGUE: refaire la calib, drone bien stable") end else etat.veloVertical = nil @@ -755,20 +786,42 @@ 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 + -- PRE-VERIFICATION AUTO-DETENDANTE: le relais exige un fond + -- CALME (3 s sous ANGLE_MAX). Si l'assiette oscille, les gains + -- sont automatiquement reduits de 30%% et l'essai reprend + -- (jusqu'a 5 fois): le systeme se stabilise TOUT SEUL avant la + -- mesure, aucune intervention manuelle. + local calme = false + for tentative = 1, 5 do + ui.progres(0.02 * tentative, + ("verification du calme (%d/5)"):format(tentative)) + local debut = os.clock() + calme = true + while os.clock() - debut < 3 do + local t, r2 = materiel.lireAssiette() + if math.max(math.abs(t), math.abs(r2)) > conf.ANGLE_MAX then + calme = false + break + end + pas() end - pas() + if calme then break end + for _, axe in ipairs({ "tangage", "roulis" }) do + local g = conf.PID[axe] + g.kp, g.ki, g.kd = g.kp * 0.7, g.ki * 0.7, g.kd * 0.7 + persisterGains(etat, Etat, axe, g) + end + journal.alerte(("calib inclinaison: assiette agitee, gains " + .. "reduits automatiquement (kp tangage=%.2f), nouvel essai") + :format(conf.PID.tangage.kp)) + local repos = os.clock() + 4 + while os.clock() < repos do pas() end + end + if not calme then + journal.erreur("calib inclinaison: assiette toujours instable " + .. "apres 5 detentes automatiques; envoyer drone.log") + ui.progres(0, "ECHEC: instable malgre les detentes") + return false end for iAxe, axe in ipairs({ "tangage", "roulis" }) do diff --git a/lib/regulation.lua b/lib/regulation.lua index 6bf3096..8fef4bb 100644 --- a/lib/regulation.lua +++ b/lib/regulation.lua @@ -131,11 +131,13 @@ function Regulation.nouveau(conf, etat, materiel, Pid, journal) s.arme = false end s.signePrec = signe + -- fenetre longue: les actionneurs lents oscillent a des + -- periodes de plusieurs secondes while s.croisements[1] - and maintenant - s.croisements[1] > 6 do + and maintenant - s.croisements[1] > 12 do table.remove(s.croisements, 1) end - if #s.croisements >= 8 then + if #s.croisements >= 6 then s.croisements = {} s.gains.kp = s.gains.kp * 0.8 s.gains.ki = s.gains.ki * 0.8