cctweaked_drone/lib/calibration.lua

--------------------------------------------------------------------
-- lib/calibration.lua : calibrations du drone.
-- Les calibrations assistees utilisent `ui` fourni par drone.lua :
--   ui.inviter(texte)     affiche l'invite sur l'ecran de conduite
--   ui.attendreToucher()  attend un toucher du moniteur (validation)
--
--   gimbal   : association des 2 angles bruts aux axes tangage /
--              roulis + signes, PROPRE A CHAQUE VAISSEAU. Assistee:
--              l'utilisateur incline le drone nez bas puis a gauche.
--   joystick : identification des faces du redstone relay. Assistee.
--   typewriter : apprentissage des codes de touches. Assistee.
--   rsc      : affectation des RSC par impulsions (mode inactif, au
--              sol, EXIGE gimbal calibre).
--   capteurs : axes ET directions des velocity sensors, par
--              mouvements commandes (en vol).
--   pid      : gains d'altitude, zero depassement (en vol).
--------------------------------------------------------------------
local Calibration = {}

local function copierGains(g)
  return { kp = g.kp, ki = g.ki, kd = g.kd }
end

--------------------------------------------------------------------
-- GIMBAL (assistee, drone au sol ou stable)
--------------------------------------------------------------------
function Calibration.gimbal(conf, etat, materiel, journal, Etat, ui)
  local seuil = conf.CALIB.IMPULSION_SEUIL

  ui.inviter("Drone a plat, touchez l'ecran")
  ui.attendreToucher()
  local x0, z0 = materiel.lireAnglesBruts()

  ui.inviter("Inclinez NEZ vers le BAS, touchez")
  ui.attendreToucher()
  local x1, z1 = materiel.lireAnglesBruts()
  local dx, dz = x1 - x0, z1 - z0
  if math.max(math.abs(dx), math.abs(dz)) < seuil then
    journal.alerte("calib gimbal: inclinaison insuffisante, abandon")
    ui.inviter("Echec: inclinaison trop faible")
    return false
  end
  local indexTangage = (math.abs(dx) >= math.abs(dz)) and 1 or 2
  local deltaT = (indexTangage == 1) and dx or dz
  -- convention: nez bas => tangage normalise NEGATIF
  local signeTangage = (deltaT < 0) and 1 or -1

  ui.inviter("Revenez a plat, touchez")
  ui.attendreToucher()
  x0, z0 = materiel.lireAnglesBruts()

  ui.inviter("Penchez a GAUCHE, touchez")
  ui.attendreToucher()
  x1, z1 = materiel.lireAnglesBruts()
  local indexRoulis = (indexTangage == 1) and 2 or 1
  local deltaR = (indexRoulis == 1) and (x1 - x0) or (z1 - z0)
  if math.abs(deltaR) < seuil then
    journal.alerte("calib gimbal: roulis insuffisant, abandon")
    ui.inviter("Echec: roulis trop faible")
    return false
  end
  -- convention: penche a gauche => roulis normalise NEGATIF
  local signeRoulis = (deltaR < 0) and 1 or -1

  etat.gimbal = {
    indexTangage = indexTangage, signeTangage = signeTangage,
    indexRoulis = indexRoulis, signeRoulis = signeRoulis,
  }
  Etat.sauver(etat)
  journal.info(("calib gimbal: tangage=angle%d(x%+d) roulis=angle%d(x%+d)")
    :format(indexTangage, signeTangage, indexRoulis, signeRoulis))
  ui.inviter("Gimbal calibre, remettez a plat")
  return true
end

