airship-autopilot/files/autopilot/stabilize.lua

-- 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