934 lines
32 KiB
Lua
934 lines
32 KiB
Lua
--------------------------------------------------------------------
|
|
-- drone.lua : controleur central du quadricoptere Create Aeronautics
|
|
--
|
|
-- Usage :
|
|
-- drone lance le controleur
|
|
-- drone maj <url_base> installe / met a jour depuis un depot
|
|
--
|
|
-- Modes : off, inactif, stationnement, vol, drone, atterrissage,
|
|
-- auto, position
|
|
-- Clavier du PC : auto <x> <z> | position <x> <y> <z>
|
|
-- pt save <nom> | pt go <nom> | pt del <nom> | pt list
|
|
-- rtb (retour au point de decollage)
|
|
--------------------------------------------------------------------
|
|
|
|
local ARGS = { ... }
|
|
local BASE = fs.getDir(shell.getRunningProgram())
|
|
|
|
--------------------------------------------------------------------
|
|
-- INSTALLEUR / MISE A JOUR INTEGRE
|
|
--------------------------------------------------------------------
|
|
local FICHIERS = {
|
|
"drone.lua",
|
|
"lib/journal.lua", "lib/conf.lua", "lib/etat.lua", "lib/pid.lua",
|
|
"lib/materiel.lua", "lib/pilotage.lua", "lib/navigation.lua",
|
|
"lib/ihm.lua", "lib/calibration.lua",
|
|
}
|
|
|
|
if ARGS[1] == "maj" then
|
|
local url = ARGS[2]
|
|
if not url then error("usage: drone maj <url_base>", 0) end
|
|
if not http then error("API http desactivee (config CC:Tweaked)", 0) end
|
|
url = url:gsub("/$", "")
|
|
for _, fichier in ipairs(FICHIERS) do
|
|
write(fichier .. " ... ")
|
|
local reponse, err = http.get(url .. "/" .. fichier)
|
|
if not reponse then error("echec: " .. tostring(err), 0) end
|
|
local contenu = reponse.readAll()
|
|
reponse.close()
|
|
local chemin = fs.combine(BASE, fichier)
|
|
fs.makeDir(fs.getDir(chemin))
|
|
local f = fs.open(chemin, "w")
|
|
f.write(contenu)
|
|
f.close()
|
|
print("ok")
|
|
end
|
|
print("mise a jour terminee, relancer: drone")
|
|
return
|
|
end
|
|
|
|
--------------------------------------------------------------------
|
|
-- CHARGEMENT DES MODULES
|
|
--------------------------------------------------------------------
|
|
local function charger(chemin)
|
|
local complet = fs.combine(BASE, chemin)
|
|
if not fs.exists(complet) then
|
|
error(chemin .. " manquant: lancer 'drone maj <url_base>'", 0)
|
|
end
|
|
return dofile(complet)
|
|
end
|
|
|
|
local journal = charger("lib/journal.lua")
|
|
local ModConf = charger("lib/conf.lua")
|
|
local conf = ModConf.charger(journal)
|
|
local Etat = charger("lib/etat.lua")
|
|
local etat = Etat.charger(journal)
|
|
local Pid = charger("lib/pid.lua")
|
|
local materiel = charger("lib/materiel.lua").initialiser(conf, etat, journal)
|
|
|
|
--------------------------------------------------------------------
|
|
-- COHERENCE DES VERSIONS: si un module a ete mis a jour sans les
|
|
-- autres, on echoue ICI avec un message clair plutot qu'en plein vol
|
|
--------------------------------------------------------------------
|
|
local function exigerInterface(objet, fichier, fonctions)
|
|
local manquantes = {}
|
|
for _, nom in ipairs(fonctions) do
|
|
if type(objet[nom]) ~= "function" then
|
|
table.insert(manquantes, nom)
|
|
end
|
|
end
|
|
if #manquantes > 0 then
|
|
error(("%s obsolete (manque: %s): resynchroniser TOUS les "
|
|
.. "fichiers avec 'drone maj <url>'")
|
|
:format(fichier, table.concat(manquantes, ", ")), 0)
|
|
end
|
|
end
|
|
|
|
exigerInterface(materiel, "lib/materiel.lua", {
|
|
"lirePression", "lireStressBrut", "rafraichirAffectations",
|
|
"actualiserPosition", "lireFacesRelay", "solPosable", "stressPrevu",
|
|
})
|
|
exigerInterface(ModConf, "lib/conf.lua", { "empreintePid" })
|
|
|
|
-- invalidation de la calibration PID si la conf a change
|
|
local empreinte = ModConf.empreintePid(conf)
|
|
if etat.pid and etat.empreintePid ~= empreinte then
|
|
journal.alerte("conf modifiee: gains PID calibres INVALIDES, "
|
|
.. "retour aux gains de la conf (relancer calib pid)")
|
|
etat.pid = nil
|
|
end
|
|
|
|
--------------------------------------------------------------------
|
|
-- PARAMETRES EDITABLES (page PARAM du moniteur)
|
|
-- Les surcharges sont persistees dans drone.etat et PRIMENT sur
|
|
-- drone.conf (sans l'ecraser).
|
|
--------------------------------------------------------------------
|
|
local PARAMS = {
|
|
{ chemin = "CALIB.TOL_DEPASSEMENT", label = "depassement tolere",
|
|
pas = 0.1, mini = 0 },
|
|
{ chemin = "ANGLE_MAX", label = "angle max (deg)", pas = 0.25,
|
|
mini = 0.25 },
|
|
{ chemin = "ANGLE_DRONE_MAX", label = "angle mode drone (deg)",
|
|
pas = 1, mini = 1 },
|
|
{ chemin = "POIDS", label = "poids A VIDE (pN)", pas = 500,
|
|
mini = 0 },
|
|
{ chemin = "POUSSEE_HELICE_MAX", label = "poussee helice (pN)",
|
|
pas = 250, mini = 1 },
|
|
{ chemin = "POUSSEE_EXPOSANT", label = "exposant poussee",
|
|
pas = 1, mini = 1, maxi = 2 },
|
|
{ chemin = "PRESSION_REF", label = "pression de reference",
|
|
pas = 0.01, mini = 0.1, maxi = 1.5 },
|
|
{ chemin = "RATIO_CHARGE_MAX", label = "ratio charge max",
|
|
pas = 0.05, mini = 0.1, maxi = 1 },
|
|
{ chemin = "ADAPTATION.SEUIL_DESEQUILIBRE",
|
|
label = "seuil desequilibre (rpm)", pas = 5, mini = 5 },
|
|
{ chemin = "MOTEUR.MARGE", label = "marge moteur", pas = 0.05,
|
|
mini = 0.05, maxi = 0.5 },
|
|
{ chemin = "MOTEUR.SECURITE", label = "securite stress",
|
|
pas = 0.02, mini = 0.5, maxi = 0.98 },
|
|
{ chemin = "MOTEUR.BOOST", label = "boost moteur (s)",
|
|
pas = 0.5, mini = 0, maxi = 10 },
|
|
{ chemin = "CALIB.ATTENTE_NIVEAU", label = "attente calib mot. (s)",
|
|
pas = 0.5, mini = 0.5, maxi = 5 },
|
|
{ chemin = "V_MONTEE_MAX", label = "v montee max (b/s)",
|
|
pas = 0.5, mini = 0.5 },
|
|
{ chemin = "V_DESCENTE_MAX", label = "v descente max (b/s)",
|
|
pas = 0.5, mini = 0.5 },
|
|
{ chemin = "VITESSE_RAMPE", label = "rampe consigne (b/s)",
|
|
pas = 0.5, mini = 0.5 },
|
|
{ chemin = "Y_VOL", label = "altitude croisiere", pas = 5 },
|
|
{ chemin = "Y_MINI", label = "altitude plancher", pas = 5 },
|
|
}
|
|
|
|
local function lireChemin(chemin)
|
|
local noeud = conf
|
|
for partie in chemin:gmatch("[^%.]+") do
|
|
if type(noeud) ~= "table" then return nil end
|
|
noeud = noeud[partie]
|
|
end
|
|
return noeud
|
|
end
|
|
|
|
local function ecrireChemin(chemin, valeur)
|
|
local noeud, precedente, cle = conf, nil, nil
|
|
for partie in chemin:gmatch("[^%.]+") do
|
|
precedente, cle = noeud, partie
|
|
noeud = noeud[partie]
|
|
end
|
|
precedente[cle] = valeur
|
|
end
|
|
|
|
etat.surcharges = etat.surcharges or {}
|
|
for chemin, valeur in pairs(etat.surcharges) do
|
|
ecrireChemin(chemin, valeur)
|
|
end
|
|
|
|
local pilotage = charger("lib/pilotage.lua")
|
|
.nouveau(conf, etat, materiel, Pid, journal)
|
|
exigerInterface(pilotage, "lib/pilotage.lua", {
|
|
"poidsEstime", "poidsMax", "trim", "razAdaptation", "pasOuvert",
|
|
"limitationStress",
|
|
})
|
|
local navigation = charger("lib/navigation.lua")
|
|
.nouveau(conf, materiel, journal)
|
|
local Calibration = charger("lib/calibration.lua")
|
|
local ihm = charger("lib/ihm.lua").nouveau(conf, materiel, journal)
|
|
|
|
os.setComputerLabel("PC_central")
|
|
|
|
--------------------------------------------------------------------
|
|
-- ETAT DE VOL
|
|
--------------------------------------------------------------------
|
|
local DT = 0.1
|
|
local mode = etat.mode or "off"
|
|
local consigneY = etat.consigneY or materiel.lireAltitude()
|
|
local cibleNav = nil
|
|
local phaseAuto = nil
|
|
local distanceCible = nil
|
|
local stress = nil
|
|
local carburantPct, autonomieMin = nil, nil
|
|
local saisieTypewriter = ""
|
|
local saisieCible = ""
|
|
local enCalibration = false
|
|
local calibEnCours = nil
|
|
local calibProgres, calibTexte = nil, nil
|
|
local rscViolet = nil
|
|
local alerteMoteurA = 0
|
|
local alerteChargeA = 0
|
|
local alerteStressA = 0
|
|
local boostMoteurFin = 0
|
|
local niveauMoteur = 15
|
|
|
|
local function sauverEtat()
|
|
etat.mode, etat.consigneY = mode, consigneY
|
|
Etat.sauver(etat)
|
|
end
|
|
|
|
--------------------------------------------------------------------
|
|
-- VERROUS: modes indisponibles tant que des calibrations manquent
|
|
--------------------------------------------------------------------
|
|
local function verrouDe(m)
|
|
local gimbalOk = etat.gimbal ~= nil
|
|
local helicesOk = #materiel.helicesManquantes() == 0
|
|
local joystickOk = etat.joystick ~= nil
|
|
|
|
if m == "off" or m == "inactif" or m == "calibrage" then return nil end
|
|
if not gimbalOk then return "calib gimbal requise" end
|
|
if m == "stationnement" then return nil end
|
|
if not helicesOk then return "calib rsc requise" end
|
|
if (m == "vol" or m == "drone") and not joystickOk then
|
|
return "calib joystick requise"
|
|
end
|
|
if m == "auto" and not materiel.lirePosition() then
|
|
return "pas de GPS"
|
|
end
|
|
return nil
|
|
end
|
|
|
|
local function verrous()
|
|
local v = {}
|
|
for _, m in ipairs({ "off", "inactif", "calibrage", "stationnement",
|
|
"vol", "drone", "atterrissage", "auto" }) do
|
|
v[m] = verrouDe(m)
|
|
end
|
|
return v
|
|
end
|
|
|
|
--------------------------------------------------------------------
|
|
-- CHANGEMENTS DE MODE
|
|
--------------------------------------------------------------------
|
|
local function changerMode(nouveau, raison)
|
|
if nouveau == mode then return end
|
|
local modePrec = mode
|
|
|
|
local verrou = verrouDe(nouveau)
|
|
if verrou then
|
|
journal.alerte(nouveau .. " refuse: " .. verrou)
|
|
ihm.message(nouveau .. ": " .. verrou)
|
|
return
|
|
end
|
|
if nouveau == "stationnement"
|
|
and not (materiel.auSol() and materiel.horizontal()) then
|
|
journal.alerte("stationnement refuse: au sol et horizontal requis")
|
|
ihm.message("stationnement: sol+plat requis")
|
|
return
|
|
end
|
|
|
|
-- pre-boost du moteur avant les regimes exigeants (evite
|
|
-- l'overstress transitoire, ex: inactif -> stationnement)
|
|
local exigeants = { stationnement = true, vol = true, drone = true,
|
|
atterrissage = true, auto = true }
|
|
if exigeants[nouveau] then
|
|
boostMoteurFin = os.clock() + conf.MOTEUR.BOOST
|
|
materiel.reglerMoteur(15)
|
|
end
|
|
|
|
local etaitAuSol = { off = true, inactif = true, calibrage = true,
|
|
stationnement = true }
|
|
mode = nouveau
|
|
pilotage.raz()
|
|
navigation.razCap()
|
|
|
|
if etaitAuSol[modePrec or ""] and (mode == "vol" or mode == "drone"
|
|
or mode == "auto" or mode == "atterrissage") then
|
|
-- nouveau vol = nouvelle cargaison: on repart de zero
|
|
pilotage.razAdaptation()
|
|
journal.info("adaptation remise a zero (nouveau vol)")
|
|
end
|
|
|
|
if mode == "vol" or mode == "drone" then
|
|
consigneY = materiel.lireAltitude()
|
|
elseif mode == "auto" then
|
|
phaseAuto = "decollage"
|
|
local x, y, z = materiel.lirePosition()
|
|
etat.decollage = { x = x, y = y, z = z }
|
|
consigneY = cibleNav.y or conf.Y_VOL
|
|
elseif mode == "off" then
|
|
pilotage.arreter()
|
|
end
|
|
|
|
journal.info("mode: " .. mode .. (raison and (" (" .. raison .. ")") or ""))
|
|
sauverEtat()
|
|
end
|
|
|
|
--------------------------------------------------------------------
|
|
-- DESCENTE SECURISEE (garde-fou ocean)
|
|
-- La descente sous Y_MINI exige un sol POSABLE detecte par
|
|
-- l'optical sensor; sinon: maintien a Y_MINI + alerte.
|
|
--------------------------------------------------------------------
|
|
local function descendre()
|
|
local cible = consigneY - conf.VITESSE_ATTERRISSAGE * DT
|
|
if cible < conf.Y_MINI and not materiel.solPosable() then
|
|
ihm.message("sol non posable: maintien Y_MINI")
|
|
return math.max(consigneY, conf.Y_MINI)
|
|
end
|
|
return cible
|
|
end
|
|
|
|
--------------------------------------------------------------------
|
|
-- UN PAS DE CONTROLE PAR MODE
|
|
--------------------------------------------------------------------
|
|
local function pasControle()
|
|
if enCalibration then return end
|
|
|
|
-- alerte moteur: dans tous les modes ou il devrait tourner
|
|
if mode ~= "off" and not materiel.moteurTourne() then
|
|
if os.clock() - alerteMoteurA > 5 then
|
|
alerteMoteurA = os.clock()
|
|
journal.erreur("le moteur ne tourne pas alors qu'il le devrait")
|
|
ihm.message("ERREUR: MOTEUR ARRETE", 5)
|
|
end
|
|
end
|
|
|
|
-- surveillance de la charge (poids estime, equilibrage)
|
|
local enVol = mode == "vol" or mode == "drone" or mode == "auto"
|
|
or mode == "atterrissage"
|
|
if enVol and os.clock() - alerteChargeA > 8 then
|
|
if pilotage.poidsEstime() > pilotage.poidsMax() then
|
|
alerteChargeA = os.clock()
|
|
journal.alerte(("SURCHARGE: poids estime %.0f > max %.0f pN")
|
|
:format(pilotage.poidsEstime(), pilotage.poidsMax()))
|
|
ihm.message("SURCHARGE POIDS", 6)
|
|
else
|
|
local tT, tR = pilotage.trim()
|
|
if math.max(math.abs(tT), math.abs(tR))
|
|
> conf.ADAPTATION.SEUIL_DESEQUILIBRE then
|
|
alerteChargeA = os.clock()
|
|
journal.alerte(("chargement desequilibre (trim %.0f/%.0f rpm)")
|
|
:format(tT, tR))
|
|
ihm.message("CHARGEMENT DESEQUILIBRE", 6)
|
|
end
|
|
end
|
|
end
|
|
|
|
local limitation = pilotage.limitationStress()
|
|
if limitation and os.clock() - alerteStressA > 6 then
|
|
alerteStressA = os.clock()
|
|
if limitation.critique then
|
|
journal.alerte(("stress insuffisant pour la sustentation "
|
|
.. "(besoin %.0f, budget %.0f)"):format(limitation.besoin,
|
|
limitation.budget))
|
|
ihm.message("STRESS INSUFFISANT", 6)
|
|
else
|
|
journal.info(("limiteur de stress actif (besoin %.0f, budget %.0f)")
|
|
:format(limitation.besoin, limitation.budget))
|
|
end
|
|
end
|
|
|
|
if mode == "off" then
|
|
pilotage.arreter()
|
|
|
|
elseif mode == "inactif" or mode == "calibrage" then
|
|
-- moteur allume, tout a zero; calibrage autorise les calibrations
|
|
materiel.toutArreter()
|
|
|
|
elseif mode == "stationnement" then
|
|
if not (materiel.auSol() and materiel.horizontal()) then
|
|
changerMode("vol", "conditions de stationnement perdues")
|
|
return
|
|
end
|
|
pilotage.plaquer()
|
|
|
|
elseif mode == "vol" then
|
|
local avance, virage = materiel.lireJoystick()
|
|
pilotage.reguler(consigneY, { tangage = 0, roulis = 0 },
|
|
avance, virage, DT)
|
|
|
|
elseif mode == "drone" then
|
|
local avance, virage = materiel.lireJoystick()
|
|
pilotage.reguler(consigneY, {
|
|
tangage = avance * conf.ANGLE_DRONE_MAX,
|
|
roulis = -virage * conf.ANGLE_DRONE_MAX,
|
|
}, 0, 0, DT)
|
|
|
|
elseif mode == "atterrissage" then
|
|
if materiel.auSol() then
|
|
changerMode("stationnement", "sol atteint")
|
|
return
|
|
end
|
|
consigneY = descendre()
|
|
pilotage.reguler(consigneY, { tangage = 0, roulis = 0 }, 0, 0, DT)
|
|
|
|
elseif mode == "auto" then
|
|
-- cible a 2 coordonnees (x, z): mission complete avec
|
|
-- atterrissage; cible a 3 (x, y, z): rallier et MAINTENIR a y
|
|
local yCroisiere = cibleNav.y or conf.Y_VOL
|
|
if cibleNav.y and cibleNav.y < materiel.lireAltitude() then
|
|
-- descente limitee par la presence d'un sol
|
|
if materiel.lireDistanceSol() <= conf.DIST_SOL + 0.5 then
|
|
yCroisiere = materiel.lireAltitude()
|
|
elseif cibleNav.y < conf.Y_MINI and not materiel.solPosable() then
|
|
yCroisiere = conf.Y_MINI
|
|
end
|
|
end
|
|
|
|
if phaseAuto == "decollage" then
|
|
local alt = pilotage.reguler(yCroisiere,
|
|
{ tangage = 0, roulis = 0 }, 0, 0, DT)
|
|
if math.abs(alt - yCroisiere) <= 0.5 then
|
|
phaseAuto = "croisiere"
|
|
journal.info("auto: croisiere vers la cible")
|
|
end
|
|
elseif phaseAuto == "croisiere" then
|
|
local avance, virage, distance = navigation.rallier(
|
|
cibleNav.x, cibleNav.z)
|
|
if not avance then
|
|
changerMode("vol", "GPS perdu")
|
|
return
|
|
end
|
|
distanceCible = distance
|
|
pilotage.reguler(yCroisiere, { tangage = 0, roulis = 0 },
|
|
avance, virage, DT)
|
|
if distance <= conf.NAV.SEUIL_ARRIVEE then
|
|
if cibleNav.y then
|
|
phaseAuto = "maintien"
|
|
journal.info("auto: cible atteinte, maintien en position")
|
|
else
|
|
phaseAuto = "atterrissage"
|
|
consigneY = materiel.lireAltitude()
|
|
journal.info("auto: cible atteinte, atterrissage")
|
|
end
|
|
end
|
|
elseif phaseAuto == "maintien" then
|
|
local avance, virage, distance = navigation.rallier(
|
|
cibleNav.x, cibleNav.z)
|
|
distanceCible = distance
|
|
pilotage.reguler(yCroisiere, { tangage = 0, roulis = 0 },
|
|
avance or 0, virage or 0, DT)
|
|
else
|
|
if materiel.auSol() then
|
|
changerMode("stationnement", "mission terminee")
|
|
return
|
|
end
|
|
consigneY = descendre()
|
|
pilotage.reguler(consigneY, { tangage = 0, roulis = 0 }, 0, 0, DT)
|
|
end
|
|
end
|
|
end
|
|
|
|
--------------------------------------------------------------------
|
|
-- CALIBRATIONS
|
|
--------------------------------------------------------------------
|
|
local ui = {
|
|
inviter = function(texte)
|
|
ihm.message(texte, 3600)
|
|
calibTexte = texte
|
|
pcall(ihm.rafraichirConduite,
|
|
{ mode = mode, consigneY = consigneY })
|
|
journal.info("[calib] " .. texte)
|
|
end,
|
|
progres = function(pct, texte)
|
|
calibProgres, calibTexte = pct, texte
|
|
end,
|
|
rscViolet = function(role) rscViolet = role end,
|
|
attendreToucher = ihm.attendreToucher,
|
|
choisirRole = ihm.choisirRole,
|
|
}
|
|
|
|
local function lancerCalibration(quoi)
|
|
enCalibration = true
|
|
calibEnCours = quoi
|
|
calibProgres, calibTexte = 0, quoi
|
|
local ok, err = pcall(function()
|
|
if quoi == "gimbal" or quoi == "joystick" or quoi == "typew"
|
|
or quoi == "rsc" or quoi == "rscman" or quoi == "moteur" then
|
|
if mode ~= "calibrage" then
|
|
journal.alerte("calib " .. quoi
|
|
.. ": passer en mode calibrage d'abord")
|
|
ihm.message("passer en mode calibrage")
|
|
return
|
|
end
|
|
end
|
|
if quoi == "gimbal" then
|
|
Calibration.gimbal(conf, etat, materiel, journal, Etat, ui)
|
|
elseif quoi == "joystick" then
|
|
Calibration.joystick(conf, etat, materiel, journal, Etat, ui)
|
|
elseif quoi == "typew" then
|
|
Calibration.typewriter(conf, etat, materiel, journal, Etat, ui)
|
|
elseif quoi == "rsc" then
|
|
Calibration.rsc(conf, etat, materiel, journal, Etat, ui)
|
|
elseif quoi == "rscman" then
|
|
Calibration.rscManuel(conf, etat, materiel, journal, Etat, ui)
|
|
elseif quoi == "moteur" then
|
|
Calibration.moteur(conf, etat, materiel, journal, Etat, ui)
|
|
elseif quoi == "capteurs" or quoi == "pid" or quoi == "pidmath" then
|
|
if mode ~= "vol" then
|
|
journal.alerte("calib " .. quoi .. ": passer en mode vol d'abord")
|
|
ihm.message("passer en mode vol")
|
|
elseif quoi == "pid" then
|
|
Calibration.pid(conf, etat, materiel, pilotage, journal, Etat,
|
|
empreinte, ui)
|
|
elseif quoi == "pidmath" then
|
|
Calibration.mathematique(conf, etat, materiel, pilotage,
|
|
journal, Etat, empreinte, ui)
|
|
else
|
|
local function tenir(duree, angles, avance)
|
|
local cible = materiel.lireAltitude()
|
|
return function()
|
|
for _ = 1, math.floor(duree / DT) do
|
|
pilotage.reguler(cible, angles, avance or 0, 0, DT)
|
|
sleep(DT)
|
|
end
|
|
end
|
|
end
|
|
Calibration.capteurs(conf, etat, materiel, journal, Etat, ui, {
|
|
monter = function()
|
|
local cible = materiel.lireAltitude() + 3
|
|
for _ = 1, 30 do
|
|
pilotage.reguler(cible, { tangage = 0, roulis = 0 },
|
|
0, 0, DT)
|
|
sleep(DT)
|
|
end
|
|
end,
|
|
avancer = tenir(3, { tangage = 0, roulis = 0 }, 0.6),
|
|
gauche = tenir(3,
|
|
{ tangage = 0, roulis = -conf.ANGLE_DRONE_MAX / 2 }, 0),
|
|
})
|
|
end
|
|
end
|
|
end)
|
|
if not ok then journal.erreur("calibration: " .. tostring(err)) end
|
|
ihm.message("", 0)
|
|
calibEnCours, rscViolet = nil, nil
|
|
enCalibration = false
|
|
end
|
|
|
|
-- statuts des calibrations pour la page CALIB
|
|
local function calibStatuts()
|
|
local faits = {
|
|
gimbal = etat.gimbal ~= nil,
|
|
joystick = etat.joystick ~= nil,
|
|
typew = etat.typewriter ~= nil,
|
|
rsc = #materiel.helicesManquantes() == 0
|
|
and #materiel.propulseursManquants() == 0,
|
|
rscman = #materiel.helicesManquantes() == 0
|
|
and #materiel.propulseursManquants() == 0,
|
|
capteurs = #materiel.axesManquants() == 0,
|
|
pid = etat.pid ~= nil,
|
|
pidmath = etat.pid ~= nil,
|
|
moteur = etat.capaciteParNiveau ~= nil,
|
|
}
|
|
local statuts = {}
|
|
for nom, fait in pairs(faits) do
|
|
if calibEnCours == nom then statuts[nom] = "encours"
|
|
elseif fait then statuts[nom] = "fait" end
|
|
end
|
|
return statuts
|
|
end
|
|
|
|
--------------------------------------------------------------------
|
|
-- POINTS NOMMES / RETOUR DECOLLAGE
|
|
--------------------------------------------------------------------
|
|
local function nbPoints()
|
|
local n = 0
|
|
for _ in pairs(etat.points) do n = n + 1 end
|
|
return n
|
|
end
|
|
|
|
local function commandePoint(mots)
|
|
if mots[2] == "save" and mots[3] then
|
|
if nbPoints() >= conf.NAV.MAX_POINTS and not etat.points[mots[3]] then
|
|
print(("maximum %d points"):format(conf.NAV.MAX_POINTS))
|
|
return
|
|
end
|
|
local x, y, z = materiel.lirePosition()
|
|
if not x then print("pas de GPS") return end
|
|
etat.points[mots[3]] = { x = x, y = y, z = z }
|
|
Etat.sauver(etat)
|
|
print(("point '%s' enregistre (%.0f %.0f %.0f)"):format(mots[3], x, y, z))
|
|
elseif mots[2] == "go" and mots[3] then
|
|
local p = etat.points[mots[3]]
|
|
if not p then print("point inconnu") return end
|
|
cibleNav = { x = p.x, z = p.z }
|
|
changerMode("auto", "pt go " .. mots[3])
|
|
elseif mots[2] == "del" and mots[3] then
|
|
etat.points[mots[3]] = nil
|
|
Etat.sauver(etat)
|
|
print("point supprime")
|
|
elseif mots[2] == "list" then
|
|
for nom, p in pairs(etat.points) do
|
|
print((" %s : %.0f %.0f %.0f"):format(nom, p.x, p.y, p.z))
|
|
end
|
|
else
|
|
print("pt save <nom> | pt go <nom> | pt del <nom> | pt list")
|
|
end
|
|
end
|
|
|
|
--------------------------------------------------------------------
|
|
-- TACHES PARALLELES
|
|
--------------------------------------------------------------------
|
|
local function tacheControle()
|
|
while true do
|
|
local ok, err = pcall(pasControle)
|
|
if not ok then
|
|
journal.erreur("controle: " .. tostring(err))
|
|
pilotage.arreter()
|
|
end
|
|
sleep(DT)
|
|
end
|
|
end
|
|
|
|
-- Moteur PREDICTIF: le niveau est choisi d'apres le besoin PREVU
|
|
-- des rpm commandes (calib moteur), avec apprentissage continu de la
|
|
-- table de capacites. Repli reactif tant que la calib n'est pas
|
|
-- faite. La SURCHARGE mesuree reste le garde-fou ultime.
|
|
local function tacheMoteur()
|
|
local enSurcharge = false
|
|
while true do
|
|
if mode == "off" then
|
|
materiel.reglerMoteur(0)
|
|
elseif os.clock() < boostMoteurFin then
|
|
niveauMoteur = 15
|
|
materiel.reglerMoteur(15)
|
|
else
|
|
stress = materiel.lireStress()
|
|
local besoin = materiel.stressPrevu()
|
|
local capacites = etat.capaciteParNiveau
|
|
|
|
if besoin and capacites then
|
|
-- PREDICTIF: plus petit niveau couvrant besoin * (1 + marge)
|
|
local vise = besoin * (1 + conf.MOTEUR.MARGE)
|
|
local cible = 15
|
|
for n = 1, 15 do
|
|
if (capacites[n] or 0) >= vise then cible = n break end
|
|
end
|
|
-- monter immediatement, descendre d'un cran max par periode
|
|
if cible > niveauMoteur then niveauMoteur = cible
|
|
elseif cible < niveauMoteur then niveauMoteur = niveauMoteur - 1 end
|
|
|
|
-- apprentissage continu de la capacite au niveau courant
|
|
local _, capMes = materiel.lireStressBrut()
|
|
if capMes and capMes > 0 then
|
|
capacites[niveauMoteur] = capacites[niveauMoteur]
|
|
and (0.9 * capacites[niveauMoteur] + 0.1 * capMes) or capMes
|
|
end
|
|
elseif stress then
|
|
-- REACTIF (calib moteur non faite)
|
|
if stress > conf.MOTEUR.STRESS_HAUT and niveauMoteur < 15 then
|
|
niveauMoteur = niveauMoteur + 1
|
|
elseif stress < conf.MOTEUR.STRESS_BAS and niveauMoteur > 1 then
|
|
niveauMoteur = niveauMoteur - 1
|
|
end
|
|
else
|
|
niveauMoteur = 15
|
|
end
|
|
|
|
-- garde-fou ultime sur la mesure
|
|
if stress and stress > conf.MOTEUR.SURCHARGE
|
|
and niveauMoteur >= 15 then
|
|
if not enSurcharge then
|
|
enSurcharge = true
|
|
journal.alerte("surcharge de stress: propulseurs reduits")
|
|
ihm.message("SURCHARGE: propulseurs reduits", 10)
|
|
end
|
|
pilotage.reglerFacteurProp(0.3)
|
|
elseif enSurcharge
|
|
and (not stress or stress < conf.MOTEUR.STRESS_HAUT) then
|
|
enSurcharge = false
|
|
pilotage.reglerFacteurProp(1.0)
|
|
journal.info("surcharge resorbee")
|
|
end
|
|
|
|
materiel.reglerMoteur(niveauMoteur)
|
|
end
|
|
sleep(conf.MOTEUR.PERIODE)
|
|
end
|
|
end
|
|
|
|
-- carburant: pourcentage de reserve + autonomie estimee par
|
|
-- APPRENTISSAGE GLISSANT (EMA) de la consommation. Les hausses de
|
|
-- quantite (ravitaillement) sont exclues de l'apprentissage.
|
|
local function tacheCarburant()
|
|
local quantitePrec, tPrec = nil, nil
|
|
local consoLissee = nil -- mB/s apprise
|
|
while true do
|
|
local quantite, capacite = materiel.lireCarburant()
|
|
if quantite then
|
|
carburantPct = quantite / capacite
|
|
local t = os.clock()
|
|
|
|
if quantitePrec and t > tPrec and mode ~= "off" then
|
|
local conso = (quantitePrec - quantite) / (t - tPrec)
|
|
if conso >= 0 then -- conso negative = ravitaillement: ignore
|
|
local a = conf.CARBURANT.LISSAGE
|
|
consoLissee = consoLissee
|
|
and (a * conso + (1 - a) * consoLissee)
|
|
or conso
|
|
end
|
|
end
|
|
quantitePrec, tPrec = quantite, t
|
|
|
|
autonomieMin = nil
|
|
if consoLissee and consoLissee > 0 then
|
|
autonomieMin = quantite / consoLissee / 60
|
|
end
|
|
|
|
if carburantPct <= conf.CARBURANT.SEUIL_ALERTE then
|
|
ihm.message("CARBURANT BAS", 4)
|
|
end
|
|
pcall(ihm.rafraichirCarburant, carburantPct, autonomieMin)
|
|
end
|
|
sleep(conf.CARBURANT.PERIODE)
|
|
end
|
|
end
|
|
|
|
-- typewriter: saisie de la consigne d'altitude (scrutation des
|
|
-- codes ASCII, clavier qwerty). Mappage par defaut integre; la
|
|
-- calibration 'typew' (etat.typewriter) le remplace si presente.
|
|
-- 0..9 chiffres de la saisie
|
|
-- Entree valider la saisie comme nouvelle consigne
|
|
-- Backspace effacer le dernier chiffre
|
|
-- + / - ajuster la consigne de +-1 (hors saisie en cours)
|
|
local ASCII = {
|
|
[13] = "valider", [10] = "valider",
|
|
[8] = "effacer", [127] = "effacer",
|
|
[43] = "plus", [45] = "moins",
|
|
}
|
|
for code = 48, 57 do ASCII[code] = string.char(code) end
|
|
|
|
local function tacheTypewriter()
|
|
local pressees = {}
|
|
while true do
|
|
if not enCalibration then
|
|
local courantes = {}
|
|
for _, code in ipairs(materiel.lireTouches()) do
|
|
courantes[code] = true
|
|
if not pressees[code] then
|
|
local symbole = (etat.typewriter or ASCII)[code]
|
|
if symbole == "valider" then
|
|
local valeur = tonumber(saisieTypewriter)
|
|
if valeur then
|
|
consigneY = valeur
|
|
journal.info(("typewriter: consigne %d"):format(valeur))
|
|
sauverEtat()
|
|
end
|
|
saisieTypewriter = ""
|
|
elseif symbole == "effacer" then
|
|
saisieTypewriter = saisieTypewriter:sub(1, -2)
|
|
elseif symbole == "plus" and #saisieTypewriter == 0 then
|
|
consigneY = consigneY + 1
|
|
sauverEtat()
|
|
elseif symbole == "moins" and #saisieTypewriter == 0 then
|
|
consigneY = consigneY - 1
|
|
sauverEtat()
|
|
elseif symbole and #symbole == 1 then
|
|
saisieTypewriter = (saisieTypewriter .. symbole):sub(1, 6)
|
|
end
|
|
end
|
|
end
|
|
pressees = courantes
|
|
end
|
|
sleep(0.1)
|
|
end
|
|
end
|
|
|
|
-- GPS: seule tache autorisee a faire l'appel bloquant (jusqu'a 2 s)
|
|
local function tachePosition()
|
|
while true do
|
|
if mode ~= "off" then
|
|
pcall(materiel.actualiserPosition)
|
|
end
|
|
sleep(3)
|
|
end
|
|
end
|
|
|
|
local function listeParams()
|
|
local liste = {}
|
|
for _, p in ipairs(PARAMS) do
|
|
table.insert(liste, { label = p.label, valeur = lireChemin(p.chemin) })
|
|
end
|
|
return liste
|
|
end
|
|
|
|
local function ajusterParam(idx, sens)
|
|
local p = PARAMS[idx]
|
|
if not p then return end
|
|
local valeur = (lireChemin(p.chemin) or 0) + sens * p.pas
|
|
if p.mini and valeur < p.mini then valeur = p.mini end
|
|
if p.maxi and valeur > p.maxi then valeur = p.maxi end
|
|
ecrireChemin(p.chemin, valeur)
|
|
etat.surcharges[p.chemin] = valeur
|
|
Etat.sauver(etat)
|
|
journal.info(("param %s = %s"):format(p.chemin, tostring(valeur)))
|
|
if etat.pid then
|
|
ihm.message("param modifie: recalibrer pid conseille")
|
|
end
|
|
end
|
|
|
|
local function contexteIhm()
|
|
local px, py, pz = materiel.lirePosition()
|
|
return {
|
|
mode = mode,
|
|
consigneY = consigneY,
|
|
stress = stress,
|
|
moteurOk = mode == "off" or materiel.moteurTourne(),
|
|
carburantPct = carburantPct,
|
|
minutes = autonomieMin,
|
|
position = px and { x = px, y = py, z = pz } or nil,
|
|
points = etat.points,
|
|
verrous = verrous(),
|
|
calibStatuts = calibStatuts(),
|
|
calibProgres = calibProgres,
|
|
calibTexte = calibTexte,
|
|
rscViolet = rscViolet,
|
|
cible = cibleNav and (cibleNav.y
|
|
and ("%d %d %d"):format(cibleNav.x, cibleNav.y, cibleNav.z)
|
|
or ("%d %d"):format(cibleNav.x, cibleNav.z)) or nil,
|
|
distanceCible = distanceCible,
|
|
saisieCible = saisieCible,
|
|
params = listeParams(),
|
|
poidsEstime = pilotage.poidsEstime(),
|
|
poidsMax = pilotage.poidsMax(),
|
|
trimTangage = select(1, pilotage.trim()),
|
|
trimRoulis = select(2, pilotage.trim()),
|
|
niveauMoteur = niveauMoteur,
|
|
stressPrevu = materiel.stressPrevu(),
|
|
}
|
|
end
|
|
|
|
-- lance la cible saisie: "x z" ou "x y z"
|
|
local function lancerCible(texte)
|
|
local nombres = {}
|
|
for n in texte:gmatch("%-?%d+") do table.insert(nombres, tonumber(n)) end
|
|
if #nombres == 2 then
|
|
cibleNav = { x = nombres[1], z = nombres[2] }
|
|
changerMode("auto", "cible x z")
|
|
elseif #nombres == 3 then
|
|
cibleNav = { x = nombres[1], y = nombres[2], z = nombres[3] }
|
|
changerMode("auto", "cible x y z")
|
|
else
|
|
ihm.message("format: x z ou x y z")
|
|
end
|
|
end
|
|
|
|
local function tacheIhm()
|
|
local prochainDessin = 0
|
|
local redessiner = false
|
|
while true do
|
|
if (os.clock() >= prochainDessin or redessiner) then
|
|
redessiner = false
|
|
pcall(ihm.rafraichirConduite, {
|
|
mode = mode, consigneY = consigneY, saisie = saisieTypewriter,
|
|
})
|
|
pcall(ihm.rafraichirMoniteur, contexteIhm())
|
|
prochainDessin = os.clock() + 1.0
|
|
end
|
|
|
|
local minuteur = os.startTimer(0.25)
|
|
local ev, a, b, c = os.pullEvent()
|
|
if ev == "monitor_touch" then
|
|
local action = not enCalibration and ihm.traiterToucher(b, c)
|
|
if action then
|
|
redessiner = true
|
|
if action.type == "mode" then
|
|
if action.verrou then
|
|
ihm.message(action.valeur .. ": " .. action.verrou)
|
|
else
|
|
changerMode(action.valeur, "moniteur")
|
|
end
|
|
elseif action.type == "delta" then
|
|
consigneY = consigneY + action.valeur
|
|
sauverEtat()
|
|
elseif action.type == "calib" then
|
|
lancerCalibration(action.valeur)
|
|
elseif action.type == "ptgo" then
|
|
local p = etat.points[action.nom]
|
|
if p then
|
|
cibleNav = { x = p.x, z = p.z }
|
|
changerMode("auto", "pt " .. action.nom)
|
|
end
|
|
elseif action.type == "annuler" then
|
|
changerMode("vol", "annulation auto")
|
|
elseif action.type == "param" then
|
|
ajusterParam(action.idx, action.sens)
|
|
elseif action.type == "pave" then
|
|
if action.valeur == "eff" then
|
|
saisieCible = saisieCible:sub(1, -2)
|
|
elseif action.valeur == "go" then
|
|
lancerCible(saisieCible)
|
|
saisieCible = ""
|
|
else
|
|
saisieCible = (saisieCible .. action.valeur):sub(1, 18)
|
|
end
|
|
end
|
|
end
|
|
end
|
|
if ev ~= "timer" or a ~= minuteur then os.cancelTimer(minuteur) end
|
|
end
|
|
end
|
|
|
|
local function tacheClavier()
|
|
while true do
|
|
local ligne = read()
|
|
local mots = {}
|
|
for mot in ligne:gmatch("%S+") do table.insert(mots, mot) end
|
|
if mots[1] == "auto" and tonumber(mots[2]) and tonumber(mots[3]) then
|
|
if tonumber(mots[4]) then
|
|
cibleNav = { x = tonumber(mots[2]), y = tonumber(mots[3]),
|
|
z = tonumber(mots[4]) }
|
|
else
|
|
cibleNav = { x = tonumber(mots[2]), z = tonumber(mots[3]) }
|
|
end
|
|
changerMode("auto", "clavier")
|
|
elseif mots[1] == "pt" then
|
|
commandePoint(mots)
|
|
elseif mots[1] == "rtb" then
|
|
if etat.decollage then
|
|
cibleNav = { x = etat.decollage.x, z = etat.decollage.z }
|
|
changerMode("auto", "retour decollage")
|
|
else
|
|
print("pas de point de decollage enregistre")
|
|
end
|
|
elseif mots[1] then
|
|
print("auto <x> <z> | auto <x> <y> <z> | pt ... | rtb")
|
|
end
|
|
end
|
|
end
|
|
|
|
journal.info(("drone: demarrage en mode %s (consigne %.0f)")
|
|
:format(mode, consigneY))
|
|
if mode == "vol" or mode == "drone" then
|
|
consigneY = materiel.lireAltitude()
|
|
end
|
|
parallel.waitForAny(tacheControle, tacheMoteur, tacheCarburant,
|
|
tachePosition, tacheTypewriter, tacheIhm, tacheClavier)
|