Stian/airship-autopilot
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
71e750bbe04419e03b2c64303afe8eca431f6827
airship-autopilot/files/autopilot/stabilize.lua
Stian 4cd2573f56 Airship autopilot: role-based wget installer for server 
Mirrors the live CC autopilot (computer 0) and cockpit (computer 4) from the
Modda wii world. install.lua fetches each role's manifest over HTTP so the
system installs on any server regardless of computer id.

Co-Authored-By: Claude Opus 4.8 (1M context) <noreply@anthropic.com>
2026-07-23 12:53:41 +02:00

572 lines
24 KiB
Lua
Raw Blame History

-- stabilize.lua -- GENERELT selvkalibrerende hoyde+pitch PID for ballong-luftskip.
--
-- Ingen ballong-spesifikke tall hardkodet:
-- * svevegass laeres av hoyde-integralet
-- * trim-skjevhet (volum/CoM) laeres av pitch-integralet
-- * pitch-fortegn auto-detekteres ved oppstart
-- Det eneste skip-spesifikke er hvilken redstone-side hver ventil sitter paa.
--
-- Sensorer (auto-funnet paa type): altitude_sensor (getHeight), gimbal_sensor (getAngles)
-- Aktuator: redstone analog 0..MAX_SIGNAL -> ventil (mer = mer gass = mer loft)
--
-- BRUK:
-- stabilize cal -> logger sensoravlesninger til cal.log (styrer ingenting)
-- stabilize test -> kjente ventil-monstre -> test.log
-- stabilize -> kjorer regulatoren -> run.log
-- ============================ KONFIG ============================
-- Skip-spesifikt: hvilke redstone-sider mater ventilene ("left"/"right"/"front"/"back")
local FORE_SIDE = "right"
local AFT_SIDE = "back" -- nil hvis bare EN ventil (da deaktiveres pitch-styring)
local TARGET_PITCH = 0 -- onsket pitch (vannrett)
local TAKEOFF_OFFSET = 5 -- mal settes saa mange blokker over startposisjon
-- Start-gains (dempet; auto-tuneren justerer og lagrer dem videre)
local A_KP, A_KI, A_KD = 0.22, 0.04, 2.2 -- hoyde
local P_KP, P_KI, P_KD = 0.22, 0.05, 0.70 -- pitch
local AUTOTUNE = true -- maaler egen svingning og justerer gains
local TUNE_INTERVAL = 12 -- sekunder mellom hver justering (KI laerer trim-skjevheten)
local MAX_SIGNAL = 7 -- maks redstone. Hold i responsivt omraade (loftet ditt topper ~5).
local MIN_SIGNAL = 1 -- gulv paa TOTAL-loft (collective) -> mykt fall, aldri fritt fall.
local LAG = 2.0 -- sek: tregheten i loft-respons. Regulatoren forutser saa langt fram.
local SLEW = 2 -- maks signal-endring per tick (demper voldsomme hopp)
local HOVER_SEED = 4.5 -- start-gjetning paa svevegass (naer ekte hover; kartet finjusterer)
-- Pitch-fortegn: 0 = auto-detekter ved oppstart, ellers +1/-1 manuelt
local PITCH_SIGN = 0
local PITCH_INDEX = 1 -- hvilket tall fra getAngles er pitch
-- Rock-solid hold (Tier 1) + place-and-forget (Tier 2)
local DEADBAND_H = 1.5 -- blokker: innenfor dette holdes hoyden rolig (ingen twitch)
local DEADBAND_P = 2.0 -- grader: innenfor dette holdes pitch rolig
local DERIV_FILTER = 0.3 -- lavpass paa avledning (0..1; lavere = mykere)
local SETTLE_TIME = 3.0 -- sek innenfor dodband for "STABIL"
-- KASKADE-regulering for hoyde (hindrer fartsoppbygging -> oversving)
local MAX_RATE = 2.0 -- blokker/sek: maks klatre-/synkerate
local K_POS = 0.4 -- posisjonsfeil -> onsket rate (5 blokker feil -> maks rate)
local K_RATE = 1.2 -- rate-feil -> gass (auto-tunes)
-- Grasios bevegelse + pitch-kaskade + robusthet
local MAX_ACCEL = 1.0 -- blokker/sek^2: myk ease-in/ut paa rate-endring
local MAX_PITCH_RATE = 4.0 -- grader/sek: maks pitch-endringsrate (grasios attityde)
local K_POS_P = 0.5 -- pitch-feil -> onsket pitch-rate
local K_PRATE = 0.08 -- pitch-rate-feil -> differanse (auto-tunes), mildt mot lag
local MAX_DIFF = 4.0 -- maks differanse: pitch kan ALDRI pumpe all gass til en ende
local GLITCH_H = 25 -- avvis hoyde-hopp storre enn dette per tick (sensor-glitch)
local DT = 0.2
-- ===============================================================
-- ---- selvlaert modell (Fase C/D): persisteres i profilen ----
local BIN_SIZE = 10 -- blokker per kart-bin
local hoverMap = {} -- hoyde-bin -> svevegass (laert kurve)
local kCouple = 0 -- collective->pitch koblingskoeffisient (laert)
local TRIM = 0 -- statisk balanse-differanse (maalt i balance-modus)
local altS = peripheral.find("altitude_sensor")
local gimS = peripheral.find("gimbal_sensor")
-- hoverFF: interpoler laert svevegass for en hoyde (fallback = HOVER_SEED)
local function hoverFF(h)
local b = h / BIN_SIZE
local lo, hi = math.floor(b), math.ceil(b)
local vlo, vhi = hoverMap[lo], hoverMap[hi]
if vlo and vhi then
if lo == hi then return vlo end
return vlo + (vhi - vlo) * (b - lo)
elseif vlo then return vlo
elseif vhi then return vhi end
local best, bestd
for k, v in pairs(hoverMap) do
local d = math.abs(k - b)
if not bestd or d < bestd then bestd, best = d, v end
end
return best or HOVER_SEED
end
-- learnHover: oppdater kart-bin med glidende snitt naar skipet er stabilt
local function learnHover(h, c)
local b = math.floor(h / BIN_SIZE + 0.5)
hoverMap[b] = hoverMap[b] and (hoverMap[b]*0.9 + c*0.1) or c
end
local function mapCount()
local n = 0; for _ in pairs(hoverMap) do n = n + 1 end; return n
end
local function logline(path, text)
local f = fs.open(path, "a"); if f then f.writeLine(text); f.close() end
end
local function angles()
local r = { gimS.getAngles() }
if type(r[1]) == "table" then r = r[1] end
return r
end
local function pitchRaw() return angles()[PITCH_INDEX] or 0 end
local function clampSig(x)
x = math.floor(x + 0.5)
return math.max(0, math.min(MAX_SIGNAL, x))
end
-- slew-begrenset utgang (mot forrige verdi)
local curFore, curAft = 0, 0
local function driveValves(fore, aft)
fore = clampSig(fore); aft = clampSig(aft)
if fore - curFore > SLEW then fore = curFore + SLEW end
if fore - curFore < -SLEW then fore = curFore - SLEW end
if aft - curAft > SLEW then aft = curAft + SLEW end
if aft - curAft < -SLEW then aft = curAft - SLEW end
curFore, curAft = fore, aft
redstone.setAnalogOutput(FORE_SIDE, fore)
if AFT_SIDE then redstone.setAnalogOutput(AFT_SIDE, aft) end
return fore, aft
end
local function allOff()
redstone.setAnalogOutput(FORE_SIDE, 0)
if AFT_SIDE then redstone.setAnalogOutput(AFT_SIDE, 0) end
curFore, curAft = 0, 0
end
-- ---- persistens: lagrede gains overlever omstart ----
local GAINS_FILE = "pid_gains.txt"
-- Lagrer gains (respons), arbeidspunkt OG laert modell (kart/kobling/lag).
local function saveState(altI, pitchI)
local f = fs.open(GAINS_FILE, "w")
f.write(textutils.serialise({A_KP=A_KP, A_KI=A_KI, A_KD=A_KD, P_KP=P_KP, P_KI=P_KI, P_KD=P_KD,
K_RATE=K_RATE, K_PRATE=K_PRATE, altI=altI, pitchI=pitchI, hoverMap=hoverMap, kCouple=kCouple, lag=LAG, TRIM=TRIM}))
f.close()
end
local function loadState()
if not fs.exists(GAINS_FILE) then return nil end
local f = fs.open(GAINS_FILE, "r"); local s = f.readAll(); f.close()
local g = textutils.unserialise(s)
if type(g) ~= "table" then return nil end
A_KP, A_KI, A_KD = g.A_KP or A_KP, g.A_KI or A_KI, g.A_KD or A_KD
P_KP, P_KI, P_KD = g.P_KP or P_KP, g.P_KI or P_KI, g.P_KD or P_KD
-- sikkerhet: kapp lagrede gains saa en korrupt profil ikke kan laste inn vanvittige verdier
A_KP, A_KD = math.min(A_KP, 0.6), math.min(A_KD, 3.0)
P_KP, P_KD = math.min(P_KP, 0.5), math.min(P_KD, 2.0)
K_RATE = math.min(g.K_RATE or K_RATE, 4.0)
K_PRATE = math.min(g.K_PRATE or K_PRATE, 1.0)
if type(g.hoverMap) == "table" then
for k, v in pairs(g.hoverMap) do hoverMap[tonumber(k) or k] = v end
end
kCouple = g.kCouple or kCouple
LAG = g.lag or LAG
TRIM = g.TRIM or TRIM
return g
end
-- ---- malhoyde: egen fil saa den overlever reload (place-and-forget) ----
local TARGET_FILE = "target.txt"
local function saveTarget(h)
local f = fs.open(TARGET_FILE, "w"); f.write(tostring(h)); f.close()
end
local function loadTarget()
if not fs.exists(TARGET_FILE) then return nil end
local f = fs.open(TARGET_FILE, "r"); local s = f.readAll(); f.close()
return tonumber(s)
end
-- ---- analyse av et feilvindu: amplitude (topp-til-topp) + null-kryssinger ----
local function analyze(win)
local mn, mx, cross = math.huge, -math.huge, 0
for i, v in ipairs(win) do
if v < mn then mn = v end
if v > mx then mx = v end
if i > 1 and ((win[i-1] < 0) ~= (v < 0)) then cross = cross + 1 end
end
return (mx - mn), cross
end
-- ============================ MENY (UI) ============================
local function pause() print(""); print("[ Enter for menu ]"); read() end
local function menu()
while true do
term.clear(); term.setCursorPos(1,1)
print("====== BALLOON AUTOPILOT ======")
print("")
print(" 1) Start stabilization")
print(" 2) Set target altitude")
print(" 3) Balance check (level the ship)")
print(" 4) Calibrate sensors")
print(" 5) Actuator test")
print(" 6) Reset profile (delete learned data)")
print(" 7) Show saved profile")
print(" 8) Exit")
print("")
local sensorsOk = peripheral.find("altitude_sensor") and peripheral.find("gimbal_sensor")
print(sensorsOk and "Sensors: OK" or "Sensors: MISSING - check wiring")
print(fs.exists(GAINS_FILE) and "Profile: saved" or "Profile: none (fresh)")
local tH = loadTarget()
print("Target: " .. (tH and string.format("%.1f", tH) or "where I start"))
print("")
write("Choice: ")
local k = read()
if k == "1" then return "run"
elseif k == "2" then
write("Target altitude (empty = where I start): ")
local s = read()
if s == "" then
if fs.exists(TARGET_FILE) then fs.delete(TARGET_FILE) end
print("Target cleared - holds where it starts.")
elseif tonumber(s) then
saveTarget(tonumber(s)); print("Target altitude set to " .. s)
else print("Invalid number.") end
pause()
elseif k == "3" then return "balance"
elseif k == "4" then return "cal"
elseif k == "5" then return "test"
elseif k == "6" then
if fs.exists(GAINS_FILE) then fs.delete(GAINS_FILE) end
print("Profile reset - fresh calibration next start."); pause()
elseif k == "7" then
if fs.exists(GAINS_FILE) then
local f = fs.open(GAINS_FILE, "r"); print(f.readAll()); f.close()
else print("No saved profile yet.") end
pause()
elseif k == "8" then return "exit"
end
end
end
-- ============================ AUTO-SIGN ============================
-- Maaler hvilken vei pitch beveger seg av positiv differanse + dodtid (lag).
-- Krever at skipet kan tilte (helst i lufta). Faller tilbake til +1 hvis ingen respons.
local function detectPitchSign()
if not AFT_SIDE then return 1 end
print("Auto-sign: measuring lag, lifting, nudging...")
local h0 = altS.getHeight()
local stepLvl = math.min(MAX_SIGNAL, 5)
local lvl = stepLvl
local t0 = os.clock()
local lagMeasured = nil
while os.clock() - t0 < 12 do
driveValves(stepLvl, stepLvl)
local rise = altS.getHeight() - h0
if rise > 1 and not lagMeasured then lagMeasured = os.clock() - t0 end
if rise > 2 then break end -- luftbaaren
sleep(0.2)
end
if lagMeasured then
LAG = math.max(0.5, math.min(6, lagMeasured))
print(string.format("Measured lag = %.1f s", LAG))
end
sleep(0.5)
local p0 = pitchRaw()
local t1 = os.clock()
while os.clock() - t1 < 3 do
driveValves(math.min(MAX_SIGNAL, lvl + 3), math.max(0, lvl - 3))
sleep(0.2)
end
local p1 = pitchRaw()
allOff()
local dp = p1 - p0
if math.abs(dp) < 1.0 then
print("Weak response -- using +1. Override PITCH_SIGN if wrong.")
return 1
end
local s = dp >= 0 and 1 or -1 -- sign(dp): gir negativ tilbakekobling
print(string.format("dpitch %.1f -> PITCH_SIGN = %d", dp, s))
sleep(1)
return s
end
-- modus fra argument, ellers vis meny
local mode = ({...})[1]
if not mode then mode = menu() end
if mode == "exit" then return end
-- alle modus trenger sensorene
if not (altS and gimS) then
print("Sensors missing (altitude_sensor + gimbal_sensor). Check wiring.")
return
end
-- ============================ AUTO-BALANCE (BRUTE-FORCE SWEEP) ============================
-- Sveiper TRIM gjennom hele omraadet, maaler pitch ved hver verdi, velger den flateste.
-- Trenger INGEN fortegn-deteksjon -> enkelt og robust. Lagres som permanent TRIM.
if mode == "balance" then
local g0 = loadState() -- behold eksisterende laerte data
local ground = altS.getHeight()
local target = ground + 10
local AIRBORNE = ground + 4 -- under dette = staar paa bakken (ugyldig)
local SWEEP_MAX, STEP = 6, 0.5
local MAXWAIT, WIN, SETTLE = 18, 14, 1.2
-- 1) LOFT skipet til luftbaaren (begge ventiler likt) FOR vi maaler noe
print("AUTO-BALANCE - lifting ship first...")
local liftOk = false
local tLift = os.clock()
while os.clock() - tLift < 18 do
local h = altS.getHeight()
local g = clampSig(HOVER_SEED + 0.7*(target - h))
driveValves(g, g)
if h > ground + 6 then liftOk = true; break end
term.clear(); term.setCursorPos(1,1)
print("AUTO-BALANCE")
print(string.format("lifting... %.1f / %.0f", h, target))
sleep(0.2)
end
if not liftOk then
allOff(); print("Could not get airborne - balloons too weak?"); return
end
-- 2) SVEIP trim, men KUN gyldig naar skipet faktisk svever (h >= AIRBORNE)
local best, bestAbs = nil, math.huge
local ok, err = pcall(function()
local t = -SWEEP_MAX
while t <= SWEEP_MAX + 0.001 do
local hist, tStart, settledP = {}, os.clock(), nil
while os.clock() - tStart < MAXWAIT do
local h = altS.getHeight()
local coll = clampSig(HOVER_SEED + 0.7*(target - h)) -- hold hoyden
local fore, aft = driveValves(coll + t, coll - t)
local p = pitchRaw()
hist[#hist+1] = p; if #hist > WIN then table.remove(hist, 1) end
local mn, mx = math.huge, -math.huge
for _, v in ipairs(hist) do mn = math.min(mn, v); mx = math.max(mx, v) end
local spread = (#hist >= WIN) and (mx - mn) or 999
if spread < SETTLE and h >= AIRBORNE then settledP = (mn + mx)/2 end -- gyldig KUN luftbaaren
term.clear(); term.setCursorPos(1,1)
print("==== AUTO-BALANCE SWEEP ====")
print(string.format(" TRIM %+.1f pitch %+.1f", t, p))
print(string.format(" fore %d/aft %d height %.1f", fore, aft, h))
print(" " .. (h < AIRBORNE and "SINKING - invalid trim" or (settledP and "settled" or "settling...")))
print(string.format(" best: %s", best and string.format("TRIM %+.1f (|p| %.1f)", best, bestAbs) or "none yet"))
if settledP then break end
sleep(0.2)
end
-- bare gyldig hvis den satte seg MENS luftbaaren
if settledP and math.abs(settledP) < bestAbs then
best, bestAbs = t, math.abs(settledP)
TRIM = best; saveState(g0 and g0.altI or 0, g0 and g0.pitchI or 0) -- lagre lopende
end
t = t + STEP
end
end)
allOff()
if best then
TRIM = best; saveState(g0 and g0.altI or 0, g0 and g0.pitchI or 0)
print(string.format("Best TRIM = %+.2f (|pitch| %.1f). Saved.", best, bestAbs))
else
print("No valid (airborne) trim found.")
end
if not ok and err ~= "Terminated" then error(err, 0) end
return
end
-- ============================ CAL ============================
if mode == "cal" then
local LOG = "cal.log"; if fs.exists(LOG) then fs.delete(LOG) end
logline(LOG, "# cal: t, height, airpressure, a1, a2, a3")
print("CALIBRATION -> cal.log (controls nothing). CTRL+T to stop.")
while true do
local h = altS.getHeight()
local press = altS.getAirPressure and altS.getAirPressure() or 0
local a = angles()
logline(LOG, string.format("%.2f, %.2f, %.2f, %.3f, %.3f, %.3f",
os.clock(), h, press or 0, a[1] or 0, a[2] or 0, a[3] or 0))
term.clear(); term.setCursorPos(1,1)
print(string.format("Height %.1f Pressure %.2f", h, press or 0))
print(string.format("Angles %.2f / %.2f / %.2f", a[1] or 0, a[2] or 0, a[3] or 0))
sleep(0.3)
end
end
-- ============================ TEST ============================
if mode == "test" then
local LOG = "test.log"; if fs.exists(LOG) then fs.delete(LOG) end
logline(LOG, "# test: t, phase, fore, aft, height, pitch")
local steps = {
{"balanced", 6, 6, 8}, {"more_FRONT", 10, 3, 8},
{"balanced", 6, 6, 6}, {"more_AFT", 3, 10, 8}, {"off", 0, 0, 4},
}
print("TEST MODE -> test.log. CTRL+T to stop.")
local ok, err = pcall(function()
for _, s in ipairs(steps) do
local phase, fore, aft, secs = s[1], s[2], s[3], s[4]
local t0 = os.clock()
while os.clock() - t0 < secs do
driveValves(fore, aft)
local h, p = altS.getHeight(), pitchRaw()
logline(LOG, string.format("%.2f, %s, %d, %d, %.2f, %.2f", os.clock(), phase, fore, aft, h, p))
term.clear(); term.setCursorPos(1,1)
print("Phase: "..phase); print(string.format("fore %d aft %d", fore, aft))
print(string.format("Height %.1f Pitch %.1f", h, p))
sleep(0.2)
end
end
end)
allOff(); print("Valves 0. Test done -> test.log.")
if not ok and err ~= "Terminated" then error(err, 0) end
return
end
-- ============================ REGULERING ============================
local loaded = loadState()
if loaded then print(string.format("Loaded profile (%d map pts, lag %.1f, kC %.2f)",
mapCount(), LAG, kCouple)) end
local sign = PITCH_SIGN
if sign == 0 then sign = detectPitchSign() end -- kan over-skrive lag med fersk maling
-- Malhoyde: lagret (place-and-forget) ELLER der skipet starter
local TARGET_FINAL = loadTarget() or (altS.getHeight() + TAKEOFF_OFFSET)
print(string.format("Target = %.1f (sign %d)", TARGET_FINAL, sign))
local LOG = "run.log"; if fs.exists(LOG) then fs.delete(LOG) end
logline(LOG, string.format("# run: target_h=%.1f sign=%d A(%.2f,%.3f,%.2f) P(%.2f,%.3f,%.2f) max=%d",
TARGET_FINAL, sign, A_KP, A_KI, A_KD, P_KP, P_KI, P_KD, MAX_SIGNAL))
logline(LOG, "# t, height, pitch, eH, eP, collective, diff, fore, aft")
-- Integralet trimmer nå bare RESTEN (hoverFF gir grunngassen) -> start nær 0
local altI = (loaded and loaded.altI) or 0
local pitchI = (loaded and loaded.pitchI) or 0
local lastH = altS.getHeight()
local lastP = pitchRaw()
local fVspd, fPr = 0, 0 -- filtrerte avledninger (lavpass)
local cmdRate = 0 -- akselerasjons-begrenset onsket rate (grasios bevegelse)
local badCount = 0 -- online fortegns-vakt
local winH, winP = {}, {} -- feilvinduer for auto-tuning
local lastTune = os.clock()
local tuneMsg = "waiting..."
local settledFor = 0 -- sek innenfor dodband
print("Self-calibrating PID active -> run.log. CTRL+T to stop.")
local ok, err = pcall(function()
while true do
-- glitch-vern: avvis urealistiske sensor-hopp (robusthet)
local rawH = altS.getHeight()
local height = (lastH and math.abs(rawH - lastH) > GLITCH_H) and lastH or rawH
local pitch = pitchRaw()
-- HOYDE KASKADE: posisjon -> begrenset+myk onsket rate -> gass
local vspd = (height - lastH) / DT; lastH = height
fVspd = DERIV_FILTER*vspd + (1-DERIV_FILTER)*fVspd -- lavpass paa stigerate
local eH = TARGET_FINAL - height
local inH = math.abs(eH) < DEADBAND_H
local ff = hoverFF(TARGET_FINAL) -- laert svevegass for malet
local desRate = math.max(-MAX_RATE, math.min(MAX_RATE, K_POS*eH)) -- ytre: begrenset rate
-- akselerasjons-grense: ramp cmdRate mot desRate -> myk ease-in/ease-out
local dr = math.max(-MAX_ACCEL*DT, math.min(MAX_ACCEL*DT, desRate - cmdRate))
cmdRate = cmdRate + dr
local rateErr = cmdRate - fVspd -- indre: hold raten
local rawColl = ff + A_KI*altI + K_RATE*rateErr
local collective = math.max(0, math.min(MAX_SIGNAL, rawColl)) -- KLEM til ventilomraade
local excess = collective - ff -- klatrekraft (for avkobling)
-- integral paa RATE-feil (IKKE posisjon!) -> trimmer FF-gass, kan ikke vinde opp paa stor eH
local satLow, satHigh = rawColl <= 0, rawColl >= MAX_SIGNAL
if (not satLow and not satHigh) or (satLow and rateErr > 0) or (satHigh and rateErr < 0) then
altI = math.max(-150, math.min(150, altI + rateErr*DT))
end
-- PITCH KASKADE: pitch-feil -> begrenset onsket pitch-rate -> differanse (grasios attityde)
local diff, eP = 0, 0
local inP = true
if AFT_SIDE then
local pr = (pitch - lastP) / DT; lastP = pitch
fPr = DERIV_FILTER*pr + (1-DERIV_FILTER)*fPr
eP = TARGET_PITCH - pitch
inP = math.abs(eP) < DEADBAND_P
local desPRate = math.max(-MAX_PITCH_RATE, math.min(MAX_PITCH_RATE, K_POS_P*eP))
local pRateErr = desPRate - fPr
-- avkobling (kCouple*excess) motvirker dreiemoment fra gass-endring + laert trim
-- dynamisk pitch-korreksjon klemmes (mot bang-bang), saa legges statisk TRIM oppaa
local dyn = sign * (K_PRATE*pRateErr + P_KI*pitchI + kCouple*excess)
dyn = math.max(-MAX_DIFF, math.min(MAX_DIFF, dyn))
diff = TRIM + dyn -- per-ventil clampSig i driveValves binder sluttverdien
-- laer decoupling: RIKTIG fortegn (gradient) -> kansellerer klatre-pitchen
if math.abs(excess) > 0.5 then
kCouple = math.max(-2, math.min(2, kCouple + 0.002 * eP * excess))
end
local fSat = (collective+diff) <= 0 or (collective+diff) >= MAX_SIGNAL
local aSat = (collective-diff) <= 0 or (collective-diff) >= MAX_SIGNAL
if not inP and not (fSat or aSat) then pitchI = math.max(-200, math.min(200, pitchI + sign*eP*DT)) end
if math.abs(pitch) > 25 then badCount = badCount + 1 else badCount = 0 end
if badCount >= 20 then
sign = -sign; pitchI = 0; badCount = 0
print("Pitch diverging -> flipping sign to " .. sign)
end
end
-- settle-deteksjon: innenfor dodband paa begge akser + naa malet
local atTarget = math.abs(TARGET_FINAL - height) < DEADBAND_H
if inH and inP and atTarget then settledFor = settledFor + DT else settledFor = 0 end
local settled = settledFor >= SETTLE_TIME
-- LAER svevegass-kart naar stabilt -> kartet overtar, integralet lekkes mot 0
if inH and inP and math.abs(fVspd) < 0.4 then
learnHover(height, collective)
altI = altI * 0.97
end
-- gulv paa TOTAL-loft (ikke per ventil): kan synke til mal + beholder pitch-differanse
collective = math.max(MIN_SIGNAL, collective)
local fore, aft = driveValves(collective + diff, collective - diff)
-- ---- AUTO-TUNING: KUN naar nær malet (hold) -> reise-transienter lurer den ikke ----
local tuneNear = math.abs(TARGET_FINAL - height) < 5
and (not AFT_SIDE or math.abs(TARGET_PITCH - pitch) < 8)
if AUTOTUNE and not tuneNear then
winH, winP = {}, {}; lastTune = os.clock(); tuneMsg = "traveling (no tune)"
elseif AUTOTUNE then
winH[#winH+1] = eH
winP[#winP+1] = (TARGET_PITCH - pitch)
if os.clock() - lastTune >= TUNE_INTERVAL then
local ampH, crH = analyze(winH)
if crH >= 4 and ampH > 4 then -- oscillating -> damp inner rate gain
K_RATE = math.max(0.2, K_RATE*0.82)
tuneMsg = "alt: damping"
elseif ampH > 5 and crH < 2 then -- genuinely sluggish near target -> mild sharpen
K_RATE = math.min(4.0, K_RATE*1.08)
tuneMsg = "alt: sharpening"
else tuneMsg = "alt: ok" end
if AFT_SIDE then
local ampP, crP = analyze(winP)
if crP >= 4 and ampP > 6 then
K_PRATE = math.max(0.03, K_PRATE*0.82)
tuneMsg = tuneMsg .. " | pitch: damping"
elseif ampP > 7 and crP < 2 then
K_PRATE = math.min(0.15, K_PRATE*1.05) -- pitch: hold mildt, svingning er verre enn treghet
tuneMsg = tuneMsg .. " | pitch: sharpening"
else tuneMsg = tuneMsg .. " | pitch: ok" end
end
saveState(altI, pitchI)
logline(LOG, string.format("# TUNE %s -> A(%.3f,%.3f,%.2f) P(%.3f,%.3f,%.2f)",
tuneMsg, A_KP, A_KI, A_KD, P_KP, P_KI, P_KD))
winH, winP = {}, {}
lastTune = os.clock()
end
end
logline(LOG, string.format("%.2f, %.2f, %.2f, %.2f, %.2f, %.2f, %.2f, %d, %d",
os.clock(), height, pitch, eH, TARGET_PITCH - pitch, collective, diff, fore, aft))
term.clear(); term.setCursorPos(1,1)
local maxed = collective >= MAX_SIGNAL-0.5 and math.abs(eH) > 5 and math.abs(fVspd) < 0.3
print(settled and "*** STABLE ***"
or (maxed and "!! MAX LIFT - target unreachable")
or (math.abs(eH) > DEADBAND_H and ">> moving..." or ".. holding .."))
print(string.format("Height %6.1f / %.0f (e %.1f)", height, TARGET_FINAL, TARGET_FINAL-height))
print(string.format("Pitch %6.1f / %.0f (e %.1f)", pitch, TARGET_PITCH, TARGET_PITCH-pitch))
print(string.format("fore %2d aft %2d %s", fore, aft, (inH and inP) and "[hold]" or ""))
print(string.format("model: ff~%.1f kR %.2f kPR %.2f kC %.2f", ff, K_RATE, K_PRATE, kCouple))
print(string.format("rate %.1f/%.1f | map %d | trim~%.1f", fVspd, cmdRate, mapCount(), sign*P_KI*pitchI))
print("tune: " .. tuneMsg)
sleep(DT)
end
end)
allOff()
saveState(altI, pitchI) -- lagre nyeste arbeidspunkt + gains
print("Valves set to 0. Operating point saved. Exited.")
if not ok and err ~= "Terminated" then error(err, 0) end
Reference in New Issue View Git Blame Copy Permalink