--------------------------------------------------------------------
-- JOYSTICK (assistee)
--------------------------------------------------------------------
function Calibration.joystick(conf, etat, materiel, journal, Etat, ui)
  if not materiel.relay then
    journal.alerte("calib joystick: pas de redstone_relay")
    return false
  end
  local seuil = conf.CALIB.JOYSTICK_SEUIL
  local mapping = {}

  local function faceActive(exclues)
    for face, valeur in pairs(materiel.lireFacesRelay()) do
      if valeur >= seuil and not exclues[face] then
        return face
      end
    end
    return nil
  end

  local function attendreNeutre()
    while true do
      local actif = false
      for _, v in pairs(materiel.lireFacesRelay()) do
        if v > 0 then actif = true end
      end
      if not actif then return end
      sleep(0.1)
    end
  end

  local exclues = {}
  for _, direction in ipairs({ "devant", "derriere", "gauche", "droite" }) do
    ui.inviter("Joystick a fond: " .. direction:upper())
    local face = nil
    while not face do
      face = faceActive(exclues)
      sleep(0.1)
    end
    mapping[direction] = face
    exclues[face] = true
    journal.info(("calib joystick: %s = %s"):format(direction, face))
    ui.inviter(direction .. " = " .. face .. ", relachez")
    attendreNeutre()
  end

  etat.joystick = mapping
  Etat.sauver(etat)
  ui.inviter("Joystick calibre")
  journal.info("calib joystick: terminee")
  return true
end

--------------------------------------------------------------------
-- TYPEWRITER (assistee): apprentissage des codes de touches
--------------------------------------------------------------------
function Calibration.typewriter(conf, etat, materiel, journal, Etat, ui)
  if not materiel.typewriter then
    journal.alerte("calib typewriter: pas de linked_typewriter")
    return false
  end
  local mapping = {}
  local connus = {}

  local function attendreRelachement()
    while #materiel.lireTouches() > 0 do sleep(0.1) end
  end

  local symboles = { "0", "1", "2", "3", "4", "5", "6", "7", "8", "9",
                     "valider", "effacer" }
  for _, symbole in ipairs(symboles) do
    ui.inviter("Typewriter: appuyez sur " .. symbole:upper())
    local code = nil
    while not code do
      for _, c in ipairs(materiel.lireTouches()) do
        if not connus[c] then code = c end
      end
      sleep(0.1)
    end
    mapping[code] = symbole
    connus[code] = true
    journal.info(("calib typewriter: %s = code %s"):format(symbole,
      tostring(code)))
    attendreRelachement()
  end

  etat.typewriter = mapping
  Etat.sauver(etat)
  ui.inviter("Typewriter calibre")
  journal.info("calib typewriter: terminee")
  return true
end

