diff --git a/README.md b/README.md index 4ffabd4..223c914 100644 --- a/README.md +++ b/README.md @@ -76,6 +76,12 @@ Via le reseau filaire (wired modems): Echelle de texte automatique (la plus grande qui loge le contenu). Navigation par onglets: +- PARAM : edition tactile des parametres (depassement tolere, angles + max, POIDS et POUSSEE de l'helice en pixelNewton, vitesses + verticales max, rampe, Y_VOL, Y_MINI). Les valeurs modifiees sont + persistees dans drone.etat et PRIMENT sur drone.conf sans + l'ecraser; modifier un parametre dont depend la calibration PID + affiche un rappel de recalibration. - MODE : boutons de modes + contenu adapte au mode courant (voir ci-dessous), art du quadricoptere (cercles des helices: blanc rpm=0, rouge rpm max, jaune en changement, vert stable; rpm des @@ -125,9 +131,48 @@ Points nommes (clavier du PC, 10 max): ## Regulation -- PID altitude: deux jeux de gains (montee / descente), derivee sur - la mesure et integrale conditionnelle (concu pour le - zero-depassement), consigne rampee (VITESSE_RAMPE). +- ASSERVISSEMENT EN CASCADE + FEEDFORWARD, ADAPTATIF (transport a + charge et centrage variables a chaque vol): + 1. feedforward de sustentation: rpm d'equilibre calcule depuis + POIDS (A VIDE) et POUSSEE_HELICE_MAX (poussee ~ rpm^EXPOSANT), + CORRIGE EN CONTINU PAR LA PRESSION mesuree (la poussee diminue + avec l'altitude): pousseeEffective = POUSSEE * pression / + PRESSION_REF. Mesurer poids a vide et poussee d'une helice et + les saisir (conf ou page PARAM). + 1b. ADAPTATION DE POIDS: en quasi-stationnaire, le residu du PID + de vitesse est transfere lentement vers le feedforward: le + drone SE PESE tout seul (poids estime affiche en vol). Remise a + zero a chaque decollage (nouvelle cargaison). Plafond: poids + total max = RATIO_CHARGE_MAX (60%%) * 4 * poussee de reference, + alerte SURCHARGE au dela. + 1c. TRIM D'ASSIETTE ADAPTATIF: en stationnaire, la composante + statique des corrections d'assiette (centre de gravite decale + par la cargaison) est absorbee par un trim par axe; alerte + "chargement desequilibre" si |trim| depasse + ADAPTATION.SEUIL_DESEQUILIBRE. + 1d. GAIN SCHEDULING: la boucle de vitesse est mise a l'echelle du + ratio poids estime / poids memorise a la calibration (borne + 0.5..2): calibrer avec n'importe quelle charge, les gains + suivent la cargaison. + Procedure par vol: charger -> mode vol -> 5-10 s de stationnaire + (pesee + trim + alertes) -> partir. Aucune recalibration. + 2. boucle externe: erreur d'altitude -> consigne de vitesse + verticale (P, bornee V_MONTEE_MAX / V_DESCENTE_MAX) + 3. boucle interne: PID sur la vitesse verticale mesuree (velocity + sensor vertical calibre; a defaut, derivee filtree de + l'altitude), deux jeux de gains montee / descente + Consigne rampee (VITESSE_RAMPE), derivee sur la mesure et + integrale conditionnelle (zero-depassement). +- Deux calibrations PID complementaires (onglet CALIB, mode vol): + - `pid` (auto-tune): recherche iterative en boucle fermee, garde + le meilleur essai valide (zero depassement, assiette bornee) + - `pidmath` (methodes mathematiques): identification du modele en + boucle ouverte autour du feedforward (echelon -> K, tau, L), + gains candidats par COHEN-COON et ZIEGLER-NICHOLS, validation en + boucle fermee des deux, conservation du meilleur valide (version + adoucie appliquee si aucun ne passe) + Conseil: regler POIDS/POUSSEE d'abord, puis pidmath (rapide), puis + pid si besoin d'affiner. - PID assiette: tangage et roulis vers leurs cibles; mixage par coin: base + corrections signees. - Moteur asservi au stressometer: augmente si l'utilisation depasse @@ -140,9 +185,33 @@ Points nommes (clavier du PC, 10 max): apprentissage glissant (EMA, CARBURANT.LISSAGE) de la consommation; les ravitaillements et le mode off sont exclus de l'apprentissage. Alerte sous SEUIL_ALERTE. -- Moteur: pre-boost a pleine puissance (MOTEUR.BOOST secondes) a - chaque passage vers un regime exigeant (ex: inactif -> - stationnement) pour eviter l'overstress transitoire. +- Moteur PREDICTIF (apres `calib moteur`, au sol en mode calibrage): + la calibration mesure la capacite produite a chacun des 15 niveaux + redstone puis les coefficients de consommation Kh (helices) et Kp + (propulseurs) en su/rpm. En vol, le besoin est PREVU d'apres les + rpm commandes et le moteur est regle au PLUS PETIT niveau couvrant + besoin * (1 + MARGE): capacite au plus pres de la consommation, + donc carburant minimal. Apprentissage continu de la table, montee + immediate, descente d'un cran par periode. Repli sur le servo + reactif tant que la calib n'est pas faite. +- LIMITEUR DE STRESS (anti-overstress transitoire): a chaque pas de + pilotage, les commandes sont bornees pour que le besoin prevu + reste sous la capacite MESUREE * SECURITE, par priorites: la + sustentation n'est jamais reduite, puis la montee, puis les + propulseurs. Pendant la montee en regime du diesel, le drone + grimpe exactement aussi vite que la capacite le permet: le + depassement transitoire devient structurellement impossible. + Alerte STRESS INSUFFISANT si meme la sustentation depasse le + budget (moteur sous-dimensionne pour la charge). +- Pre-boost a pleine puissance (MOTEUR.BOOST secondes) conserve aux + changements de regime, et garde-fou ultime SURCHARGE (delestage + propulseurs) sur la mesure. +- TYPE DE MOTEUR: le diesel repond instantanement au signal; avec + lui, BOOST peut etre mis a 0, MARGE reduite (~0.10) et + CALIB.ATTENTE_NIVEAU a 0.5 s (calib moteur en ~15 s). Ces trois + valeurs sont sur la page PARAM. Pour un moteur A TRANSITION, + garder BOOST et MARGE plus larges: le limiteur de stress couvre le + delai de montee en regime, l'architecture est inchangee. - Reactivite: le GPS (appel bloquant jusqu'a 2 s) est isole dans une tache dediee alimentant un cache; l'interface et la navigation ne bloquent jamais dessus. Messages longs de l'ecran de conduite: diff --git a/drone.lua b/drone.lua index 3e2fcc3..7312dd4 100644 --- a/drone.lua +++ b/drone.lua @@ -66,6 +66,30 @@ local etat = Etat.charger(journal) local Pid = charger("lib/pid.lua") local materiel = charger("lib/materiel.lua").initialiser(conf, etat, journal) +-------------------------------------------------------------------- +-- COHERENCE DES VERSIONS: si un module a ete mis a jour sans les +-- autres, on echoue ICI avec un message clair plutot qu'en plein vol +-------------------------------------------------------------------- +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 TOUS les " + .. "fichiers avec 'drone maj '") + :format(fichier, table.concat(manquantes, ", ")), 0) + end +end + +exigerInterface(materiel, "lib/materiel.lua", { + "lirePression", "lireStressBrut", "rafraichirAffectations", + "actualiserPosition", "lireFacesRelay", "solPosable", "stressPrevu", +}) +exigerInterface(ModConf, "lib/conf.lua", { "empreintePid" }) + -- invalidation de la calibration PID si la conf a change local empreinte = ModConf.empreintePid(conf) if etat.pid and etat.empreintePid ~= empreinte then @@ -98,6 +122,14 @@ local PARAMS = { pas = 0.05, mini = 0.1, maxi = 1 }, { chemin = "ADAPTATION.SEUIL_DESEQUILIBRE", label = "seuil desequilibre (rpm)", pas = 5, mini = 5 }, + { chemin = "MOTEUR.MARGE", label = "marge moteur", pas = 0.05, + mini = 0.05, maxi = 0.5 }, + { chemin = "MOTEUR.SECURITE", label = "securite stress", + pas = 0.02, mini = 0.5, maxi = 0.98 }, + { chemin = "MOTEUR.BOOST", label = "boost moteur (s)", + pas = 0.5, mini = 0, maxi = 10 }, + { chemin = "CALIB.ATTENTE_NIVEAU", label = "attente calib mot. (s)", + pas = 0.5, mini = 0.5, maxi = 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)", @@ -133,6 +165,10 @@ end local pilotage = charger("lib/pilotage.lua") .nouveau(conf, etat, materiel, Pid, journal) +exigerInterface(pilotage, "lib/pilotage.lua", { + "poidsEstime", "poidsMax", "trim", "razAdaptation", "pasOuvert", + "limitationStress", +}) local navigation = charger("lib/navigation.lua") .nouveau(conf, materiel, journal) local Calibration = charger("lib/calibration.lua") @@ -159,7 +195,9 @@ local calibProgres, calibTexte = nil, nil local rscViolet = nil local alerteMoteurA = 0 local alerteChargeA = 0 +local alerteStressA = 0 local boostMoteurFin = 0 +local niveauMoteur = 15 local function sauverEtat() etat.mode, etat.consigneY = mode, consigneY @@ -303,6 +341,20 @@ local function pasControle() end end + local limitation = pilotage.limitationStress() + if limitation and os.clock() - alerteStressA > 6 then + alerteStressA = os.clock() + if limitation.critique then + journal.alerte(("stress insuffisant pour la sustentation " + .. "(besoin %.0f, budget %.0f)"):format(limitation.besoin, + limitation.budget)) + ihm.message("STRESS INSUFFISANT", 6) + else + journal.info(("limiteur de stress actif (besoin %.0f, budget %.0f)") + :format(limitation.besoin, limitation.budget)) + end + end + if mode == "off" then pilotage.arreter() @@ -419,7 +471,7 @@ local function lancerCalibration(quoi) calibProgres, calibTexte = 0, quoi local ok, err = pcall(function() if quoi == "gimbal" or quoi == "joystick" or quoi == "typew" - or quoi == "rsc" or quoi == "rscman" then + or quoi == "rsc" or quoi == "rscman" or quoi == "moteur" then if mode ~= "calibrage" then journal.alerte("calib " .. quoi .. ": passer en mode calibrage d'abord") @@ -437,6 +489,8 @@ 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 == "moteur" then + Calibration.moteur(conf, etat, materiel, journal, Etat, ui) elseif quoi == "capteurs" or quoi == "pid" or quoi == "pidmath" then if mode ~= "vol" then journal.alerte("calib " .. quoi .. ": passer en mode vol d'abord") @@ -492,6 +546,7 @@ local function calibStatuts() capteurs = #materiel.axesManquants() == 0, pid = etat.pid ~= nil, pidmath = etat.pid ~= nil, + moteur = etat.capaciteParNiveau ~= nil, } local statuts = {} for nom, fait in pairs(faits) do @@ -553,41 +608,68 @@ local function tacheControle() end end --- moteur asservi au stressometer; surcharge: reduction propulseurs +-- Moteur PREDICTIF: le niveau est choisi d'apres le besoin PREVU +-- des rpm commandes (calib moteur), avec apprentissage continu de la +-- table de capacites. Repli reactif tant que la calib n'est pas +-- faite. La SURCHARGE mesuree reste le garde-fou ultime. local function tacheMoteur() - local niveau = 8 local enSurcharge = false while true do if mode == "off" then materiel.reglerMoteur(0) elseif os.clock() < boostMoteurFin then + niveauMoteur = 15 materiel.reglerMoteur(15) - niveau = 15 else stress = materiel.lireStress() - if stress then - if stress > conf.MOTEUR.STRESS_HAUT and niveau < 15 then - niveau = niveau + 1 - elseif stress < conf.MOTEUR.STRESS_BAS and niveau > 1 then - niveau = niveau - 1 + local besoin = materiel.stressPrevu() + local capacites = etat.capaciteParNiveau + + if besoin and capacites then + -- PREDICTIF: plus petit niveau couvrant besoin * (1 + marge) + local vise = besoin * (1 + conf.MOTEUR.MARGE) + local cible = 15 + for n = 1, 15 do + if (capacites[n] or 0) >= vise then cible = n break end end - -- surcharge malgre moteur au maximum: delester les propulseurs - if stress > conf.MOTEUR.SURCHARGE and niveau >= 15 then - if not enSurcharge then - enSurcharge = true - journal.alerte("surcharge de stress: propulseurs reduits") - ihm.message("SURCHARGE: propulseurs reduits", 10) - end - pilotage.reglerFacteurProp(0.3) - elseif enSurcharge and stress < conf.MOTEUR.STRESS_HAUT then - enSurcharge = false - pilotage.reglerFacteurProp(1.0) - journal.info("surcharge resorbee") + -- monter immediatement, descendre d'un cran max par periode + if cible > niveauMoteur then niveauMoteur = cible + elseif cible < niveauMoteur then niveauMoteur = niveauMoteur - 1 end + + -- apprentissage continu de la capacite au niveau courant + local _, capMes = materiel.lireStressBrut() + if capMes and capMes > 0 then + capacites[niveauMoteur] = capacites[niveauMoteur] + and (0.9 * capacites[niveauMoteur] + 0.1 * capMes) or capMes + end + elseif stress then + -- REACTIF (calib moteur non faite) + if stress > conf.MOTEUR.STRESS_HAUT and niveauMoteur < 15 then + niveauMoteur = niveauMoteur + 1 + elseif stress < conf.MOTEUR.STRESS_BAS and niveauMoteur > 1 then + niveauMoteur = niveauMoteur - 1 end else - niveau = 15 + niveauMoteur = 15 end - materiel.reglerMoteur(niveau) + + -- garde-fou ultime sur la mesure + if stress and stress > conf.MOTEUR.SURCHARGE + and niveauMoteur >= 15 then + if not enSurcharge then + enSurcharge = true + journal.alerte("surcharge de stress: propulseurs reduits") + ihm.message("SURCHARGE: propulseurs reduits", 10) + end + pilotage.reglerFacteurProp(0.3) + elseif enSurcharge + and (not stress or stress < conf.MOTEUR.STRESS_HAUT) then + enSurcharge = false + pilotage.reglerFacteurProp(1.0) + journal.info("surcharge resorbee") + end + + materiel.reglerMoteur(niveauMoteur) end sleep(conf.MOTEUR.PERIODE) end @@ -690,6 +772,29 @@ local function tachePosition() end 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 + local function contexteIhm() local px, py, pz = materiel.lirePosition() return { @@ -716,32 +821,11 @@ local function contexteIhm() poidsMax = pilotage.poidsMax(), trimTangage = select(1, pilotage.trim()), trimRoulis = select(2, pilotage.trim()), + niveauMoteur = niveauMoteur, + stressPrevu = materiel.stressPrevu(), } 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 = {} diff --git a/lib/calibration.lua b/lib/calibration.lua index 2ea6127..b8165c7 100644 --- a/lib/calibration.lua +++ b/lib/calibration.lua @@ -414,6 +414,82 @@ function Calibration.capteurs(conf, etat, materiel, journal, Etat, ui, return true end +-------------------------------------------------------------------- +-- MOTEUR (au sol, mode calibrage, roles RSC deja affectes) +-- 1. Balayage des niveaux 1..15: table capacite(niveau) en su. +-- 2. Helices seules a RPM_STRESS -> coefficient Kh (su/rpm), puis +-- propulseurs seuls -> Kp. Le moteur predictif et le limiteur de +-- stress du pilotage utilisent ces donnees. +-------------------------------------------------------------------- +function Calibration.moteur(conf, etat, materiel, journal, Etat, ui) + if not materiel.stresso then + journal.alerte("calib moteur: pas de stressometer") + return false + end + if #materiel.helicesManquantes() > 0 then + journal.alerte("calib moteur: calibrer les RSC d'abord") + return false + end + materiel.toutArreter() + local C = conf.CALIB + + -- Phase 1: capacite par niveau + local capacites = {} + for niveau = 1, 15 do + ui.progres(niveau / 15 * 0.6, ("capacite niveau %d/15"):format(niveau)) + materiel.reglerMoteur(niveau) + sleep(C.ATTENTE_NIVEAU) + local _, cap = materiel.lireStressBrut() + capacites[niveau] = cap or 0 + journal.info(("calib moteur: niveau %d -> %d su"):format( + niveau, capacites[niveau])) + end + if capacites[15] <= 0 then + journal.erreur("calib moteur: capacite nulle a pleine puissance " + .. "(moteur alimente ?)") + materiel.reglerMoteur(0) + ui.progres(0, "ECHEC: capacite nulle") + return false + end + + -- Phase 2: coefficients de consommation (moteur au maximum) + materiel.reglerMoteur(15) + sleep(C.ATTENTE_NIVEAU) + local stressBase = materiel.lireStressBrut() or 0 + + ui.progres(0.7, "coefficient helices") + for _, role in ipairs({ "lb", "rb", "lf", "rf" }) do + materiel.reglerRsc(role, C.RPM_STRESS) + end + sleep(C.ATTENTE_NIVEAU) + local stressHelices = materiel.lireStressBrut() or 0 + materiel.toutArreter() + sleep(1) + + ui.progres(0.85, "coefficient propulseurs") + materiel.reglerRsc("prop_l", C.RPM_STRESS) + materiel.reglerRsc("prop_r", C.RPM_STRESS) + sleep(C.ATTENTE_NIVEAU) + local stressProps = materiel.lireStressBrut() or 0 + materiel.toutArreter() + + local kh = math.max(0, stressHelices - stressBase) / (4 * C.RPM_STRESS) + local kp = math.max(0, stressProps - stressBase) / (2 * C.RPM_STRESS) + if kh <= 0 then + journal.erreur("calib moteur: coefficient helices nul") + ui.progres(0, "ECHEC: coeff nul") + return false + end + + etat.capaciteParNiveau = capacites + etat.stressParRpm = { helice = kh, prop = kp } + Etat.sauver(etat) + journal.info(("calib moteur: Kh=%.3f Kp=%.3f su/rpm, capa max %d su") + :format(kh, kp, capacites[15])) + ui.progres(1, "terminee") + return true +end + -------------------------------------------------------------------- -- PID MATHEMATIQUE (en vol stationnaire, marge d'altitude requise) -- 1. Identification en boucle ouverte autour du feedforward: un diff --git a/lib/conf.lua b/lib/conf.lua index 74a9d47..533ccc4 100644 --- a/lib/conf.lua +++ b/lib/conf.lua @@ -112,12 +112,23 @@ return { -- malgre tout, les PROPULSEURS sont reduits en priorite + alerte. ------------------------------------------------------------------ MOTEUR = { - STRESS_HAUT = 0.90, -- au dela: augmenter le moteur - STRESS_BAS = 0.60, -- en deca: reduire le moteur (economie) - SURCHARGE = 0.98, -- au dela: reduire les propulseurs + alerte - PERIODE = 1.0, -- periode de l'asservissement (s) - BOOST = 4.0, -- pre-boost a pleine puissance lors des - -- changements de regime exigeants (s) + -- PREDICTIF (apres calib moteur): le niveau est choisi d'apres + -- le besoin PREVU des commandes rpm, pour une capacite au plus + -- pres de la consommation sans jamais etre depassee. + -- TYPE DE MOTEUR: le diesel repond INSTANTANEMENT au signal; + -- pour lui, BOOST peut etre mis a 0 et MARGE reduite (0.10). + -- Pour un moteur A TRANSITION (autres moteurs du modpack), + -- garder BOOST > 0 et une MARGE plus large: le limiteur de + -- stress du pilotage couvre le delai de montee en regime. + MARGE = 0.15, -- capacite visee = besoin * (1 + marge) + SECURITE = 0.92, -- le pilotage borne ses commandes pour + -- que besoin <= capacite mesuree * sec. + PERIODE = 0.25, -- periode de l'asservissement (s) + -- repli REACTIF (tant que calib moteur non faite) + STRESS_HAUT = 0.90, + STRESS_BAS = 0.60, + SURCHARGE = 0.98, -- garde-fou ultime: delestage propulseurs + BOOST = 4.0, -- pre-boost aux changements de regime }, ------------------------------------------------------------------ @@ -143,6 +154,10 @@ return { RPM_MANUEL = 10, -- rsc manuel: vitesse de reperage (rpm) IMPULSION_SEUIL = 0.8, -- rsc/gimbal: variation d'angle declarante (deg) JOYSTICK_SEUIL = 8, -- joystick: niveau declarant une face (0..15) + RPM_STRESS = 32, -- calib moteur: rpm de mesure des coeff. + ATTENTE_NIVEAU = 2.5, -- calib moteur: stabilisation par niveau + -- (s); 0.5 suffit pour un moteur + -- instantane (diesel) }, } ]] diff --git a/lib/etat.lua b/lib/etat.lua index b2b7484..bd6c0a6 100644 --- a/lib/etat.lua +++ b/lib/etat.lua @@ -28,6 +28,8 @@ local DEFAUT = { points = {}, pid = nil, empreintePid = nil, + capaciteParNiveau = nil, -- calib moteur: niveau (1..15) -> su + stressParRpm = nil, -- calib moteur: { helice, prop } su/rpm decollage = nil, } diff --git a/lib/ihm.lua b/lib/ihm.lua index 7ee0604..bdf0b27 100644 --- a/lib/ihm.lua +++ b/lib/ihm.lua @@ -182,12 +182,18 @@ function Ihm.nouveau(conf, materiel, journal) local function blocStress(y, ctx) local brut, cap = materiel.lireStressBrut() - texte(1, y, ("stress %s / %s (%s) moteur %s"):format( + texte(1, y, ("stress %s / %s (%s) niv %s moteur %s"):format( brut and ("%d"):format(brut) or "?", cap and ("%d"):format(cap) or "?", ctx.stress and ("%.0f%%"):format(ctx.stress * 100) or "n/a", + ctx.niveauMoteur or "?", ctx.moteurOk and "ok" or "ARRETE"), ctx.moteurOk and colors.white or colors.red) + if ctx.stressPrevu then + texte(1, y + 1, ("besoin prevu %.0f su"):format(ctx.stressPrevu), + colors.lightGray) + return y + 2 + end return y + 1 end @@ -285,7 +291,8 @@ function Ihm.nouveau(conf, materiel, journal) colors.lightGray) local x, y = 1, 3 for _, c in ipairs({ "gimbal", "joystick", "typew", "rsc", - "rscman", "capteurs", "pid", "pidmath" }) do + "rscman", "moteur", "capteurs", "pid", + "pidmath" }) do local statut = ctx.calibStatuts[c] local couleur = colors.gray if statut == "encours" then couleur = colors.yellow @@ -317,7 +324,7 @@ function Ihm.nouveau(conf, materiel, journal) y = dessinerArt(1, y + 1, ctx) end texte(1, H - 1, - "sol: gimbal joystick typew rsc rscman (mode calibrage)", + "sol: gimbal joystick typew rsc rscman moteur (calibrage)", colors.lightGray) texte(1, H, "vol: capteurs pid pidmath (mode vol)", colors.lightGray) diff --git a/lib/materiel.lua b/lib/materiel.lua index d6b9d50..4ccc209 100644 --- a/lib/materiel.lua +++ b/lib/materiel.lua @@ -96,6 +96,10 @@ function Materiel.initialiser(conf, etat, journal) return m.altitude.getHeight() end + function m.lirePression() + return m.altitude.getAirPressure() or 1.0 + end + -- angles bruts du gimbal (table de 2) function m.lireAnglesBruts() local a = m.gimbal.getAngles() @@ -190,6 +194,20 @@ function Materiel.initialiser(conf, etat, journal) return m.stresso.getStress(), m.stresso.getStressCapacity() end + -- besoin de stress PREVU (su) d'apres les rpm commandes et les + -- coefficients de la calib moteur; nil si non calibree + function m.stressPrevu() + local k = etat.stressParRpm + if not k then return nil end + local besoin = 0 + for role, suivi in pairs(m.rpm) do + local coeff = (role == "prop_l" or role == "prop_r") + and k.prop or k.helice + besoin = besoin + (coeff or 0) * math.abs(suivi.v) + end + return besoin + end + -- capacite > 0 <=> le moteur tourne (verifie: capacite 0 a l'arret) function m.moteurTourne() if not m.stresso then return true end -- indetectable: on suppose oui diff --git a/lib/pilotage.lua b/lib/pilotage.lua index 439f948..61a05fc 100644 --- a/lib/pilotage.lua +++ b/lib/pilotage.lua @@ -46,6 +46,7 @@ function Pilotage.nouveau(conf, etat, materiel, Pid, journal) -- decollage via razAdaptation) local rpmAdapt = 0 -- correction adaptative du ff local trimTangage, trimRoulis = 0, 0 -- trim d'assiette adaptatif + local limitation = nil -- info du limiteur de stress ------------------------------------------------------------------ -- FEEDFORWARD : rpm d'equilibre d'UNE helice @@ -82,6 +83,57 @@ function Pilotage.nouveau(conf, etat, materiel, Pid, journal) return trimTangage, trimRoulis end + -- derniere limitation de stress appliquee ({besoin, budget}) ou nil + function p.limitationStress() + return limitation + end + + ------------------------------------------------------------------ + -- LIMITEUR DE STRESS (anti-overstress transitoire) + -- Borne les commandes pour que le besoin PREVU reste sous la + -- capacite MESUREE * SECURITE. Priorites: la sustentation n'est + -- jamais reduite, puis la montee, puis les propulseurs. + -- Retourne: base bornee, facteur propulseurs (0..1) + ------------------------------------------------------------------ + local function limiterStress(base, propTotal) + limitation = nil + local k = etat.stressParRpm + if not k or not k.helice then return base, 1 end + local _, capacite = materiel.lireStressBrut() + if not capacite or capacite <= 0 then return base, 1 end + + local budget = capacite * conf.MOTEUR.SECURITE + local sust = borner(p.rpmSustentation() + rpmAdapt, 0, VMAX) + local besoinHelices = k.helice * 4 * base + local besoinProps = (k.prop or 0) * propTotal + local total = besoinHelices + besoinProps + if total <= budget then return base, 1 end + + limitation = { besoin = total, budget = budget } + local excedent = total - budget + + -- 1. delester les propulseurs + local facteurP = 1 + if besoinProps > 0 then + local reduc = math.min(besoinProps, excedent) + facteurP = (besoinProps - reduc) / besoinProps + excedent = excedent - reduc + end + + -- 2. rogner la part au-dessus de la sustentation (la montee) + if excedent > 0 then + local margeMontee = k.helice * 4 * math.max(0, base - sust) + local reduc = math.min(margeMontee, excedent) + base = base - reduc / (k.helice * 4) + excedent = excedent - reduc + end + + -- 3. il reste un excedent: la sustentation elle-meme depasse le + -- budget; on la GARDE (securite > stress), signale via limitation + limitation.critique = excedent > 0 + return base, facteurP + end + function p.razAdaptation() rpmAdapt, trimTangage, trimRoulis = 0, 0, 0 end @@ -200,14 +252,22 @@ function Pilotage.nouveau(conf, etat, materiel, Pid, journal) 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) + local propR = b1(avance + virage) * VMAX * facteurProp + local propL = b1(avance - virage) * VMAX * facteurProp + + -- limiteur de stress: base et propulseurs bornes au budget + local facteurStress + base, facteurStress = limiterStress(base, + math.abs(propR) + math.abs(propL)) + + appliquerHelices(base, tangage, roulis, dt, angles, stationnaire + and math.abs(tangage) < conf.ANGLE_MAX + and math.abs(roulis) < conf.ANGLE_MAX) + materiel.reglerRsc("prop_r", propR * facteurStress) + materiel.reglerRsc("prop_l", propL * facteurStress) return altitude, consigneRampe end