cctweaked_drone/lib/regulation.lua

--------------------------------------------------------------------
-- lib/regulation.lua : stabilisation et mixage.
--
-- STRUCTURE (concue contre les oscillations constatees en V1):
--   ALTITUDE en cascade: erreur -> vitesse verticale cible (P,
--     bornee) -> PID vitesse -> delta rpm autour du rpm de
--     sustentation MESURE (calib altitude). La vitesse verticale
--     vient du velocity sensor vertical auto-detecte a la calib
--     (repli: derivee filtree de l'altitude).
--   ASSIETTE: PID par axe a derivee filtree, autorite bornee a
--     ASSIETTE_PLAFOND * rpm de sustentation, zone morte reglable.
--   SUPERVISEUR D'OSCILLATION: si une boucle oscille (passages a
--     zero repetes au-dela d'un seuil d'amplitude), ses gains sont
--     reduits automatiquement de 20%% et l'evenement journalise:
--     le systeme converge vers la stabilite meme mal regle.
--   MOTEUR: mode on/off (pleine puissance) ou adaptatif (niveau
--     regle AVANT l'ecriture des RSC en montee, apres en descente).
--------------------------------------------------------------------
local Regulation = {}

-- coefficients de mixage par coin
local SIGNES = {
  lb = { bf = -1, lr =  1 },
  rb = { bf = -1, lr = -1 },
  lf = { bf =  1, lr =  1 },
  rf = { bf =  1, lr = -1 },
}

-- Algebre du signe (demontree en V1): nez bas => tangage mesure < 0
-- => erreur (0 - mesure) > 0 => sortie PID > 0, a APPLIQUER en plus
-- a l'AVANT (bf = +1) pour relever le nez. Signes +1. Un -1 ici
-- AMPLIFIE l'inclinaison au lieu de la corriger.
local SIGNE_TANGAGE, SIGNE_ROULIS = 1, 1

function Regulation.nouveau(conf, etat, materiel, Pid, journal)
  local r = {}
  local VMAX = conf.VMAX

  -- borne HELICE courante: 256 RSC * rapport de boite
  local function vmaxHelice()
    return VMAX * materiel.rapportBoite()
  end

  local function borner(v, mini, maxi)
    if v < mini then return mini elseif v > maxi then return maxi end
    return v
  end

  -- 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)
  -- assiette: derivee filtree FORT (gimbal bruite + pas de 0.1 s)
  local pidTangage = Pid.nouveau(conf.PID.tangage, -VMAX, VMAX, 15,
    0.2)
  local pidRoulis  = Pid.nouveau(conf.PID.roulis,  -VMAX, VMAX, 15,
    0.2)

  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

  ------------------------------------------------------------------
  -- SUSTENTATION: rpm mesure par la calib altitude, corrige par la
  -- pression (la poussee diminue avec l'altitude)
  ------------------------------------------------------------------
  -- rpm HELICE de sustentation (etat.rpmHover est en unites
  -- helice: invariant au rapport de boite)
  function r.rpmHover()
    local base = etat.rpmHover or VMAX * 0.3
    if etat.pressionHover then
      local facteur = etat.pressionHover
        / math.max(materiel.lirePression(), 0.05)
      base = base * facteur
    end
    return borner(base + ffAdapt, 0, vmaxHelice())
  end

  ------------------------------------------------------------------
  -- 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
      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
      if math.abs(brute) <= conf.REGUL.V_ABERRANT then
        vFiltre = vFiltre + 0.3 * (brute - vFiltre)
      end
    end
    altPrec = altitude
    return vFiltre
  end

  ------------------------------------------------------------------
  -- SUPERVISEUR D'OSCILLATION
  -- Une boucle qui traverse zero >= 8 fois en 6 s avec une
  -- amplitude significative oscille: ses gains sont reduits de 20%%
  ------------------------------------------------------------------
  local surveillance = {
    vitesse = { seuil = 0.4, gains = conf.PID.vitesse },
    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
  end

  local function surveiller(nom, valeur)
    local s = surveillance[nom]
    local maintenant = os.clock()
    if math.abs(valeur) > s.seuil then s.arme = true end
    local signe = valeur >= 0
    if s.signePrec ~= nil and signe ~= s.signePrec and s.arme then
      table.insert(s.croisements, maintenant)
      s.arme = false
    end
    s.signePrec = signe
    while s.croisements[1]
      and maintenant - s.croisements[1] > 6 do
      table.remove(s.croisements, 1)
    end
    if #s.croisements >= 8 then
      s.croisements = {}
      s.gains.kp = s.gains.kp * 0.8
      s.gains.ki = s.gains.ki * 0.8
      s.gains.kd = s.gains.kd * 0.85
      gainsModifies = true
      journal.alerte(("oscillation %s: gains reduits (kp=%.2f)")
        :format(nom, s.gains.kp))
    end
  end

  -- le superviseur a-t-il modifie des gains (a persister) ?
  function r.gainsModifies()
    if gainsModifies then gainsModifies = false return true end
    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
  ------------------------------------------------------------------
  -- cibles: helices en rpm HELICE, propulseurs en rpm RSC; les
  -- coefficients (mesures a rapport 1/1) sont en su par rpm helice
  local function besoinPrevu(cibles)
    if not etat.coeffRsc then return nil end
    local besoin = 0
    for role, v in pairs(cibles) do
      local nom = materiel.nomParRole[role]
      local coeff = nom and etat.coeffRsc[nom]
      if coeff then besoin = besoin + coeff * math.abs(v) end
    end
    return besoin
  end

  ------------------------------------------------------------------
  -- BOITE AUTOMATIQUE: maintient la commande RSC des helices dans
  -- la bande [RSC_BAS, RSC_HAUT] en changeant le rapport d'UN cran,
  -- uniquement en quasi-stationnaire et a plat, avec temps mort.
  -- Le regulateur (unites helice) n'y voit RIEN: gains invariants.
  ------------------------------------------------------------------
  local prochainChangement = 0
  local boiteModifiee = false
  local boiteGelee = false

  -- gel de la boite (calibrations: un changement de rapport en
  -- pleine mesure la polluerait)
  function r.gelerBoite(gel)
    boiteGelee = gel and true or false
  end

  local function gererBoite(cibles, calme)
    local b = etat.boite
    if not b or not calme or boiteGelee then return end
    if injection.vitesse or injection.tangage
      or injection.roulis then return end
    if os.clock() < prochainChangement then return end
    local base = (cibles.lb + cibles.rb + cibles.lf + cibles.rf) / 4
    local rsc = base / b.rapport
    local k = math.floor(b.rapport * 16 + 0.5)
    local nouveauK = nil
    if rsc > conf.BOITE.RSC_HAUT and k < 16 then
      nouveauK = (k == 14) and 16 or (k + 1)
    elseif rsc < conf.BOITE.RSC_BAS and k > 1 then
      nouveauK = (k == 16) and 14 or (k - 1)
    end
    if nouveauK then
      local rpmHelices = { lb = cibles.lb, rb = cibles.rb,
                           lf = cibles.lf, rf = cibles.rf }
      if materiel.changerRapport(nouveauK, rpmHelices) then
        prochainChangement = os.clock() + conf.BOITE.ATTENTE
        boiteModifiee = true
        journal.info(("boite: rapport %d/16 (RSC sustentation "
          .. "~%.0f)"):format(nouveauK, base * 16 / nouveauK))
      end
    end
  end

  -- le rapport a-t-il change (etat a persister) ?
  function r.boiteModifiee()
    if boiteModifiee then boiteModifiee = false return true end
    return false
  end

  local function niveauPour(besoin)
    local capacites = etat.capaciteParNiveau
    if not capacites then return 15 end
    local vise = besoin * (1 + conf.MOTEUR.MARGE)
    for n = 1, 15 do
      if (capacites[n] or 0) >= vise then return n end
    end
    return 15
  end

  ------------------------------------------------------------------
  -- APPLICATION DES COMMANDES (moteur anticipe puis RSC)
  ------------------------------------------------------------------
  local function appliquer(cibles)
    if etat.modeMoteur == "adaptatif" then
      local besoin = besoinPrevu(cibles)
      if besoin then
        -- garde de stress: jamais plus que la capacite maximale
        local capacites = etat.capaciteParNiveau
        local capaMax = capacites and capacites[15]
        if capaMax and capaMax > 0
          and besoin > capaMax * conf.MOTEUR.SECURITE then
          local facteur = capaMax * conf.MOTEUR.SECURITE / besoin
          for role, v in pairs(cibles) do
            cibles[role] = v * facteur
          end
          besoin = besoin * facteur
        end
        local niveau = niveauPour(besoin)
        if niveau > materiel.niveauMoteur() then
          -- MONTEE de regime AVANT les RSC (anticipation)
          materiel.reglerMoteur(niveau)
          for role, v in pairs(cibles) do
            materiel.reglerRsc(role, v)
          end
          return
        else
          -- descente: RSC d'abord, moteur ensuite
          for role, v in pairs(cibles) do
            materiel.reglerRsc(role, v)
          end
          materiel.reglerMoteur(niveau)
          return
        end
      end
    end
    for role, v in pairs(cibles) do
      materiel.reglerRsc(role, v)
    end
  end

  ------------------------------------------------------------------
  -- UN PAS DE REGULATION EN VOL
  -- avance / virage: joystick (-1..1), propulseurs differentiels
  ------------------------------------------------------------------
  function r.reguler(consigneY, avance, virage, dt)
    local altitude = materiel.lireAltitude()
    local tangage, roulis = materiel.lireAssiette()
    local hover = r.rpmHover()

    -- rampe de consigne
    if consigneRampe == nil then consigneRampe = altitude end
    local pas = conf.RAMPE * dt
    consigneRampe = consigneRampe
      + borner(consigneY - consigneRampe, -pas, pas)

    -- cascade: erreur -> v cible -> delta rpm
    local vCible = borner(conf.PID.alt.kp * (consigneRampe - altitude),
      -conf.V_DESCENTE_MAX, conf.V_MONTEE_MAX)
    local vMesuree = r.vitesseVerticale(dt)
    -- 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)

    -- adaptation lente: en quasi-stationnaire le residu du PID est
    -- transfere vers la sustentation (charge variable)
    local calme = math.abs(vCible) < 0.2 and math.abs(vMesuree) < 0.3
    if not injection.vitesse and calme then
      ffAdapt = borner(ffAdapt + 0.05 * dt * deltaRpm,
        -vmaxHelice() / 2, vmaxHelice() / 2)
    end

    -- assiette: autorite proportionnelle a la sustentation
    local plafond = math.max(2, conf.ASSIETTE_PLAFOND * hover)
    pidTangage.borneMin, pidTangage.borneMax = -plafond, plafond
    pidRoulis.borneMin, pidRoulis.borneMax = -plafond, plafond
    local corrTangage = injection.tangage or (SIGNE_TANGAGE
      * pidTangage:calculer(0, tangage, dt))
    local corrRoulis = injection.roulis or (SIGNE_ROULIS
      * 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
    -- 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
        + s.lr * corrRoulis, 0, vmaxHelice())
    end
    cibles.prop_r = borner(avance + virage, -1, 1) * VMAX
    cibles.prop_l = borner(avance - virage, -1, 1) * VMAX
    appliquer(cibles)

    -- boite automatique (apres application: bascule sur des cibles
    -- fraiches, uniquement au calme et a plat)
    gererBoite(cibles, calme
      and math.abs(tangage) < conf.ANGLE_MAX / 2
      and math.abs(roulis) < conf.ANGLE_MAX / 2)

    return altitude, consigneRampe
  end

  ------------------------------------------------------------------
  -- MODES SPECIAUX
  ------------------------------------------------------------------
  -- stationnement: helices a pleine puissance INVERSEE (plaquage)
  function r.plaquer()
    local v = vmaxHelice()
    appliquer({ lb = -v, rb = -v, lf = -v, rf = -v,
                prop_l = 0, prop_r = 0 })
  end

  function r.arreter()
    materiel.toutArreter()
    r.raz()
  end

  function r.raz()
    pidVitesse:raz()
    pidTangage:raz()
    pidRoulis:raz()
    consigneRampe = nil
    altPrec, vFiltre = nil, 0
    vCapteurF, basePrec = nil, nil
  end

  function r.razAdaptation()
    ffAdapt = 0
  end

  -- calibrations: forcer une boucle ("vitesse", "tangage",
  -- "roulis") a `valeur` (nil = rendre la main au PID)
  function r.reglerInjection(boucle, valeur)
    injection[boucle] = valeur
  end

  return r
end

return Regulation