diff --git a/README.md b/README.md index 223c914..f1abcea 100644 --- a/README.md +++ b/README.md @@ -1,231 +1,131 @@ -# Quadricoptere Create Aeronautics : controleur centralise +# Drone V2 : quadricoptere Create Aeronautics / CC:Tweaked -Nouvelle architecture: un SEUL ordinateur (PC central) pilote tout via -le reseau filaire. Les anciens scripts multi-PC (helice.lua, -central.lua, input.lua, console.lua, propulsion.lua) sont obsoletes. +Reecriture complete, concue contre les oscillations permanentes de +la V1. Installation: copier drone.lua puis `drone maj ` +(Forgejo raw, domaine autorise dans la config http de CC:Tweaked). -## Materiel +## Pourquoi la V1 oscillait, et les parades de la V2 -Directement sur le PC central: -- altitude_sensor, Create_DisplayLink (affichage de conduite) -- ender modem (uniquement pour le GPS) -- sortie redstone BOTTOM vers le moteur (15 = arret, 14 = vitesse - minimale, 0 = vitesse maximale) +1. QUANTIFICATION: les RSC acceptent des rpm ENTIERS. A ~20 rpm de + sustentation, 1 rpm = 5% de poussee par coin: aucun regulateur ne + peut converger, il ne peut qu'osciller autour (cycle limite). + PARADE MATERIELLE FORTEMENT RECOMMANDEE: engrener les helices + pour que la sustentation tombe vers 60-100 rpm (meme poussee, + 3 a 5 fois plus de resolution de commande). +2. RETARD DE BOUCLE (ticks, transitions de rotation): la calibration + par RELAIS mesure la periode ultime REELLE retard compris, et + TYREUS-LUYBEN laisse ~3x de marge de gain: stabilite garantie + pour la dynamique identifiee. +3. BRUIT DE MESURE: derivee FILTREE dans le PID; vitesse verticale + par velocity sensor AUTO-DETECTE a la calib altitude (repli: + derivee filtree de l'altitude). +4. FILET DE SECURITE: un SUPERVISEUR detecte l'oscillation de + chaque boucle en vol (passages a zero repetes au-dela d'un seuil + d'amplitude) et reduit ses gains de 20% automatiquement, avec + journalisation et persistance: meme mal regle, le systeme + converge vers la stabilite. -Via le reseau filaire (wired modems): -- 6 Create_RotationSpeedController: 4 helices + 2 propulseurs - (roles affectes par `calib rsc`, persistes dans drone.etat) -- 3 velocity_sensor, un par composante (axes affectes par - `calib capteurs`) -- 1 gimbal_sensor, 1 optical_sensor (vers le sol, 15.5 = rien), - 1 stressometer, 1 redstone_relay (joystick), 1 advanced monitor - (tactile), 1 linked typewriter (EN ATTENTE: relever ses signatures - avec test_signatures, voir lib/ihm.lua) +## Modes -## Installation - -1. Copier drone.lua sur le PC, puis: `drone maj ` pour - telecharger les modules depuis votre depot (Forgejo: URL "raw" du - dossier). Autoriser le domaine dans la config http de CC:Tweaked. - Ou copier a la main drone.lua + lib/*.lua. -2. Premier lancement: `drone` cree `drone.conf`. RELIRE chaque - parametre (Y_VOL, Y_MINI, ANGLE_MAX, DIST_SOL, offsets, joystick, - moteur...) puis passer `verifie = true`. Un warning s'affiche tant - que ce n'est pas fait. -3. Calibrations depuis l'onglet CALIB, en mode CALIBRAGE pour - celles au sol, DANS CET ORDRE (les modes non calibres sont - verrouilles en rouge). Toutes sont RE-EXECUTABLES (recablage: - relancer la calibration remet ses affectations a zero et prend - effet sans relancer le programme). La calib rsc verifie la - coherence (retour de l'assiette a l'etat initial entre chaque - impulsion, detection des conflits deux RSC -> une meme helice) - et s'annule proprement en cas d'incoherence. Les calibrations - assistees affichent leurs consignes sur l'ecran de conduite et se - valident en touchant le moniteur: - - `gimbal` (assistee, en premier): incliner le nez vers le bas - puis pencher a gauche; associe les 2 angles bruts au tangage et - au roulis (l'association CHANGE d'un vaisseau a l'autre). - - `joystick` (assistee): pousser a fond dans chaque direction pour - identifier les faces du redstone relay. - - `typew` (assistee, OPTIONNELLE): le typewriter emet des codes - ASCII (qwerty), le mappage est integre par defaut (0..9, - Entree = valider, Backspace = effacer, + / - = consigne +-1). - Ne calibrer que si le clavier a un mappage exotique. - - `rsc` (drone pose): impulsion sur chaque RSC; l'effet - d'assiette designe le coin; sans effet = propulseur. prop_l / - prop_r arbitraires: si un virage gauche part a droite, les - echanger dans drone.etat. Pour un DRONE TROP LOURD (impulsions - sans effet), utiliser `rscman`. - - `rscman` (drone pose, manuelle): chaque RSC tourne a - CALIB.RPM_MANUEL (10 rpm); regarder quelle helice tourne et - toucher sa position sur l'art (4 coins + prop_l / prop_r); - unicite garantie, ANNULER restaure l'etat precedent. Fiabilise - aussi prop_l / prop_r (plus d'affectation arbitraire). - - `capteurs` (en vol): montee, avancee puis translation gauche - identifient l'axe ET la direction de chaque velocity sensor - (repere du drone: avant / lateral / vertical, positif = - avant / gauche / haut). - - `pid` (en vol stationnaire, marge d'altitude): recherche - iterative, critere ZERO depassement + temps de reponse minimal + - assiette <= ANGLE_MAX. EXPERIMENTAL. Toute modification des - parametres de conf dont dependent ces gains (VITESSE_RSC_MAX, - VITESSE_RAMPE, ANGLE_MAX) INVALIDE la calibration au prochain - demarrage (retour aux gains de la conf + alerte). - -## Interface (moniteur tactile) - -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 - propulseurs sur les cotes) -- CALIB : boutons des calibrations (gris = a faire, jaune = en - cours, vert = faite), consignes et barre de progression, art avec - le RSC en cours en VIOLET -- JOURNAL : dernieres lignes du journal de vol - -## Modes (verrouilles en rouge si une calibration manque, la raison -s'affiche au toucher) - -- `off` : moteur coupe (bottom a 15), tous les RSC a 0 -- `inactif` : moteur ALLUME, tous les RSC a 0, commandes desactivees. - Dans tous les modes sauf off, une erreur s'affiche si le moteur ne - tourne pas (stressometer a capacite nulle) alors qu'il le devrait -- `calibrage` : identique a inactif mais AUTORISE les calibrations - au sol (gimbal, joystick, typew, rsc); capteurs et pid se lancent - en mode vol -- `stationnement` : helices a pleine puissance inversee (plaquage), - propulseurs a 0, commandes desactivees. Conditions d'entree: au sol - (optical <= DIST_SOL) ET horizontal (<= ANGLE_MAX). Sortie - automatique vers `vol` si les conditions sont perdues. -- `vol` : assiette maintenue horizontale (PID tangage/roulis a 0), - altitude au PID double regime, deplacement horizontal au joystick - via les PROPULSEURS (avance/virage differentiels) -- `drone` : le joystick commande l'INCLINAISON (jusqu'a - ANGLE_DRONE_MAX), deplacement par les helices, propulseurs a 0 -- `atterrissage` : descente douce (VITESSE_ATTERRISSAGE) jusqu'au - sol, puis stationnement. Sous Y_MINI, la descente exige un sol - POSABLE (optical: bloc detecte, ni eau ni lave): garde-fou ocean -- `auto` : cible a 2 coordonnees (`auto ` au clavier, pave - tactile, ou bouton d'un point enregistre): decollage a Y_VOL, - ralliement en avancant (cap estime par GPS), atterrissage sur - place. Cible a 3 coordonnees (`auto `): rallie le point - et s'y MAINTIENT a l'altitude y (bornee par le sol et Y_MINI). - Bouton ANNULER: retour en vol stabilise a la position courante - -La consigne d'altitude se regle aux boutons +-1/+-10 du moniteur ou -au TYPEWRITER: taper les chiffres (la saisie s'affiche sur l'ecran -de conduite) puis Entree; Backspace corrige; + et - ajustent la -consigne de 1 quand aucune saisie n'est en cours. - -Points nommes (clavier du PC, 10 max): -`pt save ` | `pt go ` | `pt del ` | `pt list` | `rtb` -(retour au point de decollage de la derniere mission auto). +- `off` : moteur coupe, RSC a 0 +- `sol` : moteur allume, RSC a 0 +- `stationnement` : helices a pleine puissance INVERSEE (plaquage), + propulseurs a 0. Entree: au sol (optical <= DIST_SOL) ET + horizontal (<= ANGLE_MAX). Sortie AUTO vers vol si perdu. +- `vol` : assiette horizontale, altitude vers la consigne (+-1/+-10 + au moniteur), deplacements au joystick (propulseurs + differentiels avance/virage) +- `atterrissage` : descente a VITESSE_ATTERRISSAGE puis + stationnement. La verification d'un sol posable est au pilote. ## Regulation -- 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 - STRESS_HAUT, diminue sous STRESS_BAS (economie). Le moteur est - dimensionne pour suffire; en cas de SURCHARGE malgre le maximum, - les PROPULSEURS sont reduits en priorite (facteur 0.3) + alerte, - puis retablis quand le stress redescend. -- Carburant: deux valeurs sur le 2e display link: POURCENTAGE de - reserve (ligne 1) et AUTONOMIE en minutes (ligne 2), estimee par - apprentissage glissant (EMA, CARBURANT.LISSAGE) de la - consommation; les ravitaillements et le mode off sont exclus de - l'apprentissage. Alerte sous SEUIL_ALERTE. -- 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: - defilement par segments de mots entiers (indicateur i/n). +- ALTITUDE en cascade: erreur -> vitesse verticale cible (P bornee + V_MONTEE_MAX / V_DESCENTE_MAX) -> PID vitesse -> delta rpm autour + du rpm de sustentation MESURE (corrige par la pression). + Adaptation lente a la charge en quasi-stationnaire. +- ASSIETTE: PID par axe, derivee filtree, autorite bornee a + ASSIETTE_PLAFOND * rpm de sustentation. +- MOTEUR: bouton on/off ou adaptatif au moniteur. En adaptatif, le + besoin est PREVU (coefficients su/rpm mesures pendant la calib + rsc) et le niveau est regle AVANT l'ecriture des RSC en montee de + regime, apres en descente; garde: besoin borne a capacite max * + SECURITE. -## Fichiers +## Calibrations (onglet CALIB), dans l'ordre -- `drone.conf` : parametres utilisateur (cree si absent) -- `drone.etat` : mode, consigne, roles RSC, axes capteurs, gains - calibres, point de decollage (restaures au reboot; en vol, la - consigne repart de l'altitude reelle) -- `drone.log` : journal de vol (rotation a 64 Ko) +1. `gimbal` (mode sol): a plat, nez bas, a plat, penche a gauche. +2. `joystick` (mode sol): pousser a fond dans chaque direction. +3. `rsc` (mode sol): chaque RSC tourne a RPM_MANUEL, toucher sur + l'art la position qui tourne; mesure AUSSI les coefficients + su/rpm, la capacite moteur par niveau (mode adaptatif) et + l'association helice <-> speedometer. +3b. `boite` (mode sol, optionnelle, rsc requis): association de + CHAQUE face du second relay a SON helice (chaque helice tourne a + son tour; la face dont le signal 13 = rapport 2/16 reduit la + vitesse lue par le speedometer de cette helice est la sienne). + En vol, le meme signal est ecrit sur les 4 faces: rapport + identique sur les 4 helices, indispensable a la symetrie. +4. `altitude` (mode sol, AUTO-AMORCANTE): rampe lente jusqu'au + decollage -> rpm de sustentation MESURE. Si les boites sont + calibrees: retour au sol, CHOIX DU RAPPORT (k = plus petit k/16 + tel que la sustentation RSC <= BOITE.RSC_CIBLE, soit ~70% de la + plage avec x1.4 de marge de montee), re-decollage au rapport + definitif, verification par speedometer. Puis gains d'assiette + par defaut dimensionnes, detection du velocity sensor vertical, + relais d'Astrom-Hagglund sur la boucle de vitesse -> gains + Tyreus-Luyben, retour au sol automatique. +5. `inclinaison` (mode vol, stationnaire): relais par axe -> + gains Tyreus-Luyben. -## Points d'attention +Toutes re-executables; gains persistes en surcharges, visibles et +retouchables page PARAM (parametrage manuel guide). Le superviseur +peut ensuite les affiner a la baisse en vol si necessaire. -- tanks(): si la capacite n'est pas fournie par le jeu, la conf - CARBURANT.CAPACITE_TANK sert de repli. +## Affichages + +- Moniteur: onglets MODE / CALIB / PARAM / JOURNAL, echelle de + texte auto, art du quadricoptere (blanc 0, rouge max, jaune en + changement, vert stable, violet en calibration), autonomie en + minutes, choix du regime moteur. +- Conduite (2x22): ALT reelle > consigne, VIT globale + (sqrt(v1^2+v2^2+v3^2)); messages longs en segments de mots. +- Carburant (2x4): pourcentage de reserve. + +## Boites de vitesse (analog transmission, optionnelles) + +Une par helice, CHACUNE sur SA face du second redstone relay +(association apprise par la calib boite), plus un speedometer par +helice. Le meme signal est ecrit sur les 4 faces: rapport identique +pour les 4, indispensable a la symetrie. Rapport = (15-signal)/16 +(signal 0 = 1/1; signal 15 INTERDIT = rapport 0). Interet: a ~20 rpm +de sustentation, 1 rpm RSC entier = 5%% de poussee (oscillation +inevitable); au rapport 2/16 la sustentation RSC passe a ~160 rpm et +la resolution est multipliee par 8. + +BOITE AUTOMATIQUE: le regulateur travaille en RPM HELICE (unites +physiques); la conversion RSC = helice / rapport n'a lieu qu'a la +frontiere de l'actionneur. Le gain de boucle est donc INVARIANT au +rapport: les gains calibres une fois valent a tous les rapports. En +vol, si la commande RSC des helices sort de la bande [RSC_BAS, +RSC_HAUT] (charge accrochee en vol, perte de poussee en altitude), +le rapport change d'UN cran, uniquement en quasi-stationnaire et a +plat, avec ATTENTE de temps mort, et dans un ORDRE SUR: le +transitoire d'un tick est toujours un CREUX de poussee symetrique +(jamais un pic): montee de k = RSC reecrits puis redstone; descente += redstone puis RSC. k=15 est saute (signal 15 = rapport 0, +interdit). Les speedometers surveillent en continu que vitesse +reelle = commande * rapport (ecart > 25%% persistant 2 s -> alerte +BOITES INCOHERENTES + reassertion du signal). + +## Renumerotation des relays + +Les NOMS des peripheriques peuvent changer a la reconnexion du +reseau; les FACES des relays sont stables. Si les noms stockes ne +correspondent plus au reseau, les modes de vol sont verrouilles et +la REIDENTIFICATION AUTOMATIQUE se lance en mode sol (~3 s: +l'helice lf tourne a bas regime, le signal 13 sur sa face designe +le relay des boites via le speedometer; le joystick est l'autre +relay). Aucune action manuelle. + +Non utilises: GPS, typewriter. diff --git a/drone.lua b/drone.lua index 19fb81d..7473813 100644 --- a/drone.lua +++ b/drone.lua @@ -1,157 +1,94 @@ -------------------------------------------------------------------- --- drone.lua : controleur central du quadricoptere Create Aeronautics --- --- Usage : --- drone lance le controleur --- drone maj installe / met a jour depuis un depot --- --- Modes : off, inactif, stationnement, vol, drone, atterrissage, --- auto, position --- Clavier du PC : auto | position --- pt save | pt go | pt del | pt list --- rtb (retour au point de decollage) --------------------------------------------------------------------- - -local ARGS = { ... } -local BASE = fs.getDir(shell.getRunningProgram()) - --------------------------------------------------------------------- --- INSTALLEUR / MISE A JOUR INTEGRE +-- drone.lua : programme principal V2. +-- Usage: drone demarrer +-- drone maj mettre a jour depuis (Forgejo raw) +-- Modes: off, sol, stationnement, vol, atterrissage. -------------------------------------------------------------------- local FICHIERS = { "drone.lua", - "lib/journal.lua", "lib/conf.lua", "lib/etat.lua", "lib/pid.lua", - "lib/materiel.lua", "lib/pilotage.lua", "lib/navigation.lua", - "lib/ihm.lua", "lib/calibration.lua", + "lib/conf.lua", "lib/etat.lua", "lib/pid.lua", "lib/materiel.lua", + "lib/regulation.lua", "lib/calib.lua", "lib/ecran.lua", + "lib/journal.lua", } -if ARGS[1] == "maj" then - local url = ARGS[2] - if not url then error("usage: drone maj ", 0) end - if not http then error("API http desactivee (config CC:Tweaked)", 0) end - url = url:gsub("/$", "") +local args = { ... } +if args[1] == "maj" then + local base = args[2] + if not base then print("usage: drone maj ") return end + if base:sub(-1) ~= "/" then base = base .. "/" end for _, fichier in ipairs(FICHIERS) do - write(fichier .. " ... ") - local reponse, err = http.get(url .. "/" .. fichier) - if not reponse then error("echec: " .. tostring(err), 0) end + write("maj " .. fichier .. " ... ") + local reponse, err = http.get(base .. fichier) + if not reponse then print("ECHEC: " .. tostring(err)) return end local contenu = reponse.readAll() reponse.close() - local chemin = fs.combine(BASE, fichier) - fs.makeDir(fs.getDir(chemin)) - local f = fs.open(chemin, "w") + if fs.exists(fichier) then fs.delete(fichier) end + local f = fs.open(fichier, "w") f.write(contenu) f.close() print("ok") end - print("mise a jour terminee, relancer: drone") + print("mise a jour terminee: relancer drone") return end --------------------------------------------------------------------- --- CHARGEMENT DES MODULES --------------------------------------------------------------------- local function charger(chemin) - local complet = fs.combine(BASE, chemin) - if not fs.exists(complet) then - error(chemin .. " manquant: lancer 'drone maj '", 0) - end - return dofile(complet) + local f = assert(fs.open(chemin, "r"), + chemin .. " manquant: drone maj ") + local src = f.readAll() + f.close() + return assert(load(src, chemin))() end local journal = charger("lib/journal.lua") -local ModConf = charger("lib/conf.lua") -local conf = ModConf.charger(journal) +local conf = charger("lib/conf.lua").charger(journal) local Etat = charger("lib/etat.lua") local etat = Etat.charger(journal) local Pid = charger("lib/pid.lua") -local materiel = charger("lib/materiel.lua").initialiser(conf, etat, journal) +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 - journal.alerte("conf modifiee: gains PID calibres INVALIDES, " - .. "retour aux gains de la conf (relancer calib pid)") - 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). +-- PARAMETRES EDITABLES (page PARAM); surcharges persistees dans +-- drone.etat, elles 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 = "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 = "ASSIETTE_PLAFOND", label = "autorite assiette", - pas = 0.05, mini = 0.05, maxi = 1 }, - { chemin = "PID.tangage.kp", label = "assiette T kp", pas = 0.25, - mini = 0 }, - { chemin = "PID.tangage.ki", label = "assiette T ki", pas = 0.05, - mini = 0 }, - { chemin = "PID.tangage.kd", label = "assiette T kd", pas = 0.25, - mini = 0 }, - { chemin = "PID.roulis.kp", label = "assiette R kp", pas = 0.25, - mini = 0 }, - { chemin = "PID.roulis.ki", label = "assiette R ki", pas = 0.05, - mini = 0 }, - { chemin = "PID.roulis.kd", label = "assiette R kd", pas = 0.25, - mini = 0 }, + { chemin = "DIST_SOL", label = "distance sol", pas = 0.2, + mini = 0.4 }, + { chemin = "VITESSE_ATTERRISSAGE", label = "v atterrissage (b/s)", + pas = 0.25, mini = 0.25 }, { chemin = "V_MONTEE_MAX", label = "v montee max (b/s)", pas = 0.5, mini = 0.5 }, { chemin = "V_DESCENTE_MAX", label = "v descente max (b/s)", pas = 0.5, mini = 0.5 }, - { chemin = "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 }, + { chemin = "RAMPE", label = "rampe consigne (b/s)", pas = 0.5, + mini = 0.5 }, + { chemin = "ASSIETTE_PLAFOND", label = "autorite assiette", + pas = 0.05, mini = 0.05, maxi = 1 }, + { chemin = "PID.alt.kp", label = "altitude kp", pas = 0.1, + mini = 0.1 }, + { chemin = "PID.vitesse.kp", label = "vitesse kp", pas = 0.5, + mini = 0 }, + { chemin = "PID.vitesse.ki", label = "vitesse ki", pas = 0.5, + mini = 0 }, + { chemin = "PID.vitesse.kd", label = "vitesse kd", pas = 0.25, + mini = 0 }, + { chemin = "PID.tangage.kp", label = "tangage kp", pas = 0.25, + mini = 0 }, + { chemin = "PID.tangage.ki", label = "tangage ki", pas = 0.05, + mini = 0 }, + { chemin = "PID.tangage.kd", label = "tangage kd", pas = 0.25, + mini = 0 }, + { chemin = "PID.roulis.kp", label = "roulis kp", pas = 0.25, + mini = 0 }, + { chemin = "PID.roulis.ki", label = "roulis ki", pas = 0.05, + mini = 0 }, + { chemin = "PID.roulis.kd", label = "roulis kd", pas = 0.25, + mini = 0 }, + { chemin = "MOTEUR.MARGE", label = "marge moteur", pas = 0.05, + mini = 0.05, maxi = 0.5 }, } local function lireChemin(chemin) @@ -164,12 +101,12 @@ local function lireChemin(chemin) end local function ecrireChemin(chemin, valeur) - local noeud, precedente, cle = conf, nil, nil + local noeud, parent, cle = conf, nil, nil for partie in chemin:gmatch("[^%.]+") do - precedente, cle = noeud, partie + parent, cle = noeud, partie noeud = noeud[partie] end - precedente[cle] = valeur + parent[cle] = valeur end etat.surcharges = etat.surcharges or {} @@ -177,319 +114,307 @@ for chemin, valeur in pairs(etat.surcharges) do ecrireChemin(chemin, valeur) end -local pilotage = charger("lib/pilotage.lua") +local regulation = charger("lib/regulation.lua") .nouveau(conf, etat, materiel, Pid, journal) -exigerInterface(pilotage, "lib/pilotage.lua", { - "poidsEstime", "poidsMax", "trim", "razAdaptation", "pasOuvert", - "limitationStress", -}) -local navigation = charger("lib/navigation.lua") +local Calib = charger("lib/calib.lua") +local ecran = charger("lib/ecran.lua") .nouveau(conf, materiel, journal) -local Calibration = charger("lib/calibration.lua") -local ihm = charger("lib/ihm.lua").nouveau(conf, materiel, journal) -os.setComputerLabel("PC_central") +-- coherence des versions de modules (desynchronisation de maj) +local function exigerInterface(objet, fichier, fonctions) + local manquantes = {} + for _, nom in ipairs(fonctions) do + if type(objet[nom]) ~= "function" then + table.insert(manquantes, nom) + end + end + if #manquantes > 0 then + error(("%s obsolete (manque: %s): resynchroniser avec " + .. "'drone maj '"):format(fichier, + table.concat(manquantes, ", ")), 0) + end +end +exigerInterface(materiel, "lib/materiel.lua", { + "lireAssiette", "vitesseGlobale", "lireJoystick", "reglerMoteur", + "rafraichirAffectations", "relaisValides", +}) +exigerInterface(regulation, "lib/regulation.lua", { + "reguler", "plaquer", "rpmHover", "reglerInjection", + "gainsModifies", "vitesseVerticale", +}) +exigerInterface(ecran, "lib/ecran.lua", { + "rafraichirMoniteur", "choisirRole", "message", +}) -------------------------------------------------------------------- -- ETAT DE VOL -------------------------------------------------------------------- -local DT = 0.1 -local mode = etat.mode or "off" +local mode = etat.mode or "off" local consigneY = etat.consigneY or materiel.lireAltitude() -local cibleNav = nil -local phaseAuto = nil -local distanceCible = nil -local stress = nil -local carburantPct, autonomieMin = nil, nil -local saisieTypewriter = "" -local saisieCible = "" local enCalibration = false -local calibEnCours = nil -local calibProgres, calibTexte = nil, nil +local calibEnCours, calibProgres, calibTexte = nil, nil, nil local rscViolet = nil +local carburantPct, autonomieMin = nil, nil local alerteMoteurA = 0 -local alerteChargeA = 0 -local alerteStressA = 0 -local boostMoteurFin = 0 -local niveauMoteur = 15 +local reidentificationApres = 0 +local DT = 0.1 local function sauverEtat() - etat.mode, etat.consigneY = mode, consigneY + etat.mode = mode + etat.consigneY = consigneY Etat.sauver(etat) end -------------------------------------------------------------------- --- VERROUS: modes indisponibles tant que des calibrations manquent +-- VERROUS DE MODES (calibrations requises) -------------------------------------------------------------------- local function verrouDe(m) - local gimbalOk = etat.gimbal ~= nil - local helicesOk = #materiel.helicesManquantes() == 0 - local joystickOk = etat.joystick ~= nil - - if m == "off" or m == "inactif" or m == "calibrage" then return nil end - if not gimbalOk then return "calib gimbal requise" end - if m == "stationnement" then return nil end - if not helicesOk then return "calib rsc requise" end - if (m == "vol" or m == "drone") and not joystickOk then - return "calib joystick requise" + if m == "off" or m == "sol" then return nil end + if not etat.gimbal then return "calib gimbal requise" end + if #materiel.helicesManquantes() > 0 then + return "calib rsc requise" end - if m == "auto" and not materiel.lirePosition() then - return "pas de GPS" + if m == "vol" and not etat.calibAltitude then + return "calib altitude requise" + end + if (m == "vol" or m == "atterrissage" or m == "stationnement") + and not materiel.relaisValides() then + return "relays renumerotes: passer en mode sol (reident. auto)" + end + if m == "stationnement" then + if not materiel.auSol() then return "pas au sol" end + local t, r = materiel.lireAssiette() + if math.max(math.abs(t), math.abs(r)) > conf.ANGLE_MAX then + return "pas horizontal" + end end return nil end local function verrous() local v = {} - for _, m in ipairs({ "off", "inactif", "calibrage", "stationnement", - "vol", "drone", "atterrissage", "auto" }) do + for _, m in ipairs({ "off", "sol", "stationnement", "vol", + "atterrissage" }) do v[m] = verrouDe(m) end return v end -------------------------------------------------------------------- --- CHANGEMENTS DE MODE +-- MACHINE D'ETATS -------------------------------------------------------------------- local function changerMode(nouveau, raison) if nouveau == mode then return end - local modePrec = mode - local verrou = verrouDe(nouveau) if verrou then - journal.alerte(nouveau .. " refuse: " .. verrou) - ihm.message(nouveau .. ": " .. verrou) + ecran.message(nouveau .. ": " .. verrou) + journal.info(("mode %s refuse: %s"):format(nouveau, verrou)) return end - if nouveau == "stationnement" - and not (materiel.auSol() and materiel.horizontal()) then - journal.alerte("stationnement refuse: au sol et horizontal requis") - ihm.message("stationnement: sol+plat requis") - return + journal.info(("mode: %s (%s)"):format(nouveau, raison or "?")) + local etaitAuSol = mode == "off" or mode == "sol" + or mode == "stationnement" + mode = nouveau + regulation.raz() + if etaitAuSol and (mode == "vol" or mode == "atterrissage") then + regulation.razAdaptation() end - - -- pre-boost du moteur avant les regimes exigeants (evite - -- l'overstress transitoire, ex: inactif -> stationnement) - local exigeants = { stationnement = true, vol = true, drone = true, - atterrissage = true, auto = true } - if exigeants[nouveau] then - boostMoteurFin = os.clock() + conf.MOTEUR.BOOST + if mode == "vol" then + consigneY = materiel.lireAltitude() + elseif mode == "atterrissage" then + consigneY = materiel.lireAltitude() + elseif mode == "off" then + materiel.toutArreter() + materiel.reglerMoteur(0) + else + materiel.toutArreter() + end + if mode ~= "off" and etat.modeMoteur == "onoff" then materiel.reglerMoteur(15) end - - 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 - phaseAuto = "decollage" - local x, y, z = materiel.lirePosition() - etat.decollage = { x = x, y = y, z = z } - consigneY = cibleNav.y or conf.Y_VOL - elseif mode == "off" then - pilotage.arreter() - end - - journal.info("mode: " .. mode .. (raison and (" (" .. raison .. ")") or "")) + -- reassertion du signal des boites (rapport fixe, jamais change + -- en vol; on s'assure qu'il est bien applique avant de voler) + materiel.appliquerBoite() sauverEtat() end -------------------------------------------------------------------- --- DESCENTE SECURISEE (garde-fou ocean) --- La descente sous Y_MINI exige un sol POSABLE detecte par --- l'optical sensor; sinon: maintien a Y_MINI + alerte. +-- SURVEILLANCE DES BOITES (vol): la vitesse reelle des helices +-- (speedometers) doit suivre commande * rapport; un ecart > 25% +-- persistant 2 s declenche une alerte (cablage / signal perdu) -------------------------------------------------------------------- -local function descendre() - local cible = consigneY - conf.VITESSE_ATTERRISSAGE * DT - if cible < conf.Y_MINI and not materiel.solPosable() then - ihm.message("sol non posable: maintien Y_MINI") - return math.max(consigneY, conf.Y_MINI) +local ecartBoiteDepuis = nil +local alerteBoiteA = 0 +local function surveillerBoites() + if not (etat.boite and etat.speedoParRole) then return end + local rapport = materiel.rapportBoite() + local nb, ecarts = 0, 0 + for _, role in ipairs({ "lb", "rb", "lf", "rf" }) do + local commande = materiel.rpm[role] and materiel.rpm[role].v + local reel = materiel.lireVitesseHelice(role) + if commande and reel and math.abs(commande) > 8 then + local attendu = math.abs(commande) * rapport + if math.abs(math.abs(reel) - attendu) > attendu * 0.25 then + ecarts = ecarts + 1 + end + nb = nb + 1 + end + end + if nb > 0 and ecarts >= math.max(1, nb - 1) then + ecartBoiteDepuis = ecartBoiteDepuis or os.clock() + if os.clock() - ecartBoiteDepuis > 2 + and os.clock() - alerteBoiteA > 10 then + alerteBoiteA = os.clock() + journal.alerte("boites de vitesse incoherentes (vitesse " + .. "helices != commande * rapport): verifier le signal") + ecran.message("BOITES INCOHERENTES", 6) + materiel.appliquerBoite() -- reassertion du signal + end + else + ecartBoiteDepuis = nil end - return cible end -------------------------------------------------------------------- --- UN PAS DE CONTROLE PAR MODE +-- UN PAS DE CONTROLE -------------------------------------------------------------------- local function pasControle() - if enCalibration then return end - - -- alerte moteur: dans tous les modes ou il devrait tourner - if mode ~= "off" and not materiel.moteurTourne() then - if os.clock() - alerteMoteurA > 5 then - alerteMoteurA = os.clock() - journal.erreur("le moteur ne tourne pas alors qu'il le devrait") - ihm.message("ERREUR: MOTEUR ARRETE", 5) - 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 - - 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 + if mode ~= "off" and not materiel.moteurTourne() + and os.clock() - alerteMoteurA > 5 then + alerteMoteurA = os.clock() + journal.erreur("le moteur ne tourne pas alors qu'il le devrait") + ecran.message("MOTEUR ARRETE", 5) end if mode == "off" then - pilotage.arreter() - - elseif mode == "inactif" or mode == "calibrage" then - -- moteur allume, tout a zero; calibrage autorise les calibrations - materiel.toutArreter() - + -- tout est deja coupe + elseif mode == "sol" then + -- moteur allume, RSC a zero; reidentification AUTOMATIQUE des + -- relays si les noms stockes ne correspondent plus au reseau + if not materiel.relaisValides() + and os.clock() > reidentificationApres then + reidentificationApres = os.clock() + 30 + journal.alerte("relays renumerotes: reidentification " + .. "automatique en cours") + ecran.message("REIDENTIFICATION RELAYS", 5) + pcall(Calib.reidentifierRelais, conf, etat, materiel, journal, + Etat) + end elseif mode == "stationnement" then - if not (materiel.auSol() and materiel.horizontal()) then + -- plaquage; sortie AUTO vers vol si les conditions sont perdues + if not materiel.auSol() then changerMode("vol", "conditions de stationnement perdues") return end - pilotage.plaquer() - - elseif mode == "vol" then - local avance, virage = materiel.lireJoystick() - pilotage.reguler(consigneY, { tangage = 0, roulis = 0 }, - avance, virage, DT) - - elseif mode == "drone" then - local avance, virage = materiel.lireJoystick() - pilotage.reguler(consigneY, { - tangage = avance * conf.ANGLE_DRONE_MAX, - roulis = -virage * conf.ANGLE_DRONE_MAX, - }, 0, 0, DT) - - elseif mode == "atterrissage" then - if materiel.auSol() then - changerMode("stationnement", "sol atteint") + local t, r = materiel.lireAssiette() + if math.max(math.abs(t), math.abs(r)) > conf.ANGLE_MAX * 2 then + changerMode("vol", "assiette perdue au stationnement") return end - consigneY = descendre() - pilotage.reguler(consigneY, { tangage = 0, roulis = 0 }, 0, 0, DT) - - elseif mode == "auto" then - -- cible a 2 coordonnees (x, z): mission complete avec - -- atterrissage; cible a 3 (x, y, z): rallier et MAINTENIR a y - local yCroisiere = cibleNav.y or conf.Y_VOL - if cibleNav.y and cibleNav.y < materiel.lireAltitude() then - -- descente limitee par la presence d'un sol - if materiel.lireDistanceSol() <= conf.DIST_SOL + 0.5 then - yCroisiere = materiel.lireAltitude() - elseif cibleNav.y < conf.Y_MINI and not materiel.solPosable() then - yCroisiere = conf.Y_MINI - end + regulation.plaquer() + elseif mode == "vol" then + local avance, virage = materiel.lireJoystick() + regulation.reguler(consigneY, avance, virage, DT) + surveillerBoites() + elseif mode == "atterrissage" then + consigneY = consigneY - conf.VITESSE_ATTERRISSAGE * DT + regulation.reguler(consigneY, 0, 0, DT) + if materiel.auSol() then + changerMode("stationnement", "sol atteint") end + end - if phaseAuto == "decollage" then - local alt = pilotage.reguler(yCroisiere, - { tangage = 0, roulis = 0 }, 0, 0, DT) - if math.abs(alt - yCroisiere) <= 0.5 then - phaseAuto = "croisiere" - journal.info("auto: croisiere vers la cible") - end - elseif phaseAuto == "croisiere" then - local avance, virage, distance = navigation.rallier( - cibleNav.x, cibleNav.z) - if not avance then - changerMode("vol", "GPS perdu") - return - end - distanceCible = distance - pilotage.reguler(yCroisiere, { tangage = 0, roulis = 0 }, - avance, virage, DT) - if distance <= conf.NAV.SEUIL_ARRIVEE then - if cibleNav.y then - phaseAuto = "maintien" - journal.info("auto: cible atteinte, maintien en position") - else - phaseAuto = "atterrissage" - consigneY = materiel.lireAltitude() - journal.info("auto: cible atteinte, atterrissage") + -- persistance du rapport de boite (bascules automatiques en vol) + if regulation.boiteModifiee and regulation.boiteModifiee() then + Etat.sauver(etat) + end + + -- persistance des reductions du superviseur d'oscillation + if regulation.gainsModifies() then + for _, axe in ipairs({ "vitesse", "tangage", "roulis" }) do + local g = conf.PID[axe] + etat.surcharges["PID." .. axe .. ".kp"] = g.kp + etat.surcharges["PID." .. axe .. ".ki"] = g.ki + etat.surcharges["PID." .. axe .. ".kd"] = g.kd + end + Etat.sauver(etat) + end +end + +local function tacheControle() + while true do + if not enCalibration then + local ok, err = pcall(pasControle) + if not ok then journal.erreur("controle: " .. tostring(err)) end + end + sleep(DT) + end +end + +-------------------------------------------------------------------- +-- CARBURANT (autonomie apprise par EMA, mode off exclu) +-------------------------------------------------------------------- +local function tacheCarburant() + local quantitePrec, tPrec, conso = nil, nil, nil + local alerte = false + while true do + local ok, pct, quantite = pcall(materiel.lireCarburant) + if ok and pct then + carburantPct = pct + local t = os.clock() + if quantitePrec and t > tPrec and mode ~= "off" then + local instantanee = (quantitePrec - quantite) / (t - tPrec) + if instantanee >= 0 then + conso = conso + and (conso + conf.CARBURANT.LISSAGE + * (instantanee - conso)) + or instantanee end end - elseif phaseAuto == "maintien" then - local avance, virage, distance = navigation.rallier( - cibleNav.x, cibleNav.z) - distanceCible = distance - pilotage.reguler(yCroisiere, { tangage = 0, roulis = 0 }, - avance or 0, virage or 0, DT) - else - if materiel.auSol() then - changerMode("stationnement", "mission terminee") - return + quantitePrec, tPrec = quantite, t + autonomieMin = (conso and conso > 0.001) + and (quantite / conso / 60) or nil + if pct < conf.CARBURANT.SEUIL_ALERTE and not alerte then + alerte = true + journal.alerte("carburant sous " + .. math.floor(conf.CARBURANT.SEUIL_ALERTE * 100) .. "%") + ecran.message("CARBURANT BAS", 10) + elseif pct > conf.CARBURANT.SEUIL_ALERTE + 0.05 then + alerte = false end - consigneY = descendre() - pilotage.reguler(consigneY, { tangage = 0, roulis = 0 }, 0, 0, DT) end + sleep(conf.CARBURANT.PERIODE) end end -------------------------------------------------------------------- -- CALIBRATIONS -------------------------------------------------------------------- -local rafraichirCalib = nil -- affecte apres contexteIhm +local rafraichirCalib = nil -- affecte apres contexteEcran local ui = { - inviter = function(texte) - ihm.message(texte, 3600) - calibTexte = texte - pcall(ihm.rafraichirConduite, + inviter = function(contenu) + ecran.message(contenu, 3600) + calibTexte = contenu + pcall(ecran.rafraichirConduite, { mode = mode, consigneY = consigneY }) if rafraichirCalib then rafraichirCalib() end - journal.info("[calib] " .. texte) + journal.info("[calib] " .. contenu) end, - progres = function(pct, texte) - calibProgres, calibTexte = pct, texte + progres = function(pct, contenu) + if pct then calibProgres = pct end + if contenu then calibTexte = contenu end if rafraichirCalib then rafraichirCalib() end end, rscViolet = function(role) rscViolet = role end, - attendreToucher = ihm.attendreToucher, - choisirRole = ihm.choisirRole, + attendreToucher = ecran.attendreToucher, + choisirRole = ecran.choisirRole, } --- vide la file d'evenements (touchers accumules pendant la calib, --- qui seraient sinon rejoues sur d'anciennes zones) local function purgerEvenements() os.queueEvent("purge_evenements") while true do - local ev = os.pullEvent() - if ev == "purge_evenements" then return end + if os.pullEvent() == "purge_evenements" then return end end end @@ -497,92 +422,52 @@ local function lancerCalibration(quoi) enCalibration = true calibEnCours = quoi calibProgres, calibTexte = 0, quoi - ihm.reglerPage("calib") - local solAvecMoteur = { rsc = true, rscman = true, moteur = true } - if solAvecMoteur[quoi] and mode == "calibrage" then - -- tacheMoteur est suspendue pendant la calibration: on assure - -- la pleine puissance nous-memes - materiel.reglerMoteur(15) - end + ecran.reglerPage("calib") local ok, err = pcall(function() - if quoi == "gimbal" or quoi == "joystick" or quoi == "typew" - or quoi == "rsc" or quoi == "rscman" or quoi == "moteur" then - if mode ~= "calibrage" then - journal.alerte("calib " .. quoi - .. ": passer en mode calibrage d'abord") - ihm.message("passer en mode calibrage") + if quoi == "inclinaison" then + if mode ~= "vol" then + journal.alerte("calib inclinaison: passer en mode vol " + .. "(stationnaire)") + ecran.message("passer en mode vol") return end - end - if quoi == "gimbal" then - Calibration.gimbal(conf, etat, materiel, journal, Etat, ui) + Calib.inclinaison(conf, etat, materiel, regulation, journal, + Etat, ui) + elseif mode ~= "sol" and mode ~= "stationnement" then + journal.alerte("calib " .. quoi .. ": passer en mode sol") + ecran.message("passer en mode sol") + elseif quoi == "gimbal" then + Calib.gimbal(conf, etat, materiel, journal, Etat, ui) elseif quoi == "joystick" then - Calibration.joystick(conf, etat, materiel, journal, Etat, ui) - elseif quoi == "typew" then - Calibration.typewriter(conf, etat, materiel, journal, Etat, ui) + Calib.joystick(conf, etat, materiel, journal, Etat, ui) elseif quoi == "rsc" then - 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") - 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() - return function() - for _ = 1, math.floor(duree / DT) do - pilotage.reguler(cible, angles, avance or 0, 0, DT) - sleep(DT) - end - end - end - Calibration.capteurs(conf, etat, materiel, journal, Etat, ui, { - monter = function() - local cible = materiel.lireAltitude() + 3 - for _ = 1, 30 do - pilotage.reguler(cible, { tangage = 0, roulis = 0 }, - 0, 0, DT) - sleep(DT) - end - end, - avancer = tenir(3, { tangage = 0, roulis = 0 }, 0.6), - gauche = tenir(3, - { tangage = 0, roulis = -conf.ANGLE_DRONE_MAX / 2 }, 0), - }) - end + Calib.rsc(conf, etat, materiel, journal, Etat, ui) + elseif quoi == "boite" then + Calib.boite(conf, etat, materiel, journal, Etat, ui) + elseif quoi == "altitude" then + Calib.altitude(conf, etat, materiel, regulation, journal, + Etat, ui) end end) if not ok then journal.erreur("calibration: " .. tostring(err)) end - ihm.message("", 0) + ecran.message("", 0) calibEnCours, rscViolet = nil, nil enCalibration = false + materiel.toutArreter() + if mode ~= "off" and etat.modeMoteur == "onoff" then + materiel.reglerMoteur(15) + end purgerEvenements() end --- statuts des calibrations pour la page CALIB local function calibStatuts() local faits = { gimbal = etat.gimbal ~= nil, joystick = etat.joystick ~= nil, - typew = etat.typewriter ~= nil, - rsc = #materiel.helicesManquantes() == 0 - and #materiel.propulseursManquants() == 0, - rscman = #materiel.helicesManquantes() == 0 - and #materiel.propulseursManquants() == 0, - capteurs = #materiel.axesManquants() == 0, - pid = etat.pid ~= nil, - pidmath = etat.pid ~= nil, - moteur = etat.capaciteParNiveau ~= nil, + rsc = #materiel.helicesManquantes() == 0, + boite = etat.boite ~= nil, + altitude = etat.calibAltitude == true, + inclinaison = etat.calibInclinaison == true, } local statuts = {} for nom, fait in pairs(faits) do @@ -593,227 +478,13 @@ local function calibStatuts() end -------------------------------------------------------------------- --- POINTS NOMMES / RETOUR DECOLLAGE +-- ECRANS -------------------------------------------------------------------- -local function nbPoints() - local n = 0 - for _ in pairs(etat.points) do n = n + 1 end - return n -end - -local function commandePoint(mots) - if mots[2] == "save" and mots[3] then - if nbPoints() >= conf.NAV.MAX_POINTS and not etat.points[mots[3]] then - print(("maximum %d points"):format(conf.NAV.MAX_POINTS)) - return - end - local x, y, z = materiel.lirePosition() - if not x then print("pas de GPS") return end - etat.points[mots[3]] = { x = x, y = y, z = z } - Etat.sauver(etat) - print(("point '%s' enregistre (%.0f %.0f %.0f)"):format(mots[3], x, y, z)) - elseif mots[2] == "go" and mots[3] then - local p = etat.points[mots[3]] - if not p then print("point inconnu") return end - cibleNav = { x = p.x, z = p.z } - changerMode("auto", "pt go " .. mots[3]) - elseif mots[2] == "del" and mots[3] then - etat.points[mots[3]] = nil - Etat.sauver(etat) - print("point supprime") - elseif mots[2] == "list" then - for nom, p in pairs(etat.points) do - print((" %s : %.0f %.0f %.0f"):format(nom, p.x, p.y, p.z)) - end - else - print("pt save | pt go | pt del | pt list") - end -end - --------------------------------------------------------------------- --- TACHES PARALLELES --------------------------------------------------------------------- -local function tacheControle() - while true do - local ok, err = pcall(pasControle) - if not ok then - journal.erreur("controle: " .. tostring(err)) - pilotage.arreter() - end - sleep(DT) - end -end - --- 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 enSurcharge = false - while true do - if enCalibration then - -- les calibrations pilotent le moteur elles-memes - elseif mode == "off" then - materiel.reglerMoteur(0) - elseif os.clock() < boostMoteurFin then - niveauMoteur = 15 - materiel.reglerMoteur(15) - else - stress = materiel.lireStress() - 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 - -- 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 - niveauMoteur = 15 - end - - -- 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 -end - --- carburant: pourcentage de reserve + autonomie estimee par --- APPRENTISSAGE GLISSANT (EMA) de la consommation. Les hausses de --- quantite (ravitaillement) sont exclues de l'apprentissage. -local function tacheCarburant() - local quantitePrec, tPrec = nil, nil - local consoLissee = nil -- mB/s apprise - while true do - local quantite, capacite = materiel.lireCarburant() - if quantite then - carburantPct = quantite / capacite - local t = os.clock() - - if quantitePrec and t > tPrec and mode ~= "off" then - local conso = (quantitePrec - quantite) / (t - tPrec) - if conso >= 0 then -- conso negative = ravitaillement: ignore - local a = conf.CARBURANT.LISSAGE - consoLissee = consoLissee - and (a * conso + (1 - a) * consoLissee) - or conso - end - end - quantitePrec, tPrec = quantite, t - - autonomieMin = nil - if consoLissee and consoLissee > 0 then - autonomieMin = quantite / consoLissee / 60 - end - - if carburantPct <= conf.CARBURANT.SEUIL_ALERTE then - ihm.message("CARBURANT BAS", 4) - end - pcall(ihm.rafraichirCarburant, carburantPct, autonomieMin) - end - sleep(conf.CARBURANT.PERIODE) - end -end - --- typewriter: saisie de la consigne d'altitude (scrutation des --- codes ASCII, clavier qwerty). Mappage par defaut integre; la --- calibration 'typew' (etat.typewriter) le remplace si presente. --- 0..9 chiffres de la saisie --- Entree valider la saisie comme nouvelle consigne --- Backspace effacer le dernier chiffre --- + / - ajuster la consigne de +-1 (hors saisie en cours) -local ASCII = { - [13] = "valider", [10] = "valider", - [8] = "effacer", [127] = "effacer", - [43] = "plus", [45] = "moins", -} -for code = 48, 57 do ASCII[code] = string.char(code) end - -local function tacheTypewriter() - local pressees = {} - while true do - if not enCalibration then - local courantes = {} - for _, code in ipairs(materiel.lireTouches()) do - courantes[code] = true - if not pressees[code] then - local symbole = (etat.typewriter or ASCII)[code] - if symbole == "valider" then - local valeur = tonumber(saisieTypewriter) - if valeur then - consigneY = valeur - journal.info(("typewriter: consigne %d"):format(valeur)) - sauverEtat() - end - saisieTypewriter = "" - elseif symbole == "effacer" then - saisieTypewriter = saisieTypewriter:sub(1, -2) - elseif symbole == "plus" and #saisieTypewriter == 0 then - consigneY = consigneY + 1 - sauverEtat() - elseif symbole == "moins" and #saisieTypewriter == 0 then - consigneY = consigneY - 1 - sauverEtat() - elseif symbole and #symbole == 1 then - saisieTypewriter = (saisieTypewriter .. symbole):sub(1, 6) - end - end - end - pressees = courantes - end - sleep(0.1) - end -end - --- GPS: seule tache autorisee a faire l'appel bloquant (jusqu'a 2 s) -local function tachePosition() - while true do - if mode ~= "off" then - pcall(materiel.actualiserPosition) - end - sleep(3) - end -end - local function listeParams() local liste = {} for _, p in ipairs(PARAMS) do - table.insert(liste, { label = p.label, valeur = lireChemin(p.chemin) }) + table.insert(liste, + { label = p.label, valeur = lireChemin(p.chemin) }) end return liste end @@ -824,153 +495,118 @@ local function ajusterParam(idx, sens) local valeur = (lireChemin(p.chemin) or 0) + sens * p.pas if p.mini and valeur < p.mini then valeur = p.mini end if p.maxi and valeur > p.maxi then valeur = p.maxi end + valeur = math.floor(valeur * 100 + 0.5) / 100 ecrireChemin(p.chemin, valeur) etat.surcharges[p.chemin] = valeur Etat.sauver(etat) journal.info(("param %s = %s"):format(p.chemin, tostring(valeur))) - if etat.pid then - ihm.message("param modifie: recalibrer pid conseille") - end end -local function contexteIhm() - local px, py, pz = materiel.lirePosition() +local function contexteEcran() return { mode = mode, consigneY = consigneY, - stress = stress, moteurOk = mode == "off" or materiel.moteurTourne(), carburantPct = carburantPct, minutes = autonomieMin, - position = px and { x = px, y = py, z = pz } or nil, - points = etat.points, + modeMoteur = etat.modeMoteur, verrous = verrous(), calibStatuts = calibStatuts(), calibProgres = calibProgres, calibTexte = calibTexte, rscViolet = rscViolet, - cible = cibleNav and (cibleNav.y - and ("%d %d %d"):format(cibleNav.x, cibleNav.y, cibleNav.z) - 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()), - niveauMoteur = niveauMoteur, - stressPrevu = materiel.stressPrevu(), + rapportBoite = etat.boite + and (tostring(math.floor(etat.boite.rapport * 16)) .. "/16") + or nil, } end --- lance la cible saisie: "x z" ou "x y z" -local function lancerCible(texte) - local nombres = {} - for n in texte:gmatch("%-?%d+") do table.insert(nombres, tonumber(n)) end - if #nombres == 2 then - cibleNav = { x = nombres[1], z = nombres[2] } - changerMode("auto", "cible x z") - elseif #nombres == 3 then - cibleNav = { x = nombres[1], y = nombres[2], z = nombres[3] } - changerMode("auto", "cible x y z") - else - ihm.message("format: x z ou x y z") - end -end - rafraichirCalib = function() - pcall(ihm.rafraichirMoniteur, contexteIhm()) + pcall(ecran.rafraichirMoniteur, contexteEcran()) end -local function tacheIhm() +local function tacheEcran() local prochainDessin = 0 local redessiner = false while true do - if (os.clock() >= prochainDessin or redessiner) then + if os.clock() >= prochainDessin or redessiner then redessiner = false - pcall(ihm.rafraichirConduite, { - mode = mode, consigneY = consigneY, saisie = saisieTypewriter, - }) - pcall(ihm.rafraichirMoniteur, contexteIhm()) + pcall(ecran.rafraichirConduite, + { mode = mode, consigneY = consigneY }) + pcall(ecran.rafraichirCarburant, carburantPct) + pcall(ecran.rafraichirMoniteur, contexteEcran()) prochainDessin = os.clock() + 1.0 end - local minuteur = os.startTimer(0.25) local ev, a, b, c = os.pullEvent() - if ev == "monitor_touch" then - local action = not enCalibration and ihm.traiterToucher(b, c) + if ev == "monitor_touch" and not enCalibration then + local action = ecran.traiterToucher(b, c) if action then redessiner = true if action.type == "mode" then if action.verrou then - ihm.message(action.valeur .. ": " .. action.verrou) + ecran.message(action.valeur .. ": " .. action.verrou) else changerMode(action.valeur, "moniteur") end elseif action.type == "delta" then consigneY = consigneY + action.valeur sauverEtat() + elseif action.type == "modemoteur" then + etat.modeMoteur = action.valeur + Etat.sauver(etat) + if mode ~= "off" and etat.modeMoteur == "onoff" then + materiel.reglerMoteur(15) + end + journal.info("regime moteur: " .. action.valeur) elseif action.type == "calib" then lancerCalibration(action.valeur) - elseif action.type == "ptgo" then - local p = etat.points[action.nom] - if p then - cibleNav = { x = p.x, z = p.z } - changerMode("auto", "pt " .. action.nom) - 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) - elseif action.valeur == "go" then - lancerCible(saisieCible) - saisieCible = "" - else - saisieCible = (saisieCible .. action.valeur):sub(1, 18) - end end end end - if ev ~= "timer" or a ~= minuteur then os.cancelTimer(minuteur) end - end -end - -local function tacheClavier() - while true do - local ligne = read() - local mots = {} - for mot in ligne:gmatch("%S+") do table.insert(mots, mot) end - if mots[1] == "auto" and tonumber(mots[2]) and tonumber(mots[3]) then - if tonumber(mots[4]) then - cibleNav = { x = tonumber(mots[2]), y = tonumber(mots[3]), - z = tonumber(mots[4]) } - else - cibleNav = { x = tonumber(mots[2]), z = tonumber(mots[3]) } - end - changerMode("auto", "clavier") - elseif mots[1] == "pt" then - commandePoint(mots) - elseif mots[1] == "rtb" then - if etat.decollage then - cibleNav = { x = etat.decollage.x, z = etat.decollage.z } - changerMode("auto", "retour decollage") - else - print("pas de point de decollage enregistre") - end - elseif mots[1] then - print("auto | auto | pt ... | rtb") + if ev ~= "timer" or a ~= minuteur then + os.cancelTimer(minuteur) end end end -journal.info(("drone: demarrage en mode %s (consigne %.0f)") - :format(mode, consigneY)) -if mode == "vol" or mode == "drone" then - consigneY = materiel.lireAltitude() +-------------------------------------------------------------------- +-- CLAVIER (minimal) +-------------------------------------------------------------------- +local function tacheClavier() + while true do + write("> ") + local ligne = read() + local mots = {} + for mot in ligne:gmatch("%S+") do table.insert(mots, mot) end + if mots[1] == "mode" and mots[2] then + changerMode(mots[2], "clavier") + elseif mots[1] == "consigne" and tonumber(mots[2]) then + consigneY = tonumber(mots[2]) + sauverEtat() + else + print("mode | " + .. "consigne ") + end + end end -parallel.waitForAny(tacheControle, tacheMoteur, tacheCarburant, - tachePosition, tacheTypewriter, tacheIhm, tacheClavier) + +-------------------------------------------------------------------- +-- DEMARRAGE +-------------------------------------------------------------------- +journal.info(("drone V2: demarrage en mode %s (consigne %.0f)") + :format(mode, consigneY)) +if mode == "off" then + materiel.toutArreter() + materiel.reglerMoteur(0) +else + materiel.toutArreter() + if etat.modeMoteur == "onoff" then materiel.reglerMoteur(15) end + if mode == "vol" or mode == "atterrissage" then + consigneY = materiel.lireAltitude() + end +end + +parallel.waitForAny(tacheControle, tacheCarburant, tacheEcran, + tacheClavier) diff --git a/lib/calib.lua b/lib/calib.lua new file mode 100644 index 0000000..387d70e --- /dev/null +++ b/lib/calib.lua @@ -0,0 +1,735 @@ +-------------------------------------------------------------------- +-- 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.5, 0.08 * rpm) + g.ki = math.max(0.05, 0.008 * rpm) + g.kd = math.max(0.5, 0.15 * rpm) + persisterGains(etat, Etat, axe, g) + end + journal.info("calib altitude: gains d'assiette par defaut " + .. "dimensionnes sur la sustentation") + 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 + + 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 diff --git a/lib/calibration.lua b/lib/calibration.lua deleted file mode 100644 index ce2edd3..0000000 --- a/lib/calibration.lua +++ /dev/null @@ -1,790 +0,0 @@ --------------------------------------------------------------------- --- 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.progres(0, "mise a plat") - ui.inviter("Drone a plat, touchez l'ecran") - ui.attendreToucher() - local x0, z0 = materiel.lireAnglesBruts() - - ui.progres(0.25, "nez bas") - 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.progres(0.5, "retour a plat") - ui.inviter("Revenez a plat, touchez") - ui.attendreToucher() - x0, z0 = materiel.lireAnglesBruts() - - ui.progres(0.75, "penche a gauche") - 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.progres(1, "terminee") - 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 idx, direction in ipairs({ "devant", "derriere", "gauche", "droite" }) do - ui.progres((idx - 1) / 4, direction) - 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.progres(1, "terminee") - 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 idx, symbole in ipairs(symboles) do - ui.progres((idx - 1) / #symboles, symbole) - 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.progres(1, "terminee") - 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 - - -- RECALIBRATION COMPLETE: on repart de zero (recablage possible) - local anciensRoles = etat.roles - etat.roles = {} - materiel.rafraichirAffectations() - - local noms = {} - for nom in pairs(materiel.rsc) do table.insert(noms, nom) end - table.sort(noms) - - -- reference "a plat" pour le retour a l'etat initial entre mesures - sleep(1.0) - local refT, refR = materiel.lireAssiette() - local function retourInitial() - local fin = os.clock() + 10 - while os.clock() < fin do - local t, r = materiel.lireAssiette() - if math.abs(t - refT) < C.IMPULSION_SEUIL / 2 - and math.abs(r - refR) < C.IMPULSION_SEUIL / 2 then - sleep(0.5) - return true - end - sleep(0.2) - end - return false - end - - local function abandonner(raison) - materiel.toutArreter() - etat.roles = anciensRoles - materiel.rafraichirAffectations() - journal.erreur("calib rsc annulee: " .. raison) - ui.progres(0, "ECHEC: " .. raison) - return false - end - - local affectes = {} -- role -> nom (controle d'unicite) - local propulseurs = {} - - for i, nom in ipairs(noms) do - ui.progres((i - 1) / #noms, "impulsion " .. nom) - if not retourInitial() then - return abandonner("assiette instable avant " .. nom) - end - - journal.info("calib rsc: impulsion sur " .. 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) - - 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") - if affectes[role] then - return abandonner(("conflit: %s et %s -> %s") - :format(affectes[role], nom, role)) - end - affectes[role] = nom - etat.roles[nom] = role - ui.rscViolet(role) - journal.info(" -> helice " .. role) - else - table.insert(propulseurs, nom) - journal.info(" -> pas d'effet d'assiette: propulseur") - end - end - ui.rscViolet(nil) - - 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 - return abandonner(("%d propulseur(s) detecte(s), 2 attendus") - :format(#propulseurs)) - end - - Etat.sauver(etat) - materiel.rafraichirAffectations() -- prise d'effet immediate - ui.progres(1, "terminee") - journal.info("calib rsc: terminee") - return true -end - --------------------------------------------------------------------- --- RSC MANUELLE (drone trop lourd pour les impulsions). --- Chaque RSC tourne a RPM_MANUEL; l'utilisateur touche sur l'art la --- position de l'helice / du propulseur qui tourne. --------------------------------------------------------------------- -function Calibration.rscManuel(conf, etat, materiel, journal, Etat, ui) - -- RECALIBRATION COMPLETE - local anciensRoles = etat.roles - etat.roles = {} - materiel.rafraichirAffectations() - materiel.toutArreter() - - local noms = {} - for nom in pairs(materiel.rsc) do table.insert(noms, nom) end - table.sort(noms) - - local affectes = {} -- role -> nom - for i, nom in ipairs(noms) do - ui.progres((i - 1) / #noms, nom .. " a " - .. conf.CALIB.RPM_MANUEL .. " rpm") - materiel.rsc[nom].setTargetSpeed(conf.CALIB.RPM_MANUEL) - - 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 manuelle: annulee") - ui.progres(0, "annulee") - return false - end - affectes[role] = nom - etat.roles[nom] = role - ui.rscViolet(role) - journal.info(("calib rsc manuelle: %s -> %s"):format(nom, role)) - end - ui.rscViolet(nil) - - Etat.sauver(etat) - materiel.rafraichirAffectations() - ui.progres(1, "terminee") - journal.info("calib rsc manuelle: terminee") - 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 - - -- RECALIBRATION COMPLETE - etat.velocite = {} - materiel.rafraichirAffectations() - - ui.progres(0, "montee...") - 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.progres(0.33, "avancee...") - 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.progres(0.66, "translation gauche...") - 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) - materiel.rafraichirAffectations() - ui.progres(1, "terminee") - ui.inviter("Capteurs calibres") - journal.info("calib capteurs: terminee") - 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, mesures RSC PAR RSC - -- (un seul en rotation a la fois: pas d'overstress possible - -- pendant la calibration) - materiel.reglerMoteur(15) - sleep(C.ATTENTE_NIVEAU) - local stressBase = materiel.lireStressBrut() or 0 - - local function coefficient(roles, base01, libelle) - local somme, n = 0, 0 - for idx, role in ipairs(roles) do - ui.progres(base01 + (idx - 1) / #roles * 0.14, - libelle .. " " .. role) - materiel.reglerRsc(role, C.RPM_STRESS) - sleep(C.ATTENTE_NIVEAU) - local s = materiel.lireStressBrut() or 0 - materiel.reglerRsc(role, 0) - sleep(0.5) - local delta = s - stressBase - if delta > 0 then - somme, n = somme + delta / C.RPM_STRESS, n + 1 - end - end - if n == 0 then return 0 end - return somme / n - end - - local kh = coefficient({ "lb", "rb", "lf", "rf" }, 0.65, - "coeff helice") - local kp = coefficient({ "prop_l", "prop_r" }, 0.82, - "coeff propulseur") - materiel.toutArreter() - 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 --- 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) - 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 - local base01 = (dir.sens > 0) and 0 or 0.5 - ui.progres(base01 + (iter - 1) / C.MAX_ITER / 2, - ("%s %d/%d"):format(dir.nom, iter, C.MAX_ITER)) - 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.poidsCalibration = pilotage.poidsEstime() - journal.info(("calib pid: poids de calibration %.0f pN") - :format(etat.poidsCalibration)) - 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 - ui.progres(1, "terminee") - journal.info("calib pid: terminee") - return true -end - -return Calibration diff --git a/lib/conf.lua b/lib/conf.lua index 86f5188..6801408 100644 --- a/lib/conf.lua +++ b/lib/conf.lua @@ -1,211 +1,85 @@ -------------------------------------------------------------------- --- lib/conf.lua : fichier de configuration utilisateur (drone.conf) --- Cree le fichier avec des valeurs par defaut commentees s'il --- n'existe pas; avertit tant que `verifie` n'est pas passe a true. +-- lib/conf.lua : configuration (drone.conf), cree avec des valeurs +-- par defaut commentees. Les surcharges de la page PARAM (persistees +-- dans drone.etat) PRIMENT sur ce fichier sans l'ecraser. -------------------------------------------------------------------- local Conf = {} -local CHEMIN = "drone.conf" +local DEFAUT = [[ +-- drone.conf : configuration du drone V2. Relire puis passer +-- verifie a true pour supprimer l'alerte au demarrage. +{ + verifie = false, -local MODELE = [[ --- drone.conf : parametres utilisateur du quadricoptere. --- RELIRE ET AJUSTER chaque valeur, puis passer verifie a true. --- NB: les mappages (gimbal, joystick, typewriter, capteurs, RSC) --- ne sont PAS ici: ils sont calibres en jeu et stockes dans drone.etat. -return { - verifie = false, -- passer a true apres relecture + DIST_SOL = 1.6, -- optical: distance "au sol" (blocs) + ANGLE_MAX = 1.5, -- inclinaison max toleree (degres) + VITESSE_ATTERRISSAGE = 1.0,-- descente du mode atterrissage (b/s) + V_MONTEE_MAX = 3.0, -- vitesse verticale max montee (b/s) + V_DESCENTE_MAX = 2.0, -- vitesse verticale max descente (b/s) + RAMPE = 3.0, -- rampe de consigne d'altitude (b/s) + VMAX = 256, -- borne des RSC + ASSIETTE_PLAFOND = 0.5, -- autorite assiette: fraction du rpm + -- de sustentation - ------------------------------------------------------------------ - -- Altitudes (plan de l'altitude sensor du PC central) - ------------------------------------------------------------------ - Y_VOL = 90, -- altitude de croisiere du mode auto - Y_MINI = 70, -- plancher de securite: en dessous, la - -- descente exige un sol POSABLE detecte - -- (evite d'atterrir dans un ocean) - - ------------------------------------------------------------------ - -- Assiette - ------------------------------------------------------------------ - ANGLE_MAX = 1.5, -- inclinaison max toleree hors mode drone (deg) - ANGLE_DRONE_MAX = 15, -- inclinaison commandee max en mode drone (deg) - - ------------------------------------------------------------------ - -- Sol - ------------------------------------------------------------------ - DIST_SOL = 1.2, -- optical sensor: distance <= => au sol - -- (15.5 = aucun bloc detecte) - VITESSE_ATTERRISSAGE = 1.0, -- descente du mode atterrissage (blocs/s) - BLOCS_INTERDITS = { -- blocs sur lesquels ne PAS se poser - ["minecraft:water"] = true, - ["minecraft:lava"] = true, + -- gains par defaut; les calibrations altitude / inclinaison les + -- remplacent (persistes en surcharges, retouchables page PARAM) + PID = { + alt = { kp = 1.0 }, -- err alt -> v cible (1/s) + vitesse = { kp = 8, ki = 12, kd = 1 }, -- v -> delta rpm + tangage = { kp = 2, ki = 0.2, kd = 3 }, + roulis = { kp = 2, ki = 0.2, kd = 3 }, }, - ------------------------------------------------------------------ - -- Position reelle affichee = position GPS + ces offsets - ------------------------------------------------------------------ - OFFSET_X = 0, - OFFSET_Y = 0, - OFFSET_Z = 0, - - ------------------------------------------------------------------ - -- 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) - 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 }, - -- gains d'ASSIETTE: toujours lus ici (editables en direct page - -- PARAM), JAMAIS remplaces par les calibrations pid/pidmath - tangage = { kp = 2, ki = 0.2, kd = 3 }, - roulis = { kp = 2, ki = 0.2, kd = 3 }, - }, - - -- Autorite maximale de la correction d'assiette (et du trim), en - -- FRACTION du rpm de sustentation courant: l'autorite suit le - -- point de fonctionnement du drone (20 rpm comme 200 rpm) au lieu - -- d'etre figee a VMAX/2. - ASSIETTE_PLAFOND = 0.5, - - ------------------------------------------------------------------ - -- Ecrans (faces des deux Create_DisplayLink sur le PC) - ------------------------------------------------------------------ - ECRANS = { - conduite = "front", -- vitesse, altitudes, invites - carburant = "back", -- reserve + autonomie estimee - }, - - ------------------------------------------------------------------ - -- Carburant (2 cuves create:fluid_tank sur le reseau) - ------------------------------------------------------------------ - CARBURANT = { - CAPACITE_TANK = 8000, -- mB par cuve, si tanks() ne donne pas - -- la capacite - SEUIL_ALERTE = 0.15, -- alerte sous ce ratio de reserve - PERIODE = 5, -- periode d'echantillonnage (s) - LISSAGE = 0.1, -- apprentissage glissant de la conso - -- (EMA: 0.1 = lent et stable, 0.5 = reactif) - }, - - ------------------------------------------------------------------ - -- Moteur (sortie redstone bottom: 15=arret, 14=mini, 0=maxi) - -- Le moteur est dimensionne pour suffire; en cas de surcharge - -- malgre tout, les PROPULSEURS sont reduits en priorite + alerte. - ------------------------------------------------------------------ MOTEUR = { - -- 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 + MARGE = 0.15, -- adaptatif: capacite >= besoin*(1+m) + SECURITE = 0.95, -- garde: besoin borne a capacite*s }, - ------------------------------------------------------------------ - -- Navigation (modes auto / position, points enregistres) - ------------------------------------------------------------------ - NAV = { - SEUIL_ARRIVEE = 2.0, -- distance a la cible consideree atteinte - VITESSE_GPS_MIN = 0.5, -- vitesse mini pour estimer le cap (b/s) - MAX_POINTS = 10, -- points nommes enregistrables + CARBURANT = { + LISSAGE = 0.1, -- EMA de la consommation + PERIODE = 5, -- periode de mesure (s) + SEUIL_ALERTE = 0.15, -- alerte sous 15%% + }, + + BOITE = { + RSC_CIBLE = 180, -- calib: rpm RSC vise en sustentation + RSC_HAUT = 200, -- vol: au-dela -> rapport suivant (k+1) + RSC_BAS = 90, -- vol: en-deca -> rapport precedent + -- (k-1, meilleure resolution) + ATTENTE = 3.0, -- temps mort entre deux changements (s) }, - ------------------------------------------------------------------ - -- Calibration (voir README) - ------------------------------------------------------------------ CALIB = { - AMPLITUDE = 4, -- pid: hauteur des echelons de test (blocs) - TOL_DEPASSEMENT = 0.0, -- pid: depassement tolere (0 = strict) - BANDE = 0.3, -- pid: bande de convergence (blocs) - DELAI_STABLE = 2.0, -- pid: duree dans la bande = converge (s) - TIMEOUT = 25, -- pid: duree max d'un echelon (s) - MAX_ITER = 6, -- pid: essais par direction - IMPULSION_PAS = 16, -- rsc: increment de vitesse (rpm) - 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) + RPM_MANUEL = 10, -- rscman: rpm de reperage + SEUIL_JOYSTICK = 8, -- niveau declarant une face (0..15) + ATTENTE_NIVEAU = 0.6, -- balayage moteur: attente par niveau + RAMPE_DECOLLAGE = 2, -- calib altitude: rpm/s de la rampe + RELAIS_DUREE = 25, -- relais: duree max d'un essai (s) + RELAIS_FRACTION = 0.15, -- relais: d = fraction du rpm hover }, } ]] function Conf.charger(journal) - if not fs.exists(CHEMIN) then - local f = fs.open(CHEMIN, "w") - f.write(MODELE) + if not fs.exists("drone.conf") then + local f = fs.open("drone.conf", "w") + f.write(DEFAUT) f.close() - journal.alerte(CHEMIN .. " cree avec les valeurs par defaut: A PERSONNALISER") + journal.alerte("drone.conf cree avec les valeurs par defaut: " + .. "A PERSONNALISER") end - - local chargeur, err = loadfile(CHEMIN) - if not chargeur then - error("drone.conf invalide: " .. tostring(err), 0) + local f = fs.open("drone.conf", "r") + local contenu = f.readAll() + f.close() + local conf = textutils.unserialize(contenu) + if not conf then + error("drone.conf illisible: corriger ou supprimer le fichier", 0) end - local conf = chargeur() - if type(conf) ~= "table" then - error("drone.conf doit retourner une table", 0) - end - - if conf.verifie ~= true then - journal.alerte("drone.conf non verifie: relire les parametres puis passer verifie a true") + if not conf.verifie then + journal.alerte("drone.conf non verifie: relire les parametres " + .. "puis passer verifie a true") end return conf end --- Empreinte des parametres dont dependent les gains PID calibres. --- Si elle change, la calibration PID est invalidee. -function Conf.empreintePid(conf) - return textutils.serialize({ - 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 - return Conf diff --git a/lib/ecran.lua b/lib/ecran.lua new file mode 100644 index 0000000..f57b8e6 --- /dev/null +++ b/lib/ecran.lua @@ -0,0 +1,375 @@ +-------------------------------------------------------------------- +-- lib/ecran.lua : affichages. +-- Moniteur tactile: echelle auto, onglets MODE / CALIB / PARAM / +-- JOURNAL, art du quadricoptere, choix de role interactif. +-- Display conduite (2x22): ALT reelle -> consigne, VIT globale, +-- messages en segments de mots entiers. +-- Display carburant (2x4): pourcentage. +-------------------------------------------------------------------- +local Ecran = {} + +local function couleurRpm(suivi, vmax) + if not suivi then return colors.lightGray end + if os.clock() - suivi.tChange < 1.0 then return colors.yellow end + if suivi.v == 0 then return colors.white end + if math.abs(suivi.v) >= vmax then return colors.red end + return colors.green +end + +function Ecran.nouveau(conf, materiel, journal) + local e = {} + local moniteur = materiel.moniteur + local zones = {} + local page = "mode" + local defilParams = 0 + local L, H = 0, 0 + + -- la PLUS GRANDE echelle offrant au moins 38x14 + for _, s in ipairs({ 5, 4.5, 4, 3.5, 3, 2.5, 2, 1.5, 1, 0.5 }) do + moniteur.setTextScale(s) + L, H = moniteur.getSize() + if L >= 38 and H >= 14 then break end + end + + ------------------------------------------------------------------ + -- PRIMITIVES + ------------------------------------------------------------------ + local function texte(x, y, contenu, couleur) + moniteur.setTextColor(couleur or colors.white) + moniteur.setCursorPos(x, y) + moniteur.write(contenu) + moniteur.setTextColor(colors.white) + end + + local function bouton(x, y, libelle, action, couleur, couleurTexte) + moniteur.setCursorPos(x, y) + moniteur.setBackgroundColor(couleur or colors.gray) + moniteur.setTextColor(couleurTexte or colors.white) + moniteur.write(" " .. libelle .. " ") + moniteur.setBackgroundColor(colors.black) + moniteur.setTextColor(colors.white) + table.insert(zones, { x1 = x, y1 = y, x2 = x + #libelle + 1, + y2 = y, action = action }) + end + + ------------------------------------------------------------------ + -- CONDUITE (messages en segments de mots entiers) + ------------------------------------------------------------------ + local segments, segmentFin = nil, 0 + + function e.message(contenu, duree) + if not contenu or contenu == "" then segments = nil return end + segments = {} + local courant = "" + for mot in contenu:gmatch("%S+") do + if #courant == 0 then courant = mot + elseif #courant + 1 + #mot <= 22 then + courant = courant .. " " .. mot + else table.insert(segments, courant) courant = mot end + end + if #courant > 0 then table.insert(segments, courant) end + segmentFin = os.clock() + (duree or math.max(5, #segments * 2)) + end + + local function segmentCourant() + if not segments or os.clock() > segmentFin then return nil end + local i = 1 + math.floor((os.clock() % (#segments * 2)) / 2) + i = math.min(i, #segments) + return segments[i] + .. (#segments > 1 and (" " .. i .. "/" .. #segments) or "") + end + + -- ctx: { mode, consigneY } + function e.rafraichirConduite(ctx) + local d = materiel.ecranConduite + if not d then return end + d.clear() + d.setCursorPos(1, 1) + local seg = segmentCourant() + if seg then + d.write(seg:sub(1, 22)) + elseif ctx.mode == "vol" or ctx.mode == "atterrissage" then + d.write(("ALT %.1f > %.0f"):format(materiel.lireAltitude(), + ctx.consigneY or 0):sub(1, 22)) + else + d.write(("MODE %s"):format(ctx.mode:upper()):sub(1, 22)) + end + d.setCursorPos(1, 2) + d.write(("VIT %.2f b/s"):format(materiel.vitesseGlobale()) + :sub(1, 22)) + d.update() + end + + function e.rafraichirCarburant(pct) + local d = materiel.ecranCarburant + if not d then return end + local largeur = d.getSize() + d.clear() + d.setCursorPos(1, 1) + if pct == nil then d.write(("n/a"):sub(1, largeur)) + else + d.write(("%d%%"):format(math.floor(pct * 100 + 0.5)) + :sub(1, largeur)) + end + d.update() + end + + ------------------------------------------------------------------ + -- ART DU QUADRICOPTERE (7 lignes) + ------------------------------------------------------------------ + local function rpmDe(role) + local suivi = materiel.rpm[role] + return suivi and tostring(suivi.v) or "?" + end + + local function couleurDe(role, ctx) + if ctx.rscViolet == role then return colors.purple end + return couleurRpm(materiel.rpm[role], conf.VMAX) + end + + local function dessinerArt(x, y, ctx) + local function coin(cx, cy, role, aGauche) + local couleur = couleurDe(role, ctx) + texte(cx, cy, "O", couleur) + local r = rpmDe(role) + if aGauche then texte(cx - #r - 1, cy, r, couleur) + else texte(cx + 2, cy, r, couleur) end + end + local g, d = x + 6, x + 19 + texte(x + 10, y, "AVANT", colors.lightGray) + coin(g, y + 1, "lf", true) + coin(d, y + 1, "rf", false) + texte(g + 1, y + 1, ("-"):rep(d - g - 1)) + for i = 2, 4 do + texte(g, y + i, "|") + texte(d, y + i, "|") + end + texte(x, y + 3, rpmDe("prop_l"), couleurDe("prop_l", ctx)) + texte(d + 2, y + 3, rpmDe("prop_r"), couleurDe("prop_r", ctx)) + coin(g, y + 5, "lb", true) + coin(d, y + 5, "rb", false) + texte(g + 1, y + 5, ("-"):rep(d - g - 1)) + return y + 7 + end + + ------------------------------------------------------------------ + -- PAGES + ------------------------------------------------------------------ + local function dessinerPageMode(ctx) + local x, y = 1, 2 + for _, m in ipairs({ "off", "sol", "stationnement", "vol", + "atterrissage" }) do + local couleur = colors.gray + if m == ctx.mode then couleur = colors.green + elseif ctx.verrous[m] then couleur = colors.red end + if x + #m + 2 > L then x, y = 1, y + 1 end + bouton(x, y, m, { type = "mode", valeur = m, + verrou = ctx.verrous[m] }, couleur) + x = x + #m + 3 + end + y = y + 2 + + texte(1, y, ("carburant %s autonomie %s"):format( + ctx.carburantPct + and ("%.0f%%"):format(ctx.carburantPct * 100) or "n/a", + ctx.minutes and ("%dmin"):format(math.floor(ctx.minutes)) + or "--")) + y = y + 1 + if ctx.mode ~= "off" then + local stress, capacite = materiel.lireStressBrut() + texte(1, y, ("stress %s/%s niv %d moteur %s"):format( + stress and ("%d"):format(stress) or "?", + capacite and ("%d"):format(capacite) or "?", + materiel.niveauMoteur(), + ctx.moteurOk and "ok" or "ARRETE"), + ctx.moteurOk and colors.white or colors.red) + y = y + 1 + end + -- choix du mode moteur depuis le moniteur + texte(1, y, "regime moteur:") + bouton(16, y, "on/off", { type = "modemoteur", valeur = "onoff" }, + ctx.modeMoteur == "onoff" and colors.green or colors.gray) + bouton(25, y, "adaptatif", + { type = "modemoteur", valeur = "adaptatif" }, + ctx.modeMoteur == "adaptatif" and colors.green or colors.gray) + y = y + 2 + + if ctx.mode == "stationnement" or ctx.mode == "vol" + or ctx.mode == "atterrissage" then + y = dessinerArt(1, y, ctx) + end + + if ctx.mode == "vol" then + texte(1, y, ("vit %.2f b/s alt %.1f -> %.0f boite %s") + :format(materiel.vitesseGlobale(), materiel.lireAltitude(), + ctx.consigneY or 0, ctx.rapportBoite or "1/1")) + y = y + 1 + texte(1, y, "consigne:") + bouton(11, y, "-10", { type = "delta", valeur = -10 }) + bouton(17, y, "-1", { type = "delta", valeur = -1 }) + bouton(22, y, "+1", { type = "delta", valeur = 1 }) + bouton(27, y, "+10", { type = "delta", valeur = 10 }) + elseif ctx.mode == "atterrissage" then + texte(1, y, ("distance sol %.1f (sol posable: au pilote)") + :format(materiel.lireDistanceSol())) + end + end + + local function dessinerPageCalib(ctx) + texte(1, 2, "gris: a faire jaune: en cours vert: fait", + colors.lightGray) + local x, y = 1, 3 + for _, c in ipairs({ "gimbal", "joystick", "rsc", "boite", + "altitude", "inclinaison" }) do + local statut = ctx.calibStatuts[c] + local couleur = colors.gray + if statut == "encours" then couleur = colors.yellow + elseif statut == "fait" then couleur = colors.green end + if x + #c + 2 > L then x, y = 1, y + 1 end + bouton(x, y, c, { type = "calib", valeur = c }, couleur, + statut == "encours" and colors.black or colors.white) + x = x + #c + 3 + end + y = y + 2 + if ctx.calibTexte then + texte(1, y, ctx.calibTexte:sub(1, L), colors.yellow) + y = y + 1 + end + if ctx.calibProgres then + local largeur = L - 2 + local rempli = math.floor(ctx.calibProgres * largeur + 0.5) + moniteur.setCursorPos(1, y) + moniteur.setBackgroundColor(colors.green) + moniteur.write((" "):rep(rempli)) + moniteur.setBackgroundColor(colors.gray) + moniteur.write((" "):rep(largeur - rempli)) + moniteur.setBackgroundColor(colors.black) + y = y + 2 + end + if ctx.rscViolet then + y = dessinerArt(1, y, ctx) + end + texte(1, H - 1, "sol: gimbal joystick rsc boite altitude", + colors.lightGray) + texte(1, H, "vol stationnaire: inclinaison", colors.lightGray) + end + + local function dessinerPageParams(ctx) + local params = ctx.params or {} + local parPage = H - 3 + local maxDefil = math.max(0, #params - parPage) + if defilParams > maxDefil then defilParams = maxDefil end + texte(1, 2, ("surcharges dans drone.etat (%d-%d/%d)"):format( + defilParams + 1, + math.min(defilParams + parPage, #params), #params), + colors.lightGray) + bouton(L - 9, 2, "^", { type = "defil", sens = -1 }) + bouton(L - 4, 2, "v", { type = "defil", sens = 1 }) + local y = 3 + for i = defilParams + 1, + math.min(defilParams + parPage, #params) do + local p = params[i] + texte(1, y, ("%-22s"):format(p.label)) + texte(24, y, tostring(p.valeur), colors.yellow) + bouton(L - 9, y, "-", { type = "param", idx = i, sens = -1 }) + bouton(L - 4, y, "+", { type = "param", idx = i, sens = 1 }) + y = y + 1 + end + end + + local function dessinerPageJournal() + for i, ligne in ipairs(journal.recents(H - 2)) do + texte(1, 1 + i, ligne:sub(1, L), + ligne:find("ERREUR") and colors.red + or ligne:find("ALERTE") and colors.orange or colors.white) + end + end + + ------------------------------------------------------------------ + -- RENDU ET TOUCHERS + ------------------------------------------------------------------ + function e.rafraichirMoniteur(ctx) + zones = {} + moniteur.setBackgroundColor(colors.black) + moniteur.clear() + local onglets = { mode = "MODE", calib = "CALIB", + params = "PARAM", journal = "JOURNAL" } + local x = 1 + 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 + end + if page == "mode" then dessinerPageMode(ctx) + elseif page == "calib" then dessinerPageCalib(ctx) + elseif page == "params" then dessinerPageParams(ctx) + else dessinerPageJournal() end + end + + function e.traiterToucher(tx, ty) + for _, z in ipairs(zones) do + if tx >= z.x1 and tx <= z.x2 and ty >= z.y1 and ty <= z.y2 then + if z.action.type == "page" then + page = z.action.valeur + return { type = "redessiner" } + elseif z.action.type == "defil" then + defilParams = math.max(0, defilParams + z.action.sens * 4) + return { type = "redessiner" } + end + return z.action + end + end + return nil + end + + function e.reglerPage(p) page = p end + + function e.attendreToucher() + os.pullEvent("monitor_touch") + end + + -- art interactif plein ecran pour la calib rsc + function e.choisirRole(titre, affectes) + local locales = {} + local function boutonLocal(x, y, libelle, role, couleur) + moniteur.setCursorPos(x, y) + moniteur.setBackgroundColor(couleur) + moniteur.write(" " .. libelle .. " ") + moniteur.setBackgroundColor(colors.black) + table.insert(locales, { x1 = x, y1 = y, x2 = x + #libelle + 1, + y2 = y, role = role }) + end + moniteur.setBackgroundColor(colors.black) + moniteur.clear() + texte(1, 1, titre:sub(1, L), colors.yellow) + texte(1, 2, "Touchez la position qui TOURNE", colors.lightGray) + local function couleurChoix(role) + return affectes[role] and colors.gray or colors.blue + end + local g, d = 8, L - 12 + texte(math.floor(L / 2) - 2, 4, "AVANT", colors.lightGray) + boutonLocal(g, 5, "lf", "lf", couleurChoix("lf")) + boutonLocal(d, 5, "rf", "rf", couleurChoix("rf")) + boutonLocal(1, 7, "prop_l", "prop_l", couleurChoix("prop_l")) + boutonLocal(L - 8, 7, "prop_r", "prop_r", couleurChoix("prop_r")) + boutonLocal(g, 9, "lb", "lb", couleurChoix("lb")) + boutonLocal(d, 9, "rb", "rb", couleurChoix("rb")) + boutonLocal(1, 11, "ANNULER", nil, colors.red) + while true do + local _, _, tx, ty = os.pullEvent("monitor_touch") + for _, z in ipairs(locales) do + if tx >= z.x1 and tx <= z.x2 and ty >= z.y1 + and ty <= z.y2 then + if z.role == nil then return nil end + if not affectes[z.role] then return z.role end + texte(1, 12, "deja affecte, choisir une autre position", + colors.orange) + end + end + end + end + + return e +end + +return Ecran diff --git a/lib/etat.lua b/lib/etat.lua index bd6c0a6..1f8282b 100644 --- a/lib/etat.lua +++ b/lib/etat.lua @@ -1,63 +1,49 @@ -------------------------------------------------------------------- --- lib/etat.lua : persistance de l'etat du drone (drone.etat) --- Restaure apres un redemarrage: mode, consigne, et tous les --- MAPPAGES CALIBRES (propres a chaque vaisseau): --- roles nom RSC -> lb|rb|lf|rf|prop_l|prop_r --- gimbal { indexTangage, signeTangage, indexRoulis, signeRoulis } --- convention normalisee: tangage negatif = nez bas, --- roulis negatif = penche a gauche --- joystick direction (devant|derriere|gauche|droite) -> face relay --- typewriter code de touche -> symbole ("0".."9", "valider", "effacer") --- velocite nom capteur -> { axe = avant|lateral|vertical, --- signe = 1|-1 } (positif = avant/gauche/haut) --- points nom -> { x, y, z } (max NAV.MAX_POINTS) --- empreintePid parametres de conf au moment de la calib PID +-- lib/etat.lua : etat persistant (drone.etat), ecrit par le +-- programme: calibrations, surcharges de parametres, mode. -------------------------------------------------------------------- local Etat = {} -local CHEMIN = "drone.etat" - local DEFAUT = { - mode = "off", + mode = "off", consigneY = nil, - roles = {}, - gimbal = nil, - joystick = nil, - typewriter = nil, - velocite = {}, - points = {}, - pid = nil, - empreintePid = nil, - capaciteParNiveau = nil, -- calib moteur: niveau (1..15) -> su - stressParRpm = nil, -- calib moteur: { helice, prop } su/rpm - decollage = nil, + roles = {}, -- nom RSC -> lb|rb|lf|rf|prop_l|prop_r + gimbal = nil, -- { indexTangage, signeTangage, indexRoulis, + -- signeRoulis } + joystick = nil, -- { relais, faces = direction -> face } + rpmHover = nil, -- rpm de sustentation mesure (calib altitude) + pressionHover = nil, -- pression lors de cette mesure + coeffRsc = nil, -- nom RSC -> su/rpm (mesure pendant rscman) + speedoParRole = nil, -- role -> nom du speedometer (calib rsc) + boite = nil, -- { relais, faces = role -> face, signal, + -- rapport } boites de vitesse (une face + -- PAR boite, meme signal sur les 4) + capaciteParNiveau = nil, -- niveau moteur (1..15) -> su + modeMoteur = "onoff", -- "onoff" | "adaptatif" (bouton du moniteur) + calibAltitude = nil, -- calib altitude effectuee + calibInclinaison = nil, + surcharges = {}, -- chemin conf -> valeur (page PARAM) } function Etat.charger(journal) local etat = {} - for k, v in pairs(DEFAUT) do etat[k] = v end - - if fs.exists(CHEMIN) then - local f = fs.open(CHEMIN, "r") - local contenu = f.readAll() + for cle, valeur in pairs(DEFAUT) do etat[cle] = valeur end + if fs.exists("drone.etat") then + local f = fs.open("drone.etat", "r") + local lu = textutils.unserialize(f.readAll() or "") f.close() - local lu = textutils.unserialize(contenu) - if type(lu) == "table" then - for k, v in pairs(lu) do etat[k] = v end - journal.info(("etat restaure (mode %s)"):format(etat.mode)) - else - journal.alerte("drone.etat illisible, etat par defaut") + if lu then + for cle, valeur in pairs(lu) do etat[cle] = valeur end + journal.info("etat restaure (mode " .. tostring(etat.mode) .. ")") end end return etat end function Etat.sauver(etat) - local f = fs.open(CHEMIN, "w") - if f then - f.write(textutils.serialize(etat)) - f.close() - end + local f = fs.open("drone.etat", "w") + f.write(textutils.serialize(etat)) + f.close() end return Etat diff --git a/lib/ihm.lua b/lib/ihm.lua deleted file mode 100644 index 8611440..0000000 --- a/lib/ihm.lua +++ /dev/null @@ -1,470 +0,0 @@ --------------------------------------------------------------------- --- lib/ihm.lua : interfaces homme-machine. --- Moniteur tactile : navigation par onglets (MODE / CALIB / --- JOURNAL), echelle de texte auto (la plus grande qui loge le --- contenu), pages adaptees au mode, art du quadricoptere. --- Display Link conduite : altitudes / vitesse / messages longs --- defiles par segments de mots entiers. --- Display Link carburant : % + autonomie. --------------------------------------------------------------------- -local Ihm = {} - --- couleur d'un rpm: blanc=0, rouge=max, jaune=en changement, vert=stable -local function couleurRpm(suivi, vmax) - if not suivi then return colors.lightGray end - if os.clock() - suivi.tChange < 1.0 then return colors.yellow end - if suivi.v == 0 then return colors.white end - if math.abs(suivi.v) >= vmax then return colors.red end - return colors.green -end - -function Ihm.nouveau(conf, materiel, journal) - local ihm = {} - local moniteur = materiel.moniteur - local zones = {} - local page = "mode" -- mode | calib | journal - local L, H = 0, 0 -- taille en caracteres - - -- Echelle auto: la PLUS GRANDE echelle offrant au moins 38x14 - local function reglerEchelle() - for _, s in ipairs({ 5, 4.5, 4, 3.5, 3, 2.5, 2, 1.5, 1, 0.5 }) do - moniteur.setTextScale(s) - L, H = moniteur.getSize() - if L >= 38 and H >= 14 then return end - end - end - reglerEchelle() - - ------------------------------------------------------------------ - -- MESSAGES DE CONDUITE (defilement par segments de mots entiers) - ------------------------------------------------------------------ - local segments, segmentFin, segmentIdx = nil, 0, 1 - - function ihm.message(texte, duree) - if texte == "" then segments = nil return end - -- decoupe en segments <= 22 caracteres sans couper les mots - segments = {} - local courant = "" - for mot in texte:gmatch("%S+") do - if #courant == 0 then courant = mot - elseif #courant + 1 + #mot <= 22 then courant = courant .. " " .. mot - else table.insert(segments, courant) courant = mot end - end - if #courant > 0 then table.insert(segments, courant) end - segmentIdx = 1 - segmentFin = os.clock() + (duree or math.max(5, #segments * 2)) - end - - local function segmentCourant() - if not segments or os.clock() > segmentFin then return nil end - -- avance d'un segment toutes les 2 s - local i = 1 + math.floor((os.clock() % (#segments * 2)) / 2) - return segments[math.min(i, #segments)] - .. (#segments > 1 and (" " .. i .. "/" .. #segments) or "") - end - - -- ctx: { mode, consigneY, saisie } - function ihm.rafraichirConduite(ctx) - local e = materiel.ecranConduite - e.clear() - e.setCursorPos(1, 1) - local seg = segmentCourant() - if seg then - e.write(seg:sub(1, 22)) - elseif ctx.saisie and #ctx.saisie > 0 then - e.write(("SAISIE: " .. ctx.saisie):sub(1, 22)) - elseif ctx.mode == "vol" or ctx.mode == "drone" - or ctx.mode == "atterrissage" or ctx.mode == "auto" then - e.write(("ALT %.1f / %.0f"):format( - materiel.lireAltitude(), ctx.consigneY or 0)) - else - e.write("MODE " .. ctx.mode:upper()) - end - e.setCursorPos(1, 2) - e.write(("VIT %.2f b/s"):format(materiel.vitesseHorizontale())) - e.update() - end - - function ihm.rafraichirCarburant(pct, minutes) - local e = materiel.ecranCarburant - local largeur = e.getSize() - e.clear() - e.setCursorPos(1, 1) - if pct == nil then - e.write(("n/a"):sub(1, largeur)) - else - e.write(("%d%%"):format(math.floor(pct * 100 + 0.5)):sub(1, largeur)) - e.setCursorPos(1, 2) - e.write((minutes and ("%dm"):format(math.floor(minutes)) or "--m") - :sub(1, largeur)) - end - e.update() - end - - ------------------------------------------------------------------ - -- PRIMITIVES MONITEUR - ------------------------------------------------------------------ - local function bouton(x, y, texte, action, couleur, couleurTexte) - moniteur.setCursorPos(x, y) - moniteur.setBackgroundColor(couleur or colors.gray) - moniteur.setTextColor(couleurTexte or colors.white) - moniteur.write(" " .. texte .. " ") - moniteur.setBackgroundColor(colors.black) - moniteur.setTextColor(colors.white) - table.insert(zones, { - x1 = x, y1 = y, x2 = x + #texte + 1, y2 = y, action = action, - }) - end - - local function texte(x, y, contenu, couleur) - moniteur.setTextColor(couleur or colors.white) - moniteur.setCursorPos(x, y) - moniteur.write(contenu) - moniteur.setTextColor(colors.white) - end - - ------------------------------------------------------------------ - -- ART DU QUADRICOPTERE - -- rectangle, 4 cercles aux coins (helices, rpm a cote), rpm des - -- propulseurs sur les cotes; violet = RSC en cours de calibration - ------------------------------------------------------------------ - local function couleurDe(role, ctx) - if ctx.rscViolet == role then return colors.purple end - return couleurRpm(materiel.rpm[role], conf.VITESSE_RSC_MAX) - end - - local function rpmDe(role) - local s = materiel.rpm[role] - return s and tostring(s.v) or "?" - end - - -- occupe 7 lignes a partir de y; largeur ~ 30 - local function dessinerArt(x, y, ctx) - local function coin(cx, cy, role, rpmAGauche) - texte(cx, cy, "O", couleurDe(role, ctx)) - local r = rpmDe(role) - if rpmAGauche then texte(cx - #r - 1, cy, r, couleurDe(role, ctx)) - else texte(cx + 2, cy, r, couleurDe(role, ctx)) end - end - local g, d = x + 5, x + 18 -- colonnes des coins - texte(x + 9, y, "AVANT", colors.lightGray) - coin(g, y + 1, "lf", true) - coin(d, y + 1, "rf", false) - for i = 2, 4 do - texte(g, y + i, "|") - texte(d, y + i, "|") - end - texte(g + 1, y + 1, ("-"):rep(d - g - 1)) - texte(g + 1, y + 5, ("-"):rep(d - g - 1)) - texte(x, y + 3, rpmDe("prop_l"), couleurDe("prop_l", ctx)) - texte(d + 2, y + 3, rpmDe("prop_r"), couleurDe("prop_r", ctx)) - coin(g, y + 5, "lb", true) - coin(d, y + 5, "rb", false) - return y + 7 - end - - ------------------------------------------------------------------ - -- PAGE MODE (contenu adapte au mode courant) - ------------------------------------------------------------------ - local function blocCommun(y, ctx) - texte(1, y, ("carburant %s autonomie %s"):format( - ctx.carburantPct and ("%.0f%%"):format(ctx.carburantPct * 100) - or "n/a", - ctx.minutes and ("%dmin"):format(math.floor(ctx.minutes)) or "--")) - if ctx.position then - texte(1, y + 1, ("gps %.0f / %.0f"):format( - ctx.position.x, ctx.position.z)) - else - texte(1, y + 1, "gps n/a", colors.red) - end - return y + 2 - end - - local function blocStress(y, ctx) - local brut, cap = materiel.lireStressBrut() - 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 - - local function dessinerPageMode(ctx) - local modes = { "off", "inactif", "calibrage", "stationnement", - "vol", "drone", "atterrissage", "auto" } - local x, y = 1, 2 - for _, m in ipairs(modes) do - local verrou = ctx.verrous[m] - local couleur = colors.gray - if m == ctx.mode then couleur = colors.green - elseif verrou then couleur = colors.red end - if x + #m + 2 > L then x, y = 1, y + 1 end - bouton(x, y, m, { type = "mode", valeur = m, verrou = verrou }, - couleur) - x = x + #m + 3 - end - y = y + 2 - - local mode = ctx.mode - y = blocCommun(y, ctx) - if mode ~= "off" then y = blocStress(y, ctx) end - y = y + 1 - - if mode == "stationnement" or mode == "vol" or mode == "drone" - or mode == "auto" then - y = dessinerArt(1, y, ctx) - end - - if mode == "vol" or mode == "drone" then - texte(1, y, ("vit %.2f b/s alt %.1f -> %.0f"):format( - 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 }) - bouton(17, y, "+10", { type = "delta", valeur = 10 }) - y = y + 1 - if mode == "drone" then - texte(1, y, "INCLINAISON LIBRE (joystick = assiette)", - colors.orange) - y = y + 1 - end - elseif mode == "atterrissage" then - texte(1, y, ("distance au sol: %.1f"):format( - materiel.lireDistanceSol())) - y = y + 1 - elseif mode == "auto" then - texte(1, y, ("vit %.2f b/s alt %.1f cible %s dist %s"):format( - materiel.vitesseHorizontale(), materiel.lireAltitude(), - ctx.cible or "-", ctx.distanceCible - and ("%.0f"):format(ctx.distanceCible) or "-")) - y = y + 1 - -- points enregistres en boutons + annulation - local x2 = 1 - for nom in pairs(ctx.points or {}) do - if x2 + #nom + 2 > L - 10 then break end - bouton(x2, y, nom, { type = "ptgo", nom = nom }, colors.blue) - x2 = x2 + #nom + 3 - end - bouton(L - 9, y, "ANNULER", { type = "annuler" }, colors.red) - y = y + 1 - -- zone de saisie + pave numerique (coordonnees: "x z" ou "x y z") - texte(1, y, "coord: " .. (ctx.saisieCible or "") .. "_") - y = y + 1 - local paves = { "1","2","3","4","5","6","7","8","9","0","-" } - local x3 = 1 - for _, p in ipairs(paves) do - bouton(x3, y, p, { type = "pave", valeur = p }) - x3 = x3 + 4 - end - bouton(x3, y, "esp", { type = "pave", valeur = " " }) - x3 = x3 + 6 - bouton(x3, y, "eff", { type = "pave", valeur = "eff" }) - x3 = x3 + 6 - bouton(x3, y, "GO", { type = "pave", valeur = "go" }, colors.green) - end - end - - ------------------------------------------------------------------ - -- PAGE CALIBRATIONS - ------------------------------------------------------------------ - local function dessinerPageCalib(ctx) - texte(1, 2, "gris: a faire jaune: en cours vert: fait", - colors.lightGray) - local x, y = 1, 3 - for _, c in ipairs({ "gimbal", "joystick", "typew", "rsc", - "rscman", "moteur", "capteurs", "pid", - "pidmath" }) do - local statut = ctx.calibStatuts[c] - local couleur = colors.gray - if statut == "encours" then couleur = colors.yellow - elseif statut == "fait" then couleur = colors.green end - if x + #c + 2 > L then x, y = 1, y + 1 end - bouton(x, y, c, { type = "calib", valeur = c }, couleur, - statut == "encours" and colors.black or colors.white) - x = x + #c + 3 - end - y = y + 2 - - if ctx.calibTexte then - texte(1, y, ctx.calibTexte:sub(1, L), colors.yellow) - y = y + 1 - end - if ctx.calibProgres then - local largeur = L - 2 - local rempli = math.floor(ctx.calibProgres * largeur + 0.5) - moniteur.setCursorPos(1, y) - moniteur.setBackgroundColor(colors.green) - moniteur.write((" "):rep(rempli)) - moniteur.setBackgroundColor(colors.gray) - moniteur.write((" "):rep(largeur - rempli)) - moniteur.setBackgroundColor(colors.black) - y = y + 2 - end - if ctx.rscViolet then - texte(1, y, "en violet: RSC en cours", colors.purple) - y = dessinerArt(1, y + 1, ctx) - end - texte(1, H - 1, - "sol: gimbal joystick typew rsc rscman moteur (calibrage)", - colors.lightGray) - texte(1, H, "vol: capteurs pid pidmath (mode vol)", - colors.lightGray) - end - - ------------------------------------------------------------------ - -- PAGE PARAMS (edition tactile; surcharges dans drone.etat) - ------------------------------------------------------------------ - local defilParams = 0 - local function dessinerPageParams(ctx) - local params = ctx.params or {} - local parPage = H - 3 - local maxDefil = math.max(0, #params - parPage) - if defilParams > maxDefil then defilParams = maxDefil end - texte(1, 2, ("surcharges dans drone.etat (%d-%d/%d)"):format( - defilParams + 1, math.min(defilParams + parPage, #params), - #params), colors.lightGray) - bouton(L - 9, 2, "^", { type = "defilparam", sens = -1 }) - bouton(L - 4, 2, "v", { type = "defilparam", sens = 1 }) - local y = 3 - for i2 = defilParams + 1, math.min(defilParams + parPage, #params) do - local p = params[i2] - 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 - end - end - - ------------------------------------------------------------------ - -- PAGE JOURNAL - ------------------------------------------------------------------ - local function dessinerPageJournal() - local lignes = journal.recents(H - 2) - for i, ligne in ipairs(lignes) do - texte(1, 1 + i, ligne:sub(1, L), - ligne:find("ERREUR") and colors.red - or ligne:find("ALERTE") and colors.orange or colors.white) - end - end - - ------------------------------------------------------------------ - -- RENDU GLOBAL - ------------------------------------------------------------------ - function ihm.rafraichirMoniteur(ctx) - zones = {} - moniteur.setBackgroundColor(colors.black) - moniteur.clear() - - -- barre d'onglets - local onglets = { mode = "MODE", calib = "CALIB", - params = "PARAM", journal = "JOURNAL" } - local x = 1 - 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 - end - texte(math.min(x + 1, L - #ctx.mode), 1, ctx.mode, colors.lightGray) - - if page == "mode" then dessinerPageMode(ctx) - elseif page == "calib" then dessinerPageCalib(ctx) - elseif page == "params" then dessinerPageParams(ctx) - else dessinerPageJournal() end - end - - function ihm.traiterToucher(tx, ty) - for _, z in ipairs(zones) do - if tx >= z.x1 and tx <= z.x2 and ty >= z.y1 and ty <= z.y2 then - if z.action.type == "page" then - page = z.action.valeur - return { type = "redessiner" } - elseif z.action.type == "defilparam" then - defilParams = math.max(0, defilParams + z.action.sens * 4) - return { type = "redessiner" } - end - return z.action - end - end - return nil - end - - function ihm.reglerPage(p) page = p end - - -- Art interactif plein ecran: l'utilisateur touche la position de - -- l'helice/du propulseur qui tourne. Retourne le role choisi, ou - -- nil (annulation). `affectes` (role -> nom) grise le deja pris. - function ihm.choisirRole(titre, affectes) - local locales = {} - local function boutonLocal(x, y, texteB, role, couleur) - moniteur.setCursorPos(x, y) - moniteur.setBackgroundColor(couleur) - moniteur.write(" " .. texteB .. " ") - moniteur.setBackgroundColor(colors.black) - table.insert(locales, { - x1 = x, y1 = y, x2 = x + #texteB + 1, y2 = y, role = role, - }) - end - - moniteur.setBackgroundColor(colors.black) - moniteur.clear() - texte(1, 1, titre:sub(1, L), colors.yellow) - texte(1, 2, "Touchez la position qui TOURNE", colors.lightGray) - - local function couleurChoix(role) - if affectes[role] then return colors.gray end - return colors.blue - end - local g, d = 8, L - 12 - texte(math.floor(L / 2) - 2, 4, "AVANT", colors.lightGray) - boutonLocal(g, 5, "lf", "lf", couleurChoix("lf")) - boutonLocal(d, 5, "rf", "rf", couleurChoix("rf")) - boutonLocal(1, 7, "prop_l", "prop_l", couleurChoix("prop_l")) - boutonLocal(L - 8, 7, "prop_r", "prop_r", couleurChoix("prop_r")) - boutonLocal(g, 9, "lb", "lb", couleurChoix("lb")) - boutonLocal(d, 9, "rb", "rb", couleurChoix("rb")) - boutonLocal(1, 11, "ANNULER", nil, colors.red) - - while true do - local _, _, tx, ty = os.pullEvent("monitor_touch") - for _, z in ipairs(locales) do - if tx >= z.x1 and tx <= z.x2 and ty >= z.y1 and ty <= z.y2 then - if z.role == nil then return nil end - if not affectes[z.role] then return z.role end - texte(1, 12, "deja affecte, choisir une autre position", - colors.orange) - end - end - end - end - - function ihm.attendreToucher() - while true do - local ev = os.pullEvent("monitor_touch") - if ev then return end - end - end - - return ihm -end - -return Ihm diff --git a/lib/materiel.lua b/lib/materiel.lua index 4ccc209..c47937c 100644 --- a/lib/materiel.lua +++ b/lib/materiel.lua @@ -1,59 +1,86 @@ -------------------------------------------------------------------- --- lib/materiel.lua : resolution des peripheriques et adaptateurs. --- Tous les formats sont VERIFIES en jeu (inventaire du 17/07/2026). --- --- Directs : altitude_sensor (top), 2 Create_DisplayLink (faces dans --- conf.ECRANS), ender modem (gps), moteur diesel en bottom pilote --- par redstone analogique (15=arret, 14=mini, 0=maxi). --- Reseau : RSC (roles calibres), velocity_sensor x3 (axes/directions --- calibres), gimbal_sensor (axes calibres), optical_sensor, --- Create_Stressometer, redstone_relay (joystick calibre), --- linked_typewriter, monitor, 2 cuves create:fluid_tank. +-- lib/materiel.lua : acces au materiel. +-- Peripheriques: 6 RSC (4 helices + 2 propulseurs), gimbal, +-- altitude (top), optical (vers le sol), stressometer, redstone +-- relay (joystick), moniteur tactile, 2 display links (conduite +-- 2x22, carburant 2x4), velocity sensors (vitesse globale), moteur +-- diesel pilote par redstone analogique bottom (15=arret, 0=maxi). -------------------------------------------------------------------- local Materiel = {} -local function trouver(type_, obligatoire, journal, note) - local p = peripheral.find(type_) - if not p and obligatoire then - error("peripherique manquant: " .. type_, 0) - end - if not p and journal then - journal.alerte(type_ .. " introuvable" .. (note and (": " .. note) or "")) - end - return p -end - function Materiel.initialiser(conf, etat, journal) local m = {} - m.rpm = {} -- role -> { v, tChange } (pour l'affichage) - local posCache = nil, nil - local posDate = 0 + m.rpm = {} -- role -> { v, tChange } (affichage + besoin moteur) - m.altitude = trouver("altitude_sensor", true) - m.gimbal = trouver("gimbal_sensor", true) - m.optical = trouver("optical_sensor", true) - m.moniteur = trouver("monitor", true) - m.relay = trouver("redstone_relay", false, journal, "joystick inactif") - m.stresso = trouver("Create_Stressometer", false, journal, - "detection moteur et surcharge inactives") - m.typewriter = trouver("linked_typewriter", false, journal, - "consigne au moniteur uniquement") - - -- les deux display links, identifies par leur face (conf.ECRANS) - local function ecran(face, usage) - local p = peripheral.wrap(face) - if not p or peripheral.getType(face) ~= "Create_DisplayLink" then - error(("Create_DisplayLink attendu en '%s' (%s), voir conf.ECRANS") - :format(face, usage), 0) + local function premier(type_) + for _, nom in ipairs(peripheral.getNames()) do + if peripheral.getType(nom) == type_ then + return peripheral.wrap(nom), nom + end end - return p end - m.ecranConduite = ecran(conf.ECRANS.conduite, "conduite") - m.ecranCarburant = ecran(conf.ECRANS.carburant, "carburant") - -- rotation speed controllers et velocity sensors: les affectations - -- (etat.roles / etat.velocite) sont rechargeables A CHAUD apres - -- une recalibration via m.rafraichirAffectations() + m.altitude = premier("altitude_sensor") + m.gimbal = premier("gimbal_sensor") + m.optical = premier("optical_sensor") + m.stresso = premier("Create_Stressometer") + m.moniteur = premier("monitor") + + -- relays multiples: joystick sur l'un, boites de vitesse sur un + -- autre (identifies par leurs calibrations respectives) + m.relays = {} + for _, nom in ipairs(peripheral.getNames()) do + if peripheral.getType(nom) == "redstone_relay" then + m.relays[nom] = peripheral.wrap(nom) + end + end + -- Les NOMS des relays peuvent changer (renumerotation reseau); + -- les FACES, elles, sont stables. Resolution robuste: + -- joystick: nom stocke s'il existe encore, sinon "un relay qui + -- n'est pas celui des boites", sinon le premier + local function relayJoystick() + local j = etat.joystick + if j and j.relais and m.relays[j.relais] then + return m.relays[j.relais] + end + local boite = etat.boite and etat.boite.relais + for nom, p in pairs(m.relays) do + if nom ~= boite then return p end + end + for _, p in pairs(m.relays) do return p end + end + + -- les noms stockes correspondent-ils encore au reseau ? + function m.relaisValides() + if etat.boite and not m.relays[etat.boite.relais] then + return false + end + if etat.joystick and etat.joystick.relais + and not m.relays[etat.joystick.relais] then + return false + end + return true + end + + for _, obligatoire in ipairs({ "altitude", "gimbal", "moniteur" }) do + if not m[obligatoire] then + error("peripherique manquant: " .. obligatoire, 0) + end + end + + -- display links: conduite = le plus large, carburant = l'autre + local liens = {} + for _, nom in ipairs(peripheral.getNames()) do + if peripheral.getType(nom) == "Create_DisplayLink" then + table.insert(liens, peripheral.wrap(nom)) + end + end + table.sort(liens, function(a, b) + return (a.getSize() or 0) > (b.getSize() or 0) + end) + m.ecranConduite, m.ecranCarburant = liens[1], liens[2] + + -- RSC et velocity sensors m.rsc, m.velocite = {}, {} for _, nom in ipairs(peripheral.getNames()) do local type_ = peripheral.getType(nom) @@ -64,29 +91,82 @@ function Materiel.initialiser(conf, etat, journal) end end + -- speedometers (vitesse REELLE des helices, apres boite) + m.speedo = {} + for _, nom in ipairs(peripheral.getNames()) do + if peripheral.getType(nom) == "Create_Speedometer" then + m.speedo[nom] = peripheral.wrap(nom) + end + end + + function m.lireSpeedo(nom) + local p = m.speedo[nom] + if not p then return nil end + return p.getSpeed() or 0 + end + + -- vitesse reelle d'une helice via son speedometer associe + function m.lireVitesseHelice(role) + local assoc = etat.speedoParRole and etat.speedoParRole[role] + if not assoc then return nil end + return m.lireSpeedo(assoc) + end + + ------------------------------------------------------------------ + -- BOITES DE VITESSE (analog transmission): + -- signal 0 -> rapport 16/16 ; sinon rapport = (15-signal)/16 + -- signal 15 -> rapport 0: INTERDIT (helices stoppees) + -- Les 4 boites partagent UNE face d'UN relay (meme rapport pour + -- les 4: indispensable a la symetrie). Le rapport est choisi AU + -- SOL (calib altitude) et JAMAIS change en vol. + ------------------------------------------------------------------ + -- etat.boite = { relais, faces = { lb, rb, lf, rf -> face }, + -- signal, rapport } : une face PAR boite, meme + -- signal (donc meme rapport) sur les 4 + function m.rapportBoite() + local b = etat.boite + if not b then return 1 end + return b.rapport or 1 + end + + function m.signalPourRapportK(k) + if k >= 16 then return 0 end + return 15 - k + end + + -- ecrit le signal sur les 4 faces des boites + function m.appliquerBoite() + local b = etat.boite + if not b or not m.relays[b.relais] or not b.faces then return end + for _, face in pairs(b.faces) do + m.relays[b.relais].setAnalogOutput(face, b.signal or 0) + end + end + + function m.reglerSignalBoite(nomRelais, face, signal) + local p = m.relays[nomRelais] + if p then p.setAnalogOutput(face, signal) end + end + + -- affectations rechargeables A CHAUD apres recalibration function m.rafraichirAffectations() - m.rscParRole, m.veloParAxe = {}, {} + m.rscParRole, m.nomParRole = {}, {} for nom, p in pairs(m.rsc) do local role = etat.roles[nom] - if role then m.rscParRole[role] = p end - end - for nom, p in pairs(m.velocite) do - local aff = etat.velocite[nom] - if aff then m.veloParAxe[aff.axe] = { p = p, signe = aff.signe } end + if role then + m.rscParRole[role] = p + m.nomParRole[role] = nom + end end end m.rafraichirAffectations() - -- cuves de carburant (create:fluid_tank; exclut le moteur diesel, - -- qui est fluid_storage mais pas fluid_tank) - m.cuves = {} - for _, nom in ipairs(peripheral.getNames()) do - if peripheral.hasType(nom, "create:fluid_tank") then - table.insert(m.cuves, peripheral.wrap(nom)) + function m.helicesManquantes() + local manquantes = {} + for _, role in ipairs({ "lb", "rb", "lf", "rf" }) do + if not m.rscParRole[role] then table.insert(manquantes, role) end end - end - if #m.cuves == 0 then - journal.alerte("aucune cuve create:fluid_tank: suivi carburant inactif") + return manquantes end ------------------------------------------------------------------ @@ -100,163 +180,124 @@ function Materiel.initialiser(conf, etat, journal) return m.altitude.getAirPressure() or 1.0 end - -- angles bruts du gimbal (table de 2) + -- angles bruts du gimbal: { a1, a2 } (mapping variable par + -- vaisseau, d'ou la calibration) function m.lireAnglesBruts() local a = m.gimbal.getAngles() return a[1] or 0, a[2] or 0 end - -- assiette NORMALISEE via la calibration gimbal : - -- tangage negatif = nez bas, roulis negatif = penche a gauche + -- assiette NORMALISEE par la calib gimbal: + -- tangage NEGATIF = nez bas, roulis NEGATIF = penche a gauche function m.lireAssiette() local g = etat.gimbal - if not g then return m.lireAnglesBruts() end + if not g then return 0, 0 end local a = m.gimbal.getAngles() return (a[g.indexTangage] or 0) * g.signeTangage, (a[g.indexRoulis] or 0) * g.signeRoulis end + -- distance au sol (optical vers le bas); 15.5 = rien devant function m.lireDistanceSol() - return m.optical.getDistance() + if not m.optical then return 99 end + local d = m.optical.getDistance() + if not d or d >= 15.4 then return 99 end + return d end function m.auSol() return m.lireDistanceSol() <= conf.DIST_SOL end - -- sol detecte ET posable (pas d'eau/lave: garde-fou ocean) - function m.solPosable() - if not m.optical.hasHit() then return false end - local bloc = m.optical.getBlock() - return not conf.BLOCS_INTERDITS[bloc] - end - - function m.horizontal() - local t, r = m.lireAssiette() - return math.abs(t) <= conf.ANGLE_MAX and math.abs(r) <= conf.ANGLE_MAX - end - - -- vitesse le long d'un axe calibre ("avant"|"lateral"|"vertical") - -- signe: positif = avant / gauche / haut - function m.lireVitesseAxe(axe) - local c = m.veloParAxe[axe] - if not c then return 0 end - return (c.p.getVelocity() or 0) * c.signe - end - - function m.vitesseHorizontale() - local va = m.lireVitesseAxe("avant") - local vl = m.lireVitesseAxe("lateral") - return math.sqrt(va * va + vl * vl) - end - - -- joystick calibre: avance (devant-derriere), virage (gauche-droite) - -- dans -1..1; pas de zone morte (joystick de mod, zero franc) - function m.lireJoystick() - local j = etat.joystick - if not (m.relay and j) then return 0, 0 end - local function lire(direction) - local face = j[direction] - if not face then return 0 end - return m.relay.getAnalogInput(face) / 15 - end - return lire("devant") - lire("derriere"), - lire("gauche") - lire("droite") - end - - function m.lireFacesRelay() - if not m.relay then return {} end - local lectures = {} - for _, face in ipairs({ "top", "bottom", "left", "right", - "front", "back" }) do - lectures[face] = m.relay.getAnalogInput(face) - end - return lectures - end - - -- typewriter: table des codes de touches actuellement pressees - function m.lireTouches() - if not m.typewriter then return {} end - return m.typewriter.getPressedKeyCodes() or {} - end - - -- stressometer (formats verifies): ratio 0..1, ou nil - function m.lireStress() - if not m.stresso then return nil end - local cap = m.stresso.getStressCapacity() - if not cap or cap <= 0 then return nil end - return m.stresso.getStress() / cap - end - - -- valeurs brutes (su) pour l'affichage: stress, capacite function m.lireStressBrut() if not m.stresso then return nil end 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) + -- capacite > 0 <=> le moteur tourne (verifie: 0 a l'arret) function m.moteurTourne() - if not m.stresso then return true end -- indetectable: on suppose oui - return (m.stresso.getStressCapacity() or 0) > 0 + if not m.stresso then return true end + local _, capacite = m.lireStressBrut() + return (capacite or 0) > 0 end - -- carburant: quantite totale (mB) et capacite totale + -- vitesse GLOBALE = sqrt(v1^2 + v2^2 + v3^2): les directions des + -- capteurs n'ont pas d'importance + function m.vitesseGlobale() + local somme = 0 + for _, p in pairs(m.velocite) do + local v = p.getVelocity() or 0 + somme = somme + v * v + end + return math.sqrt(somme) + end + + function m.lireVitesseCapteur(nom) + local p = m.velocite[nom] + return p and (p.getVelocity() or 0) or 0 + end + + -- joystick via redstone relay (calib: direction -> face) + -- retourne avance (-1..1, devant positif) et virage (-1..1, + -- droite positif); 0,0 si non calibre + function m.lireJoystick() + local j = etat.joystick + local relay = relayJoystick() + if not j or not j.faces or not relay then return 0, 0 end + local function niveau(direction) + local face = j.faces[direction] + if not face then return 0 end + return (relay.getAnalogInput(face) or 0) / 15 + end + return niveau("devant") - niveau("derriere"), + niveau("droite") - niveau("gauche") + end + + -- lecture d'une face d'un relay nomme (calibrations) + function m.lireFaceRelayDe(nomRelais, face) + local p = m.relays[nomRelais] + if not p then return 0 end + return p.getAnalogInput(face) or 0 + end + + -- carburant: cuves du reseau (le moteur diesel en bottom est + -- aussi un fluid_storage: exclu du comptage) function m.lireCarburant() - if #m.cuves == 0 then return nil end local quantite, capacite = 0, 0 - for _, cuve in ipairs(m.cuves) do - for _, t in ipairs(cuve.tanks() or {}) do - quantite = quantite + (t.amount or 0) - capacite = capacite + (t.capacity or conf.CARBURANT.CAPACITE_TANK) + for _, nom in ipairs(peripheral.getNames()) do + if nom ~= "bottom" + and peripheral.hasType(nom, "fluid_storage") then + local p = peripheral.wrap(nom) + for _, cuve in ipairs(p.tanks() or {}) do + quantite = quantite + (cuve.amount or 0) + end + if p.getInfo then + local info = p.getInfo() + capacite = capacite + (info and info.capacity or 0) + else + capacite = capacite + 16000 + end end end - if capacite <= 0 then capacite = conf.CARBURANT.CAPACITE_TANK * #m.cuves end - return quantite, capacite - end - - -- ACTUALISATION GPS (BLOQUANTE, jusqu'a 2 s): a appeler depuis une - -- tache dediee uniquement - function m.actualiserPosition() - local x, y, z = gps.locate(2) - if x then - posCache = { x = x + conf.OFFSET_X, y = y + conf.OFFSET_Y, - z = z + conf.OFFSET_Z } - posDate = os.clock() - end - end - - -- position GPS en CACHE (non bloquante); nil si absente ou > 8 s - function m.lirePosition() - if not posCache or os.clock() - posDate > 8 then return nil end - return posCache.x, posCache.y, posCache.z + if capacite <= 0 then return nil end + return quantite / capacite, quantite end ------------------------------------------------------------------ - -- SORTIES + -- COMMANDES ------------------------------------------------------------------ - function m.reglerMoteur(niveau) -- 0 (arret) .. 15 (maxi) - niveau = math.max(0, math.min(15, math.floor(niveau + 0.5))) - redstone.setAnalogOutput("bottom", 15 - niveau) - end + local HELICES = { lb = true, rb = true, lf = true, rf = true } + -- vitesse en RPM HELICE pour les 4 helices (conversion RSC = + -- helice / rapport a la frontiere de l'actionneur: le regulateur + -- travaille en unites physiques, invariantes au rapport), en rpm + -- RSC pour les propulseurs (pas de boite) function m.reglerRsc(role, vitesse) local p = m.rscParRole[role] if not p then return end - vitesse = math.floor(vitesse + 0.5) + if HELICES[role] then vitesse = vitesse / m.rapportBoite() end + vitesse = math.max(-256, math.min(256, + math.floor(vitesse + 0.5))) local suivi = m.rpm[role] if not suivi or suivi.v ~= vitesse then m.rpm[role] = { v = vitesse, tChange = os.clock() } @@ -264,38 +305,76 @@ function Materiel.initialiser(conf, etat, journal) p.setTargetSpeed(vitesse) end + ------------------------------------------------------------------ + -- CHANGEMENT DE RAPPORT EN VOL (ordre SUR: le transitoire d'un + -- tick est toujours un CREUX de poussee, jamais un pic) + -- k monte (rapport augmente): RSC reecrits D'ABORD (avec le + -- nouveau rapport: valeurs plus basses, creux), redstone + -- ENSUITE (restaure) + -- k descend: redstone D'ABORD (creux), RSC reecrits ENSUITE + -- rpmHelices: role -> rpm HELICE a maintenir pendant la bascule + ------------------------------------------------------------------ + function m.changerRapport(k, rpmHelices) + local b = etat.boite + if not b then return false end + if k == 15 then k = (b.rapport * 16 < 15) and 14 or 16 end + k = math.max(1, math.min(16, k)) + local nouveauRapport = k / 16 + if nouveauRapport == b.rapport then return false end + local monte = nouveauRapport > b.rapport + + local function ecrireRsc() + for role in pairs(HELICES) do + local p = m.rscParRole[role] + local v = rpmHelices[role] + if p and v then + local rsc = math.max(-256, math.min(256, + math.floor(v / nouveauRapport + 0.5))) + m.rpm[role] = { v = rsc, tChange = os.clock() } + p.setTargetSpeed(rsc) + end + end + end + + local signal = m.signalPourRapportK(k) + local function ecrireFaces() + for _, face in pairs(b.faces or {}) do + m.reglerSignalBoite(b.relais, face, signal) + end + end + if monte then + ecrireRsc() + ecrireFaces() + else + ecrireFaces() + ecrireRsc() + end + b.signal, b.rapport = signal, nouveauRapport + return true + end + function m.toutArreter() for _, p in pairs(m.rsc) do p.setTargetSpeed(0) end for role, suivi in pairs(m.rpm) do - if suivi.v ~= 0 then m.rpm[role] = { v = 0, tChange = os.clock() } end + if suivi.v ~= 0 then + m.rpm[role] = { v = 0, tChange = os.clock() } + end end end - ------------------------------------------------------------------ - -- DIAGNOSTIC DES CALIBRATIONS - ------------------------------------------------------------------ - function m.helicesManquantes() - local manquantes = {} - for _, role in ipairs({ "lb", "rb", "lf", "rf" }) do - if not m.rscParRole[role] then table.insert(manquantes, role) end + -- niveau 0 = arret, 15 = pleine puissance (redstone inverse: + -- signal bottom 15 = arret, 0 = maxi) + local niveauCourant = -1 + function m.reglerMoteur(niveau) + niveau = math.max(0, math.min(15, math.floor(niveau + 0.5))) + if niveau ~= niveauCourant then + redstone.setAnalogOutput("bottom", 15 - niveau) + niveauCourant = niveau end - return manquantes end - function m.propulseursManquants() - local manquants = {} - for _, role in ipairs({ "prop_l", "prop_r" }) do - if not m.rscParRole[role] then table.insert(manquants, role) end - end - return manquants - end - - function m.axesManquants() - local manquants = {} - for _, axe in ipairs({ "avant", "lateral", "vertical" }) do - if not m.veloParAxe[axe] then table.insert(manquants, axe) end - end - return manquants + function m.niveauMoteur() + return math.max(0, niveauCourant) end return m diff --git a/lib/navigation.lua b/lib/navigation.lua deleted file mode 100644 index 669006c..0000000 --- a/lib/navigation.lua +++ /dev/null @@ -1,77 +0,0 @@ --------------------------------------------------------------------- --- lib/navigation.lua : position GPS, estimation du cap, ralliement. --- --- Le cap n'est pas mesurable directement (pas de boussole): il est --- estime a partir du deplacement GPS quand la vitesse est --- suffisante. Le ralliement avance donc "en aveugle" au depart puis --- corrige le cap des que le drone bouge. --------------------------------------------------------------------- -local Navigation = {} - -function Navigation.nouveau(conf, materiel, journal) - local n = {} - local posPrec, tPrec = nil, nil - local cap = nil -- radians, atan2(dx, dz) du deplacement - - function n.position() - return materiel.lirePosition() - end - - -- A appeler periodiquement: met a jour l'estimation du cap - function n.actualiser() - local x, y, z = materiel.lirePosition() - if not x then return nil end - local t = os.clock() - if posPrec and t > tPrec then - local dx, dz = x - posPrec.x, z - posPrec.z - local dist = math.sqrt(dx * dx + dz * dz) - local vitesse = dist / (t - tPrec) - if vitesse >= conf.NAV.VITESSE_GPS_MIN then - cap = math.atan2(dx, dz) - end - end - posPrec, tPrec = { x = x, y = y, z = z }, t - return x, y, z - end - - -- Commandes propulseurs pour rallier (cibleX, cibleZ). - -- Retourne avance, virage, distance (ou nil si pas de GPS). - function n.rallier(cibleX, cibleZ) - local x, y, z = n.actualiser() - if not x then return nil end - - local dx, dz = cibleX - x, cibleZ - z - local distance = math.sqrt(dx * dx + dz * dz) - if distance <= conf.NAV.SEUIL_ARRIVEE then - return 0, 0, distance - end - - local capCible = math.atan2(dx, dz) - local virage = 0 - if cap then - local ecart = capCible - cap - while ecart > math.pi do ecart = ecart - 2 * math.pi end - while ecart < -math.pi do ecart = ecart + 2 * math.pi end - -- proportionnel borne: plein virage au dela de 45 degres - virage = math.max(-1, math.min(1, ecart / (math.pi / 4))) - end - - -- avance reduite tant que le cap est incertain ou tres desaxe - local avance = 0.8 - if cap == nil then - avance = 0.4 -- demarrage: avancer pour estimer le cap - elseif math.abs(virage) > 0.7 then - avance = 0.2 -- virage serre: privilegier la rotation - end - - return avance, virage, distance - end - - function n.razCap() - cap, posPrec, tPrec = nil, nil, nil - end - - return n -end - -return Navigation diff --git a/lib/pid.lua b/lib/pid.lua index d8986ea..d83590b 100644 --- a/lib/pid.lua +++ b/lib/pid.lua @@ -1,28 +1,31 @@ -------------------------------------------------------------------- --- lib/pid.lua : regulateur PID concu pour le zero-depassement --- - derivee calculee sur la MESURE (pas de coup de derivee au --- changement de consigne) --- - integrale conditionnelle: gelee quand la sortie sature dans le --- sens de l'erreur (anti-emballement) +-- lib/pid.lua : regulateur PID concu pour le zero-depassement. +-- - derivee sur la MESURE (pas de coup au changement de consigne) +-- et FILTREE (EMA) contre le bruit des capteurs +-- - integrale conditionnelle: gelee quand la sortie sature dans +-- le sens de l'erreur (anti-emballement) -------------------------------------------------------------------- local Pid = {} Pid.__index = Pid --- gains: { kp, ki, kd } ; bornes de sortie ; borne d'integrale -function Pid.nouveau(gains, borneMin, borneMax, iMax) +-- gains { kp, ki, kd } lus PAR REFERENCE a chaque pas (edition vive) +function Pid.nouveau(gains, borneMin, borneMax, iMax, filtreD) return setmetatable({ gains = gains, borneMin = borneMin, borneMax = borneMax, iMax = iMax or 40, + filtreD = filtreD or 0.5, -- 1 = brut, 0.5 = lisse integrale = 0, mesurePrec = nil, + deriveeF = 0, }, Pid) end function Pid:raz() self.integrale = 0 self.mesurePrec = nil + self.deriveeF = 0 end function Pid:reglerGains(gains) @@ -33,21 +36,21 @@ function Pid:calculer(consigne, mesure, dt) local g = self.gains local erreur = consigne - mesure - local derivee = 0 if self.mesurePrec and dt > 0 then - derivee = (mesure - self.mesurePrec) / dt + local brute = (mesure - self.mesurePrec) / dt + self.deriveeF = self.deriveeF + + self.filtreD * (brute - self.deriveeF) end self.mesurePrec = mesure - -- tentative d'integration bornee local integrale = self.integrale + erreur * dt if integrale > self.iMax then integrale = self.iMax end if integrale < -self.iMax then integrale = -self.iMax end - local sortie = g.kp * erreur + g.ki * integrale - g.kd * derivee + local sortie = g.kp * erreur + g.ki * integrale + - g.kd * self.deriveeF if sortie > self.borneMax then - -- sature haut: on n'integre que si l'erreur tire vers le bas if erreur < 0 then self.integrale = integrale end return self.borneMax elseif sortie < self.borneMin then diff --git a/lib/pilotage.lua b/lib/pilotage.lua deleted file mode 100644 index b499cbf..0000000 --- a/lib/pilotage.lua +++ /dev/null @@ -1,332 +0,0 @@ --------------------------------------------------------------------- --- lib/pilotage.lua : regulation et mixage. --- --- 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 = {} - -local SIGNES = { - lb = { bf = -1, lr = 1 }, - rb = { bf = -1, lr = -1 }, - lf = { bf = 1, lr = 1 }, - rf = { bf = 1, lr = -1 }, -} - --- Assiette normalisee (calib gimbal): tangage NEGATIF = nez bas, --- roulis NEGATIF = penche a gauche. --- Algebre du signe: nez bas => mesure < 0 => erreur (consigne 0 - --- mesure) > 0 => sortie PID > 0. Il faut alors POUSSER PLUS a --- l'AVANT (bf = +1) pour relever le nez, donc la correction --- s'applique telle quelle: signes +1. (Un -1 ici AMPLIFIE --- l'inclinaison au lieu de la corriger.) -local SIGNE_TANGAGE, SIGNE_ROULIS = 1, 1 - -function Pilotage.nouveau(conf, etat, materiel, Pid, journal) - local p = {} - local VMAX = conf.VITESSE_RSC_MAX - - local function borner(v, mini, maxi) - if v < mini then return mini elseif v > maxi then return maxi end - return v - end - - -- Gains: montee/descente viennent des calibrations (etat.pid), - -- l'assiette vient TOUJOURS de la conf (reference vive: la page - -- PARAM agit immediatement) - local gains = { - alt = conf.PID.alt, - montee = (etat.pid and etat.pid.montee) or conf.PID.montee, - descente = (etat.pid and etat.pid.descente) or conf.PID.descente, - tangage = conf.PID.tangage, - roulis = conf.PID.roulis, - } - - 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 - 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 - local limitation = nil -- info du limiteur de stress - - ------------------------------------------------------------------ - -- 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 - - -- 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 - - ------------------------------------------------------------------ - -- 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 - - function p.reglerGains(nouveaux) - gains.alt = nouveaux.alt or gains.alt - gains.montee = nouveaux.montee or gains.montee - gains.descente = nouveaux.descente or gains.descente - -- seuls les gains verticaux sont persistes: l'assiette reste - -- pilotee par la conf / page PARAM - etat.pid = { montee = gains.montee, descente = gains.descente } - pidMontee:reglerGains(gains.montee) - pidDescente:reglerGains(gains.descente) - end - - function p.reglerFacteurProp(f) - facteurProp = math.max(0, math.min(1, f)) - end - - function p.raz() - pidMontee:raz() - pidDescente:raz() - pidTangage:raz() - pidRoulis:raz() - consigneRampe = nil - regimePrec = nil - altPrec, vFiltre = nil, 0 - 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) - -- autorite d'assiette proportionnelle au point de fonctionnement - local plafond = math.max(2, - conf.ASSIETTE_PLAFOND * (p.rpmSustentation() + rpmAdapt)) - pidTangage.borneMin, pidTangage.borneMax = -plafond, plafond - pidRoulis.borneMin, pidRoulis.borneMax = -plafond, plafond - - 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 = borner(trimTangage + taux * corrTangage, - -plafond, plafond) - trimRoulis = borner(trimRoulis + taux * corrRoulis, - -plafond, plafond) - 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() - - -- rampe de consigne - if consigneRampe == nil then consigneRampe = altitude end - local pas = conf.VITESSE_RAMPE * dt - consigneRampe = consigneRampe - + borner(consigneY - consigneRampe, -pas, pas) - - -- 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 - regimePrec = montee - local deltaRpm = pid:calculer(vCible, vMesuree, dt) - - -- 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 - - -- 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 - - local base = borner(p.rpmSustentation() + rpmAdapt + deltaRpm, - 0, VMAX) - - -- propulseurs (differentiel, delestables en surcharge) - local function b1(v) return borner(v, -1, 1) end - 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 - - ------------------------------------------------------------------ - -- 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) - end - materiel.reglerRsc("prop_l", 0) - materiel.reglerRsc("prop_r", 0) - end - - function p.arreter() - materiel.toutArreter() - p.raz() - end - - return p -end - -return Pilotage diff --git a/lib/regulation.lua b/lib/regulation.lua new file mode 100644 index 0000000..ce29809 --- /dev/null +++ b/lib/regulation.lua @@ -0,0 +1,346 @@ +-------------------------------------------------------------------- +-- lib/regulation.lua : stabilisation et mixage. +-- +-- STRUCTURE (concue contre les oscillations constatees en V1): +-- ALTITUDE en cascade: erreur -> vitesse verticale cible (P, +-- bornee) -> PID vitesse -> delta rpm autour du rpm de +-- sustentation MESURE (calib altitude). La vitesse verticale +-- vient du velocity sensor vertical auto-detecte a la calib +-- (repli: derivee filtree de l'altitude). +-- ASSIETTE: PID par axe a derivee filtree, autorite bornee a +-- ASSIETTE_PLAFOND * rpm de sustentation, zone morte reglable. +-- SUPERVISEUR D'OSCILLATION: si une boucle oscille (passages a +-- zero repetes au-dela d'un seuil d'amplitude), ses gains sont +-- reduits automatiquement de 20%% et l'evenement journalise: +-- le systeme converge vers la stabilite meme mal regle. +-- MOTEUR: mode on/off (pleine puissance) ou adaptatif (niveau +-- regle AVANT l'ecriture des RSC en montee, apres en descente). +-------------------------------------------------------------------- +local Regulation = {} + +-- coefficients de mixage par coin +local SIGNES = { + lb = { bf = -1, lr = 1 }, + rb = { bf = -1, lr = -1 }, + lf = { bf = 1, lr = 1 }, + rf = { bf = 1, lr = -1 }, +} + +-- Algebre du signe (demontree en V1): nez bas => tangage mesure < 0 +-- => erreur (0 - mesure) > 0 => sortie PID > 0, a APPLIQUER en plus +-- a l'AVANT (bf = +1) pour relever le nez. Signes +1. Un -1 ici +-- AMPLIFIE l'inclinaison au lieu de la corriger. +local SIGNE_TANGAGE, SIGNE_ROULIS = 1, 1 + +function Regulation.nouveau(conf, etat, materiel, Pid, journal) + local r = {} + local VMAX = conf.VMAX + + -- borne HELICE courante: 256 RSC * rapport de boite + local function vmaxHelice() + return VMAX * materiel.rapportBoite() + end + + local function borner(v, mini, maxi) + if v < mini then return mini elseif v > maxi then return maxi end + return v + end + + -- gains: conf.PID est la table VIVE (surcharges appliquees au + -- demarrage, page PARAM et calibrations la modifient en place) + local pidVitesse = Pid.nouveau(conf.PID.vitesse, -VMAX, VMAX, 30) + local pidTangage = Pid.nouveau(conf.PID.tangage, -VMAX, VMAX, 15) + local pidRoulis = Pid.nouveau(conf.PID.roulis, -VMAX, VMAX, 15) + + local consigneRampe = nil + local ffAdapt = 0 -- adaptation lente (charge variable) + local altPrec, vFiltre = nil, 0 + local injection = {} -- relais de calibration par axe + local gainsModifies = false -- superviseur: a persister + + ------------------------------------------------------------------ + -- SUSTENTATION: rpm mesure par la calib altitude, corrige par la + -- pression (la poussee diminue avec l'altitude) + ------------------------------------------------------------------ + -- rpm HELICE de sustentation (etat.rpmHover est en unites + -- helice: invariant au rapport de boite) + function r.rpmHover() + local base = etat.rpmHover or VMAX * 0.3 + if etat.pressionHover then + local facteur = etat.pressionHover + / math.max(materiel.lirePression(), 0.05) + base = base * facteur + end + return borner(base + ffAdapt, 0, vmaxHelice()) + end + + ------------------------------------------------------------------ + -- VITESSE VERTICALE: capteur auto-detecte, repli derivee filtree + ------------------------------------------------------------------ + function r.vitesseVerticale(dt) + local vv = etat.veloVertical + if vv then + return materiel.lireVitesseCapteur(vv.nom) * vv.signe + end + local altitude = materiel.lireAltitude() + if altPrec and dt and dt > 0 then + local brute = (altitude - altPrec) / dt + vFiltre = vFiltre + 0.3 * (brute - vFiltre) + end + altPrec = altitude + return vFiltre + end + + ------------------------------------------------------------------ + -- SUPERVISEUR D'OSCILLATION + -- Une boucle qui traverse zero >= 8 fois en 6 s avec une + -- amplitude significative oscille: ses gains sont reduits de 20%% + ------------------------------------------------------------------ + local surveillance = { + vitesse = { seuil = 0.4, gains = conf.PID.vitesse }, + tangage = { seuil = 0.4, gains = conf.PID.tangage }, + roulis = { seuil = 0.4, gains = conf.PID.roulis }, + } + for _, s in pairs(surveillance) do + s.croisements, s.signePrec, s.arme = {}, nil, false + end + + local function surveiller(nom, valeur) + local s = surveillance[nom] + local maintenant = os.clock() + if math.abs(valeur) > s.seuil then s.arme = true end + local signe = valeur >= 0 + if s.signePrec ~= nil and signe ~= s.signePrec and s.arme then + table.insert(s.croisements, maintenant) + s.arme = false + end + s.signePrec = signe + while s.croisements[1] + and maintenant - s.croisements[1] > 6 do + table.remove(s.croisements, 1) + end + if #s.croisements >= 8 then + s.croisements = {} + s.gains.kp = s.gains.kp * 0.8 + s.gains.ki = s.gains.ki * 0.8 + s.gains.kd = s.gains.kd * 0.85 + gainsModifies = true + journal.alerte(("oscillation %s: gains reduits (kp=%.2f)") + :format(nom, s.gains.kp)) + end + end + + -- le superviseur a-t-il modifie des gains (a persister) ? + function r.gainsModifies() + if gainsModifies then gainsModifies = false return true end + return false + end + + ------------------------------------------------------------------ + -- MOTEUR: besoin prevu et niveau anticipe + ------------------------------------------------------------------ + -- cibles: helices en rpm HELICE, propulseurs en rpm RSC; les + -- coefficients (mesures a rapport 1/1) sont en su par rpm helice + local function besoinPrevu(cibles) + if not etat.coeffRsc then return nil end + local besoin = 0 + for role, v in pairs(cibles) do + local nom = materiel.nomParRole[role] + local coeff = nom and etat.coeffRsc[nom] + if coeff then besoin = besoin + coeff * math.abs(v) end + end + return besoin + end + + ------------------------------------------------------------------ + -- BOITE AUTOMATIQUE: maintient la commande RSC des helices dans + -- la bande [RSC_BAS, RSC_HAUT] en changeant le rapport d'UN cran, + -- uniquement en quasi-stationnaire et a plat, avec temps mort. + -- Le regulateur (unites helice) n'y voit RIEN: gains invariants. + ------------------------------------------------------------------ + local prochainChangement = 0 + local boiteModifiee = false + + local function gererBoite(cibles, calme) + local b = etat.boite + if not b or not calme then return end + if os.clock() < prochainChangement then return end + local base = (cibles.lb + cibles.rb + cibles.lf + cibles.rf) / 4 + local rsc = base / b.rapport + local k = math.floor(b.rapport * 16 + 0.5) + local nouveauK = nil + if rsc > conf.BOITE.RSC_HAUT and k < 16 then + nouveauK = (k == 14) and 16 or (k + 1) + elseif rsc < conf.BOITE.RSC_BAS and k > 1 then + nouveauK = (k == 16) and 14 or (k - 1) + end + if nouveauK then + local rpmHelices = { lb = cibles.lb, rb = cibles.rb, + lf = cibles.lf, rf = cibles.rf } + if materiel.changerRapport(nouveauK, rpmHelices) then + prochainChangement = os.clock() + conf.BOITE.ATTENTE + boiteModifiee = true + journal.info(("boite: rapport %d/16 (RSC sustentation " + .. "~%.0f)"):format(nouveauK, base * 16 / nouveauK)) + end + end + end + + -- le rapport a-t-il change (etat a persister) ? + function r.boiteModifiee() + if boiteModifiee then boiteModifiee = false return true end + return false + end + + local function niveauPour(besoin) + local capacites = etat.capaciteParNiveau + if not capacites then return 15 end + local vise = besoin * (1 + conf.MOTEUR.MARGE) + for n = 1, 15 do + if (capacites[n] or 0) >= vise then return n end + end + return 15 + end + + ------------------------------------------------------------------ + -- APPLICATION DES COMMANDES (moteur anticipe puis RSC) + ------------------------------------------------------------------ + local function appliquer(cibles) + if etat.modeMoteur == "adaptatif" then + local besoin = besoinPrevu(cibles) + if besoin then + -- garde de stress: jamais plus que la capacite maximale + local capacites = etat.capaciteParNiveau + local capaMax = capacites and capacites[15] + if capaMax and capaMax > 0 + and besoin > capaMax * conf.MOTEUR.SECURITE then + local facteur = capaMax * conf.MOTEUR.SECURITE / besoin + for role, v in pairs(cibles) do + cibles[role] = v * facteur + end + besoin = besoin * facteur + end + local niveau = niveauPour(besoin) + if niveau > materiel.niveauMoteur() then + -- MONTEE de regime AVANT les RSC (anticipation) + materiel.reglerMoteur(niveau) + for role, v in pairs(cibles) do + materiel.reglerRsc(role, v) + end + return + else + -- descente: RSC d'abord, moteur ensuite + for role, v in pairs(cibles) do + materiel.reglerRsc(role, v) + end + materiel.reglerMoteur(niveau) + return + end + end + end + for role, v in pairs(cibles) do + materiel.reglerRsc(role, v) + end + end + + ------------------------------------------------------------------ + -- UN PAS DE REGULATION EN VOL + -- avance / virage: joystick (-1..1), propulseurs differentiels + ------------------------------------------------------------------ + function r.reguler(consigneY, avance, virage, dt) + local altitude = materiel.lireAltitude() + local tangage, roulis = materiel.lireAssiette() + local hover = r.rpmHover() + + -- rampe de consigne + if consigneRampe == nil then consigneRampe = altitude end + local pas = conf.RAMPE * dt + consigneRampe = consigneRampe + + borner(consigneY - consigneRampe, -pas, pas) + + -- cascade: erreur -> v cible -> delta rpm + local vCible = borner(conf.PID.alt.kp * (consigneRampe - altitude), + -conf.V_DESCENTE_MAX, conf.V_MONTEE_MAX) + local vMesuree = r.vitesseVerticale(dt) + pidVitesse.borneMin = -vmaxHelice() + pidVitesse.borneMax = vmaxHelice() + local deltaRpm = injection.vitesse + or pidVitesse:calculer(vCible, vMesuree, dt) + surveiller("vitesse", vCible - vMesuree) + + -- adaptation lente: en quasi-stationnaire le residu du PID est + -- transfere vers la sustentation (charge variable) + local calme = math.abs(vCible) < 0.2 and math.abs(vMesuree) < 0.3 + if not injection.vitesse and calme then + ffAdapt = borner(ffAdapt + 0.05 * dt * deltaRpm, + -vmaxHelice() / 2, vmaxHelice() / 2) + end + + -- assiette: autorite proportionnelle a la sustentation + local plafond = math.max(2, conf.ASSIETTE_PLAFOND * hover) + pidTangage.borneMin, pidTangage.borneMax = -plafond, plafond + pidRoulis.borneMin, pidRoulis.borneMax = -plafond, plafond + local corrTangage = injection.tangage or (SIGNE_TANGAGE + * pidTangage:calculer(0, tangage, dt)) + local corrRoulis = injection.roulis or (SIGNE_ROULIS + * pidRoulis:calculer(0, roulis, dt)) + surveiller("tangage", tangage) + surveiller("roulis", roulis) + + -- mixage (helices en rpm HELICE) + local base = borner(hover + deltaRpm, 0, vmaxHelice()) + local cibles = {} + for role, s in pairs(SIGNES) do + cibles[role] = borner(base + s.bf * corrTangage + + s.lr * corrRoulis, 0, vmaxHelice()) + end + cibles.prop_r = borner(avance + virage, -1, 1) * VMAX + cibles.prop_l = borner(avance - virage, -1, 1) * VMAX + appliquer(cibles) + + -- boite automatique (apres application: bascule sur des cibles + -- fraiches, uniquement au calme et a plat) + gererBoite(cibles, calme + and math.abs(tangage) < conf.ANGLE_MAX / 2 + and math.abs(roulis) < conf.ANGLE_MAX / 2) + + return altitude, consigneRampe + end + + ------------------------------------------------------------------ + -- MODES SPECIAUX + ------------------------------------------------------------------ + -- stationnement: helices a pleine puissance INVERSEE (plaquage) + function r.plaquer() + local v = vmaxHelice() + appliquer({ lb = -v, rb = -v, lf = -v, rf = -v, + prop_l = 0, prop_r = 0 }) + end + + function r.arreter() + materiel.toutArreter() + r.raz() + end + + function r.raz() + pidVitesse:raz() + pidTangage:raz() + pidRoulis:raz() + consigneRampe = nil + altPrec, vFiltre = nil, 0 + end + + function r.razAdaptation() + ffAdapt = 0 + end + + -- calibrations: forcer une boucle ("vitesse", "tangage", + -- "roulis") a `valeur` (nil = rendre la main au PID) + function r.reglerInjection(boucle, valeur) + injection[boucle] = valeur + end + + return r +end + +return Regulation