ploush/cctweaked_drone
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions
cctweaked_drone/lib/calib.lua
Ploush a84108e4fd fix excess
2026-07-19 23:24:19 +02:00

790 lines
28 KiB
Lua
Raw Blame History

--------------------------------------------------------------------
-- lib/calib.lua : calibrations.
-- gimbal : conventions d'assiette (nez bas, penche a gauche)
-- joystick : faces du redstone relay
-- rsc : procedure MANUELLE (chaque RSC tourne a
-- RPM_MANUEL, toucher sa position sur l'art); mesure
-- au passage le coefficient su/rpm de chaque RSC et
-- la capacite du moteur par niveau (mode adaptatif)
-- altitude : AUTO-AMORCANTE depuis le sol: rampe lente jusqu'au
-- decollage -> rpm de sustentation mesure; detection
-- du velocity sensor VERTICAL; gains d'assiette par
-- defaut dimensionnes sur ce rpm; puis relais
-- d'Astrom-Hagglund sur la boucle de vitesse ->
-- gains Tyreus-Luyben; retour au sol
-- inclinaison : relais par axe (tangage, roulis) en stationnaire
-- Toutes persistees (etat + surcharges page PARAM), re-executables.
--------------------------------------------------------------------
local Calib = {}
--------------------------------------------------------------------
-- OUTILS PARTAGES
--------------------------------------------------------------------
-- persiste des gains PID dans les surcharges (visibles page PARAM)
local function persisterGains(etat, Etat, axe, g)
etat.surcharges = etat.surcharges or {}
etat.surcharges["PID." .. axe .. ".kp"] = g.kp
etat.surcharges["PID." .. axe .. ".ki"] = g.ki
etat.surcharges["PID." .. axe .. ".kd"] = g.kd
Etat.sauver(etat)
end
-- Relais d'Astrom-Hagglund sur une boucle de la regulation.
-- boucle : "vitesse" | "tangage" | "roulis"
-- mesure : function() -> valeur regulee (v verticale ou angle)
-- pas : function() -> un pas de regulation (le relais y est
-- injecte via reglerInjection)
-- d : amplitude du relais (rpm)
-- garde : function(valeur) -> false pour interrompre (securite)
-- Retourne Ku, Tu ou nil, raison ("amplitude" | "oscillation")
local function relais(conf, regulation, boucle, mesure, pas, d, garde)
local croisements, pics = {}, {}
local signePrec, picCourant = nil, 0
local debut = os.clock()
while os.clock() - debut < conf.CALIB.RELAIS_DUREE do
local valeur = mesure()
if not garde(valeur) then
regulation.reglerInjection(boucle, nil)
return nil, "amplitude"
end
picCourant = math.max(picCourant, math.abs(valeur))
local signe = valeur >= 0
if signePrec ~= nil and signe ~= signePrec then
table.insert(croisements, os.clock())
table.insert(pics, picCourant)
picCourant = 0
if #croisements >= 9 then break end
end
signePrec = signe
-- relais oppose a la valeur (meme sens que la correction)
regulation.reglerInjection(boucle, (valeur < 0) and d or -d)
pas()
end
regulation.reglerInjection(boucle, nil)
if #croisements < 6 then return nil, "oscillation" end
-- moyennes sur les cycles etablis (2 premiers ecartes)
local sommeT, n = 0, 0
for i = 4, #croisements - 1, 2 do
sommeT = sommeT + (croisements[i + 1] - croisements[i - 1])
n = n + 1
end
local sommeA = 0
for i = 3, #pics do sommeA = sommeA + pics[i] end
local Tu = sommeT / math.max(n, 1)
local a = sommeA / math.max(#pics - 2, 1)
if a <= 0.02 or Tu <= 0 then return nil, "oscillation" end
return 4 * d / (math.pi * a), Tu
end
-- essais de relais avec auto-ajustement de d, puis Tyreus-Luyben
local function accorder(conf, regulation, journal, ui, boucle,
mesure, pas, d, garde, stabiliser)
local Ku, Tu
for essai = 1, 4 do
ui.progres(nil, ("%s: relais d=%.1f rpm (essai %d)")
:format(boucle, d, essai))
journal.info(("calib %s: relais d=%.1f"):format(boucle, d))
local raison
Ku, Tu = relais(conf, regulation, boucle, mesure, pas, d, garde)
raison = Tu
if Ku then break end
if raison == "amplitude" then d = d / 2 else d = d * 1.7 end
stabiliser()
end
if not Ku then return nil end
journal.info(("calib %s: Ku=%.2f Tu=%.2f"):format(boucle, Ku, Tu))
-- Tyreus-Luyben PID (conservateur, ~3x de marge de gain)
return { kp = Ku / 2.2, ki = (Ku / 2.2) / (2.2 * Tu),
kd = (Ku / 2.2) * Tu / 6.3 }, Tu
end
--------------------------------------------------------------------
-- GIMBAL (au sol): conventions d'assiette
--------------------------------------------------------------------
function Calib.gimbal(conf, etat, materiel, journal, Etat, ui)
ui.progres(0, "mise a plat")
ui.inviter("Drone a plat, touchez l'ecran")
ui.attendreToucher()
local plat1, plat2 = materiel.lireAnglesBruts()
ui.progres(0.25, "nez bas")
ui.inviter("Inclinez NEZ vers le BAS, touchez")
ui.attendreToucher()
local nez1, nez2 = materiel.lireAnglesBruts()
local d1, d2 = nez1 - plat1, nez2 - plat2
local indexTangage = (math.abs(d1) >= math.abs(d2)) and 1 or 2
local deltaTangage = (indexTangage == 1) and d1 or d2
if math.abs(deltaTangage) < 2 then
journal.erreur("calib gimbal: inclinaison insuffisante")
ui.progres(0, "ECHEC: incliner davantage")
return false
end
ui.progres(0.5, "retour a plat")
ui.inviter("Revenez a plat, touchez")
ui.attendreToucher()
plat1, plat2 = materiel.lireAnglesBruts()
ui.progres(0.75, "penche a gauche")
ui.inviter("Penchez a GAUCHE, touchez")
ui.attendreToucher()
local gauche1, gauche2 = materiel.lireAnglesBruts()
local indexRoulis = (indexTangage == 1) and 2 or 1
local deltaRoulis = (indexRoulis == 1) and (gauche1 - plat1)
or (gauche2 - plat2)
if math.abs(deltaRoulis) < 2 then
journal.erreur("calib gimbal: penchement insuffisant")
ui.progres(0, "ECHEC: pencher davantage")
return false
end
-- conventions: nez bas -> tangage NEGATIF, gauche -> roulis NEGATIF
etat.gimbal = {
indexTangage = indexTangage,
signeTangage = (deltaTangage < 0) and 1 or -1,
indexRoulis = indexRoulis,
signeRoulis = (deltaRoulis < 0) and 1 or -1,
}
Etat.sauver(etat)
ui.progres(1, "terminee")
ui.inviter("Gimbal calibre, remettez a plat")
journal.info("calib gimbal: terminee")
return true
end
--------------------------------------------------------------------
-- JOYSTICK (au sol): faces du redstone relay
--------------------------------------------------------------------
function Calib.joystick(conf, etat, materiel, journal, Etat, ui)
local FACES = { "front", "back", "left", "right", "top", "bottom" }
local relais, faces, exclues = nil, {}, {}
for idx, direction in ipairs({ "devant", "derriere", "gauche",
"droite" }) do
ui.progres((idx - 1) / 4, direction)
ui.inviter("Joystick a fond: " .. direction:upper()
.. ", puis touchez")
ui.attendreToucher()
local meilleure, meilleurRelais = nil, nil
local niveau = conf.CALIB.SEUIL_JOYSTICK - 1
for nomRelais in pairs(materiel.relays) do
if not relais or relais == nomRelais then
for _, face in ipairs(FACES) do
if not exclues[face] then
local n = materiel.lireFaceRelayDe(nomRelais, face)
if n > niveau then
meilleure, meilleurRelais, niveau = face, nomRelais, n
end
end
end
end
end
if not meilleure then
journal.erreur("calib joystick: aucune face active pour "
.. direction)
ui.progres(0, "ECHEC " .. direction)
return false
end
relais = meilleurRelais
faces[direction] = meilleure
exclues[meilleure] = true
journal.info(("calib joystick: %s -> %s / %s"):format(direction,
relais, meilleure))
end
etat.joystick = { relais = relais, faces = faces }
Etat.sauver(etat)
ui.progres(1, "terminee")
ui.inviter("Joystick calibre")
journal.info("calib joystick: terminee")
return true
end
--------------------------------------------------------------------
-- RSC (au sol, moteur allume): procedure MANUELLE + coefficients de
-- stress + capacites moteur par niveau (pour le mode adaptatif)
--------------------------------------------------------------------
function Calib.rsc(conf, etat, materiel, journal, Etat, ui)
local anciensRoles = etat.roles
etat.roles = {}
materiel.rafraichirAffectations()
materiel.toutArreter()
materiel.reglerMoteur(15)
sleep(0.5)
local noms = {}
for nom in pairs(materiel.rsc) do table.insert(noms, nom) end
table.sort(noms)
local stressBase = materiel.lireStressBrut() or 0
local coeffs, affectes = {}, {}
for i, nom in ipairs(noms) do
ui.progres((i - 1) / (#noms + 1), nom .. " a "
.. conf.CALIB.RPM_MANUEL .. " rpm")
materiel.rsc[nom].setTargetSpeed(conf.CALIB.RPM_MANUEL)
sleep(0.6)
-- coefficient su/rpm (un seul RSC tourne: mesure propre)
local stress = materiel.lireStressBrut() or 0
coeffs[nom] = math.max(0,
(stress - stressBase) / conf.CALIB.RPM_MANUEL)
-- speedometer associe: le seul qui tourne en ce moment
local speedoActif = nil
for nomSpeedo in pairs(materiel.speedo) do
if math.abs(materiel.lireSpeedo(nomSpeedo) or 0) > 1 then
speedoActif = nomSpeedo
break
end
end
local role = ui.choisirRole(
("%s (%d/%d) tourne a %d rpm"):format(nom, i, #noms,
conf.CALIB.RPM_MANUEL), affectes)
materiel.rsc[nom].setTargetSpeed(0)
if role == nil then
etat.roles = anciensRoles
materiel.rafraichirAffectations()
journal.info("calib rsc: annulee")
ui.progres(0, "annulee")
return false
end
affectes[role] = nom
etat.roles[nom] = role
if speedoActif then
etat.speedoParRole = etat.speedoParRole or {}
etat.speedoParRole[role] = speedoActif
end
ui.rscViolet(role)
journal.info(("calib rsc: %s -> %s (%.1f su/rpm%s)")
:format(nom, role, coeffs[nom],
speedoActif and (", speedo " .. speedoActif) or ""))
sleep(0.3)
end
ui.rscViolet(nil)
-- capacite du moteur a chaque niveau (mode adaptatif)
local capacites = {}
for niveau = 1, 15 do
ui.progres(#noms / (#noms + 1)
+ niveau / 15 / (#noms + 1), ("capacite niveau %d"):format(niveau))
materiel.reglerMoteur(niveau)
sleep(conf.CALIB.ATTENTE_NIVEAU)
local _, capacite = materiel.lireStressBrut()
capacites[niveau] = capacite or 0
end
materiel.reglerMoteur(15)
etat.coeffRsc = coeffs
etat.capaciteParNiveau = capacites
Etat.sauver(etat)
materiel.rafraichirAffectations()
ui.progres(1, "terminee")
journal.info(("calib rsc: terminee (capa max %d su)")
:format(capacites[15] or 0))
return true
end
--------------------------------------------------------------------
-- BOITES DE VITESSE (au sol, rsc requis): association de CHAQUE
-- face du relay a SON helice. Chaque helice tourne a un RSC fixe;
-- la face qui, recevant le signal 13 (rapport 2/16), reduit la
-- vitesse lue par le speedometer de cette helice, est la sienne.
-- Le meme signal sera ensuite ecrit sur les 4 faces (rapport
-- identique sur les 4: symetrie).
--------------------------------------------------------------------
function Calib.boite(conf, etat, materiel, journal, Etat, ui)
if #materiel.helicesManquantes() > 0 then
journal.alerte("calib boite: calibrer les RSC d'abord")
return false
end
local ROLES = { "lb", "rb", "lf", "rf" }
for _, role in ipairs(ROLES) do
if not (etat.speedoParRole and etat.speedoParRole[role]) then
journal.alerte("calib boite: speedometer de " .. role
.. " non associe (refaire la calib rsc avec les "
.. "speedometers en place)")
return false
end
end
local FACES = { "front", "back", "left", "right", "top", "bottom" }
local RPM_TEST = 64
materiel.toutArreter()
materiel.reglerMoteur(15)
-- relays candidats: tous sauf celui du joystick
local relaisJoystick = etat.joystick and etat.joystick.relais
local candidats = {}
for nomRelais in pairs(materiel.relays) do
if nomRelais ~= relaisJoystick then
table.insert(candidats, nomRelais)
end
end
table.sort(candidats)
if #candidats == 0 then
journal.erreur("calib boite: aucun relay disponible (celui du "
.. "joystick est exclu)")
ui.progres(0, "ECHEC: pas de relay")
return false
end
-- association: chaque helice tourne a son tour; la face qui,
-- recevant le signal 13 (rapport 2/16), REDUIT la vitesse lue par
-- le speedometer de CETTE helice, est la sienne
local relaisRetenu, faces = nil, {}
local facesPrises = {}
for iRole, role in ipairs(ROLES) do
ui.progres((iRole - 1) / (#ROLES + 1), role .. " a "
.. RPM_TEST .. " rpm")
materiel.reglerRsc(role, RPM_TEST)
sleep(1.0)
local reference = materiel.lireVitesseHelice(role) or 0
if math.abs(reference) < RPM_TEST * 0.5 then
materiel.toutArreter()
journal.erreur("calib boite: le speedometer de " .. role
.. " ne lit pas la rotation")
ui.progres(0, "ECHEC: speedometer " .. role)
return false
end
local trouvee = nil
for _, nomRelais in ipairs(candidats) do
if (not relaisRetenu or relaisRetenu == nomRelais)
and not trouvee then
for _, face in ipairs(FACES) do
if not facesPrises[face] then
materiel.reglerSignalBoite(nomRelais, face, 13)
sleep(0.8)
local mesure = materiel.lireVitesseHelice(role) or 0
materiel.reglerSignalBoite(nomRelais, face, 0)
sleep(0.4)
local rapport = math.abs(mesure) / math.abs(reference)
if rapport > 0.06 and rapport < 0.25 then -- ~0.125
trouvee = face
relaisRetenu = nomRelais
journal.info(("calib boite: %s -> %s / %s "
.. "(rapport observe %.3f)"):format(role,
nomRelais, face, rapport))
break
end
end
end
end
end
materiel.reglerRsc(role, 0)
if not trouvee then
materiel.toutArreter()
journal.erreur("calib boite: face introuvable pour " .. role
.. " (cablage ?)")
ui.progres(0, "ECHEC: face de " .. role)
return false
end
faces[role] = trouvee
facesPrises[trouvee] = true
end
materiel.toutArreter()
-- rapport 1/1 par defaut; la calib altitude choisira le rapport
etat.boite = { relais = relaisRetenu, faces = faces,
signal = 0, rapport = 1 }
Etat.sauver(etat)
materiel.appliquerBoite()
ui.progres(1, "terminee (rapport choisi par calib altitude)")
journal.info("calib boite: terminee (relay " .. relaisRetenu .. ")")
return true
end
--------------------------------------------------------------------
-- REIDENTIFICATION DES RELAYS (au sol, moteur allume, ~3 s).
-- Les noms des relays changent a la renumerotation du reseau mais
-- les FACES sont stables: l'helice lf tourne a bas regime, le
-- signal 13 est envoye sur SA face de chaque relay candidat, et le
-- speedometer designe le relay des boites; celui du joystick est
-- l'autre. Lancee AUTOMATIQUEMENT en mode sol si les noms stockes
-- ne correspondent plus au reseau.
--------------------------------------------------------------------
function Calib.reidentifierRelais(conf, etat, materiel, journal, Etat)
local b = etat.boite
if not (b and b.faces and b.faces.lf) then return false end
if not (etat.speedoParRole and etat.speedoParRole.lf) then
return false
end
materiel.reglerMoteur(15)
materiel.reglerRsc("lf", conf.CALIB.RPM_MANUEL * b.rapport)
sleep(1.0)
local reference = materiel.lireVitesseHelice("lf") or 0
if math.abs(reference) < 2 then
materiel.reglerRsc("lf", 0)
journal.alerte("reidentification relays: speedometer lf muet")
return false
end
local noms = {}
for nom in pairs(materiel.relays) do table.insert(noms, nom) end
table.sort(noms)
local trouve = nil
for _, nom in ipairs(noms) do
materiel.reglerSignalBoite(nom, b.faces.lf, 13)
sleep(0.8)
local mesure = materiel.lireVitesseHelice("lf") or 0
materiel.reglerSignalBoite(nom, b.faces.lf, 0)
sleep(0.4)
local rapportObserve = math.abs(mesure)
/ math.max(math.abs(reference), 0.1)
if rapportObserve > 0.06 and rapportObserve < 0.5 then
trouve = nom
break
end
end
materiel.reglerRsc("lf", 0)
if not trouve then
journal.erreur("reidentification relays: relay des boites "
.. "introuvable (cablage ?)")
return false
end
b.relais = trouve
-- le joystick est un relay qui n'est pas celui des boites
if etat.joystick then
local autre = nil
local nb = 0
for nom in pairs(materiel.relays) do
if nom ~= trouve then autre, nb = nom, nb + 1 end
end
if nb == 1 then
etat.joystick.relais = autre
elseif not materiel.relays[etat.joystick.relais or ""] then
journal.alerte("reidentification relays: plusieurs candidats "
.. "pour le joystick, refaire sa calibration")
etat.joystick.relais = nil
end
end
Etat.sauver(etat)
materiel.appliquerBoite()
journal.info(("relays reidentifies: boites = %s, joystick = %s")
:format(trouve, etat.joystick and tostring(etat.joystick.relais)
or "n/a"))
return true
end
--------------------------------------------------------------------
-- ALTITUDE (depuis le sol, gimbal + rsc requis): auto-amorcante
--------------------------------------------------------------------
function Calib.altitude(conf, etat, materiel, regulation, journal,
Etat, ui)
if #materiel.helicesManquantes() > 0 then
journal.alerte("calib altitude: calibrer les RSC d'abord")
return false
end
if not etat.gimbal then
journal.alerte("calib altitude: calibrer le gimbal d'abord")
return false
end
if not materiel.auSol() then
journal.alerte("calib altitude: le drone doit etre au sol")
return false
end
local dt = 0.1
regulation.raz()
regulation.razAdaptation()
materiel.reglerMoteur(15)
-- rampe jusqu'au decollage; retourne le rpm RSC de sustentation
local function rampeDecollage(depart)
local distSol0 = materiel.lireDistanceSol()
local rpm = depart or 0
while rpm < conf.VMAX do
rpm = rpm + conf.CALIB.RAMPE_DECOLLAGE * dt
for _, role in ipairs({ "lb", "rb", "lf", "rf" }) do
materiel.reglerRsc(role, rpm)
end
if materiel.lireDistanceSol() > distSol0 + 0.4 then
return rpm
end
sleep(dt)
end
return nil
end
-- Phase 1: rampe a rapport 1/1 -> sustentation cote helices
ui.progres(0.05, "rampe de decollage (rapport 1/1)")
ui.inviter("Calib altitude: decollage automatique")
if etat.boite then
etat.boite.signal, etat.boite.rapport = 0, 1
materiel.appliquerBoite()
sleep(0.3)
end
local rpm = rampeDecollage(0)
if not rpm then
materiel.toutArreter()
journal.erreur("calib altitude: pas de decollage a VMAX "
.. "(poussee insuffisante ?)")
ui.progres(0, "ECHEC: pas de decollage")
return false
end
-- rpm est en unites HELICE (rampe faite a rapport 1/1: RSC =
-- helice). etat.rpmHover est TOUJOURS en unites helice: le
-- regulateur est invariant au rapport de boite.
etat.rpmHover = rpm
etat.pressionHover = materiel.lirePression()
Etat.sauver(etat)
journal.info(("calib altitude: sustentation %.0f rpm helice "
.. "(pression %.3f)"):format(rpm, etat.pressionHover))
-- Phase 1b (boites presentes): retour au sol, choix du rapport
-- INITIAL (vise RSC_CIBLE en sustentation: resolution maximale
-- avec marge), puis re-decollage PAR LA REGULATION (le
-- feedforward est desormais connu, en unites helice). En vol, la
-- boite automatique ajustera ensuite le rapport d'un cran a la
-- fois selon la charge et l'altitude.
if etat.boite then
ui.progres(0.15, "retour au sol (choix du rapport)")
regulation.plaquer()
local fin = os.clock() + 8
while os.clock() < fin and not materiel.auSol() do sleep(0.2) end
sleep(1.0)
materiel.toutArreter()
local k = math.max(1, math.min(16,
math.ceil(16 * rpm / conf.BOITE.RSC_CIBLE)))
if k == 15 then k = 16 end
etat.boite.signal = materiel.signalPourRapportK(k)
etat.boite.rapport = k / 16
materiel.appliquerBoite()
Etat.sauver(etat)
journal.info(("calib altitude: rapport initial %d/16 (signal "
.. "%d), sustentation ~%.0f rpm RSC"):format(k,
etat.boite.signal, rpm * 16 / k))
sleep(0.5)
ui.progres(0.2, ("re-decollage au rapport %d/16"):format(k))
regulation.raz()
local cible = materiel.lireAltitude() + 2
local finDecollage = os.clock() + 15
while os.clock() < finDecollage
and materiel.lireAltitude() < cible - 0.3 do
regulation.reguler(cible, 0, 0, dt)
sleep(dt)
end
if materiel.lireAltitude() < cible - 0.5 then
materiel.toutArreter()
journal.erreur("calib altitude: pas de re-decollage au "
.. "nouveau rapport")
ui.progres(0, "ECHEC: re-decollage")
return false
end
-- verification par speedometer: helice reelle ~ commande
local commande = materiel.rpm.lf and materiel.rpm.lf.v
local reel = materiel.lireVitesseHelice("lf")
if commande and reel then
local attendu = math.abs(commande) * etat.boite.rapport
if math.abs(math.abs(reel) - attendu)
> math.max(3, 0.25 * attendu) then
journal.alerte(("calib altitude: speedometer lf %.0f rpm "
.. "vs attendu %.0f: verifier les boites"):format(reel,
attendu))
end
end
end
local releves = {} -- nom -> somme des vitesses (capteur vertical)
-- 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
if etat.speedoParRole and etat.speedoParRole.lf then
ui.progres(0.28, "mesure du retard d'actionnement")
local stabiliser = os.clock() + 2
local altTenue = materiel.lireAltitude()
while os.clock() < stabiliser do
regulation.reguler(altTenue, 0, 0, dt)
sleep(dt)
end
local v0 = math.abs(materiel.lireVitesseHelice("lf") or 0)
local cibleRpm = v0 * 1.2
regulation.reglerInjection("vitesse", 0.2 * rpm)
local t0, t63 = os.clock(), nil
while os.clock() - t0 < 3 do
regulation.reguler(altTenue, 0, 0, dt)
local v = math.abs(materiel.lireVitesseHelice("lf") or 0)
if not t63 and v >= v0 + 0.632 * (cibleRpm - v0) then
t63 = os.clock() - t0
end
sleep(dt)
end
regulation.reglerInjection("vitesse", nil)
if t63 then
journal.info(("calib altitude: retard d'actionnement ~%.2f s "
.. "(constante 63%%)"):format(t63))
else
journal.alerte("calib altitude: retard d'actionnement > 3 s "
.. "(transition d'helice tres lente ?)")
end
-- redescendre la base tranquillement
local calmer = os.clock() + 2
while os.clock() < calmer do
regulation.reguler(altTenue, 0, 0, dt)
sleep(dt)
end
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")
local altDepart = materiel.lireAltitude()
local consigne = altDepart + 3
local debut = os.clock()
while os.clock() - debut < 8
and materiel.lireAltitude() < consigne - 0.3 do
regulation.reguler(consigne, 0, 0, dt)
for nom in pairs(materiel.velocite) do
releves[nom] = (releves[nom] or 0)
+ materiel.lireVitesseCapteur(nom)
end
sleep(dt)
end
local meilleur, somme = nil, 0.5
for nom, s in pairs(releves) do
if math.abs(s) > math.abs(somme) then meilleur, somme = nom, s end
end
if meilleur then
etat.veloVertical = { nom = meilleur, signe = (somme > 0) and 1 or -1 }
journal.info("calib altitude: capteur vertical = " .. meilleur)
else
etat.veloVertical = nil
journal.alerte("calib altitude: capteur vertical non detecte, "
.. "repli sur la derivee d'altitude")
end
Etat.sauver(etat)
-- Phase 3: relais sur la boucle de vitesse -> gains Tyreus-Luyben
ui.progres(0.5, "relais boucle de vitesse")
local altRef = materiel.lireAltitude()
local d = math.max(2, conf.CALIB.RELAIS_FRACTION * rpm)
local gains = accorder(conf, regulation, journal, ui, "vitesse",
function() return regulation.vitesseVerticale(dt) end,
function()
regulation.reguler(altRef, 0, 0, dt)
sleep(dt)
end,
d,
function()
return math.abs(materiel.lireAltitude() - altRef) < 4
end,
function()
local fin = os.clock() + 4
while os.clock() < fin do
regulation.reguler(altRef, 0, 0, dt)
sleep(dt)
end
end)
if gains then
conf.PID.vitesse.kp = gains.kp
conf.PID.vitesse.ki = gains.ki
conf.PID.vitesse.kd = gains.kd
persisterGains(etat, Etat, "vitesse", gains)
else
journal.alerte("calib altitude: relais non concluant, gains "
.. "par defaut conserves")
end
-- Phase 4: retour au sol en douceur
ui.progres(0.9, "retour au sol")
local fin = os.clock() + 60
local cible = materiel.lireAltitude()
while os.clock() < fin and not materiel.auSol() do
cible = cible - conf.VITESSE_ATTERRISSAGE * dt
regulation.reguler(cible, 0, 0, dt)
sleep(dt)
end
regulation.plaquer()
etat.calibAltitude = true
Etat.sauver(etat)
ui.progres(1, "terminee")
journal.info("calib altitude: terminee")
return true
end
--------------------------------------------------------------------
-- INCLINAISON (en vol stationnaire): relais par axe
--------------------------------------------------------------------
function Calib.inclinaison(conf, etat, materiel, regulation, journal,
Etat, ui)
local dt = 0.1
local altRef = materiel.lireAltitude()
local hover = regulation.rpmHover()
local function pas()
regulation.reguler(altRef, 0, 0, dt)
sleep(dt)
end
-- PRE-VERIFICATION: le relais exige un fond CALME. 3 s d'assiette
-- tenue sous ANGLE_MAX, sinon abandon avec la marche a suivre.
ui.progres(0.02, "verification du calme")
local debut = os.clock()
while os.clock() - debut < 3 do
local t, r2 = materiel.lireAssiette()
if math.max(math.abs(t), math.abs(r2)) > conf.ANGLE_MAX then
journal.erreur("calib inclinaison: assiette instable AVANT le "
.. "relais; reduire tangage/roulis kp (page PARAM) ou "
.. "verifier la calib altitude, puis relancer")
ui.progres(0, "ECHEC: stabiliser d'abord (voir journal)")
return false
end
pas()
end
for iAxe, axe in ipairs({ "tangage", "roulis" }) do
local base01 = (iAxe - 1) * 0.5
ui.progres(base01, axe)
local mesure = function()
local t, r = materiel.lireAssiette()
return (axe == "tangage") and t or r
end
local gains = accorder(conf, regulation, journal, ui, axe,
mesure, pas,
math.max(1, conf.CALIB.RELAIS_FRACTION * hover),
function(valeur) return math.abs(valeur) < conf.ANGLE_MAX * 2 end,
function()
local fin = os.clock() + 3
while os.clock() < fin do pas() end
end)
if not gains then
journal.erreur("calib inclinaison " .. axe .. ": relais "
.. "impossible")
ui.progres(0, "ECHEC " .. axe)
return false
end
conf.PID[axe].kp = gains.kp
conf.PID[axe].ki = gains.ki
conf.PID[axe].kd = gains.kd
persisterGains(etat, Etat, axe, gains)
journal.info(("calib inclinaison %s: kp=%.2f ki=%.3f kd=%.2f")
:format(axe, gains.kp, gains.ki, gains.kd))
end
etat.calibInclinaison = true
Etat.sauver(etat)
ui.progres(1, "terminee")
journal.info("calib inclinaison: terminee")
return true
end
return Calib
Reference in a new issue View git blame Copy permalink