airship-autopilot/files/autopilot/autopilot/calibration.lua

-- calibration.lua (§6) -- SELV-LAERENDE sysID. Maaler alt skip-spesifikt og AVLEDER sin egen
-- regulator-gain fra den maalte planten -> universell uten hand-tuning.
--
-- Hvorfor avledet gain: en proporsjonal hastighetsregulator paa denne lav-drag/stor-etterslep
-- planten er stabil bare hvis Kp ~ lambda/g (skip-spesifikt). Fast Kp gir grensesyklus paa ett
-- skip og treghet paa et annet. Vi maaler lambda/g i trappas kryss-trinn og setter Kp = K*lambda/g.
--
-- Flyt: trappe-fra-bunn (finn hover + steg-respons -> lambda/g -> avled Kp) -> hover-hold ->
--       pitch-fortegn -> pitch-trim. Bundet proporsjonal styring + treg hoverEst-trim. Dither
--       i driveValves lar skipet hvile paa sann (desimal) hover.

local utils     = require("utils")
local Estimator = require("estimator")
local Logger    = require("logger")

local M = {}
local function now() return os.epoch("utc") / 1000 end

-- ---- trappe ----
local STAIR_STEP      = 0.5
local STAIR_DWELL     = 6.0    -- sek per UNDER-hover trinn (>= etterslep)
local STAIR_DETECT    = 0.30   -- b/s sustained = crossing
local STAIR_DET_HOLD  = 1.0
local STAIR_HARDV     = 1.5    -- b/s -> bryt uansett
local STAIR_SINK      = 0.40   -- b/s ned -> airborne & under hover: steg opp
local STAIR_SINK_HOLD = 0.6
local FIT_WINDOW      = 9.0    -- sek hold paa kryss-kommando for lambda/g-fit (steg-respons)

-- ---- avledet hastighetsregulator (universell: Kp = K*lambda/g) ----
local K_GAIN_VEL = 1.5
-- KP_MAX lavt nok til at kpVel*v holder seg innenfor VEL_BAND for v opp til ~7 b/s
-- -> alltid LINEAER (ingen bang-bang) -> grensesyklus strukturelt utelukket. Lavt = trygt.
local KP_MIN, KP_MAX = 0.05, 0.20
local Ki_ff      = 0.05        -- TREG hoverEst-trim (airborne hold)
local FF_BAND    = 1.5
local VEL_BAND   = 1.5         -- omslutter hover for enhver plant
local CLIMB_RATE = 0.4         -- lav -> lite momentum -> liten coast
local HOVER_VTOL = 0.40        -- loosere: nok til pitch-maaling, unngaa grensesyklus-jag
local HOVER_HOLD = 2.0
local HOLD_MAX_T = 90.0
local Kp_diff    = 0.20        -- pitch-nivellering (best-effort)
local PITCH_WARN = 3.0

-- ---- vakter ----
local V_RUNAWAY, RUNAWAY_HOLD, GROUND_DROP = 4.0, 3.0, 2.0

-- v(t) = v0 + a_inf*(t - (1-e^{-lam t})/lam) ; 2-param LSQ for hver lam, min SSE
local function fitFirstOrder(samples)
  if #samples < 8 then return nil, nil, false end
  local bestSSE, bestL, bestA = math.huge, nil, nil
  for gi = 0, 30 do
    local lam = 0.1 + (2.0 - 0.1) * gi / 30
    local s11, s1p, spp, s1v, spv = 0, 0, 0, 0, 0
    for _, s in ipairs(samples) do
      local t, v = s[1], s[2]
      local phi = t - (1 - math.exp(-lam * t)) / lam
      s11 = s11 + 1; s1p = s1p + phi; spp = spp + phi * phi
      s1v = s1v + v; spv = spv + phi * v
    end
    local det = s11 * spp - s1p * s1p
    if math.abs(det) > 1e-9 then
      local c0 = (spp * s1v - s1p * spv) / det
      local c1 = (s11 * spv - s1p * s1v) / det
      local sse = 0
      for _, s in ipairs(samples) do
        local t, v = s[1], s[2]
        local phi = t - (1 - math.exp(-lam * t)) / lam
        sse = sse + (v - (c0 + c1 * phi)) ^ 2
      end
      if sse < bestSSE then bestSSE, bestL, bestA = sse, lam, c1 end
    end
  end
  if not bestL then return nil, nil, false end
  return bestL, bestA, (bestA ~= nil and bestA > 0.02 and bestL > 0.11 and bestL < 1.99)
end

