cctweaked_drone/drone.lua

--------------------------------------------------------------------
-- drone.lua : programme principal V2.
-- Usage: drone            demarrer
--        drone maj <url>  mettre a jour depuis <url> (Forgejo raw)
-- Modes: off, sol, stationnement, vol, atterrissage.
--------------------------------------------------------------------
local FICHIERS = {
  "drone.lua",
  "lib/conf.lua", "lib/etat.lua", "lib/pid.lua", "lib/materiel.lua",
  "lib/regulation.lua", "lib/calib.lua", "lib/ecran.lua",
  "lib/journal.lua",
}

local args = { ... }
if args[1] == "maj" then
  local base = args[2]
  if not base then print("usage: drone maj <url_base>") return end
  if base:sub(-1) ~= "/" then base = base .. "/" end
  for _, fichier in ipairs(FICHIERS) do
    write("maj " .. fichier .. " ... ")
    local reponse, err = http.get(base .. fichier)
    if not reponse then print("ECHEC: " .. tostring(err)) return end
    local contenu = reponse.readAll()
    reponse.close()
    if fs.exists(fichier) then fs.delete(fichier) end
    local f = fs.open(fichier, "w")
    f.write(contenu)
    f.close()
    print("ok")
  end
  print("mise a jour terminee: relancer drone")
  return
end

local function charger(chemin)
  local f = assert(fs.open(chemin, "r"),
    chemin .. " manquant: drone maj <url>")
  local src = f.readAll()
  f.close()
  return assert(load(src, chemin))()
end

local journal    = charger("lib/journal.lua")
local conf       = charger("lib/conf.lua").charger(journal)
local Etat       = charger("lib/etat.lua")
local etat       = Etat.charger(journal)
local Pid        = charger("lib/pid.lua")
local materiel   = charger("lib/materiel.lua")
  .initialiser(conf, etat, journal)

