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