fix oscillation

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")

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