From f21bcb7b114030219b6ba6ddd467054424ee4e79 Mon Sep 17 00:00:00 2001 From: Sterister Date: Sat, 25 Jul 2026 18:13:40 +0200 Subject: [PATCH] Pitch-compensated lift feedforward: hold altitude under forward thrust Reported symptom: the ship sinks as soon as it moves forward. Confirmed in code -- altitude.lua had no pitch input at all. Pitch only fed the front/aft differential; nothing compensated the common-mode lift. So when the propeller pushes the nose up, the lift vector tilts with the hull and its vertical component drops ~cos(pitch), and only the slow altitude integrator (~5-10 s) fought it -> a multi-block sag every time you accelerate. - altitude.lua takes measured pitch and boosts common-mode lift by 1/cos(pitch), bounded by cfg.pitchLiftMax (default 1.5 = full comp to ~48 deg, 1.0 = off). Sign-agnostic (cos is even); feedforward adds no loop gain so it can't destabilize -- altitude feedback still owns steady-state. - FLY HUD shows the live boost (liftFF xN.NN); u_sum in fly.log reflects it. Physically motivated but unverified against the mod's exact lift model: if lift were world-vertical the ship would climb slightly on accel instead (feedback corrects it) -- still better than sinking. Tune pitchLiftMax from flight. Co-Authored-By: Claude Opus 4.8 (1M context) --- files/autopilot/autopilot/altitude.lua | 21 ++++++++++++++++++--- files/autopilot/autopilot/config.lua | 5 +++++ files/autopilot/autopilot/main.lua | 4 ++-- 3 files changed, 25 insertions(+), 5 deletions(-) diff --git a/files/autopilot/autopilot/altitude.lua b/files/autopilot/autopilot/altitude.lua index bd98d48..4c39065 100644 --- a/files/autopilot/autopilot/altitude.lua +++ b/files/autopilot/autopilot/altitude.lua @@ -22,8 +22,9 @@ function M.new(cfg, calib) end -- vMeas: MAALT vertikal fart (b/s). dt: faktisk steg (s). saturated: mixeren klemte en utgang --- (anti-windup -> frys integralet). Returnerer u_front_common, u_rear_common + diag. -function Alt:compute(altitude, target, vMeas, dt, saturated, lowAltTighten) +-- (anti-windup -> frys integralet). pitchDeg: MAALT pitch (grader) for loft-feedforward. +-- Returnerer u_front_common, u_rear_common + diag. +function Alt:compute(altitude, target, vMeas, dt, saturated, lowAltTighten, pitchDeg) local cfg, calib = self.cfg, self.calib local uHoverSum = calib.uHoverFront + calib.uHoverRear -- noeytral baseline; uTrim laerer resten @@ -46,10 +47,24 @@ function Alt:compute(altitude, target, vMeas, dt, saturated, lowAltTighten) end local u_sum = utils.clamp(uHoverSum + self.uTrim + cfg.Kp_climb * vErr, 2 * cfg.absFloor, 2 * cfg.maxSignal) + -- PITCH-KOMPENSERT LOFT (feedforward). Naar skroget pitcher (typisk fordi propellen dytter og + -- nesa loefter seg), tilter loftvektoren med skroget og den VERTIKALE komponenten faller ~cos(pitch) + -- -> skipet synker "saa fort du gir framdrift". Vi gjenoppretter den vertikale komponenten + -- UMIDDELBART (1/cos), bundet, i stedet for aa vente paa den trege loft-integratoren. Fortegns- + -- uavhengig (cos er jamn): nese opp ELLER ned taper vertikalt loft likt. Feedforward tilfoerer + -- ingen sloyfe-gain -> destabiliserer ikke; hoyde-feedbacken eier fortsatt steady-state. + local ffBoost = 1.0 + if pitchDeg then + local c = math.cos(math.rad(pitchDeg)) + if c > 0.2 then ffBoost = utils.clamp(1 / c, 1.0, cfg.pitchLiftMax or 1.0) end + end + u_sum = utils.clamp(u_sum * ffBoost, 2 * cfg.absFloor, 2 * cfg.maxSignal) + -- fordel paa segmentene etter trim-forholdet (pitch-loopen legger differansen oppaa) local u_front = u_sum * calib.uHoverFront / uHoverSum local u_rear = u_sum * calib.uHoverRear / uHoverSum - return u_front, u_rear, { v_des = v_des, vErr = vErr, uTrim = self.uTrim, u_sum = u_sum, hold = self.hold } + return u_front, u_rear, + { v_des = v_des, vErr = vErr, uTrim = self.uTrim, u_sum = u_sum, hold = self.hold, pitchFF = ffBoost } end return M diff --git a/files/autopilot/autopilot/config.lua b/files/autopilot/autopilot/config.lua index 21bcad7..ef6f1f4 100644 --- a/files/autopilot/autopilot/config.lua +++ b/files/autopilot/autopilot/config.lua @@ -46,6 +46,11 @@ return { vMaxDown = 0.8, -- b/s maks synkerate altDeadband = 3.0, -- blokker -- innenfor baandet: HOLD, ikke chase eksakt hoyde altBandHyst = 2.0, -- ekstra margin foer vi forlater baandet (hysterese mot grensesyklus) + -- Pitch-kompensert loft (feedforward, §altitude): naar skroget pitcher taper loftet sin vertikale + -- komponent (~cos(pitch)) og skipet synker naar du gir framdrift. Vi booster felles-loftet med + -- 1/cos(pitch), bundet av dette taket. 1.0 = AV. 1.5 = full komp opp til ~48 grader pitch. + -- Hev om skipet fremdeles synker under framdrift; senk om det STIGER naar du akselererer. + pitchLiftMax = 1.5, -- ============ HOYDE: G-FRI FART-PI (Lag 6, self-learning) ============ -- Erstatter den g-avhengige modell-inversjonen (svingte fordi g/hover var ukalibrert). uTrim er diff --git a/files/autopilot/autopilot/main.lua b/files/autopilot/autopilot/main.lua index 77ed106..eba74d1 100644 --- a/files/autopilot/autopilot/main.lua +++ b/files/autopilot/autopilot/main.lua @@ -785,7 +785,7 @@ local function runFly() local frontHeat, rearHeat, sat, diag, u_diff = nil, nil, true, {}, 0 if state == "FLYING" and valid then local lowAlt = (est.altitude - launchAlt) < cfg.lowAltCaution - local uFc, uRc, ad = actl:compute(est.altitude, target, est.verticalVelocity, dt, prevSat, lowAlt) + local uFc, uRc, ad = actl:compute(est.altitude, target, est.verticalVelocity, dt, prevSat, lowAlt, est.pitch) 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 @@ -832,7 +832,7 @@ local function runFly() print(string.format("Pitch %7.1f pRate %+.1f trim %+.2f", est.pitch, est.pitchRate, pctl.iTrim)) print(string.format("v_meas %6.2f v_hat %6.2f (e_v %+.2f)", est.verticalVelocity, obs.v_hat, e_v)) print(string.format("u_sum %.1f hTrim %+.2f vDes %+.2f", diag.u_sum or 0, actl.uTrim, diag.v_des or 0)) - print(string.format("fore %2d aft %2d", of, oa)) + print(string.format("fore %2d aft %2d liftFF x%.2f", of, oa, diag.pitchFF or 1.0)) -- kringkast status til pilot-konsollen link.publishStatus({