Stian/airship-autopilot
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Sign test: measure ALL gimbal axes + self-healing altitude supervisor; link diagnostics

Browse Source 
- Sign test measures rate response on every gimbal axis and picks the one
  that actually answers the differential push -> pitchIndex is now MEASURED
  per ship (build orientation no longer matters). Verified both-direction
  response accepted at half threshold. Tilt guard watches all axes.
- Altitude supervisor: drifting out of the measuring band re-centers and
  retries instead of aborting; single 7-min deadline. Faster hover trim
  (0.05 -> 0.12, matches verified Ki_climb) plus an 8 s settle phase.
- Single-balloon ships: sign test marks itself done, FLY zeroes u_diff.
- Cockpit terminal now explains link status (OFFLINE checklist); autopilot
  standby screen warns loudly when no modem is attached.
- Failure text points at cross-wired links (both burners firing on one pulse).

Co-Authored-By: Claude Fable 5 <noreply@anthropic.com>
This commit is contained in:
Sterister
2026-07-23 23:45:11 +02:00
parent 54963e5b42
commit d012246d0c
3 changed files with 215 additions and 55 deletions
Download Patch File Download Diff File Expand all files Collapse all files

232
files/autopilot/autopilot/main.lua
View File

@@ -162,15 +162,73 @@ local function runSigntest()
signtestAirborne = false
local h0 = sensors.readAltitude()
if not h0 then print("No altitude sensor."); return false end
-- ett-ballong-skip: ingen differensiell pitch-autoritet -> testen er meningsloes
if not cfg.aftSide then
print("Single-balloon ship (no AFT side wired).")
print("Pitch can't be steered differentially, so")
print("the sign test does not apply. Marking done.")
local c = loadCalib(); c.pitchSign = 1; c.signMeasured = true; saveCalib(c)
print(""); print("[Enter]"); read()
return true
end
local t0 = now()
local hover = nil -- laeres i trappa (1/4), trimmes videre av fart-holdet
local levelDiff = 0 -- treg nivellering under nedstigning (naar fortegnet er kjent)
-- MULTI-AKSE maaling: vi VET ikke hvilken gimbal-vinkel som er pitch paa DETTE skipet
-- (avhenger av byggeretning!). Derfor maales rate-responsen paa ALLE aksene, og aksen
-- som faktisk svarer paa dyttet blir pitchIndex. Orientering blir dermed likegyldig.
local NAX = 0 -- antall gimbal-akser (settes ved foerste lesing, maks 3)
local axRings = {} -- per-akse (t, vinkel)-ring -> rate via least-squares slope
local a0, lastAx = nil, nil -- vinkler ved start (tilt-referanse) + siste lesing
local function readAx()
local a = sensors.readAngles()
if type(a) ~= "table" or type(a[1]) ~= "number" then return nil end
if NAX == 0 then
NAX = math.min(3, #a)
a0 = {}
for i = 1, NAX do axRings[i] = utils.newRing(1.0); a0[i] = a[i] end
end
return a
end
local function axRates()
local r = {}
for i = 1, NAX do r[i] = axRings[i]:slope() end
return r
end
-- stoerste vinkelavvik fra start-attityden paa NOEN akse (fortegns-/akse-uavhengig tilt-vakt)
local function maxTilt()
if not (lastAx and a0) then return 0 end
local m = 0
for i = 1, NAX do
local v = lastAx[i]
if type(v) == "number" then
local d = math.abs(v - a0[i])
if d > m then m = d end
end
end
return m
end
local function tick(fore, aft, phase)
local f, a = driveValves(fore, aft)
local h, p = sensors.readAltitude(), sensors.readPitch()
if h and p then est:update(now(), h, p) end
log:row({ now() - t0, phase, f, a, est.altitude, est.pitch, est.verticalVelocity, est.pitchRate })
local f, oa = driveValves(fore, aft)
local h, t = sensors.readAltitude(), now()
local ax = readAx()
if ax then
lastAx = ax
for i = 1, NAX do
local v = ax[i]
if type(v) == "number" and v == v then axRings[i]:push(t, v) end
end
end
local p = ax and ax[cfg.pitchIndex or 1] or nil
if h and p then est:update(t, h, p) end
log:row({ t - t0, phase, f, oa, est.altitude, est.pitch, est.verticalVelocity, est.pitchRate })
sleep(cfg.dt)
end
@@ -185,9 +243,11 @@ local function runSigntest()
print("CTRL+T = abort (holds lift, no freefall)")
end
-- fart-P-hold rundt laert hover (+ treg hover-trim), med valgfri differanse (fore - aft)
-- fart-P-hold rundt laert hover (+ hover-trim), med valgfri differanse (fore - aft).
-- Trim-rate 0.12 = samme verdi som Ki_climb (verifisert stabil for skipets ~5 s lag);
-- 0.05 var for tregt -- skipet rakk aa drive ut av maalebaandet foer trimmen tok igjen.
local function velTick(vDes, diff, phase)
hover = clamp(hover + 0.05 * (vDes - est.verticalVelocity) * cfg.dt, cfg.absFloor, cfg.maxSignal - 0.5)
hover = clamp(hover + 0.12 * (vDes - est.verticalVelocity) * cfg.dt, cfg.absFloor, cfg.maxSignal - 0.5)
local u = hover + clamp(1.2 * (vDes - est.verticalVelocity), -1.5, 1.5)
local f, a = mixer.preserveDifferenceClamp(u + diff, u - diff, cfg.absFloor, cfg.maxSignal)
tick(f, a, phase)
@@ -222,9 +282,12 @@ local function runSigntest()
end
local function abortMsg(code)
if code == "tilt" then return string.format("Tilted past %.0f deg -- aborted for safety.", s.maxTiltAbort) end
if code == "alt" then return "Drifted too far up -- aborted." end
if code == "low" then return "Got too close to the ground -- aborted." end
if code == "tilt" then
return string.format("Tilted past %.0f deg with BALANCED lift --\nship is out of balance. Aborted.", s.maxTiltAbort)
end
if code == "time" then
return "Ran out of time -- could not hold a\nsteady measuring height. Check burner\nfuel / ship weight and try again."
end
return "Aborted."
end
@@ -235,6 +298,7 @@ local function runSigntest()
hud("1/4", "Lifting off (auto power search)...", {
string.format("power %.1f / %d", cmd, cfg.maxSignal),
})
if maxTilt() > s.maxTiltAbort then return bail(abortMsg("tilt")) end
if est.verticalVelocity > 0.25 then riseHold = riseHold + cfg.dt else riseHold = 0 end
if riseHold >= 1.0 and est.altitude - h0 > 1.5 then break end
if now() - stepT >= 4.0 then
@@ -248,81 +312,139 @@ local function runSigntest()
hover = math.max(cfg.absFloor, cmd - 0.25) -- kryssingen er ~her: foerste hover-estimat
signtestAirborne = true
-- ---- 2/4: kontrollert klatring til trygg maalehoyde (hover-trimmen konvergerer underveis) ----
local tC = now()
while est.altitude < h0 + 8 do
velTick(0.4, 0, "climb")
hud("2/4", "Climbing to measuring height...", {
string.format("target +8 hover ~%.1f", hover),
})
if math.abs(est.pitch) > s.maxTiltAbort then return bail(abortMsg("tilt")) end
if now() - tC > 60 then return bail("Could not reach measuring height.") end
-- ---- 2/4: klatre til maalehoyde og SETTLE (hover-trimmen laerer ekte hover) ----
-- SUPERVISOR-prinsipp: hoyden overvaakes gjennom hele maalefasen. Driver skipet ut av
-- baandet -> fly tilbake til maalehoyden og proev maalingen igjen (selv-reparerende).
-- Bare EN total-deadline kan felle testen -- ikke haartrigger-aborter paa drift.
local BAND_LO, BAND_HI, BAND_TARGET = 4, 25, 12
local deadline = now() + 420 -- hele testen ferdig innen 7 min, ellers ryddig bail
local function recenter(step, reason)
while now() < deadline do
local dh = est.altitude - (h0 + BAND_TARGET)
if math.abs(dh) < 1.5 then return true end
velTick(dh > 0 and -0.4 or 0.4, 0, "recenter")
hud(step, "Flying to measuring height (" .. reason .. ")", {
string.format("target +%d now %+.1f hover ~%.1f", BAND_TARGET, est.altitude - h0, hover),
})
if maxTilt() > s.maxTiltAbort then return false, "tilt" end
end
return false, "time"
end
-- rate-stabil maaling: hold hoyden (vDes=0) med gitt differanse til pitch-raten roer seg.
-- Lag-bevisst: minHold >= loft-etterslepet foer vi godtar. Timeout -> bruk snittet likevel.
local okC, cErr = recenter("2/4", "initial climb")
if not okC then return bail(abortMsg(cErr)) end
local tSettle = now()
while now() - tSettle < 8 do
velTick(0, 0, "settle0")
hud("2/4", "Settling (learning true hover)...", {
string.format("hover ~%.2f %.0fs left", hover, math.max(0, 8 - (now() - tSettle))),
})
end
-- rate-stabil maaling PER AKSE: hold hoyden (vDes=0) med gitt differanse til ratene roer
-- seg. Returnerer tabell {avgRate per akse}. Lag-bevisst: minHold >= loft-etterslepet.
local function measureRate(diff, step, title, minHold, maxWait)
local tS, win = now(), {}
local avg = 0
local avg = nil
while now() - tS < maxWait do
velTick(0, diff, title)
win[#win + 1] = { t = now(), pr = est.pitchRate }
win[#win + 1] = { t = now(), r = axRates() }
while #win > 1 and (now() - win[1].t) > 1.5 do table.remove(win, 1) end
local sum, lo, hi = 0, math.huge, -math.huge
for _, e in ipairs(win) do sum = sum + e.pr; lo = math.min(lo, e.pr); hi = math.max(hi, e.pr) end
avg = sum / #win
local span = (#win > 1) and (hi - lo) or math.huge
avg = {}
local spanMax = 0
for i = 1, NAX do
local sum, lo, hi = 0, math.huge, -math.huge
for _, e in ipairs(win) do
local v = e.r[i]
sum = sum + v; lo = math.min(lo, v); hi = math.max(hi, v)
end
avg[i] = sum / #win
local sp = (#win > 1) and (hi - lo) or 99
if sp > spanMax then spanMax = sp end
end
local rline = ""
for i = 1, NAX do rline = rline .. string.format(" a%d %+.2f", i, avg[i]) end
hud(step, title, {
string.format("diff %+.0f rate avg %+.2f span %.2f", diff, avg, span),
string.format("stable when span < %.1f deg/s", s.rateCalm),
string.format("diff %+.0f rates:%s", diff, rline),
string.format("calm when spread < %.1f (now %.2f)", s.rateCalm, spanMax),
})
if math.abs(est.pitch) > s.maxTiltAbort then return nil, "tilt" end
if est.altitude - h0 > 35 then return nil, "alt" end
if est.altitude - h0 < 2 then return nil, "low" end
if now() - tS >= minHold and #win >= 5 and span < s.rateCalm then return avg end
if maxTilt() > s.maxTiltAbort then
-- kraftig tilt UNDER EN NUDGE er i seg selv et sterkt, entydig signal: bruk
-- maalingen og slutt aa dytte. Tilt med balansert loft = ubalansert skip -> abort.
if diff ~= 0 then return avg end
return nil, "tilt"
end
local rel = est.altitude - h0
if rel < BAND_LO or rel > BAND_HI then return nil, "band" end
if now() > deadline then return nil, "time" end
if now() - tS >= minHold and #win >= 5 and spanMax < s.rateCalm then return avg end
end
return avg -- timeout: retningen er som regel riktig selv om roen aldri kom
end
-- ---- 3/4: baseline -> nudge -> MOTSATT nudge, med automatisk opptrapping ved svak respons ----
local nudge, sign, verified = s.nudge, 0, false
-- selv-reparerende maaling: hoyde-drift -> re-sentrer og proev IGJEN i stedet for abort
local function measureSafe(diff, title, minHold, maxWait)
while true do
local r, code = measureRate(diff, "3/4", title, minHold, maxWait)
if r then return r end
if code ~= "band" then return nil, code end
local okR, rErr = recenter("3/4", "altitude drift")
if not okR then return nil, rErr end
end
end
-- ---- 3/4: baseline -> nudge -> MOTSATT nudge; aksen som SVARER blir pitchIndex ----
-- Verifisert respons (motsatt dytt gir motsatt rate) godtas ved HALV terskel: to
-- uavhengige, konsistente maalinger er sterkere bevis enn en stor enkeltmaaling.
local nudge = s.nudge
local maxNudge = math.max(s.nudge, math.floor(cfg.maxSignal / 2) + 1)
local bestVer, bestAny = nil, nil
for attempt = 1, 3 do
local tag = string.format(" (try %d, push %d)", attempt, nudge)
local r0, e0 = measureRate(0, "3/4", "Baseline (balanced)" .. tag, s.baseMinHold, s.baseMaxWait)
local r0, e0 = measureSafe(0, "Baseline (balanced)" .. tag, s.baseMinHold, s.baseMaxWait)
if not r0 then return bail(abortMsg(e0)) end
local r1, e1 = measureRate(nudge, "3/4", "Push: more FRONT" .. tag, s.nudgeMinHold, s.nudgeMaxWait)
local r1, e1 = measureSafe(nudge, "Push: more FRONT" .. tag, s.nudgeMinHold, s.nudgeMaxWait)
if not r1 then return bail(abortMsg(e1)) end
local r2, e2 = measureRate(-nudge, "3/4", "Verify: more AFT" .. tag, s.nudgeMinHold, s.nudgeMaxWait)
local r2, e2 = measureSafe(-nudge, "Verify: more AFT" .. tag, s.nudgeMinHold, s.nudgeMaxWait)
if not r2 then return bail(abortMsg(e2)) end
local dF, dR = r1 - r0, r2 - r0 -- rate-endring ved mer front / mer bak
if math.abs(dF) >= s.drMin and utils.sign(dR) == -utils.sign(dF) and math.abs(dR) >= s.drMin * 0.5 then
sign, verified = utils.sign(dF), true
break
for i = 1, NAX do
local dF, dR = r1[i] - r0[i], r2[i] - r0[i] -- rate-endring: mer front / mer bak
local cand = { axis = i, sign = utils.sign(dF), mag = math.abs(dF) }
local consistent = utils.sign(dR) == -utils.sign(dF) and math.abs(dR) >= s.drMin * 0.25
if consistent and cand.mag >= s.drMin * 0.5 then
if not bestVer or cand.mag > bestVer.mag then bestVer = cand end
elseif cand.mag >= s.drMin then
if not bestAny or cand.mag > bestAny.mag then bestAny = cand end
end
end
if math.abs(dF) >= s.drMin then sign = utils.sign(dF) end -- kandidat, men uverifisert
nudge = math.min(maxNudge, nudge + 1) -- auto: proev sterkere dytt
if bestVer then break end
nudge = math.min(maxNudge, nudge + 1) -- auto: proev sterkere dytt
end
if sign == 0 then
return bail("No measurable pitch response, even with\na stronger push. Are both balloons on\nthe same fore/aft line? Re-check wiring.")
local pick = bestVer or bestAny
if not pick then
return bail("No pitch response on ANY gimbal axis,\neven with a stronger push. Likely causes:\n- links cross-wired: did BOTH burners fire\n on one pulse in Wiring? Re-run Wiring.\n- balloons not on the fore/aft line.")
end
local sign, axis, verified = pick.sign, pick.axis, (bestVer ~= nil)
-- ---- lagre FOER nedstigningen (Ctrl+T etterpaa mister ingenting) ----
local c = loadCalib()
c.pitchSign, c.pitchGain, c.alphaPitch = sign, 1.0, 1.0
c.pitchIndex = axis -- MAALT: aksen som faktisk svarte paa dyttet
cfg.pitchIndex = axis -- gjeld umiddelbart (estimator + nedstigning)
c.pitchTrim = 0 -- nullstill laert trim ved ny fortegns-maaling
c.uHoverFront, c.uHoverRear = hover, hover -- grov men EKTE hover-maaling ('cal' kan forfine)
c.hoverTrim = 0 -- ny baseline -> gammel loft-trim er ugyldig
c.signMeasured = true -- profilen er komplett (wizard/standby sjekker denne)
saveCalib(c)
-- ---- 4/4: kontrollert ned + hold hover (nivellerer med det nymaalte fortegnet) ----
-- ---- 4/4: kontrollert ned + hold hover (nivellerer med nymaalt akse + fortegn) ----
descendAndHold(sign)
term.clear(); term.setCursorPos(1, 1)
print("==== SIGN TEST: SUCCESS ====")
print(string.format("pitchSign %+d %s", sign,
verified and "(verified both directions)" or "(UNVERIFIED -- reverse push unclear)"))
print(string.format("pitch = gimbal axis a%d, sign %+d", axis, sign))
print(verified and "(verified in both directions)" or "(UNVERIFIED -- reverse push unclear)")
print(string.format("hover ~%.1f (saved as baseline)", hover))
print("")
print("Ship is holding hover. Ready to FLY.")
@@ -565,6 +687,7 @@ local function runFly()
local lowAlt = (est.altitude - launchAlt) < cfg.lowAltCaution
local uFc, uRc, ad = actl:compute(est.altitude, target, est.verticalVelocity, dt, prevSat, lowAlt)
u_diff = pctl:compute(est.pitch, est.pitchRate, dt, prevSat)
if not cfg.aftSide then u_diff = 0 end -- ett-ballong-skip: ingen differensiell autoritet
local floor
frontHeat, rearHeat, floor = mixer.mix(cfg, calib, uFc, uRc, u_diff)
sat = (frontHeat <= floor + 0.01 or frontHeat >= cfg.maxSignal - 0.01
@@ -693,8 +816,9 @@ local function awaitCmd(secs)
end
local function runStandby()
link.open()
while true do
-- re-proev modem hver runde: fanger et modem som plugges til mens vi staar i standby
local hasModem = link.open()
local h = sensors.readAltitude() or 0
local p = sensors.readPitch() or 0
local flyable = fs.exists(CALIB_FILE) -- kan fly (evt. eldre profil uten maalt-flagg)
@@ -707,6 +831,9 @@ local function runStandby()
if complete then print("Profile: READY")
elseif flyable then print("Profile: LEGACY ([S] = re-run setup)")
else print("Profile: NOT SET UP -> press [S]") end
if hasModem then print("Link: modem OK (broadcasting to cockpit)")
else print("Link: NO MODEM -- cockpit shows OFFLINE!")
print(" attach a modem to this PC") end
print(string.format("Height %.1f Pitch %+.1f", h, p))
print("Waiting for ENGAGE from the pilot console...")
print("[S] setup wizard CTRL+T then 'ap' = menu")
@@ -758,8 +885,8 @@ local function showProfile()
print("==== PROFILE ====")
print(string.format("uHoverFront %.2f uHoverRear %.2f", c.uHoverFront, c.uHoverRear))
print(string.format("lambda %.3f g %.3f", c.lambda, c.g))
print(string.format("pitchSign %d (%s) pitchGain %.3f", c.pitchSign,
c.signMeasured and "measured" or "DEFAULT -- run setup", c.pitchGain))
print(string.format("pitchSign %d (%s) axis a%d", c.pitchSign,
c.signMeasured and "measured" or "DEFAULT -- run setup", c.pitchIndex or cfg.pitchIndex))
print(string.format("wiring: fore=%s aft=%s%s", cfg.foreSide, tostring(cfg.aftSide),
fs.exists(SIDES_FILE) and "" or " (config default)"))
print("calib.txt: " .. (fs.exists(CALIB_FILE) and "saved" or "none (defaults)"))
@@ -820,6 +947,9 @@ local args = { ... }
-- bruk persistert wiring (front/aft-sider) hvis satt -- overstyrer config-defaults per skip
local sv = loadSides()
if sv then cfg.foreSide = sv.fore; cfg.aftSide = sv.aft end
-- bruk MAALT gimbal-akse for pitch hvis sign-testen har funnet den (bygg-orientering varierer)
local cal0 = loadCalib()
if cal0.pitchIndex then cfg.pitchIndex = cal0.pitchIndex end
-- bruk persisterte live-innstillinger (klatre-fart / hold-baand / maks-effekt)
local sset = loadSettings()
if sset then