248 lines
8.9 KiB
Lua
248 lines
8.9 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: une helice AVANT pousse plus quand le tangage
|
|
-- est negatif (nez bas), une helice GAUCHE quand le roulis est
|
|
-- negatif (penche a gauche)
|
|
local SIGNE_TANGAGE, SIGNE_ROULIS = -1, -1
|
|
|
|
function Pilotage.nouveau(conf, etat, materiel, Pid, journal)
|
|
local p = {}
|
|
local VMAX = conf.VITESSE_RSC_MAX
|
|
local gains = etat.pid or conf.PID
|
|
|
|
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
|
|
|
|
------------------------------------------------------------------
|
|
-- FEEDFORWARD : rpm d'equilibre d'UNE helice
|
|
-- poussee(rpm) = pousseeEffective * (rpm/VMAX)^exp, avec la
|
|
-- poussee CORRIGEE PAR LA PRESSION (elle diminue avec l'altitude):
|
|
-- pousseeEffective = POUSSEE_HELICE_MAX * pression / PRESSION_REF
|
|
-- equilibre: 4 * poussee(rpm) = POIDS (a vide) + charge estimee
|
|
------------------------------------------------------------------
|
|
local function pousseeEffective()
|
|
local facteur = materiel.lirePression() / conf.PRESSION_REF
|
|
return conf.POUSSEE_HELICE_MAX * math.max(facteur, 0.05)
|
|
end
|
|
|
|
function p.rpmSustentation()
|
|
local ratio = (conf.POIDS / 4) / pousseeEffective()
|
|
if ratio <= 0 then return 0 end
|
|
if ratio >= 1 then return VMAX end
|
|
return VMAX * ratio ^ (1 / conf.POUSSEE_EXPOSANT)
|
|
end
|
|
|
|
-- poids total estime (a vide + charge), inverse de la courbe de
|
|
-- poussee au rpm de sustentation adapte, a la pression courante
|
|
function p.poidsEstime()
|
|
local rpm = math.max(0, math.min(VMAX, p.rpmSustentation() + rpmAdapt))
|
|
return 4 * pousseeEffective() * (rpm / VMAX) ^ conf.POUSSEE_EXPOSANT
|
|
end
|
|
|
|
-- plafond de poids total (regle des 60% a la pression de reference)
|
|
function p.poidsMax()
|
|
return conf.RATIO_CHARGE_MAX * 4 * conf.POUSSEE_HELICE_MAX
|
|
end
|
|
|
|
function p.trim()
|
|
return trimTangage, trimRoulis
|
|
end
|
|
|
|
function p.razAdaptation()
|
|
rpmAdapt, trimTangage, trimRoulis = 0, 0, 0
|
|
end
|
|
|
|
------------------------------------------------------------------
|
|
-- MESURE DE VITESSE VERTICALE
|
|
-- capteur calibre si present, sinon derivee filtree de l'altitude
|
|
------------------------------------------------------------------
|
|
function p.vitesseVerticale(altitude, dt)
|
|
if materiel.veloParAxe["vertical"] then
|
|
return materiel.lireVitesseAxe("vertical")
|
|
end
|
|
if altPrec and dt > 0 then
|
|
local brute = (altitude - altPrec) / dt
|
|
vFiltre = vFiltre + 0.3 * (brute - vFiltre)
|
|
end
|
|
altPrec = altitude
|
|
return vFiltre
|
|
end
|
|
|
|
function p.gains()
|
|
return gains
|
|
end
|
|
|
|
function p.reglerGains(nouveaux)
|
|
gains = nouveaux
|
|
etat.pid = nouveaux
|
|
pidMontee:reglerGains(gains.montee)
|
|
pidDescente:reglerGains(gains.descente)
|
|
pidTangage:reglerGains(gains.tangage)
|
|
pidRoulis:reglerGains(gains.roulis)
|
|
end
|
|
|
|
function p.reglerFacteurProp(f)
|
|
facteurProp = math.max(0, math.min(1, f))
|
|
end
|
|
|
|
function p.raz()
|
|
pidMontee:raz()
|
|
pidDescente:raz()
|
|
pidTangage:raz()
|
|
pidRoulis:raz()
|
|
consigneRampe = nil
|
|
regimePrec = nil
|
|
altPrec, vFiltre = nil, 0
|
|
end
|
|
|
|
local function borner(v, mini, maxi)
|
|
if v < mini then return mini elseif v > maxi then return maxi end
|
|
return v
|
|
end
|
|
|
|
-- applique base + trim + corrections d'assiette aux 4 helices.
|
|
-- `adapter` (booleen): en quasi-stationnaire, le trim absorbe
|
|
-- lentement la composante statique des corrections (CG decale par
|
|
-- la cargaison); le PID reste centre sur la dynamique.
|
|
local function appliquerHelices(base, tangage, roulis, dt,
|
|
anglesCibles, adapter)
|
|
local corrTangage = SIGNE_TANGAGE
|
|
* pidTangage:calculer(anglesCibles.tangage or 0, tangage, dt)
|
|
local corrRoulis = SIGNE_ROULIS
|
|
* pidRoulis:calculer(anglesCibles.roulis or 0, roulis, dt)
|
|
if adapter then
|
|
local taux = conf.ADAPTATION.TAUX_TRIM * dt
|
|
trimTangage = trimTangage + taux * corrTangage
|
|
trimRoulis = trimRoulis + taux * corrRoulis
|
|
end
|
|
for role, s in pairs(SIGNES) do
|
|
materiel.reglerRsc(role, borner(base
|
|
+ s.bf * (trimTangage + corrTangage)
|
|
+ s.lr * (trimRoulis + corrRoulis), 0, VMAX))
|
|
end
|
|
end
|
|
|
|
------------------------------------------------------------------
|
|
-- UN PAS DE REGULATION EN VOL
|
|
------------------------------------------------------------------
|
|
function p.reguler(consigneY, angles, avance, virage, dt)
|
|
local altitude = materiel.lireAltitude()
|
|
local tangage, roulis = materiel.lireAssiette()
|
|
|
|
-- 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)
|
|
appliquerHelices(base, tangage, roulis, dt, angles, stationnaire
|
|
and math.abs(tangage) < conf.ANGLE_MAX
|
|
and math.abs(roulis) < conf.ANGLE_MAX)
|
|
|
|
-- propulseurs (differentiel, delestables en surcharge)
|
|
local function b1(v) return borner(v, -1, 1) end
|
|
materiel.reglerRsc("prop_r", b1(avance + virage) * VMAX * facteurProp)
|
|
materiel.reglerRsc("prop_l", b1(avance - virage) * VMAX * facteurProp)
|
|
|
|
return altitude, consigneRampe
|
|
end
|
|
|
|
------------------------------------------------------------------
|
|
-- 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
|