diff --git a/README.md b/README.md index 0e86927..9a25363 100644 --- a/README.md +++ b/README.md @@ -29,10 +29,13 @@ side drives which balloon, pitch sign, hover level) is measured on the ship by the **setup wizard**, which the install flow takes you straight into: 1. **Wiring** — the wizard pulses each free side; you say which burner fired. -2. **Sign test** — the ship lifts off by itself, finds its own lift power, - pushes the front balloon, verifies the response in both directions - (auto-escalating the push if the response is weak), then descends and holds - hover. No config editing, no freefall. Takes 1–3 minutes. +2. **Sign test** — the ship lifts off by itself, finds its own lift power, then + *rocks* between front and aft pushes. The sign comes from + `rate(+push) − rate(−push)`, which cancels any constant lean exactly, so a + ship hanging crooked measures fine — that lean **is** the thing being + measured. It then learns the differential trim that levels the ship, saves + it as a warm-start, and descends to hover. No config editing, no freefall. + Takes 2–4 minutes. After that the ship is ready: `FLY` from the menu or `ENGAGE` from the cockpit console. Pitch and altitude keep self-learning in flight. The wizard can be diff --git a/files/autopilot/autopilot/config.lua b/files/autopilot/autopilot/config.lua index 20c6ace..21bcad7 100644 --- a/files/autopilot/autopilot/config.lua +++ b/files/autopilot/autopilot/config.lua @@ -124,6 +124,9 @@ return { nudgeMinHold = 6.0, -- s min under nudge (>= ~5 s loft-etterslep) foer godtatt nudgeMaxWait = 14.0, -- s timeout nudge drMin = 0.3, -- deg/s min rate-endring for aa godta/lagre fortegn - maxTiltAbort = 25.0, -- deg: stopp open-loop maaling om skipet roterer for langt + -- IKKE en abort: et skjevt skip er nettopp det testen finnes for. Naar tiltet passerer + -- dette OG maalevinduet er fullt, avslutter vi bare maalingen tidlig (vi har svaret -- + -- unoedig aa tippe videre). Balansefasen retter skjevheten etterpaa. + maxTiltAbort = 25.0, -- deg fra start-attityde: "godt nok maalt, slutt aa dytte" }, } diff --git a/files/autopilot/autopilot/main.lua b/files/autopilot/autopilot/main.lua index 706decf..22df7dd 100644 --- a/files/autopilot/autopilot/main.lua +++ b/files/autopilot/autopilot/main.lua @@ -145,14 +145,21 @@ local function runMonitor() end end --- ============================ SIGN TEST (§13.2, v2 -- SELVGAAENDE) ============================ --- Maaler pitch-responsen paa en differanse-nudge for aa finne pitchSign. Bruker pitch-RATEN, ikke --- vinkelen: open-loop under et vedvarende moment er skipet en naer-integrator -> VINKELEN settler --- aldri, men RATEN gjor det. Hele testen er selvgaaende -- INGEN config-redigering: --- 1/4 trapper seg luftbaaren fra bunnen (laerer ~hover paa kjoepet, ingen fast liftLevel) --- 2/4 klatrer kontrollert til trygg maalehoyde (fart-P-hold rundt laert hover) --- 3/4 baseline -> nudge front -> MOTSATT nudge (verifisering); svak respons -> auto sterkere dytt --- 4/4 lagrer, senker skipet kontrollert tilbake og HOLDER hover -- ALDRI frittfall +-- ============================ SIGN TEST (§13.2, v3 -- GYNGE-PROBE) ============================ +-- GRUNNPREMISS: et SKJEVT skip er nettopp skipet denne testen finnes for. Skjevhet med balansert +-- loft er MAALEDATA (det er ubalansen vi skal laere aa trimme bort), ikke en feiltilstand. Derfor +-- finnes ingen "for skjevt"-abort -- testen maaler seg gjennom skjevheten og retter den til slutt. +-- +-- Fortegnet trenger INGEN balansert baseline: sign = sign(rate(+dytt) - rate(-dytt)). Differansen +-- mellom de to dyttene kansellerer enhver konstant skjev-drift eksakt. Og fordi vi VEKSLER mellom +-- dyttene gynger skipet rundt utgangspunktet i stedet for aa akkumulere tilt -- selvbegrensende. +-- +-- 1/5 trapper seg luftbaaren fra bunnen (laerer ~hover paa kjoepet, ingen fast liftLevel) +-- 2/5 klatrer kontrollert til trygg maalehoyde (fart-P-hold rundt laert hover) +-- 3/5 GYNGE-PROBE: front-dytt <-> bak-dytt, 2 sykluser; aksen som svarer blir pitchIndex. +-- Svak respons -> auto sterkere dytt. Maaler ALLE gimbal-akser (byggeretning likegyldig) +-- 4/5 BALANSE: med fortegnet kjent laeres differanse-trimmen som holder skipet vannrett +-- 5/5 lagrer, senker skipet kontrollert tilbake og HOLDER hover -- ALDRI frittfall local signtestAirborne = false -- for runMode-cleanup: hold hover hvis luftbaaren, ellers alt av @@ -261,8 +268,9 @@ local function runSigntest() levelDiff = clamp(levelDiff - pSign * 0.2 * est.pitch * cfg.dt, -1.5, 1.5) end velTick(-0.4, levelDiff, "descend") - hud("4/4", "Coming back down...", { + hud("5/5", "Coming back down...", { string.format("descending to +3 hover ~%.1f", hover), + string.format("trim %+.2f holding it level", levelDiff), }) end local tS = now() @@ -282,9 +290,6 @@ local function runSigntest() end local function abortMsg(code) - 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 @@ -295,10 +300,9 @@ local function runSigntest() local cmd, stepT, riseHold, tLift = cfg.absFloor, now(), 0, now() while true do tick(cmd, cmd, "lift") - hud("1/4", "Lifting off (auto power search)...", { + hud("1/5", "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 @@ -323,21 +327,21 @@ local function runSigntest() 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") + velTick(dh > 0 and -0.4 or 0.4, levelDiff, "recenter") hud(step, "Flying to measuring height (" .. reason .. ")", { string.format("target +%d now %+.1f hover ~%.1f", BAND_TARGET, est.altitude - h0, hover), + string.format("tilt %+.0f deg (fine -- that's what we fix)", maxTilt()), }) - if maxTilt() > s.maxTiltAbort then return false, "tilt" end end return false, "time" end - local okC, cErr = recenter("2/4", "initial climb") + local okC, cErr = recenter("2/5", "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)...", { + hud("2/5", "Settling (learning true hover)...", { string.format("hover ~%.2f %.0fs left", hover, math.max(0, 8 - (now() - tSettle))), }) end @@ -369,12 +373,9 @@ local function runSigntest() string.format("diff %+.0f rates:%s", diff, rline), string.format("calm when spread < %.1f (now %.2f)", s.rateCalm, spanMax), }) - 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 + -- INGEN tilt-abort: skjevhet er maalingens formaal. Har raten allerede svart tydelig, + -- slutter vi likevel aa dytte tidlig -- vi har det vi kom for, unoedig aa tippe mer. + if maxTilt() > s.maxTiltAbort and #win >= 5 then return avg 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 @@ -386,65 +387,102 @@ local function runSigntest() -- 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) + local r, code = measureRate(diff, "3/5", 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") + local okR, rErr = recenter("3/5", "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. + -- ---- 3/5: GYNGE-PROBE -- front-dytt <-> bak-dytt; aksen som SVARER blir pitchIndex ---- + -- d = rate(+dytt) - rate(-dytt) trenger INGEN baseline: en konstant skjev-drift finnes i + -- begge leddene og kanselleres eksakt. To sykluser som er ENIGE om fortegnet = verifisert. local nudge = s.nudge local maxNudge = math.max(s.nudge, math.floor(cfg.maxSignal / 2) + 1) - local bestVer, bestAny = nil, nil + local pick = nil for attempt = 1, 3 do - local tag = string.format(" (try %d, push %d)", attempt, nudge) - local r0, e0 = measureSafe(0, "Baseline (balanced)" .. tag, s.baseMinHold, s.baseMaxWait) - if not r0 then return bail(abortMsg(e0)) end - 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 = measureSafe(-nudge, "Verify: more AFT" .. tag, s.nudgeMinHold, s.nudgeMaxWait) - if not r2 then return bail(abortMsg(e2)) end + local ds = {} + for c = 1, 2 do + local tag = string.format(" (cycle %d/2, push %d)", c, nudge) + local rF, eF = measureSafe(nudge, "Rocking: push FRONT" .. tag, s.nudgeMinHold, s.nudgeMaxWait) + if not rF then return bail(abortMsg(eF)) end + local rA, eA = measureSafe(-nudge, "Rocking: push AFT" .. tag, s.nudgeMinHold, s.nudgeMaxWait) + if not rA then return bail(abortMsg(eA)) end + local d = {} + for i = 1, NAX do d[i] = rF[i] - rA[i] end + ds[c] = d + end + -- aksen med sterkest ENIG respons vinner (begge sykluser samme retning) 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 + local d1, d2 = ds[1][i], ds[2][i] + local sg = utils.sign(d1) + local mag = (math.abs(d1) + math.abs(d2)) / 2 + if sg ~= 0 and utils.sign(d2) == sg and mag >= s.drMin then + if not pick or mag > pick.mag then pick = { axis = i, sign = sg, mag = mag } end end end - if bestVer then break end + if pick then break end nudge = math.min(maxNudge, nudge + 1) -- auto: proev sterkere dytt end - 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.") + 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 spaced along the hull.") end - local sign, axis, verified = pick.sign, pick.axis, (bestVer ~= nil) + local sign, axis = pick.sign, pick.axis + cfg.pitchIndex = axis -- gjeld umiddelbart (estimator, balansefase, nedstigning) + + -- ---- 4/5: BALANSE -- laer differanse-trimmen som holder DETTE skipet vannrett ---- + -- Dette er hele poenget: skipet henger skjevt, og vi maaler nettopp trimmen som retter det. + -- Integralet laerer den varige trimmen; P/D bare demper transienten og forsvinner ved likevekt. + -- Konvergerer den ikke: vi lagrer likevel beste trim + advarer (begrenset pitch-autoritet). + local Kp_lvl, Kd_lvl, Ki_lvl = 0.15, 0.08, 0.03 + local trimMax = cfg.maxSignal / 2 + local levelTrim, calm, tBal = 0, 0, now() + local pitchNow, rateNow = 0, 0 + while now() - tBal < 90 and now() < deadline do + pitchNow = (lastAx and lastAx[axis]) or 0 + rateNow = axRings[axis]:slope() + levelTrim = clamp(levelTrim + Ki_lvl * sign * (-pitchNow) * cfg.dt, -trimMax, trimMax) + levelDiff = clamp(levelTrim + sign * (-Kp_lvl * pitchNow - Kd_lvl * rateNow), -trimMax, trimMax) + velTick(0, levelDiff, "balance") + hud("4/5", "Learning the balance trim...", { + string.format("tilt %+.1f deg rate %+.2f", pitchNow, rateNow), + string.format("trim %+.2f level for %.1f/4.0 s", levelTrim, calm), + }) + if math.abs(pitchNow) < 2.0 and math.abs(rateNow) < 0.3 then calm = calm + cfg.dt else calm = 0 end + if calm >= 4.0 then break end + local rel = est.altitude - h0 + if rel < BAND_LO or rel > BAND_HI then + local okR, rErr = recenter("4/5", "altitude drift") + if not okR then return bail(abortMsg(rErr)) end + end + end + local balanced = calm >= 4.0 + levelDiff = levelTrim -- nedstigningen fortsetter fra den laerte trimmen -- ---- 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.pitchTrim = 2 * levelTrim -- diff -> D_des-enheter (mixer: front = u + D/2) 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 nymaalt akse + fortegn) ---- + -- ---- 5/5: kontrollert ned + hold hover (nivellert med den nylaerte trimmen) ---- descendAndHold(sign) term.clear(); term.setCursorPos(1, 1) print("==== SIGN TEST: SUCCESS ====") 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("balance trim %+.2f (saved warm-start)", 2 * levelTrim)) + if balanced then + print("Ship levelled out under this trim.") + else + print(string.format("NOTE: still %+.0f deg off level -- limited", pitchNow)) + print("pitch authority. FLY keeps learning it.") + end print(string.format("hover ~%.1f (saved as baseline)", hover)) print("") print("Ship is holding hover. Ready to FLY.")