-------------------------------------------------------------------- -- drone.lua : controleur central du quadricoptere Create Aeronautics -- -- Usage : -- drone lance le controleur -- drone maj installe / met a jour depuis un depot -- -- Modes : off, inactif, stationnement, vol, drone, atterrissage, -- auto, position -- Clavier du PC : auto | position -- pt save | pt go | pt del | pt list -- rtb (retour au point de decollage) -------------------------------------------------------------------- local ARGS = { ... } local BASE = fs.getDir(shell.getRunningProgram()) -------------------------------------------------------------------- -- INSTALLEUR / MISE A JOUR INTEGRE -------------------------------------------------------------------- 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 ", 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 '", 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) -- 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 local pilotage = charger("lib/pilotage.lua") .nouveau(conf, etat, materiel, Pid, journal) 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 boostMoteurFin = 0 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 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 mode = nouveau pilotage.raz() navigation.razCap() 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 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" 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 == "capteurs" or quoi == "pid" 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) 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, } 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 | pt go | pt del | pt list") end end -------------------------------------------------------------------- -- TACHES PARALLELES -------------------------------------------------------------------- local function tacheControle() while true do local ok, err = pcall(pasControle) if not ok then journal.erreur("controle: " .. tostring(err)) pilotage.arreter() end sleep(DT) end end -- moteur asservi au stressometer; surcharge: reduction propulseurs local function tacheMoteur() local niveau = 8 local enSurcharge = false while true do if mode == "off" then materiel.reglerMoteur(0) elseif os.clock() < boostMoteurFin then materiel.reglerMoteur(15) niveau = 15 else stress = materiel.lireStress() if stress then if stress > conf.MOTEUR.STRESS_HAUT and niveau < 15 then niveau = niveau + 1 elseif stress < conf.MOTEUR.STRESS_BAS and niveau > 1 then niveau = niveau - 1 end -- surcharge malgre moteur au maximum: delester les propulseurs if stress > conf.MOTEUR.SURCHARGE and niveau >= 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 stress < conf.MOTEUR.STRESS_HAUT then enSurcharge = false pilotage.reglerFacteurProp(1.0) journal.info("surcharge resorbee") end else niveau = 15 end materiel.reglerMoteur(niveau) 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 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, } 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 == "pave" then if action.valeur == "eff" then saisieCible = saisieCible:sub(1, -2) elseif action.valeur == "go" then lancerCible(saisieCible) saisieCible = "" else saisieCible = (saisieCible .. action.valeur):sub(1, 18) end end end end if ev ~= "timer" or a ~= minuteur then os.cancelTimer(minuteur) end end end local function tacheClavier() while true do local ligne = read() local mots = {} for mot in ligne:gmatch("%S+") do table.insert(mots, mot) end if mots[1] == "auto" and tonumber(mots[2]) and tonumber(mots[3]) then if tonumber(mots[4]) then cibleNav = { x = tonumber(mots[2]), y = tonumber(mots[3]), z = tonumber(mots[4]) } else cibleNav = { x = tonumber(mots[2]), z = tonumber(mots[3]) } end changerMode("auto", "clavier") elseif mots[1] == "pt" then commandePoint(mots) elseif mots[1] == "rtb" then if etat.decollage then cibleNav = { x = etat.decollage.x, z = etat.decollage.z } changerMode("auto", "retour decollage") else print("pas de point de decollage enregistre") end elseif mots[1] then print("auto | auto | pt ... | rtb") 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)