--------------------------------------------------------------------
-- RSC (mode inactif, au sol, gimbal deja calibre)
--------------------------------------------------------------------
function Calibration.rsc(conf, etat, materiel, journal, Etat, ui)
  if not etat.gimbal then
    journal.alerte("calib rsc: calibrer le GIMBAL d'abord")
    return false
  end
  if not materiel.auSol() then
    journal.alerte("calib rsc: le drone doit etre pose au sol")
    return false
  end
  materiel.toutArreter()
  local C = conf.CALIB

  local nonAffectes = {}
  for nom in pairs(materiel.rsc) do
    if not etat.roles[nom] then table.insert(nonAffectes, nom) end
  end
  table.sort(nonAffectes)
  if #nonAffectes == 0 then
    journal.info("calib rsc: tous les RSC sont deja affectes")
    return true
  end

  local propulseurs = {}
  for _, nom in ipairs(nonAffectes) do
    journal.info("calib rsc: impulsion sur " .. nom)
    ui.inviter("Impulsion: " .. nom)
    local p = materiel.rsc[nom]
    local t0, r0 = materiel.lireAssiette()
    local vitesse, dt_, dr = 0, 0, 0

    while vitesse < conf.VITESSE_RSC_MAX do
      vitesse = math.min(vitesse + C.IMPULSION_PAS, conf.VITESSE_RSC_MAX)
      p.setTargetSpeed(vitesse)
      sleep(0.5)
      local t, r = materiel.lireAssiette()
      dt_, dr = t - t0, r - r0
      if math.abs(dt_) >= C.IMPULSION_SEUIL
        and math.abs(dr) >= C.IMPULSION_SEUIL then
        break
      end
    end
    p.setTargetSpeed(0)
    sleep(1.0)

    if math.abs(dt_) >= C.IMPULSION_SEUIL
      and math.abs(dr) >= C.IMPULSION_SEUIL then
      -- assiette normalisee: tangage>0 = nez haut => helice a l'AVANT
      -- roulis>0 = penche a droite => helice a GAUCHE
      local role = ((dr > 0) and "l" or "r") .. ((dt_ > 0) and "f" or "b")
      etat.roles[nom] = role
      journal.info("  -> helice " .. role)
    else
      table.insert(propulseurs, nom)
      journal.info("  -> pas d'effet d'assiette: propulseur")
    end
  end

  if #propulseurs == 2 then
    etat.roles[propulseurs[1]] = "prop_l"
    etat.roles[propulseurs[2]] = "prop_r"
    journal.alerte("calib rsc: prop_l/prop_r affectes ARBITRAIREMENT: "
      .. "si un virage gauche part a droite, les echanger dans drone.etat")
  elseif #propulseurs > 0 then
    journal.alerte(("calib rsc: %d propulseur(s), 2 attendus")
      :format(#propulseurs))
  end

  Etat.sauver(etat)
  journal.info("calib rsc: terminee (relancer drone pour recharger)")
  ui.inviter("RSC calibres: relancer drone")
  return true
end

--------------------------------------------------------------------
-- VELOCITY SENSORS (en vol): axes ET directions.
-- mouvements = { monter, avancer, gauche } (fonctions bloquantes qui
-- commandent le deplacement pendant que ce module mesure)
--------------------------------------------------------------------
function Calibration.capteurs(conf, etat, materiel, journal, Etat, ui,
                              mouvements)
  local noms = {}
  for nom in pairs(materiel.velocite) do table.insert(noms, nom) end
  table.sort(noms)
  if #noms ~= 3 then
    journal.alerte(("calib capteurs: %d velocity sensors, 3 attendus")
      :format(#noms))
    return false
  end

  -- lance un mouvement en parallele des mesures; retourne, pour le
  -- capteur non exclu a la plus grande amplitude, (nom, moyenne)
  local function mesurerPendant(mouvement, exclus)
    local sommes, maxis, n = {}, {}, 0
    parallel.waitForAll(mouvement, function()
      for _ = 1, 30 do
        for _, nom in ipairs(noms) do
          if not exclus[nom] then
            local v = materiel.velocite[nom].getVelocity() or 0
            sommes[nom] = (sommes[nom] or 0) + v
            maxis[nom] = math.max(maxis[nom] or 0, math.abs(v))
          end
        end
        n = n + 1
        sleep(0.1)
      end
    end)
    local meilleur, amplitude = nil, -1
    for _, nom in ipairs(noms) do
      if not exclus[nom] and (maxis[nom] or 0) > amplitude then
        meilleur, amplitude = nom, maxis[nom]
      end
    end
    return meilleur, (sommes[meilleur] or 0) / math.max(n, 1)
  end

  local function signeDe(moyenne) return (moyenne >= 0) and 1 or -1 end

  ui.inviter("Calib capteurs: montee...")
  local capteur, moyenne = mesurerPendant(mouvements.monter, {})
  etat.velocite[capteur] = { axe = "vertical", signe = signeDe(moyenne) }
  journal.info(("  vertical: %s (x%+d)"):format(capteur, signeDe(moyenne)))
  local exclus = { [capteur] = true }

  ui.inviter("Calib capteurs: avancee...")
  capteur, moyenne = mesurerPendant(mouvements.avancer, exclus)
  etat.velocite[capteur] = { axe = "avant", signe = signeDe(moyenne) }
  journal.info(("  avant: %s (x%+d)"):format(capteur, signeDe(moyenne)))
  exclus[capteur] = true

  ui.inviter("Calib capteurs: translation gauche...")
  capteur, moyenne = mesurerPendant(mouvements.gauche, exclus)
  etat.velocite[capteur] = { axe = "lateral", signe = signeDe(moyenne) }
  journal.info(("  lateral: %s (x%+d)"):format(capteur, signeDe(moyenne)))

  Etat.sauver(etat)
  ui.inviter("Capteurs calibres: relancer drone")
  journal.info("calib capteurs: terminee (relancer drone pour recharger)")
  return true
end

--------------------------------------------------------------------
-- PID D'ALTITUDE (en vol stationnaire, marge d'altitude requise)
--------------------------------------------------------------------
function Calibration.pid(conf, etat, materiel, pilotage, journal, Etat,
                         empreinte)
  local C = conf.CALIB
  local base = materiel.lireAltitude()
  local gains = pilotage.gains()
  local dt = 0.1

  local function mesurer(cible, direction)
    local debut = os.clock()
    local extremum = materiel.lireAltitude()
    local assietteMax = 0
    local dansBande, atteint, tempsReponse = nil, false, nil

    while os.clock() - debut < C.TIMEOUT do
      local alt = pilotage.reguler(cible, { tangage = 0, roulis = 0 },
        0, 0, dt)
      local t, r = materiel.lireAssiette()
      assietteMax = math.max(assietteMax, math.abs(t), math.abs(r))
      if direction > 0 then extremum = math.max(extremum, alt)
      else extremum = math.min(extremum, alt) end

      if math.abs(alt - cible) <= C.BANDE then
        dansBande = dansBande or os.clock()
        if os.clock() - dansBande >= C.DELAI_STABLE then
          atteint, tempsReponse = true, dansBande - debut
          break
        end
      else
        dansBande = nil
      end
      sleep(dt)
    end

    return {
      atteint = atteint,
      tempsReponse = tempsReponse or math.huge,
      depassement = (direction > 0)
        and math.max(0, extremum - cible)
        or math.max(0, cible - extremum),
      assietteMax = assietteMax,
    }
  end

  for _, dir in ipairs({
    { nom = "montee", sens = 1 },
    { nom = "descente", sens = -1 },
  }) do
    local g = gains[dir.nom]
    local meilleurs, meilleurTemps = nil, math.huge

    for iter = 1, C.MAX_ITER do
      journal.info(("[calib %s %d/%d] kp=%.1f ki=%.1f kd=%.1f")
        :format(dir.nom, iter, C.MAX_ITER, g.kp, g.ki, g.kd))
      pilotage.reglerGains(gains)

      local m = mesurer(base + dir.sens * C.AMPLITUDE, dir.sens)
      mesurer(base, -dir.sens)   -- retour a la base

      local valide = m.atteint
        and m.depassement <= C.TOL_DEPASSEMENT
        and m.assietteMax <= conf.ANGLE_MAX
      journal.info(("  t=%.1fs dep=%.2f incl=%.2f %s"):format(
        m.tempsReponse == math.huge and -1 or m.tempsReponse,
        m.depassement, m.assietteMax, valide and "VALIDE" or "rejete"))

      if valide and m.tempsReponse < meilleurTemps then
        meilleurs, meilleurTemps = copierGains(g), m.tempsReponse
      end

      if not m.atteint then
        g.kp = g.kp * 1.3
      elseif m.depassement > C.TOL_DEPASSEMENT then
        g.kd = g.kd * 1.4
        g.kp = g.kp * 0.9
      elseif m.assietteMax > conf.ANGLE_MAX then
        g.kp = g.kp * 0.8
      else
        g.kp = g.kp * 1.25
      end
      g.ki = g.kp * 0.15
    end

    if meilleurs then
      gains[dir.nom] = meilleurs
      journal.info(("calib %s: kp=%.1f ki=%.1f kd=%.1f (t=%.1fs)")
        :format(dir.nom, meilleurs.kp, meilleurs.ki, meilleurs.kd,
          meilleurTemps))
    else
      journal.alerte("calib " .. dir.nom .. ": aucun essai valide")
    end
  end

  pilotage.reglerGains(gains)
  etat.empreintePid = empreinte
  Etat.sauver(etat)

  local fin = os.clock() + C.TIMEOUT
  while os.clock() < fin do
    local alt = pilotage.reguler(base, { tangage = 0, roulis = 0 }, 0, 0, dt)
    if math.abs(alt - base) <= C.BANDE then break end
    sleep(dt)
  end
  journal.info("calib pid: terminee")
  return true
end

return Calibration