diff --git a/drone.lua b/drone.lua index c599fae..3e2fcc3 100644 --- a/drone.lua +++ b/drone.lua @@ -74,6 +74,63 @@ if etat.pid and etat.empreintePid ~= empreinte then etat.pid = nil end +-------------------------------------------------------------------- +-- PARAMETRES EDITABLES (page PARAM du moniteur) +-- Les surcharges sont persistees dans drone.etat et PRIMENT sur +-- drone.conf (sans l'ecraser). +-------------------------------------------------------------------- +local PARAMS = { + { chemin = "CALIB.TOL_DEPASSEMENT", label = "depassement tolere", + pas = 0.1, mini = 0 }, + { chemin = "ANGLE_MAX", label = "angle max (deg)", pas = 0.25, + mini = 0.25 }, + { chemin = "ANGLE_DRONE_MAX", label = "angle mode drone (deg)", + pas = 1, mini = 1 }, + { chemin = "POIDS", label = "poids A VIDE (pN)", pas = 500, + mini = 0 }, + { chemin = "POUSSEE_HELICE_MAX", label = "poussee helice (pN)", + pas = 250, mini = 1 }, + { chemin = "POUSSEE_EXPOSANT", label = "exposant poussee", + pas = 1, mini = 1, maxi = 2 }, + { chemin = "PRESSION_REF", label = "pression de reference", + pas = 0.01, mini = 0.1, maxi = 1.5 }, + { chemin = "RATIO_CHARGE_MAX", label = "ratio charge max", + pas = 0.05, mini = 0.1, maxi = 1 }, + { chemin = "ADAPTATION.SEUIL_DESEQUILIBRE", + label = "seuil desequilibre (rpm)", pas = 5, mini = 5 }, + { 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 = "VITESSE_RAMPE", label = "rampe consigne (b/s)", + pas = 0.5, mini = 0.5 }, + { chemin = "Y_VOL", label = "altitude croisiere", pas = 5 }, + { chemin = "Y_MINI", label = "altitude plancher", pas = 5 }, +} + +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, precedente, cle = conf, nil, nil + for partie in chemin:gmatch("[^%.]+") do + precedente, cle = noeud, partie + noeud = noeud[partie] + end + precedente[cle] = valeur +end + +etat.surcharges = etat.surcharges or {} +for chemin, valeur in pairs(etat.surcharges) do + ecrireChemin(chemin, valeur) +end + local pilotage = charger("lib/pilotage.lua") .nouveau(conf, etat, materiel, Pid, journal) local navigation = charger("lib/navigation.lua") @@ -101,6 +158,7 @@ local calibEnCours = nil local calibProgres, calibTexte = nil, nil local rscViolet = nil local alerteMoteurA = 0 +local alerteChargeA = 0 local boostMoteurFin = 0 local function sauverEtat() @@ -143,6 +201,7 @@ end -------------------------------------------------------------------- local function changerMode(nouveau, raison) if nouveau == mode then return end + local modePrec = mode local verrou = verrouDe(nouveau) if verrou then @@ -166,10 +225,19 @@ local function changerMode(nouveau, raison) materiel.reglerMoteur(15) end + local etaitAuSol = { off = true, inactif = true, calibrage = true, + stationnement = true } mode = nouveau pilotage.raz() navigation.razCap() + if etaitAuSol[modePrec or ""] and (mode == "vol" or mode == "drone" + or mode == "auto" or mode == "atterrissage") then + -- nouveau vol = nouvelle cargaison: on repart de zero + pilotage.razAdaptation() + journal.info("adaptation remise a zero (nouveau vol)") + end + if mode == "vol" or mode == "drone" then consigneY = materiel.lireAltitude() elseif mode == "auto" then @@ -214,6 +282,27 @@ local function pasControle() end end + -- surveillance de la charge (poids estime, equilibrage) + local enVol = mode == "vol" or mode == "drone" or mode == "auto" + or mode == "atterrissage" + if enVol and os.clock() - alerteChargeA > 8 then + if pilotage.poidsEstime() > pilotage.poidsMax() then + alerteChargeA = os.clock() + journal.alerte(("SURCHARGE: poids estime %.0f > max %.0f pN") + :format(pilotage.poidsEstime(), pilotage.poidsMax())) + ihm.message("SURCHARGE POIDS", 6) + else + local tT, tR = pilotage.trim() + if math.max(math.abs(tT), math.abs(tR)) + > conf.ADAPTATION.SEUIL_DESEQUILIBRE then + alerteChargeA = os.clock() + journal.alerte(("chargement desequilibre (trim %.0f/%.0f rpm)") + :format(tT, tR)) + ihm.message("CHARGEMENT DESEQUILIBRE", 6) + end + end + end + if mode == "off" then pilotage.arreter() @@ -348,13 +437,16 @@ local function lancerCalibration(quoi) Calibration.rsc(conf, etat, materiel, journal, Etat, ui) elseif quoi == "rscman" then Calibration.rscManuel(conf, etat, materiel, journal, Etat, ui) - elseif quoi == "capteurs" or quoi == "pid" then + elseif quoi == "capteurs" or quoi == "pid" or quoi == "pidmath" then if mode ~= "vol" then journal.alerte("calib " .. quoi .. ": passer en mode vol d'abord") ihm.message("passer en mode vol") elseif quoi == "pid" then Calibration.pid(conf, etat, materiel, pilotage, journal, Etat, empreinte, ui) + elseif quoi == "pidmath" then + Calibration.mathematique(conf, etat, materiel, pilotage, + journal, Etat, empreinte, ui) else local function tenir(duree, angles, avance) local cible = materiel.lireAltitude() @@ -399,6 +491,7 @@ local function calibStatuts() and #materiel.propulseursManquants() == 0, capteurs = #materiel.axesManquants() == 0, pid = etat.pid ~= nil, + pidmath = etat.pid ~= nil, } local statuts = {} for nom, fait in pairs(faits) do @@ -618,9 +711,37 @@ local function contexteIhm() or ("%d %d"):format(cibleNav.x, cibleNav.z)) or nil, distanceCible = distanceCible, saisieCible = saisieCible, + params = listeParams(), + poidsEstime = pilotage.poidsEstime(), + poidsMax = pilotage.poidsMax(), + trimTangage = select(1, pilotage.trim()), + trimRoulis = select(2, pilotage.trim()), } end +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 + ecrireChemin(p.chemin, valeur) + etat.surcharges[p.chemin] = valeur + Etat.sauver(etat) + journal.info(("param %s = %s"):format(p.chemin, tostring(valeur))) + if etat.pid then + ihm.message("param modifie: recalibrer pid conseille") + end +end + -- lance la cible saisie: "x z" ou "x y z" local function lancerCible(texte) local nombres = {} @@ -674,6 +795,8 @@ local function tacheIhm() end elseif action.type == "annuler" then changerMode("vol", "annulation auto") + elseif action.type == "param" then + ajusterParam(action.idx, action.sens) elseif action.type == "pave" then if action.valeur == "eff" then saisieCible = saisieCible:sub(1, -2) diff --git a/lib/calibration.lua b/lib/calibration.lua index 6cb3e3f..2ea6127 100644 --- a/lib/calibration.lua +++ b/lib/calibration.lua @@ -415,7 +415,180 @@ function Calibration.capteurs(conf, etat, materiel, journal, Etat, ui, end -------------------------------------------------------------------- --- PID D'ALTITUDE (en vol stationnaire, marge d'altitude requise) +-- PID MATHEMATIQUE (en vol stationnaire, marge d'altitude requise) +-- 1. Identification en boucle ouverte autour du feedforward: un +-- echelon de +deltaRpm est applique, la reponse en vitesse +-- verticale donne le modele FOPDT (gain K, constante de temps +-- tau, retard L). +-- 2. Gains candidats calcules par COHEN-COON et ZIEGLER-NICHOLS +-- (reponse indicielle), pour la montee puis la descente +-- (echelon negatif). +-- 3. Validation en boucle fermee de chaque candidat (memes criteres +-- que l'auto-tune: zero depassement, assiette bornee): le +-- meilleur temps de reponse VALIDE est conserve. +-------------------------------------------------------------------- +local function identifierFOPDT(pilotage, deltaRpm, dt, duree, journal) + -- stabiliser au feedforward pur + for _ = 1, math.floor(1.5 / dt) do + pilotage.pasOuvert(0, dt) + sleep(dt) + end + -- echelon et enregistrement + local temps, vitesses = {}, {} + local debut = os.clock() + while os.clock() - debut < duree do + local v = pilotage.pasOuvert(deltaRpm, dt) + table.insert(temps, os.clock() - debut) + table.insert(vitesses, v) + sleep(dt) + end + pilotage.pasOuvert(0, dt) + + -- valeur finale: moyenne du dernier quart + local somme, n = 0, 0 + for i = math.floor(#vitesses * 0.75), #vitesses do + somme, n = somme + vitesses[i], n + 1 + end + local vFinale = somme / math.max(n, 1) + if math.abs(vFinale) < 0.05 then + journal.alerte("pidmath: reponse trop faible, augmenter l'echelon") + return nil + end + + local K = vFinale / deltaRpm + -- retard L: premier passage a 5% de vFinale; tau: 63.2% - L + local L, t63 = nil, nil + for i, v in ipairs(vitesses) do + local ratio = v / vFinale + if not L and ratio >= 0.05 then L = temps[i] end + if not t63 and ratio >= 0.632 then t63 = temps[i] end + end + if not L or not t63 or t63 <= L then + journal.alerte("pidmath: identification impossible (bruit ?)") + return nil + end + local tau = t63 - L + L = math.max(L, dt) -- retard minimal: une periode + journal.info(("pidmath: K=%.4f tau=%.2f L=%.2f"):format(K, tau, L)) + return K, tau, L +end + +local function gainsCohenCoon(K, tau, L) + local r = L / tau + local kp = (1 / K) * (tau / L) * (4 / 3 + r / 4) + local Ti = L * (32 + 6 * r) / (13 + 8 * r) + local Td = L * 4 / (11 + 2 * r) + return { kp = kp, ki = kp / Ti, kd = kp * Td } +end + +local function gainsZieglerNichols(K, tau, L) + local kp = 1.2 * tau / (K * L) + return { kp = kp, ki = kp / (2 * L), kd = kp * 0.5 * L } +end + +function Calibration.mathematique(conf, etat, materiel, pilotage, + journal, Etat, empreinte, ui) + local C = conf.CALIB + local dt = 0.1 + local base = materiel.lireAltitude() + local gains = pilotage.gains() + local deltaRpm = math.max(8, conf.VITESSE_RSC_MAX * 0.05) + + -- meme critere de validation que l'auto-tune + local function valider(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 + local depassement = (direction > 0) + and math.max(0, extremum - cible) + or math.max(0, cible - extremum) + local valide = atteint and depassement <= C.TOL_DEPASSEMENT + and assietteMax <= conf.ANGLE_MAX + return valide, tempsReponse or math.huge, depassement + end + + for iDir, dir in ipairs({ + { nom = "montee", sens = 1 }, + { nom = "descente", sens = -1 }, + }) do + ui.progres((iDir - 1) / 2, dir.nom .. ": identification") + journal.info("pidmath: identification " .. dir.nom) + local K, tau, L = identifierFOPDT(pilotage, dir.sens * deltaRpm, + dt, 8, journal) + if K then + local candidats = { + { nom = "cohen-coon", g = gainsCohenCoon(math.abs(K), tau, L) }, + { nom = "ziegler-nichols", + g = gainsZieglerNichols(math.abs(K), tau, L) }, + } + local meilleur, meilleurTemps = nil, math.huge + for iC, c in ipairs(candidats) do + ui.progres((iDir - 1) / 2 + iC * 0.2, dir.nom .. ": " .. c.nom) + journal.info(("pidmath %s %s: kp=%.1f ki=%.1f kd=%.1f") + :format(dir.nom, c.nom, c.g.kp, c.g.ki, c.g.kd)) + gains[dir.nom] = c.g + pilotage.reglerGains(gains) + -- retour a la base puis echelon de validation + valider(base, -dir.sens) + local valide, temps, dep = valider( + base + dir.sens * C.AMPLITUDE, dir.sens) + valider(base, -dir.sens) + journal.info((" %s t=%.1f dep=%.2f"):format( + valide and "VALIDE" or "rejete", + temps == math.huge and -1 or temps, dep)) + if valide and temps < meilleurTemps then + meilleur, meilleurTemps = c, temps + end + end + if meilleur then + gains[dir.nom] = meilleur.g + journal.info(("pidmath %s: retenu %s"):format(dir.nom, + meilleur.nom)) + else + -- aucun candidat brut valide: version adoucie de cohen-coon + local g = gainsCohenCoon(math.abs(K), tau, L) + g.kp, g.ki, g.kd = g.kp * 0.6, g.ki * 0.6, g.kd * 0.6 + gains[dir.nom] = g + journal.alerte("pidmath " .. dir.nom + .. ": candidats rejetes, version adoucie appliquee") + end + pilotage.reglerGains(gains) + end + end + + etat.empreintePid = empreinte + etat.poidsCalibration = pilotage.poidsEstime() + journal.info(("pidmath: poids de calibration %.0f pN") + :format(etat.poidsCalibration)) + Etat.sauver(etat) + valider(base, 1) + ui.progres(1, "terminee") + journal.info("pidmath: terminee") + return true +end + +-------------------------------------------------------------------- +-- PID AUTO-TUNE (en vol stationnaire, marge d'altitude requise) +-- Recherche iterative sur la boucle de vitesse verticale. -------------------------------------------------------------------- function Calibration.pid(conf, etat, materiel, pilotage, journal, Etat, empreinte, ui) @@ -514,6 +687,9 @@ function Calibration.pid(conf, etat, materiel, pilotage, journal, Etat, pilotage.reglerGains(gains) etat.empreintePid = empreinte + etat.poidsCalibration = pilotage.poidsEstime() + journal.info(("calib pid: poids de calibration %.0f pN") + :format(etat.poidsCalibration)) Etat.sauver(etat) local fin = os.clock() + C.TIMEOUT diff --git a/lib/conf.lua b/lib/conf.lua index a34b3dd..74a9d47 100644 --- a/lib/conf.lua +++ b/lib/conf.lua @@ -48,13 +48,40 @@ return { OFFSET_Z = 0, ------------------------------------------------------------------ - -- Regulation + -- Regulation (cascade altitude -> vitesse verticale -> rpm) ------------------------------------------------------------------ VITESSE_RAMPE = 3.0, -- rampe de consigne d'altitude (blocs/s) VITESSE_RSC_MAX = 256, -- borne des rotation speed controllers + V_MONTEE_MAX = 3.0, -- vitesse verticale max en montee (b/s) + V_DESCENTE_MAX = 2.0, -- vitesse verticale max en descente (b/s) + + -- Feedforward de sustentation: le rpm d'equilibre est calcule + -- depuis le poids et la poussee, le PID ne corrige qu'autour. + -- Transport a charge variable: POIDS est le poids A VIDE; la + -- charge est estimee en vol (adaptation) et la poussee est + -- corrigee par la pression mesuree (elle diminue avec l'altitude). + POIDS = 32000, -- poids A VIDE du drone (pixelNewton) + POUSSEE_HELICE_MAX = 10000, -- poussee d'UNE helice a + -- VITESSE_RSC_MAX et PRESSION_REF (pN) + POUSSEE_EXPOSANT = 1, -- poussee ~ rpm^exp (1 lineaire, 2 carre) + PRESSION_REF = 1.0, -- pression a laquelle POUSSEE_HELICE_MAX + -- est mesuree (getAirPressure) + RATIO_CHARGE_MAX = 0.60, -- poids total max = ratio * 4 * poussee + -- de reference (alerte SURCHARGE au dela) + + -- Adaptation en vol (le drone se pese et se trime en stationnaire) + ADAPTATION = { + TAUX_POIDS = 0.05, -- vitesse de transfert vers le + -- feedforward (1/s); petit = stable + TAUX_TRIM = 0.05, -- idem pour le trim d'assiette + SEUIL_DESEQUILIBRE = 15, -- |trim| (rpm) declenchant l'alerte + -- "chargement desequilibre" + }, + PID = { -- gains par defaut (remplaces par calib) - montee = { kp = 12, ki = 2.0, kd = 18 }, - descente = { kp = 10, ki = 1.5, kd = 22 }, + alt = { kp = 1.2 }, -- erreur alt -> v cible (1/s) + montee = { kp = 40, ki = 60, kd = 5 }, -- v -> delta rpm + descente = { kp = 40, ki = 60, kd = 5 }, tangage = { kp = 4, ki = 0.5, kd = 8 }, roulis = { kp = 4, ki = 0.5, kd = 8 }, }, @@ -150,6 +177,11 @@ function Conf.empreintePid(conf) vmax = conf.VITESSE_RSC_MAX, rampe = conf.VITESSE_RAMPE, angle = conf.ANGLE_MAX, + poids = conf.POIDS, + poussee = conf.POUSSEE_HELICE_MAX, + exposant = conf.POUSSEE_EXPOSANT, + vmont = conf.V_MONTEE_MAX, + vdesc = conf.V_DESCENTE_MAX, }) end diff --git a/lib/ihm.lua b/lib/ihm.lua index 130abe7..7ee0604 100644 --- a/lib/ihm.lua +++ b/lib/ihm.lua @@ -222,6 +222,15 @@ function Ihm.nouveau(conf, materiel, journal) materiel.vitesseHorizontale(), materiel.lireAltitude(), ctx.consigneY or 0)) y = y + 1 + local surcharge = ctx.poidsEstime + and ctx.poidsMax and ctx.poidsEstime > ctx.poidsMax + texte(1, y, ("poids est. %s / max %s trim %s/%s"):format( + ctx.poidsEstime and ("%.0f"):format(ctx.poidsEstime) or "?", + ctx.poidsMax and ("%.0f"):format(ctx.poidsMax) or "?", + ctx.trimTangage and ("%+.0f"):format(ctx.trimTangage) or "?", + ctx.trimRoulis and ("%+.0f"):format(ctx.trimRoulis) or "?"), + surcharge and colors.red or colors.white) + y = y + 1 bouton(1, y, "-10", { type = "delta", valeur = -10 }) bouton(7, y, "-1", { type = "delta", valeur = -1 }) bouton(12, y, "+1", { type = "delta", valeur = 1 }) @@ -276,7 +285,7 @@ function Ihm.nouveau(conf, materiel, journal) colors.lightGray) local x, y = 1, 3 for _, c in ipairs({ "gimbal", "joystick", "typew", "rsc", - "rscman", "capteurs", "pid" }) do + "rscman", "capteurs", "pid", "pidmath" }) do local statut = ctx.calibStatuts[c] local couleur = colors.gray if statut == "encours" then couleur = colors.yellow @@ -310,7 +319,24 @@ function Ihm.nouveau(conf, materiel, journal) texte(1, H - 1, "sol: gimbal joystick typew rsc rscman (mode calibrage)", colors.lightGray) - texte(1, H, "vol: capteurs pid (mode vol)", colors.lightGray) + texte(1, H, "vol: capteurs pid pidmath (mode vol)", + colors.lightGray) + end + + ------------------------------------------------------------------ + -- PAGE PARAMS (edition tactile; surcharges dans drone.etat) + ------------------------------------------------------------------ + local function dessinerPageParams(ctx) + texte(1, 2, "surcharges sauvees dans drone.etat", colors.lightGray) + local y = 3 + for i2, p in ipairs(ctx.params or {}) do + texte(1, y, ("%-24s"):format(p.label)) + texte(26, y, tostring(p.valeur), colors.yellow) + bouton(L - 9, y, "-", { type = "param", idx = i2, sens = -1 }) + bouton(L - 4, y, "+", { type = "param", idx = i2, sens = 1 }) + y = y + 1 + if y > H then break end + end end ------------------------------------------------------------------ @@ -334,9 +360,10 @@ function Ihm.nouveau(conf, materiel, journal) moniteur.clear() -- barre d'onglets - local onglets = { mode = "MODE", calib = "CALIB", journal = "JOURNAL" } + local onglets = { mode = "MODE", calib = "CALIB", + params = "PARAM", journal = "JOURNAL" } local x = 1 - for _, id in ipairs({ "mode", "calib", "journal" }) do + for _, id in ipairs({ "mode", "calib", "params", "journal" }) do bouton(x, 1, onglets[id], { type = "page", valeur = id }, page == id and colors.green or colors.gray) x = x + #onglets[id] + 3 @@ -345,6 +372,7 @@ function Ihm.nouveau(conf, materiel, journal) if page == "mode" then dessinerPageMode(ctx) elseif page == "calib" then dessinerPageCalib(ctx) + elseif page == "params" then dessinerPageParams(ctx) else dessinerPageJournal() end end diff --git a/lib/pilotage.lua b/lib/pilotage.lua index cfd2892..439f948 100644 --- a/lib/pilotage.lua +++ b/lib/pilotage.lua @@ -1,14 +1,17 @@ -------------------------------------------------------------------- -- lib/pilotage.lua : regulation et mixage. -- --- base = PID d'altitude (double regime montee / descente) --- assiette = PID tangage + PID roulis vers des angles cibles --- (0 sauf en mode drone ou le joystick les commande) --- helice(coin) = base + signeBF*corrTangage + signeLR*corrRoulis --- propulseurs = mixage differentiel avance / virage --- --- La consigne d'altitude passe par une rampe (VITESSE_RAMPE) pour --- garantir des transitions douces et sans depassement. +-- ASSERVISSEMENT EN CASCADE + FEEDFORWARD : +-- 1. feedforward : rpm d'equilibre (sustentation) calcule depuis +-- POIDS et POUSSEE_HELICE_MAX; le PID ne corrige qu'autour de +-- ce point au lieu de devoir le trouver par integrale +-- 2. boucle externe : erreur d'altitude -> consigne de vitesse +-- verticale (P simple, bornee V_MONTEE_MAX / V_DESCENTE_MAX) +-- 3. boucle interne : PID sur la vitesse verticale MESUREE +-- (velocity sensor vertical calibre; repli: derivee filtree de +-- l'altitude) -> delta rpm autour du feedforward +-- 4. assiette : PID tangage + roulis, corrections signees par coin +-- 5. propulseurs : mixage differentiel avance / virage -------------------------------------------------------------------- local Pilotage = {} @@ -19,10 +22,9 @@ local SIGNES = { rf = { bf = 1, lr = -1 }, } --- materiel.lireAssiette() est NORMALISE par la calibration gimbal: --- tangage negatif = nez bas, roulis negatif = penche a gauche. --- Une helice AVANT pousse plus quand le tangage est negatif, une --- helice GAUCHE pousse plus quand le roulis est negatif => signes -1 +-- assiette normalisee: une helice AVANT pousse plus quand le tangage +-- est negatif (nez bas), une helice GAUCHE quand le roulis est +-- negatif (penche a gauche) local SIGNE_TANGAGE, SIGNE_ROULIS = -1, -1 function Pilotage.nouveau(conf, etat, materiel, Pid, journal) @@ -30,25 +32,80 @@ function Pilotage.nouveau(conf, etat, materiel, Pid, journal) local VMAX = conf.VITESSE_RSC_MAX local gains = etat.pid or conf.PID - local pidMontee = Pid.nouveau(gains.montee, 0, VMAX) - local pidDescente = Pid.nouveau(gains.descente, 0, VMAX) + local pidMontee = Pid.nouveau(gains.montee, -VMAX, VMAX, 20) + local pidDescente = Pid.nouveau(gains.descente, -VMAX, VMAX, 20) local pidTangage = Pid.nouveau(gains.tangage, -VMAX / 2, VMAX / 2) local pidRoulis = Pid.nouveau(gains.roulis, -VMAX / 2, VMAX / 2) local consigneRampe = nil local regimePrec = nil - local facteurProp = 1.0 -- reduit en cas de surcharge du moteur + local facteurProp = 1.0 + local altPrec, vFiltre = nil, 0 -- repli de mesure de vitesse + + -- ADAPTATION (transport a charge variable, remise a zero au + -- decollage via razAdaptation) + local rpmAdapt = 0 -- correction adaptative du ff + local trimTangage, trimRoulis = 0, 0 -- trim d'assiette adaptatif + + ------------------------------------------------------------------ + -- FEEDFORWARD : rpm d'equilibre d'UNE helice + -- poussee(rpm) = pousseeEffective * (rpm/VMAX)^exp, avec la + -- poussee CORRIGEE PAR LA PRESSION (elle diminue avec l'altitude): + -- pousseeEffective = POUSSEE_HELICE_MAX * pression / PRESSION_REF + -- equilibre: 4 * poussee(rpm) = POIDS (a vide) + charge estimee + ------------------------------------------------------------------ + local function pousseeEffective() + local facteur = materiel.lirePression() / conf.PRESSION_REF + return conf.POUSSEE_HELICE_MAX * math.max(facteur, 0.05) + end + + function p.rpmSustentation() + local ratio = (conf.POIDS / 4) / pousseeEffective() + if ratio <= 0 then return 0 end + if ratio >= 1 then return VMAX end + return VMAX * ratio ^ (1 / conf.POUSSEE_EXPOSANT) + end + + -- poids total estime (a vide + charge), inverse de la courbe de + -- poussee au rpm de sustentation adapte, a la pression courante + function p.poidsEstime() + local rpm = math.max(0, math.min(VMAX, p.rpmSustentation() + rpmAdapt)) + return 4 * pousseeEffective() * (rpm / VMAX) ^ conf.POUSSEE_EXPOSANT + end + + -- plafond de poids total (regle des 60% a la pression de reference) + function p.poidsMax() + return conf.RATIO_CHARGE_MAX * 4 * conf.POUSSEE_HELICE_MAX + end + + function p.trim() + return trimTangage, trimRoulis + end + + function p.razAdaptation() + rpmAdapt, trimTangage, trimRoulis = 0, 0, 0 + end + + ------------------------------------------------------------------ + -- MESURE DE VITESSE VERTICALE + -- capteur calibre si present, sinon derivee filtree de l'altitude + ------------------------------------------------------------------ + function p.vitesseVerticale(altitude, dt) + if materiel.veloParAxe["vertical"] then + return materiel.lireVitesseAxe("vertical") + end + if altPrec and dt > 0 then + local brute = (altitude - altPrec) / dt + vFiltre = vFiltre + 0.3 * (brute - vFiltre) + end + altPrec = altitude + return vFiltre + end function p.gains() return gains end - -- 0..1 : applique aux propulseurs (priorite de delestage en - -- cas de surcharge du reseau de stress) - function p.reglerFacteurProp(f) - facteurProp = math.max(0, math.min(1, f)) - end - function p.reglerGains(nouveaux) gains = nouveaux etat.pid = nouveaux @@ -58,6 +115,10 @@ function Pilotage.nouveau(conf, etat, materiel, Pid, journal) pidRoulis:reglerGains(gains.roulis) end + function p.reglerFacteurProp(f) + facteurProp = math.max(0, math.min(1, f)) + end + function p.raz() pidMontee:raz() pidDescente:raz() @@ -65,12 +126,39 @@ function Pilotage.nouveau(conf, etat, materiel, Pid, journal) pidRoulis:raz() consigneRampe = nil regimePrec = nil + altPrec, vFiltre = nil, 0 end - -- Un pas de regulation en vol. - -- consigneY : altitude cible - -- angles : { tangage = deg, roulis = deg } cibles - -- avance, virage : commande propulseurs dans -1..1 + local function borner(v, mini, maxi) + if v < mini then return mini elseif v > maxi then return maxi end + return v + end + + -- applique base + trim + corrections d'assiette aux 4 helices. + -- `adapter` (booleen): en quasi-stationnaire, le trim absorbe + -- lentement la composante statique des corrections (CG decale par + -- la cargaison); le PID reste centre sur la dynamique. + local function appliquerHelices(base, tangage, roulis, dt, + anglesCibles, adapter) + local corrTangage = SIGNE_TANGAGE + * pidTangage:calculer(anglesCibles.tangage or 0, tangage, dt) + local corrRoulis = SIGNE_ROULIS + * pidRoulis:calculer(anglesCibles.roulis or 0, roulis, dt) + if adapter then + local taux = conf.ADAPTATION.TAUX_TRIM * dt + trimTangage = trimTangage + taux * corrTangage + trimRoulis = trimRoulis + taux * corrRoulis + end + for role, s in pairs(SIGNES) do + materiel.reglerRsc(role, borner(base + + s.bf * (trimTangage + corrTangage) + + s.lr * (trimRoulis + corrRoulis), 0, VMAX)) + end + end + + ------------------------------------------------------------------ + -- UN PAS DE REGULATION EN VOL + ------------------------------------------------------------------ function p.reguler(consigneY, angles, avance, virage, dt) local altitude = materiel.lireAltitude() local tangage, roulis = materiel.lireAssiette() @@ -78,44 +166,69 @@ function Pilotage.nouveau(conf, etat, materiel, Pid, journal) -- rampe de consigne if consigneRampe == nil then consigneRampe = altitude end local pas = conf.VITESSE_RAMPE * dt - local ecart = consigneY - consigneRampe - if ecart > pas then ecart = pas elseif ecart < -pas then ecart = -pas end - consigneRampe = consigneRampe + ecart + consigneRampe = consigneRampe + + borner(consigneY - consigneRampe, -pas, pas) - -- double regime : montee ou descente - local montee = (consigneRampe - altitude) >= 0 + -- boucle externe: erreur alt -> vitesse verticale cible + local vCible = borner(gains.alt.kp * (consigneRampe - altitude), + -conf.V_DESCENTE_MAX, conf.V_MONTEE_MAX) + + -- boucle interne: vitesse verticale -> delta rpm (double regime) + local vMesuree = p.vitesseVerticale(altitude, dt) + local montee = vCible >= 0 local pid = montee and pidMontee or pidDescente - if regimePrec ~= nil and regimePrec ~= montee then - pid:raz() - end + if regimePrec ~= nil and regimePrec ~= montee then pid:raz() end regimePrec = montee - local base = pid:calculer(consigneRampe, altitude, dt) + local deltaRpm = pid:calculer(vCible, vMesuree, dt) - -- assiette - local corrTangage = SIGNE_TANGAGE - * pidTangage:calculer(angles.tangage or 0, tangage, dt) - local corrRoulis = SIGNE_ROULIS - * pidRoulis:calculer(angles.roulis or 0, roulis, dt) - - for role, s in pairs(SIGNES) do - local v = base + s.bf * corrTangage + s.lr * corrRoulis - if v < 0 then v = 0 elseif v > VMAX then v = VMAX end - materiel.reglerRsc(role, v) + -- GAIN SCHEDULING: la boucle de vitesse est mise a l'echelle du + -- poids (delta de poussee requis proportionnel a la masse) + if etat.poidsCalibration and etat.poidsCalibration > 0 then + deltaRpm = deltaRpm + * borner(p.poidsEstime() / etat.poidsCalibration, 0.5, 2.0) end - -- propulseurs (differentiel) - local function borner(v) - if v > 1 then return 1 elseif v < -1 then return -1 end - return v + -- ADAPTATION DE POIDS: en quasi-stationnaire, le residu du PID + -- est transfere lentement vers le feedforward (le drone se pese) + local stationnaire = math.abs(vCible) < 0.2 + and math.abs(vMesuree) < 0.3 + if stationnaire then + rpmAdapt = borner( + rpmAdapt + conf.ADAPTATION.TAUX_POIDS * dt * deltaRpm, + -VMAX / 2, VMAX / 2) end - materiel.reglerRsc("prop_r", borner(avance + virage) * VMAX * facteurProp) - materiel.reglerRsc("prop_l", borner(avance - virage) * VMAX * facteurProp) + + local base = borner(p.rpmSustentation() + rpmAdapt + deltaRpm, + 0, VMAX) + appliquerHelices(base, tangage, roulis, dt, angles, stationnaire + and math.abs(tangage) < conf.ANGLE_MAX + and math.abs(roulis) < conf.ANGLE_MAX) + + -- propulseurs (differentiel, delestables en surcharge) + local function b1(v) return borner(v, -1, 1) end + materiel.reglerRsc("prop_r", b1(avance + virage) * VMAX * facteurProp) + materiel.reglerRsc("prop_l", b1(avance - virage) * VMAX * facteurProp) return altitude, consigneRampe end - -- Stationnement: helices a pleine puissance inversee (plaquage), - -- propulseurs a 0 + ------------------------------------------------------------------ + -- PAS EN BOUCLE OUVERTE (identification pour pidmath) + -- applique rpmSustentation + deltaRpm (assiette toujours asservie) + -- et retourne la vitesse verticale mesuree + ------------------------------------------------------------------ + function p.pasOuvert(deltaRpm, dt) + local altitude = materiel.lireAltitude() + local tangage, roulis = materiel.lireAssiette() + local base = borner(p.rpmSustentation() + rpmAdapt + deltaRpm, + 0, VMAX) + appliquerHelices(base, tangage, roulis, dt, + { tangage = 0, roulis = 0 }, false) + materiel.reglerRsc("prop_l", 0) + materiel.reglerRsc("prop_r", 0) + return p.vitesseVerticale(altitude, dt), altitude + end + function p.plaquer() for role in pairs(SIGNES) do materiel.reglerRsc(role, -VMAX)