332 lines
12 KiB
Lua
332 lines
12 KiB
Lua
--------------------------------------------------------------------
|
|
-- 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
|