function M.run(cfg, sensors, drive)
  local log = Logger.new("cal.log", "# cal v3: phase, t, fore, aft, height, vVel, pitch, pitchRate")
  local est = Estimator.new(cfg)
  local s   = cfg.signtest
  local hoverGuess = (cfg.calib.uHoverFront + cfg.calib.uHoverRear) / 2

  local calib = {}
  for k, v in pairs(cfg.calib) do calib[k] = v end

  local startAlt = sensors.readAltitude()
  if not startAlt then return nil, "no altitude sensor" end

  local hoverEst = hoverGuess
  local ffBase   = hoverGuess
  local uDiff    = 0
  local kpVel    = 0.10          -- foreloepig; AVLEDES fra lambda/g etter trappa
  local runHold  = 0

  local function tick(fore, aft, phase)
    local f, a = drive(fore, aft)
    local h, p = sensors.readAltitude(), sensors.readPitch()
    if h and p then est:update(now(), h, p) end
    log:row({ phase, now(), f, a, est.altitude, est.verticalVelocity, est.pitch, est.pitchRate })
    sleep(cfg.dt)
    return est.altitude, est.verticalVelocity, est.pitch, est.pitchRate
  end

  local function hud(title, lines)
    term.clear(); term.setCursorPos(1, 1); print("==== CALIBRATION ===="); print(title)
    for _, l in ipairs(lines) do print(l) end
  end

  -- bundet proporsjonal hastighets-steg med AVLEDET kpVel (kan ikke runaway/grensesykle)
  local function velCtl(vDes, levelPitch, trimFF, tag)
    local v = est.verticalVelocity
    if trimFF then
      hoverEst = utils.clamp(hoverEst + Ki_ff * (vDes - v) * cfg.dt,
        math.max(cfg.absFloor, ffBase - FF_BAND), math.min(cfg.maxSignal - 0.5, ffBase + FF_BAND))
    end
    local uCommon = utils.clamp(hoverEst + utils.clamp(kpVel * (vDes - v), -VEL_BAND, VEL_BAND),
      cfg.absFloor, cfg.maxSignal)
    if levelPitch then
      uDiff = utils.clamp(uDiff - calib.pitchSign * Kp_diff * est.pitch * cfg.dt,
        -cfg.maxSignal / 2, cfg.maxSignal / 2)
    end
    return tick(uCommon + uDiff, uCommon - uDiff, tag)
  end

  local function guard(v)
    if est.altitude < startAlt - GROUND_DROP then return "sank below launch point (lift too low?)" end
    if math.abs(v) > V_RUNAWAY then runHold = runHold + cfg.dt else runHold = 0 end
    if runHold > RUNAWAY_HOLD then return "velocity runaway (unexpected)" end
    return nil
  end

  local function holdHover(levelP, tag, title)
    local t0, held = now(), 0
    while now() - t0 < HOLD_MAX_T do
      local _, v, p = velCtl(0, levelP, true, tag)
      local err = guard(v); if err then return false, err end
      if math.abs(v) < HOVER_VTOL then held = held + cfg.dt else held = 0 end
      hud(title, {
        string.format("hoverEst %.2f  uDiff %+.2f  kpVel %.3f", hoverEst, uDiff, kpVel),
        string.format("vVel %.2f  pitch %.2f", v, p),
        string.format("settle %.1f / %.1f s", held, HOVER_HOLD),
      })
      if held >= HOVER_HOLD then return true end
    end
    return false, "hover-hold did not settle in time"
  end

  -- ---------- STAGE 1: TRAPPE fra bunnen -> hover + lambda/g-fit (kryss-trinnet) ----------
  do
    local cmd = cfg.absFloor
    local crossing, fitSamples = nil, nil
    while cmd <= cfg.maxSignal + 1e-9 and not crossing do
      local tStep, det, sink, samples, crossed = now(), 0, 0, {}, false
      while true do
        local _, v = tick(cmd, cmd, "S1_stair")
        samples[#samples + 1] = { now() - tStep, v }
        if v > STAIR_DETECT then det = det + cfg.dt else det = 0 end
        if v < -STAIR_SINK then sink = sink + cfg.dt else sink = 0 end
        if not crossed and (det >= STAIR_DET_HOLD or v > STAIR_HARDV) then crossed = true end
        hud("Stage 1: staircase hover-find", {
          string.format("cmd %.2f  vVel %.2f", cmd, v),
          crossed and string.format("CROSSING -> fitting %.1f/%.1f s", now() - tStep, FIT_WINDOW)
            or string.format("rise-detect %.1f / %.1f s", det, STAIR_DET_HOLD),
          string.format("alt +%.1f", est.altitude - startAlt),
        })
        if crossed then
          if now() - tStep >= FIT_WINDOW then break end          -- nok steg-respons-data
        else
          if now() - tStep >= STAIR_DWELL or sink >= STAIR_SINK_HOLD then break end
        end
      end
      if crossed then crossing, fitSamples = cmd, samples else cmd = cmd + STAIR_STEP end
    end
    if not crossing then return nil, "could not lift at any command up to maxSignal (too heavy?)" end
    hoverEst = crossing - STAIR_STEP / 2
    ffBase = hoverEst

    -- lambda/g fra steg-responsen (excess = STAIR_STEP/2 over hoverEst)
    local lam, ainf, ok = fitFirstOrder(fitSamples)
    if ok then calib.lambda = lam else calib.lambda = 1.0 / cfg.responseDelayDefault end
    if ok and ainf > 0 then
      calib.g = ainf * hoverEst / (STAIR_STEP / 2)   -- g = a_inf/(L-1), L-1 = (step/2)/hoverEst
    end
    -- AVLED regulator-gain fra maalt plant (universell)
    kpVel = utils.clamp(K_GAIN_VEL * calib.lambda / math.max(0.1, calib.g), KP_MIN, KP_MAX)
    log:raw(string.format("# S1: crossing=%.2f hoverEst=%.2f lambda=%.3f g=%.3f -> kpVel=%.3f",
      crossing, hoverEst, calib.lambda, calib.g, kpVel))
  end

  -- ---------- STAGE 2: hover-hold (arrester klatringen, settle) ----------
  do
    local ok, err = holdHover(false, "S2_hold", "Stage 2: settling to hover")
    if not ok then return nil, err end
  end

  -- ---------- STAGE 3: pitch-fortegn + autoritet (rolig ved hover) ----------
  do
    local function avgPitch(fore, aft, secs, phase)
      local t0, sum, n = now(), 0, 0
      while now() - t0 < secs do local _, _, p = tick(fore, aft, phase); sum = sum + p; n = n + 1 end
      return (n > 0) and (sum / n) or 0
    end
    hud("Stage 3: measuring pitch sign", { "balanced..." })
    local pBefore = avgPitch(hoverEst, hoverEst, s.settleTime, "S3_pre")
    local prMax, t0 = 0, now()
    while now() - t0 < s.nudgeTime do
      local _, _, _, pr = tick(hoverEst + s.nudge, math.max(0, hoverEst - s.nudge), "S3_nudge")
      if math.abs(pr) > math.abs(prMax) then prMax = pr end
      hud("Stage 3: nudge (more front)", { string.format("pitchRate %.2f", pr) })
    end
    local pAfter = avgPitch(hoverEst + s.nudge, math.max(0, hoverEst - s.nudge), s.settleTime, "S3_post")
    local dp = pAfter - pBefore
    if math.abs(dp) < 1.0 then
      log:raw(string.format("# S3: WEAK dp=%.2f -> keeping pitchSign=%d", dp, calib.pitchSign))
    else
      calib.pitchSign  = utils.sign(dp)
      calib.pitchGain  = math.max(0.05, math.abs(dp) / (2 * s.nudge))
      calib.alphaPitch = calib.pitchGain
      log:raw(string.format("# S3: dp=%.2f -> pitchSign=%d pitchGain=%.3f", dp, calib.pitchSign, calib.pitchGain))
    end
  end

  -- ---------- STAGE 4: hover + pitch-trim (best-effort) ----------
  do
    local ok, err = holdHover(true, "S4_level", "Stage 4: hover + level (patient)")
    if not ok then return nil, err end
    calib.uHoverFront   = utils.clamp(hoverEst + uDiff, cfg.absFloor, cfg.maxSignal)
    calib.uHoverRear    = utils.clamp(hoverEst - uDiff, cfg.absFloor, cfg.maxSignal)
    calib.pitchResidual = est.pitch
    log:raw(string.format("# S4: uHoverFront=%.2f uHoverRear=%.2f residualPitch=%.2f",
      calib.uHoverFront, calib.uHoverRear, est.pitch))
    if math.abs(est.pitch) > PITCH_WARN then
      log:raw(string.format("# S4: WARNING limited pitch authority (residual %.1f deg)", est.pitch))
    end
  end

  drive(calib.uHoverFront, calib.uHoverRear)   -- hold hover ved utgang
  return calib
end

return M