--------------------------------------------------------------------
-- PARAMETRES EDITABLES (page PARAM); surcharges persistees dans
-- drone.etat, elles PRIMENT sur drone.conf sans l'ecraser
--------------------------------------------------------------------
local PARAMS = {
  { chemin = "ANGLE_MAX", label = "angle max (deg)", pas = 0.25,
    mini = 0.25 },
  { chemin = "DIST_SOL", label = "distance sol", pas = 0.2,
    mini = 0.4 },
  { chemin = "VITESSE_ATTERRISSAGE", label = "v atterrissage (b/s)",
    pas = 0.25, mini = 0.25 },
  { chemin = "V_MONTEE_MAX", label = "v montee max (b/s)",
    pas = 0.5, mini = 0.5 },
  { chemin = "V_DESCENTE_MAX", label = "v descente max (b/s)",
    pas = 0.5, mini = 0.5 },
  { chemin = "RAMPE", label = "rampe consigne (b/s)", pas = 0.5,
    mini = 0.5 },
  { chemin = "ASSIETTE_PLAFOND", label = "autorite assiette",
    pas = 0.05, mini = 0.05, maxi = 1 },
  { chemin = "PID.alt.kp", label = "altitude kp", pas = 0.1,
    mini = 0.1 },
  { chemin = "PID.vitesse.kp", label = "vitesse kp", pas = 0.5,
    mini = 0 },
  { chemin = "PID.vitesse.ki", label = "vitesse ki", pas = 0.5,
    mini = 0 },
  { chemin = "PID.vitesse.kd", label = "vitesse kd", pas = 0.25,
    mini = 0 },
  { chemin = "PID.tangage.kp", label = "tangage kp", pas = 0.25,
    mini = 0 },
  { chemin = "PID.tangage.ki", label = "tangage ki", pas = 0.05,
    mini = 0 },
  { chemin = "PID.tangage.kd", label = "tangage kd", pas = 0.25,
    mini = 0 },
  { chemin = "PID.roulis.kp", label = "roulis kp", pas = 0.25,
    mini = 0 },
  { chemin = "PID.roulis.ki", label = "roulis ki", pas = 0.05,
    mini = 0 },
  { chemin = "PID.roulis.kd", label = "roulis kd", pas = 0.25,
    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)
  local noeud = conf
  for partie in chemin:gmatch("[^%.]+") do
    if type(noeud) ~= "table" then return nil end
    noeud = noeud[partie]
  end
  return noeud
end

local function ecrireChemin(chemin, valeur)
  local noeud, parent, cle = conf, nil, nil
  for partie in chemin:gmatch("[^%.]+") do
    parent, cle = noeud, partie
    noeud = noeud[partie]
  end
  parent[cle] = valeur
end

etat.surcharges = etat.surcharges or {}
for chemin, valeur in pairs(etat.surcharges) do
  ecrireChemin(chemin, valeur)
end

local regulation = charger("lib/regulation.lua")
  .nouveau(conf, etat, materiel, Pid, journal)
local Calib      = charger("lib/calib.lua")
local ecran      = charger("lib/ecran.lua")
  .nouveau(conf, materiel, journal)

-- coherence des versions de modules (desynchronisation de maj)
local function exigerInterface(objet, fichier, fonctions)
  local manquantes = {}
  for _, nom in ipairs(fonctions) do
    if type(objet[nom]) ~= "function" then
      table.insert(manquantes, nom)
    end
  end
  if #manquantes > 0 then
    error(("%s obsolete (manque: %s): resynchroniser avec "
      .. "'drone maj <url>'"):format(fichier,
      table.concat(manquantes, ", ")), 0)
  end
end
exigerInterface(materiel, "lib/materiel.lua", {
  "lireAssiette", "vitesseGlobale", "lireJoystick", "reglerMoteur",
  "rafraichirAffectations", "relaisValides",
})
exigerInterface(regulation, "lib/regulation.lua", {
  "reguler", "plaquer", "rpmHover", "reglerInjection",
  "gainsModifies", "vitesseVerticale", "gelerBoite",
})
exigerInterface(ecran, "lib/ecran.lua", {
  "rafraichirMoniteur", "choisirRole", "message",
})

--------------------------------------------------------------------
-- ETAT DE VOL
--------------------------------------------------------------------
local mode = etat.mode or "off"
local consigneY = etat.consigneY or materiel.lireAltitude()
local enCalibration = false
local calibEnCours, calibProgres, calibTexte = nil, nil, nil
local rscViolet = nil
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
  etat.consigneY = consigneY
  Etat.sauver(etat)
end

--------------------------------------------------------------------
-- VERROUS DE MODES (calibrations requises)
--------------------------------------------------------------------
local function verrouDe(m)
  if m == "off" or m == "sol" then return nil end
  if not etat.gimbal then return "calib gimbal requise" end
  if #materiel.helicesManquantes() > 0 then
    return "calib rsc requise"
  end
  if m == "vol" and not etat.calibAltitude then
    return "calib altitude requise"
  end
  if (m == "vol" or m == "atterrissage" or m == "stationnement")
    and not materiel.relaisValides() then
    return "relays renumerotes: passer en mode sol (reident. auto)"
  end
  if m == "stationnement" then
    if not materiel.auSol() then return "pas au sol" end
    local t, r = materiel.lireAssiette()
    if math.max(math.abs(t), math.abs(r)) > conf.ANGLE_MAX then
      return "pas horizontal"
    end
  end
  return nil
end

local function verrous()
  local v = {}
  for _, m in ipairs({ "off", "sol", "stationnement", "vol",
                       "atterrissage" }) do
    v[m] = verrouDe(m)
  end
  return v
end

--------------------------------------------------------------------
-- MACHINE D'ETATS
--------------------------------------------------------------------
local function changerMode(nouveau, raison)
  if nouveau == mode then return end
  local verrou = verrouDe(nouveau)
  if verrou then
    ecran.message(nouveau .. ": " .. verrou)
    journal.info(("mode %s refuse: %s"):format(nouveau, verrou))
    return
  end
  journal.info(("mode: %s (%s)"):format(nouveau, raison or "?"))
  local etaitAuSol = mode == "off" or mode == "sol"
    or mode == "stationnement"
  mode = nouveau
  regulation.raz()
  if etaitAuSol and (mode == "vol" or mode == "atterrissage") then
    regulation.razAdaptation()
  end
  if mode == "vol" then
    consigneY = materiel.lireAltitude()
  elseif mode == "atterrissage" then
    consigneY = materiel.lireAltitude()
  elseif mode == "off" then
    materiel.toutArreter()
    materiel.reglerMoteur(0)
  else
    materiel.toutArreter()
  end
  if mode ~= "off" and etat.modeMoteur == "onoff" then
    materiel.reglerMoteur(15)
  end
  -- reassertion du signal des boites (rapport fixe, jamais change
  -- en vol; on s'assure qu'il est bien applique avant de voler)
  materiel.appliquerBoite()
  sauverEtat()
end

--------------------------------------------------------------------
-- SURVEILLANCE DES BOITES (vol): la vitesse reelle des helices
-- (speedometers) doit suivre commande * rapport; un ecart > 25%
-- persistant 2 s declenche une alerte (cablage / signal perdu)
--------------------------------------------------------------------
local ecartBoiteDepuis = nil
local alerteBoiteA = 0
local function surveillerBoites()
  if not (etat.boite and etat.speedoParRole) then return end
  local rapport = materiel.rapportBoite()
  local nb, ecarts = 0, 0
  for _, role in ipairs({ "lb", "rb", "lf", "rf" }) do
    local commande = materiel.rpm[role] and materiel.rpm[role].v
    local reel = materiel.lireVitesseHelice(role)
    if commande and reel and math.abs(commande) > 8 then
      local attendu = math.abs(commande) * rapport
      if math.abs(math.abs(reel) - attendu) > attendu * 0.25 then
        ecarts = ecarts + 1
      end
      nb = nb + 1
    end
  end
  if nb > 0 and ecarts >= math.max(1, nb - 1) then
    ecartBoiteDepuis = ecartBoiteDepuis or os.clock()
    if os.clock() - ecartBoiteDepuis > 2
      and os.clock() - alerteBoiteA > 10 then
      alerteBoiteA = os.clock()
      journal.alerte("boites de vitesse incoherentes (vitesse "
        .. "helices != commande * rapport): verifier le signal")
      ecran.message("BOITES INCOHERENTES", 6)
      materiel.appliquerBoite()   -- reassertion du signal
    end
  else
    ecartBoiteDepuis = nil
  end
end

--------------------------------------------------------------------
-- UN PAS DE CONTROLE
--------------------------------------------------------------------
local function pasControle()
  if mode ~= "off" and not materiel.moteurTourne()
    and os.clock() - alerteMoteurA > 5 then
    alerteMoteurA = os.clock()
    journal.erreur("le moteur ne tourne pas alors qu'il le devrait")
    ecran.message("MOTEUR ARRETE", 5)
  end

  if mode == "off" then
    -- tout est deja coupe
  elseif mode == "sol" then
    -- moteur allume, RSC a zero; reidentification AUTOMATIQUE des
    -- relays si les noms stockes ne correspondent plus au reseau
    if not materiel.relaisValides()
      and os.clock() > reidentificationApres then
      reidentificationApres = os.clock() + 30
      journal.alerte("relays renumerotes: reidentification "
        .. "automatique en cours")
      ecran.message("REIDENTIFICATION RELAYS", 5)
      pcall(Calib.reidentifierRelais, conf, etat, materiel, journal,
        Etat)
    end
  elseif mode == "stationnement" then
    -- plaquage; sortie AUTO vers vol si les conditions sont perdues
    if not materiel.auSol() then
      changerMode("vol", "conditions de stationnement perdues")
      return
    end
    local t, r = materiel.lireAssiette()
    if math.max(math.abs(t), math.abs(r)) > conf.ANGLE_MAX * 2 then
      changerMode("vol", "assiette perdue au stationnement")
      return
    end
    regulation.plaquer()
  elseif mode == "vol" then
    local avance, virage = materiel.lireJoystick()
    regulation.reguler(consigneY, avance, virage, DT_REEL)
    surveillerBoites()
  elseif mode == "atterrissage" then
    consigneY = consigneY - conf.VITESSE_ATTERRISSAGE * DT_REEL
    regulation.reguler(consigneY, 0, 0, DT_REEL)
    if materiel.auSol() then
      changerMode("stationnement", "sol atteint")
    end
  end

  -- persistance du rapport de boite (bascules automatiques en vol)
  if regulation.boiteModifiee and regulation.boiteModifiee() then
    Etat.sauver(etat)
  end

  -- persistance des reductions du superviseur d'oscillation
  if regulation.gainsModifies() then
    for _, axe in ipairs({ "vitesse", "tangage", "roulis" }) do
      local g = conf.PID[axe]
      etat.surcharges["PID." .. axe .. ".kp"] = g.kp
      etat.surcharges["PID." .. axe .. ".ki"] = g.ki
      etat.surcharges["PID." .. axe .. ".kd"] = g.kd
    end
    Etat.sauver(etat)
  end
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
end

--------------------------------------------------------------------
-- CARBURANT (autonomie apprise par EMA, mode off exclu)
--------------------------------------------------------------------
local function tacheCarburant()
  local quantitePrec, tPrec, conso = nil, nil, nil
  local alerte = false
  while true do
    local ok, pct, quantite = pcall(materiel.lireCarburant)
    if ok and pct then
      carburantPct = pct
      local t = os.clock()
      if quantitePrec and t > tPrec and mode ~= "off" then
        local instantanee = (quantitePrec - quantite) / (t - tPrec)
        if instantanee >= 0 then
          conso = conso
            and (conso + conf.CARBURANT.LISSAGE
              * (instantanee - conso))
            or instantanee
        end
      end
      quantitePrec, tPrec = quantite, t
      autonomieMin = (conso and conso > 0.001)
        and (quantite / conso / 60) or nil
      if pct < conf.CARBURANT.SEUIL_ALERTE and not alerte then
        alerte = true
        journal.alerte("carburant sous "
          .. math.floor(conf.CARBURANT.SEUIL_ALERTE * 100) .. "%")
        ecran.message("CARBURANT BAS", 10)
      elseif pct > conf.CARBURANT.SEUIL_ALERTE + 0.05 then
        alerte = false
      end
    end
    sleep(conf.CARBURANT.PERIODE)
  end
end

--------------------------------------------------------------------
-- CALIBRATIONS
--------------------------------------------------------------------
local rafraichirCalib = nil     -- affecte apres contexteEcran

local ui = {
  inviter = function(contenu)
    ecran.message(contenu, 3600)
    calibTexte = contenu
    pcall(ecran.rafraichirConduite,
      { mode = mode, consigneY = consigneY })
    if rafraichirCalib then rafraichirCalib() end
    journal.info("[calib] " .. contenu)
  end,
  progres = function(pct, contenu)
    if pct then calibProgres = pct end
    if contenu then calibTexte = contenu end
    if rafraichirCalib then rafraichirCalib() end
  end,
  rscViolet = function(role) rscViolet = role end,
  attendreToucher = ecran.attendreToucher,
  choisirRole = ecran.choisirRole,
}

local function purgerEvenements()
  os.queueEvent("purge_evenements")
  while true do
    if os.pullEvent() == "purge_evenements" then return end
  end
end

local function lancerCalibration(quoi)
  enCalibration = true
  calibEnCours = quoi
  calibProgres, calibTexte = 0, quoi
  ecran.reglerPage("calib")
  -- la boite ne change JAMAIS de rapport pendant une calibration
  regulation.gelerBoite(true)
  local ok, err = pcall(function()
    if quoi == "inclinaison" then
      if mode ~= "vol" then
        journal.alerte("calib inclinaison: passer en mode vol "
          .. "(stationnaire)")
        ecran.message("passer en mode vol")
        return
      end
      Calib.inclinaison(conf, etat, materiel, regulation, journal,
        Etat, ui)
    elseif mode ~= "sol" and mode ~= "stationnement" then
      journal.alerte("calib " .. quoi .. ": passer en mode sol")
      ecran.message("passer en mode sol")
    elseif quoi == "gimbal" then
      Calib.gimbal(conf, etat, materiel, journal, Etat, ui)
    elseif quoi == "joystick" then
      Calib.joystick(conf, etat, materiel, journal, Etat, ui)
    elseif quoi == "rsc" then
      Calib.rsc(conf, etat, materiel, journal, Etat, ui)
    elseif quoi == "boite" then
      Calib.boite(conf, etat, materiel, journal, Etat, ui)
    elseif quoi == "altitude" then
      Calib.altitude(conf, etat, materiel, regulation, journal,
        Etat, ui)
    end
  end)
  if not ok then journal.erreur("calibration: " .. tostring(err)) end
  regulation.gelerBoite(false)
  ecran.message("", 0)
  calibEnCours, rscViolet = nil, nil
  enCalibration = false
  materiel.toutArreter()
  if mode ~= "off" and etat.modeMoteur == "onoff" then
    materiel.reglerMoteur(15)
  end
  purgerEvenements()
end

local function calibStatuts()
  local faits = {
    gimbal = etat.gimbal ~= nil,
    joystick = etat.joystick ~= nil,
    rsc = #materiel.helicesManquantes() == 0,
    boite = etat.boite ~= nil,
    altitude = etat.calibAltitude == true,
    inclinaison = etat.calibInclinaison == true,
  }
  local statuts = {}
  for nom, fait in pairs(faits) do
    if calibEnCours == nom then statuts[nom] = "encours"
    elseif fait then statuts[nom] = "fait" end
  end
  return statuts
end

--------------------------------------------------------------------
-- ECRANS
--------------------------------------------------------------------
local function listeParams()
  local liste = {}
  for _, p in ipairs(PARAMS) do
    table.insert(liste,
      { label = p.label, valeur = lireChemin(p.chemin) })
  end
  return liste
end

local function ajusterParam(idx, sens)
  local p = PARAMS[idx]
  if not p then return end
  local valeur = (lireChemin(p.chemin) or 0) + sens * p.pas
  if p.mini and valeur < p.mini then valeur = p.mini end
  if p.maxi and valeur > p.maxi then valeur = p.maxi end
  valeur = math.floor(valeur * 100 + 0.5) / 100
  ecrireChemin(p.chemin, valeur)
  etat.surcharges[p.chemin] = valeur
  Etat.sauver(etat)
  journal.info(("param %s = %s"):format(p.chemin, tostring(valeur)))
end

local function contexteEcran()
  return {
    mode = mode,
    consigneY = consigneY,
    moteurOk = mode == "off" or materiel.moteurTourne(),
    carburantPct = carburantPct,
    minutes = autonomieMin,
    modeMoteur = etat.modeMoteur,
    verrous = verrous(),
    calibStatuts = calibStatuts(),
    calibProgres = calibProgres,
    calibTexte = calibTexte,
    rscViolet = rscViolet,
    params = listeParams(),
    rapportBoite = etat.boite
      and (tostring(math.floor(etat.boite.rapport * 16)) .. "/16")
      or nil,
  }
end

rafraichirCalib = function()
  pcall(ecran.rafraichirMoniteur, contexteEcran())
end

local function tacheEcran()
  local prochainDessin = 0
  local redessiner = false
  while true do
    if os.clock() >= prochainDessin or redessiner then
      redessiner = false
      pcall(ecran.rafraichirConduite,
        { mode = mode, consigneY = consigneY })
      pcall(ecran.rafraichirCarburant, carburantPct)
      pcall(ecran.rafraichirMoniteur, contexteEcran())
      prochainDessin = os.clock() + 1.0
    end
    local minuteur = os.startTimer(0.25)
    local ev, a, b, c = os.pullEvent()
    if ev == "monitor_touch" and not enCalibration then
      local action = ecran.traiterToucher(b, c)
      if action then
        redessiner = true
        if action.type == "mode" then
          if action.verrou then
            ecran.message(action.valeur .. ": " .. action.verrou)
          else
            changerMode(action.valeur, "moniteur")
          end
        elseif action.type == "delta" then
          consigneY = consigneY + action.valeur
          sauverEtat()
        elseif action.type == "modemoteur" then
          etat.modeMoteur = action.valeur
          Etat.sauver(etat)
          if mode ~= "off" and etat.modeMoteur == "onoff" then
            materiel.reglerMoteur(15)
          end
          journal.info("regime moteur: " .. action.valeur)
        elseif action.type == "calib" then
          lancerCalibration(action.valeur)
        end
      end
    end
    if ev ~= "timer" or a ~= minuteur then
      os.cancelTimer(minuteur)
    end
  end
end

--------------------------------------------------------------------
-- CLAVIER (minimal)
--------------------------------------------------------------------
local function tacheClavier()
  while true do
    write("> ")
    local ligne = read()
    local mots = {}
    for mot in ligne:gmatch("%S+") do table.insert(mots, mot) end
    if mots[1] == "mode" and mots[2] then
      changerMode(mots[2], "clavier")
    elseif mots[1] == "consigne" and tonumber(mots[2]) then
      consigneY = tonumber(mots[2])
      sauverEtat()
    else
      print("mode <off|sol|stationnement|vol|atterrissage> | "
        .. "consigne <y>")
    end
  end
end

--------------------------------------------------------------------
-- DEMARRAGE
--------------------------------------------------------------------
journal.info(("drone V2: demarrage en mode %s (consigne %.0f)")
  :format(mode, consigneY))
if mode == "off" then
  materiel.toutArreter()
  materiel.reglerMoteur(0)
else
  materiel.toutArreter()
  if etat.modeMoteur == "onoff" then materiel.reglerMoteur(15) end
  if mode == "vol" or mode == "atterrissage" then
    consigneY = materiel.lireAltitude()
  end
end

parallel.waitForAny(tacheControle, tacheCarburant, tacheEcran,
  tacheClavier)