fix oscillation

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)
+    -- base de reference moyennee sur 1 s
-    local cibleRpm = v0 * 1.2
+    local v0, n0 = 0, 0
-    regulation.reglerInjection("vitesse", 0.2 * rpm)
+    local fenetre = os.clock() + 1
-    local t0, t63 = os.clock(), nil
+    while os.clock() < fenetre do
-    while os.clock() - t0 < 3 do
       regulation.reguler(altTenue, 0, 0, dt)
-      local v = math.abs(materiel.lireVitesseHelice("lf") or 0)
+      local v = vitesseMoyenne()
-      if not t63 and v >= v0 + 0.632 * (cibleRpm - v0) then
+      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
+    if math.abs(s) > math.abs(correlation) then
-      second = math.abs(somme)
+      seconde = math.abs(correlation)
-      meilleur, somme = nom, s
+      meilleur, correlation = nom, s
-    elseif math.abs(s) > second then
+    elseif math.abs(s) > seconde then
-      second = math.abs(s)
+      seconde = math.abs(s)
     end
   end
-  if meilleur then
+  if meilleur and math.abs(correlation) > 0.5 then
-    etat.veloVertical = { nom = meilleur, signe = (somme > 0) and 1 or -1 }
+    etat.veloVertical = { nom = meilleur,
-    journal.info("calib altitude: capteur vertical = " .. meilleur)
+                          signe = (correlation > 0) and 1 or -1 }
-    if second > 0.5 * math.abs(somme) then
+    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): "
+        .. "AMBIGUE: refaire la calib, drone bien stable")
-        .. "refaire la calib drone bien immobile")
     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
+  -- PRE-VERIFICATION AUTO-DETENDANTE: le relais exige un fond
-  -- tenue sous ANGLE_MAX, sinon abandon avec la marche a suivre.
+  -- CALME (3 s sous ANGLE_MAX). Si l'assiette oscille, les gains
-  ui.progres(0.02, "verification du calme")
+  -- sont automatiquement reduits de 30%% et l'essai reprend
-  local debut = os.clock()
+  -- (jusqu'a 5 fois): le systeme se stabilise TOUT SEUL avant la
-  while os.clock() - debut < 3 do
+  -- mesure, aucune intervention manuelle.
-    local t, r2 = materiel.lireAssiette()
+  local calme = false
-    if math.max(math.abs(t), math.abs(r2)) > conf.ANGLE_MAX then
+  for tentative = 1, 5 do
-      journal.erreur("calib inclinaison: assiette instable AVANT le "
+    ui.progres(0.02 * tentative,
-        .. "relais; reduire tangage/roulis kp (page PARAM) ou "
+      ("verification du calme (%d/5)"):format(tentative))
-        .. "verifier la calib altitude, puis relancer")
+    local debut = os.clock()
-      ui.progres(0, "ECHEC: stabiliser d'abord (voir journal)")
+    calme = true
-      return false
+    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

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