ploush/cctweaked_drone
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

changement complet de logiciel

Browse source
This commit is contained in:
Ploush 2026-07-19 20:08:24 +02:00
parent 9137a98af3
commit 810b1c5e27
13 changed files with 2328 additions and 3063 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

338
README.md
View file

@ -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 <url_base>`
(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 <url_base>` 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 <x> <z>` 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 <x> <y> <z>`): 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 <nom>` | `pt go <nom>` | `pt del <nom>` | `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.

1062
drone.lua
View file

File diff suppressed because it is too large Load diff

735
lib/calib.lua Normal file
View file

@ -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

790
lib/calibration.lua
View file

@ -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

238
lib/conf.lua
View file

@ -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,
},
------------------------------------------------------------------
-- 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
-- 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 },
},
-- 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

375
lib/ecran.lua Normal file
View file

@ -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

66
lib/etat.lua
View file

@ -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",
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
local f = fs.open("drone.etat", "w")
f.write(textutils.serialize(etat))
f.close()
end
end
return Etat

470
lib/ihm.lua
View file

@ -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

479
lib/materiel.lua
View file

@ -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
if role then
m.rscParRole[role] = p
m.nomParRole[role] = nom
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
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
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

77
lib/navigation.lua
View file

@ -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

27
lib/pid.lua
View file

@ -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

332
lib/pilotage.lua
View file

@ -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

346
lib/regulation.lua Normal file
View file

@ -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