From b073746fc89e4e80b56dd5b28fc8041741f9432a Mon Sep 17 00:00:00 2001 From: Grzegorz Sterniczuk Date: Sat, 19 Sep 2026 20:57:23 +0200 Subject: [PATCH 01/33] feat(chirp): share the trust band and let the estimator pool several windows --- src/core/PSestimateFreqResponse.m | 86 ++++++++++++++++------------- src/core/PStrustBand.m | 35 ++++++++++++ src/plot/PSplotBode.m | 16 +----- tests/test_PSestimateMultiWindow.m | 87 ++++++++++++++++++++++++++++++ tests/test_PStrustBand.m | 58 ++++++++++++++++++++ 5 files changed, 230 insertions(+), 52 deletions(-) create mode 100644 src/core/PStrustBand.m create mode 100644 tests/test_PSestimateMultiWindow.m create mode 100644 tests/test_PStrustBand.m diff --git a/src/core/PSestimateFreqResponse.m b/src/core/PSestimateFreqResponse.m index 15322ac..ec27c74 100644 --- a/src/core/PSestimateFreqResponse.m +++ b/src/core/PSestimateFreqResponse.m @@ -1,8 +1,9 @@ -function [G, C, freq] = PSestimateFreqResponse(inp, out, Fs, Nest, Noverlap) +function [G, C, freq, nSeg] = PSestimateFreqResponse(inp, out, Fs, Nest, Noverlap) %% PSestimateFreqResponse - Welch cross-spectral frequency response estimation % Based on pichim/bf_controller_tuning estimate_frequency_response.m -% inp - input signal (excitation-correlated, column vector) -% out - output signal (response, column vector) +% inp - input signal (excitation-correlated, column vector), or a cell +% array of several windows of the same system +% out - output signal (response), matching inp in shape and count % Fs - sample rate (Hz) % Nest - FFT segment length (default: round(2.5*Fs)) % Noverlap - segment overlap (default: round(0.9*Nest)) @@ -11,62 +12,73 @@ % G - complex frequency response (one-sided, Nhalf x 1) % C - coherence 0..1 (Nhalf x 1) % freq - frequency vector in Hz (Nhalf x 1) +% nSeg - how many segments went into the average +% +% Several windows pool their Suu/Syu/Syy accumulators before G and C are +% formed, which weights each window by how hard it was driven. Averaging the +% finished G estimates instead would give a barely excited window the same say +% as a well excited one. if nargin < 4 || isempty(Nest), Nest = round(2.5 * Fs); end if nargin < 5 || isempty(Noverlap), Noverlap = round(0.9 * Nest); end -inp = inp(:); -out = out(:); -N = min(length(inp), length(out)); -inp = inp(1:N) - mean(inp(1:N)); -out = out(1:N) - mean(out(1:N)); - -w = hann(Nest, 'periodic'); -Nstep = Nest - Noverlap; -Nseg = floor((N - Nest) / Nstep) + 1; - -if Nseg < 1 - Nhalf = floor(Nest/2) + 1; - G = zeros(Nhalf, 1); - C = zeros(Nhalf, 1); - freq = (0:Nhalf-1)' * Fs / Nest; - return +if ~iscell(inp), inp = {inp}; end +if ~iscell(out), out = {out}; end +if numel(inp) ~= numel(out) + error('PSestimateFreqResponse: %d input windows against %d output windows', ... + numel(inp), numel(out)); end Nhalf = floor(Nest/2) + 1; +freq = (0:Nhalf-1)' * Fs / Nest; + +w = hann(Nest, 'periodic'); +Nstep = Nest - Noverlap; W = sum(w) / Nest / 2; Suu = zeros(Nhalf, 1); Syu = zeros(Nhalf, 1); Syy = zeros(Nhalf, 1); +nSeg = 0; -for s = 1:Nseg - i0 = (s-1) * Nstep + 1; - idx = i0 : (i0 + Nest - 1); +for k = 1:numel(inp) + u = inp{k}(:); + y = out{k}(:); + N = min(length(u), length(y)); + if N < Nest, continue; end + u = u(1:N) - mean(u(1:N)); + y = y(1:N) - mean(y(1:N)); - u_seg = inp(idx) .* w; - y_seg = out(idx) .* w; + for s = 1:(floor((N - Nest) / Nstep) + 1) + i0 = (s-1) * Nstep + 1; + idx = i0 : (i0 + Nest - 1); - U = fft(u_seg, Nest); U = U(1:Nhalf) / (Nest * W); - Y = fft(y_seg, Nest); Y = Y(1:Nhalf) / (Nest * W); + U = fft(u(idx) .* w, Nest); U = U(1:Nhalf) / (Nest * W); + Y = fft(y(idx) .* w, Nest); Y = Y(1:Nhalf) / (Nest * W); - % DC and Nyquist: undo single-sided doubling - U(1) = U(1) / 2; U(end) = U(end) / 2; - Y(1) = Y(1) / 2; Y(end) = Y(end) / 2; + % DC and Nyquist: undo single-sided doubling + U(1) = U(1) / 2; U(end) = U(end) / 2; + Y(1) = Y(1) / 2; Y(end) = Y(end) / 2; - Suu = Suu + U .* conj(U); - Syu = Syu + Y .* conj(U); - Syy = Syy + Y .* conj(Y); + Suu = Suu + U .* conj(U); + Syu = Syu + Y .* conj(U); + Syy = Syy + Y .* conj(Y); + nSeg = nSeg + 1; + end end -Suu = Suu / Nseg; -Syu = Syu / Nseg; -Syy = Syy / Nseg; +if nSeg < 1 + G = zeros(Nhalf, 1); + C = zeros(Nhalf, 1); + return +end + +Suu = Suu / nSeg; +Syu = Syu / nSeg; +Syy = Syy / nSeg; delta = max(Suu) * 1e-12; G = Syu ./ (Suu + delta); C = abs(Syu).^2 ./ (Suu .* Syy + delta); -freq = (0:Nhalf-1)' * Fs / Nest; - end diff --git a/src/core/PStrustBand.m b/src/core/PStrustBand.m new file mode 100644 index 0000000..4d1dda7 --- /dev/null +++ b/src/core/PStrustBand.m @@ -0,0 +1,35 @@ +function fTrust = PStrustBand(freq, C, cohMin, runLen) +%% PStrustBand - top of the frequency band a chirp measurement can be trusted to +% freq - frequency vector (Hz) +% C - coherence 0..1 on that grid +% cohMin - coherence below this counts as lost (default 0.8, the line the +% Bode plot already draws) +% runLen - how many bins in a row must be lost before the band ends (default 5) +% +% This bounds where a prediction may be evaluated. It is never a filter cutoff: +% the chirp excitation falls about 20 dB above 30 Hz, so coherence drops for +% reasons that have nothing to do with gyro noise. Reading a noise floor out of +% it is what cost Betaflight issue #5258. + +if nargin < 3 || isempty(cohMin), cohMin = 0.8; end +if nargin < 4 || isempty(runLen), runLen = 5; end + +freq = freq(:); +C = C(:); + +% Below 2 Hz a 2.5 s Welch segment holds too few cycles for coherence to mean +% anything, and the sweep itself starts at 0.2 Hz. +low = freq > 2 & C < cohMin; + +iBad = []; +if numel(low) >= runLen + iBad = find(conv(double(low), ones(runLen, 1), 'valid') == runLen, 1); +end + +if isempty(iBad) + fTrust = max(freq); +else + fTrust = freq(max(iBad - 1, 2)); +end + +end diff --git a/src/plot/PSplotBode.m b/src/plot/PSplotBode.m index e2ab1fd..92251f6 100644 --- a/src/plot/PSplotBode.m +++ b/src/plot/PSplotBode.m @@ -27,21 +27,7 @@ function PSplotBode(freq, G_track, G_plant, C, stepData, titleStr, pred) fPlot = freq(freq > 0); % skip DC for log plot fMask = freq > 0; -% The prediction is only worth as much as the plant under it, and the plant -% stops meaning anything once the chirp no longer moves the airframe. Cut it at -% the same 0.8 coherence the plot already draws a line at, and take a run of -% bins rather than the first one so a single noisy bin does not end the band. -low = freq > 2 & C(:) < 0.8; -run = 5; -iBad = []; -if numel(low) >= run - iBad = find(conv(double(low), ones(run, 1), 'valid') == run, 1); -end -if isempty(iBad) - fTrust = max(freq); -else - fTrust = freq(max(iBad - 1, 2)); -end +fTrust = PStrustBand(freq, C); predMask = fMask & freq <= fTrust; fPred = freq(predMask); diff --git a/tests/test_PSestimateMultiWindow.m b/tests/test_PSestimateMultiWindow.m new file mode 100644 index 0000000..0021f3d --- /dev/null +++ b/tests/test_PSestimateMultiWindow.m @@ -0,0 +1,87 @@ +% test_PSestimateMultiWindow.m - pooling several chirp windows into one estimate + +%!test +%! % The segment count is reported, and it follows the documented geometry +%! Fs = 1000; N = 20000; +%! randn('state', 1); +%! x = randn(N, 1); +%! [~, ~, ~, nSeg] = PSestimateFreqResponse(x, x, Fs); +%! Nest = round(2.5*Fs); Nstep = Nest - round(0.9*Nest); +%! assert(nSeg, floor((N - Nest)/Nstep) + 1); + +%!test +%! % One window in a cell is the same as passing it bare +%! Fs = 1000; N = 20000; +%! randn('state', 2); +%! x = randn(N, 1); y = filter(1, [1 -0.8], x); +%! [G1, C1, f1, n1] = PSestimateFreqResponse(x, y, Fs); +%! [G2, C2, f2, n2] = PSestimateFreqResponse({x}, {y}, Fs); +%! assert(max(abs(G1 - G2)), 0, 1e-12); +%! assert(max(abs(C1 - C2)), 0, 1e-12); +%! assert(f1, f2); assert(n1, n2); + +%!test +%! % Two windows contribute their segments to one estimate +%! Fs = 1000; N = 20000; +%! randn('state', 3); +%! x1 = randn(N,1); x2 = randn(N,1); +%! y1 = 2*x1; y2 = 2*x2; +%! [G, C, freq, nSeg] = PSestimateFreqResponse({x1, x2}, {y1, y2}, Fs); +%! [~, ~, ~, n1] = PSestimateFreqResponse(x1, y1, Fs); +%! assert(nSeg, 2*n1); +%! mid = freq > 10 & freq < 400; +%! assert(max(abs(abs(G(mid)) - 2)) < 0.01, 'pooled gain must still be 2'); +%! assert(min(C(mid)) > 0.99); + +%!test +%! % Pooling the Welch accumulators is not the same as averaging the two G's, +%! % and it is the right one: a weakly driven window carries less weight instead +%! % of the same weight. Averaging G would let the noisy window pull the answer. +%! Fs = 1000; N = 20000; +%! randn('state', 5); +%! trueG = 2; +%! xStrong = 10*randn(N,1); % well excited +%! xWeak = 0.2*randn(N,1); % barely excited +%! nz = 0.5*randn(N,1); % same output noise on both +%! yStrong = trueG*xStrong + nz; +%! yWeak = trueG*xWeak + 0.5*randn(N,1); +%! [Gp, ~, freq] = PSestimateFreqResponse({xStrong, xWeak}, {yStrong, yWeak}, Fs); +%! Gs = PSestimateFreqResponse(xStrong, yStrong, Fs); +%! Gw = PSestimateFreqResponse(xWeak, yWeak, Fs); +%! Gmean = (Gs + Gw) / 2; +%! mid = freq > 10 & freq < 400; +%! ePooled = mean(abs(Gp(mid) - trueG)); +%! eMean = mean(abs(Gmean(mid) - trueG)); +%! assert(ePooled < eMean, 'pooled accumulators must beat averaging the G estimates'); +%! assert(ePooled < 0.05, 'pooled estimate must land near the true gain'); + +%!test +%! % A window too short to hold one segment is skipped, not an error +%! Fs = 1000; N = 20000; +%! randn('state', 7); +%! x = randn(N,1); y = 3*x; +%! [G, ~, freq, nSeg] = PSestimateFreqResponse({x, randn(100,1)}, {y, randn(100,1)}, Fs); +%! [~, ~, ~, n1] = PSestimateFreqResponse(x, y, Fs); +%! assert(nSeg, n1, 'the short window must contribute nothing'); +%! mid = freq > 10 & freq < 400; +%! assert(max(abs(abs(G(mid)) - 3)) < 0.01); + +%!test +%! % Mismatched cell lengths are a caller bug and must say so +%! Fs = 1000; +%! x = randn(20000,1); +%! err = ''; +%! try +%! PSestimateFreqResponse({x, x}, {x}, Fs); +%! catch e +%! err = e.message; +%! end +%! assert(~isempty(err), 'mismatched window counts must raise'); + +%!test +%! % No usable window at all returns zeros rather than NaN +%! Fs = 1000; +%! [G, C, freq, nSeg] = PSestimateFreqResponse({randn(50,1)}, {randn(50,1)}, Fs); +%! assert(nSeg, 0); +%! assert(all(G == 0)); assert(all(C == 0)); +%! assert(numel(freq), floor(round(2.5*Fs)/2) + 1); diff --git a/tests/test_PStrustBand.m b/tests/test_PStrustBand.m new file mode 100644 index 0000000..35b8a4c --- /dev/null +++ b/tests/test_PStrustBand.m @@ -0,0 +1,58 @@ +% test_PStrustBand.m - tests for the coherence-gated top of the usable band + +%!test +%! % Coherence never drops: the whole grid is usable +%! freq = (0:0.5:500)'; +%! C = 0.95 * ones(size(freq)); +%! assert(PStrustBand(freq, C), 500); + +%!test +%! % A sustained drop cuts the band just before it starts +%! freq = (0:0.5:500)'; +%! C = 0.95 * ones(size(freq)); +%! C(freq >= 120) = 0.4; +%! f = PStrustBand(freq, C); +%! assert(f >= 119 && f < 120, 'band must end just below the drop'); + +%!test +%! % One noisy bin is not the end of the band - that is why a run is required +%! freq = (0:0.5:500)'; +%! C = 0.95 * ones(size(freq)); +%! C(freq == 200) = 0.1; +%! assert(PStrustBand(freq, C), 500); +%! % but five in a row are +%! C(freq >= 200 & freq <= 202) = 0.1; +%! assert(PStrustBand(freq, C) < 200); + +%!test +%! % Low frequency is exempt: a 2.5 s Welch segment holds too few cycles there +%! % for coherence to mean anything, and the chirp starts at 0.2 Hz +%! freq = (0:0.5:500)'; +%! C = 0.95 * ones(size(freq)); +%! C(freq <= 2) = 0.1; +%! assert(PStrustBand(freq, C), 500); + +%!test +%! % Threshold and run length are settable, and both actually bind +%! freq = (0:0.5:500)'; +%! C = 0.95 * ones(size(freq)); +%! C(freq >= 100) = 0.7; +%! assert(PStrustBand(freq, C, 0.8), 99.5); +%! assert(PStrustBand(freq, C, 0.6), 500); +%! C2 = 0.95 * ones(size(freq)); +%! C2(freq >= 100 & freq <= 101) = 0.1; % 3 bins +%! assert(PStrustBand(freq, C2, 0.8, 5), 500); +%! assert(PStrustBand(freq, C2, 0.8, 3) < 100); + +%!test +%! % A grid shorter than the run length cannot show a sustained drop +%! freq = (0:0.5:1)'; +%! C = [0.1; 0.1; 0.1]; +%! assert(PStrustBand(freq, C), 1); + +%!test +%! % Row or column, same answer - callers pass both +%! freq = (0:0.5:500)'; +%! C = 0.95 * ones(size(freq)); +%! C(freq >= 120) = 0.4; +%! assert(PStrustBand(freq(:)', C(:)'), PStrustBand(freq, C)); From f367eef1cea838de9a286f8a86f6d766a2af4600 Mon Sep 17 00:00:00 2001 From: Grzegorz Sterniczuk Date: Sat, 19 Sep 2026 21:25:05 +0200 Subject: [PATCH 02/33] feat(chirp): score every chirp run in a log, not just the longest --- src/core/PSchirpWindows.m | 98 +++++++++++++++++++++++++++++++ src/core/PSfindChirpWindow.m | 32 ++++------ tests/test_PSchirpWindows.m | 111 +++++++++++++++++++++++++++++++++++ 3 files changed, 219 insertions(+), 22 deletions(-) create mode 100644 src/core/PSchirpWindows.m create mode 100644 tests/test_PSchirpWindows.m diff --git a/src/core/PSchirpWindows.m b/src/core/PSchirpWindows.m new file mode 100644 index 0000000..6bd1a61 --- /dev/null +++ b/src/core/PSchirpWindows.m @@ -0,0 +1,98 @@ +function w = PSchirpWindows(sinarg_raw, gyro, axisSum, thr, Fs, si, axisIdx) +%% PSchirpWindows - score every chirp run in a log, one struct per run +% sinarg_raw - debug_0_ column (sinarg * 5000) +% gyro - gyro for the axis under test +% axisSum - P+I+D+F for that axis, or [] when the PID terms are not logged +% thr - setpoint_3_ (mixer throttle), or [] when absent +% Fs - sample rate (Hz), or [] to leave the timed fields unknown +% si - setupInfo cell array {param, value} +% axisIdx - 0=Roll, 1=Pitch, 2=Yaw, picks which pidsum limit applies +% +% Everything from axisSum on is optional. What cannot be worked out comes back +% NaN and complete comes back false, so a run nobody could time never reads as +% a run that passed. +% +% Fields: i0/i1/nSamp/durSec, chirpTime/complete, gyroVar, satFrac/pidsumLimit, +% thrStd. + +if nargin < 3, axisSum = []; end +if nargin < 4, thr = []; end +if nargin < 5, Fs = []; end +if nargin < 6, si = {}; end +if nargin < 7, axisIdx = []; end + +sinarg_raw = sinarg_raw(:); +gyro = gyro(:); +axisSum = axisSum(:); +thr = thr(:); + +w = struct([]); + +% sinarg is a phase wrapping through [0, 2*pi], so it passes through zero on +% every cycle - hundreds of times a second at the top of the sweep. Only a real +% gap between runs separates two of them, not a wrap or a sample landing on one. +maxGap = 50; +act = find(sinarg_raw ~= 0); +if isempty(act), return; end + +brk = find(diff(act) > maxGap); +starts = act([1; brk+1]); +ends = act([brk; numel(act)]); + +chirpTime = NaN; +pidsumLimit = NaN; +if ~isempty(si) + chirpTime = hnum(si, 'chirp_time_seconds'); + % yaw runs on its own limit, and scoring it against the roll one would hide + % a third of the saturation + if isequal(axisIdx, 2) + pidsumLimit = hnum(si, 'pidsum_limit_yaw'); + elseif ~isempty(axisIdx) + pidsumLimit = hnum(si, 'pidsum_limit'); + end +end + +haveFs = ~isempty(Fs) && Fs > 0; +canSat = ~isempty(axisSum) && isfinite(pidsumLimit) && pidsumLimit > 0; + +for k = 1:numel(starts) + i0 = starts(k); + i1 = ends(k); + + w(k).i0 = i0; + w(k).i1 = i1; + w(k).nSamp = i1 - i0 + 1; + + w(k).durSec = NaN; + if haveFs, w(k).durSec = w(k).nSamp / Fs; end + w(k).chirpTime = chirpTime; + % a sweep cut short never reached the top of the band, yet fTrust off it + % still comes out looking sensible + w(k).complete = w(k).durSec >= 0.95 * chirpTime; + + w(k).gyroVar = var(gyro(i0:i1)); + + w(k).satFrac = NaN; + if canSat + % the loop stops being linear before the sum reaches the clip, which + % makes the plant measured off a saturated run a fiction + w(k).satFrac = mean(abs(axisSum(i0:i1)) >= 0.98 * pidsumLimit); + end + w(k).pidsumLimit = pidsumLimit; + + w(k).thrStd = NaN; + if ~isempty(thr), w(k).thrStd = std(thr(i0:i1)); end +end + +end + + +function v = hnum(si, key) + v = NaN; + for k = 1:size(si, 1) + if strcmp(strtrim(si{k,1}), key) + v = str2double(strtrim(si{k,2})); + return; + end + end +end diff --git a/src/core/PSfindChirpWindow.m b/src/core/PSfindChirpWindow.m index 4720375..1b25439 100644 --- a/src/core/PSfindChirpWindow.m +++ b/src/core/PSfindChirpWindow.m @@ -6,35 +6,23 @@ if nargin < 3, varThresh = 500; end -sinarg_raw = sinarg_raw(:); +w = PSchirpWindows(sinarg_raw, gyro); -% sinarg is a phase wrapping through [0, 2*pi], so it passes near zero on every -% cycle - hundreds of times a second at the top of the sweep. Only a real gap -% between runs separates two windows, not a wrap or a sample landing on one. -maxGap = 50; -act = find(sinarg_raw ~= 0); - -if isempty(act) +if isempty(w) idx_start = 1; idx_end = length(sinarg_raw); return end -brk = find(diff(act) > maxGap); -starts = act([1; brk+1]); -ends = act([brk; numel(act)]); - % pick longest window with sufficient gyro variance best_len = 0; -idx_start = starts(1); -idx_end = ends(1); -for k = 1:length(starts) - seg_len = ends(k) - starts(k) + 1; - if seg_len < 100, continue; end - gvar = var(gyro(starts(k):ends(k))); - if gvar > varThresh && seg_len > best_len - best_len = seg_len; - idx_start = starts(k); - idx_end = ends(k); +idx_start = w(1).i0; +idx_end = w(1).i1; +for k = 1:numel(w) + if w(k).nSamp < 100, continue; end + if w(k).gyroVar > varThresh && w(k).nSamp > best_len + best_len = w(k).nSamp; + idx_start = w(k).i0; + idx_end = w(k).i1; end end diff --git a/tests/test_PSchirpWindows.m b/tests/test_PSchirpWindows.m new file mode 100644 index 0000000..431a192 --- /dev/null +++ b/tests/test_PSchirpWindows.m @@ -0,0 +1,111 @@ +% test_PSchirpWindows.m - scoring every chirp block in a log, not just the longest + +%!test +%! % Three runs separated by real gaps are three blocks, and a phase wrap that +%! % lands exactly on zero inside a run must not split it +%! blk = round(5000 * mod((1:10000)' * 0.01, 2*pi) / (2*pi)); +%! blk(500:500:9500) = 0; +%! gap = zeros(2000, 1); +%! sinarg = [blk; gap; blk; gap; blk]; +%! randn('state', 11); +%! gyro = 100 * randn(size(sinarg)); +%! w = PSchirpWindows(sinarg, gyro); +%! assert(numel(w) == 3, 'a wrap through zero must not split a run'); +%! assert([w.i0], [1 12001 24001]); +%! assert([w.i1], [10000 22000 34000]); +%! assert([w.nSamp], [10000 10000 10000]); + +%!test +%! % A sweep that ran to the end is complete; one cut short is not, and the +%! % short one still reports a plausible window +%! Fs = 1000; +%! si = {'chirp_time_seconds', '20'}; +%! sinarg = [ones(20000,1); zeros(2000,1); ones(10000,1)]; +%! randn('state', 12); +%! gyro = 100 * randn(size(sinarg)); +%! w = PSchirpWindows(sinarg, gyro, [], [], Fs, si, 0); +%! assert(numel(w), 2); +%! assert(w(1).durSec, 20, 1e-9); +%! assert(w(1).chirpTime, 20); +%! assert(w(1).complete); +%! assert(w(2).durSec, 10, 1e-9); +%! assert(~w(2).complete, 'half a sweep has no top of band'); + +%!test +%! % Without chirp_time_seconds nothing can be certified complete, and the +%! % caller can tell it apart from a short sweep because chirpTime is unknown +%! Fs = 1000; +%! randn('state', 13); +%! w = PSchirpWindows(ones(20000,1), 100*randn(20000,1), [], [], Fs, {}, 0); +%! assert(isnan(w.chirpTime)); +%! assert(~w.complete, 'unknown sweep length must not read as complete'); + +%!test +%! % Saturation counts from 0.98 of pidsum_limit, in both directions - the loop +%! % stops being linear before the sum reaches the clip +%! Fs = 1000; +%! si = {'chirp_time_seconds', '10'; 'pidsum_limit', '800'; 'pidsum_limit_yaw', '600'}; +%! randn('state', 14); +%! axisSum = 100 * ones(10000, 1); +%! axisSum(1:1500) = 790; +%! axisSum(2001:3000) = -790; +%! w = PSchirpWindows(ones(10000,1), 100*randn(10000,1), axisSum, [], Fs, si, 0); +%! assert(w.pidsumLimit, 800); +%! assert(w.satFrac, 0.25, 1e-12); + +%!test +%! % Yaw has its own limit - scoring it against the roll one hides a third of +%! % the saturation +%! Fs = 1000; +%! si = {'chirp_time_seconds', '10'; 'pidsum_limit', '800'; 'pidsum_limit_yaw', '600'}; +%! randn('state', 15); +%! gyro = 100 * randn(10000, 1); +%! axisSum = 100 * ones(10000, 1); +%! axisSum(1:1500) = 700; % over 0.98*600, under 0.98*800 +%! wRoll = PSchirpWindows(ones(10000,1), gyro, axisSum, [], Fs, si, 0); +%! wYaw = PSchirpWindows(ones(10000,1), gyro, axisSum, [], Fs, si, 2); +%! assert(wYaw.pidsumLimit, 600); +%! assert(wRoll.satFrac, 0, 1e-12); +%! assert(wYaw.satFrac, 0.15, 1e-12); + +%!test +%! % Variance is per block, so a run that swept another axis scores low on this +%! % one - that is how a three-axis log keeps its axes apart +%! Fs = 1000; +%! si = {'chirp_time_seconds', '10'}; +%! randn('state', 16); +%! sinarg = [ones(10000,1); zeros(2000,1); ones(10000,1)]; +%! gyro = [0.5*randn(10000,1); zeros(2000,1); 100*randn(10000,1)]; +%! w = PSchirpWindows(sinarg, gyro, [], [], Fs, si, 0); +%! assert(w(1).gyroVar < 10); +%! assert(w(2).gyroVar > 1000); + +%!test +%! % Throttle spread is per block as well - TPA moves with throttle, so a block +%! % flown on a drifting stick is not the same measurement as a steady one +%! Fs = 1000; +%! si = {'chirp_time_seconds', '10'}; +%! randn('state', 17); +%! sinarg = [ones(10000,1); zeros(2000,1); ones(10000,1)]; +%! gyro = 100 * randn(size(sinarg)); +%! thr = [1500*ones(10000,1); zeros(2000,1); [1400*ones(5000,1); 1600*ones(5000,1)]]; +%! w = PSchirpWindows(sinarg, gyro, [], thr, Fs, si, 0); +%! assert(w(1).thrStd, 0, 1e-12); +%! assert(w(2).thrStd, 100, 0.02); + +%!test +%! % A log with no chirp has no blocks to score +%! randn('state', 18); +%! w = PSchirpWindows(zeros(5000,1), randn(5000,1)); +%! assert(isempty(w)); + +%!test +%! % PSfindChirpWindow needs only the blocks and their variance, so the rest is +%! % optional and reports as unknown rather than as a passing score +%! randn('state', 19); +%! w = PSchirpWindows(ones(3000,1), 100*randn(3000,1)); +%! assert(w.i0, 1); assert(w.i1, 3000); +%! assert(w.gyroVar > 1000); +%! assert(isnan(w.durSec)); assert(isnan(w.satFrac)); assert(isnan(w.thrStd)); +%! assert(isnan(w.pidsumLimit)); +%! assert(~w.complete); From 7e163690a3e2e9f5952c65377447fde0c9dfbf69 Mon Sep 17 00:00:00 2001 From: Grzegorz Sterniczuk Date: Sat, 19 Sep 2026 21:25:29 +0200 Subject: [PATCH 03/33] fix(chirp): pick the run that moved the axis, not the longest one --- src/core/PSfindChirpWindow.m | 11 +++++++---- tests/test_PSfindChirpWindow.m | 29 +++++++++++++++++++++++++++++ 2 files changed, 36 insertions(+), 4 deletions(-) diff --git a/src/core/PSfindChirpWindow.m b/src/core/PSfindChirpWindow.m index 1b25439..a1c169b 100644 --- a/src/core/PSfindChirpWindow.m +++ b/src/core/PSfindChirpWindow.m @@ -13,14 +13,17 @@ return end -% pick longest window with sufficient gyro variance -best_len = 0; +% Every run in a log lasts chirp_time_seconds, so run length differs only by +% sampling noise and says nothing about which axis a run swept - the response +% does. Cross-axis coupling on a real log reaches 1e3, which clears varThresh, +% so the threshold only answers "was this axis swept at all". +best_var = 0; idx_start = w(1).i0; idx_end = w(1).i1; for k = 1:numel(w) if w(k).nSamp < 100, continue; end - if w(k).gyroVar > varThresh && w(k).nSamp > best_len - best_len = w(k).nSamp; + if w(k).gyroVar > varThresh && w(k).gyroVar > best_var + best_var = w(k).gyroVar; idx_start = w(k).i0; idx_end = w(k).i1; end diff --git a/tests/test_PSfindChirpWindow.m b/tests/test_PSfindChirpWindow.m index 0e4012b..8f92daf 100644 --- a/tests/test_PSfindChirpWindow.m +++ b/tests/test_PSfindChirpWindow.m @@ -48,6 +48,35 @@ %! [i0, i1] = PSfindChirpWindow(sinarg_raw, gyro); %! assert(abs(i0 - 19*Fs) < 0.01*Fs, 'must pick the run where the axis responded'); +%!test +%! % Every run in a log is the same length by construction, so run length is +%! % noise and cannot decide which axis a run swept. Block sizes and variances +%! % here are the ones measured on 20250907_flipmini_00, where roll answers in +%! % blocks 1 and 4 while the other four still carry 1e3 of cross-axis coupling - +%! % well clear of the 500 floor, and the longest block of the six is #6. +%! nSamp = [39987 39988 39942 39950 39986 39996]; +%! gvar = [561000 853 211 562000 1365 225]; +%! randn('state', 31); +%! sinarg = []; gyro = []; +%! for k = 1:6 +%! sinarg = [sinarg; 15000*ones(nSamp(k),1); zeros(2000,1)]; +%! gyro = [gyro; sqrt(gvar(k))*randn(nSamp(k),1); zeros(2000,1)]; +%! end +%! [i0, i1] = PSfindChirpWindow(sinarg, gyro); +%! starts = 1 + cumsum([0 nSamp(1:5) + 2000]); +%! picked = find(starts == i0); +%! assert(any(picked == [1 4]), 'must pick a run that actually moved this axis'); + +%!test +%! % Two longer runs carrying only cross-axis coupling clear the variance floor +%! % but must not outrank the run that was swept +%! randn('state', 20); +%! sinarg = [ones(3000,1); zeros(2000,1); ones(9000,1); zeros(2000,1); ones(5000,1)]; +%! gyro = [700*randn(3000,1); zeros(2000,1); 30*randn(9000,1); ... +%! zeros(2000,1); 30*randn(5000,1)]; +%! [i0, i1] = PSfindChirpWindow(sinarg, gyro); +%! assert(isequal([i0 i1], [1 3000]), 'the swept run wins over the longer ones'); + %!test %! % A sample landing exactly on the wrap logs a zero; it must not split the run %! Fs = 2000; Tc = 10; N = 30*Fs; From 5e530601987b944e8a482309c24d171b32a9cad1 Mon Sep 17 00:00:00 2001 From: Grzegorz Sterniczuk Date: Sat, 19 Sep 2026 21:25:29 +0200 Subject: [PATCH 04/33] test: fix two assertions that passed a message where a tolerance goes --- tests/test_PSestimateMultiWindow.m | 2 +- tests/test_PSparseBFversion.m | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/tests/test_PSestimateMultiWindow.m b/tests/test_PSestimateMultiWindow.m index 0021f3d..4c31c82 100644 --- a/tests/test_PSestimateMultiWindow.m +++ b/tests/test_PSestimateMultiWindow.m @@ -62,7 +62,7 @@ %! x = randn(N,1); y = 3*x; %! [G, ~, freq, nSeg] = PSestimateFreqResponse({x, randn(100,1)}, {y, randn(100,1)}, Fs); %! [~, ~, ~, n1] = PSestimateFreqResponse(x, y, Fs); -%! assert(nSeg, n1, 'the short window must contribute nothing'); +%! assert(nSeg == n1, 'the short window must contribute nothing'); %! mid = freq > 10 & freq < 400; %! assert(max(abs(abs(G(mid)) - 3)) < 0.01); diff --git a/tests/test_PSparseBFversion.m b/tests/test_PSparseBFversion.m index 39719c1..6ddd24b 100644 --- a/tests/test_PSparseBFversion.m +++ b/tests/test_PSparseBFversion.m @@ -50,6 +50,6 @@ %! % BF 2025.12 new header format (no slash, from real BBL) %! si = {'Firmware revision', 'Betaflight 2025.12.0-beta (080b2f5da) STM32F405'}; %! [t, maj, mnr] = PSparseBFversion(si); -%! assert(strcmp(t, 'Betaflight'), true, 'fwType should be Betaflight'); +%! assert(strcmp(t, 'Betaflight'), 'fwType should be Betaflight'); %! assert(maj, 2025); %! assert(mnr, 12); From 453ff5719cf60368de3da71179d6e6a8fab9e5fb Mon Sep 17 00:00:00 2001 From: Grzegorz Sterniczuk Date: Sat, 19 Sep 2026 22:03:46 +0200 Subject: [PATCH 05/33] refactor(chirp): split identification out of the plotting --- src/core/PSidentifyChirp.m | 109 ++++++++++++++++++++++++++++++++ src/plot/PSrunChirpAnalysis.m | 82 +++--------------------- tests/test_PSidentifyChirp.m | 114 ++++++++++++++++++++++++++++++++++ 3 files changed, 231 insertions(+), 74 deletions(-) create mode 100644 src/core/PSidentifyChirp.m create mode 100644 tests/test_PSidentifyChirp.m diff --git a/src/core/PSidentifyChirp.m b/src/core/PSidentifyChirp.m new file mode 100644 index 0000000..d46e0bf --- /dev/null +++ b/src/core/PSidentifyChirp.m @@ -0,0 +1,109 @@ +function id = PSidentifyChirp(T, setupInfo, Fs, tIND, axisIdx) +%% PSidentifyChirp - measure the plant and the tracking response from a chirp run +% T - data struct for one file +% setupInfo - header cell array {param, value} +% Fs - sample rate (Hz) +% tIND - logical time index mask +% axisIdx - 0=Roll, 1=Pitch, 2=Yaw +% +% Always returns the same fields, so three axes concatenate into a struct +% array. Nothing is drawn, printed or popped up - the caller decides how to +% report ok and msg, which is what lets all three axes run without opening +% three windows. + +axNames = {'Roll', 'Pitch', 'Yaw'}; +axSuffix = {'_0_', '_1_', '_2_'}; +ax = axisIdx + 1; + +id = blank(); +id.axisIdx = axisIdx; +id.axisName = axNames{ax}; + +if ~isfield(T, 'debug_0_') + id.msg = 'No debug data in log - chirp analysis requires debug_mode = CHIRP'; + return +end + +sinarg_raw = T.debug_0_(tIND); +sinarg = sinarg_raw / 5000; + +if max(abs(sinarg)) < 0.1 + id.msg = 'No chirp data found in debug_0_ (sinarg ≈ 0). Set debug_mode = CHIRP in BF.'; + return +end + +gyro = T.(['gyroADC' axSuffix{ax}])(tIND); +sp = T.(['setpoint' axSuffix{ax}])(tIND); + +hasAxisSum = isfield(T, ['axisP' axSuffix{ax}]) && isfield(T, ['axisI' axSuffix{ax}]); +axisSum = []; Dterm = []; Fterm = []; +if hasAxisSum + P = T.(['axisP' axSuffix{ax}])(tIND); + I = T.(['axisI' axSuffix{ax}])(tIND); + Dterm = zeros(size(P)); + Fterm = zeros(size(P)); + if isfield(T, ['axisD' axSuffix{ax}]), Dterm = T.(['axisD' axSuffix{ax}])(tIND); end + if isfield(T, ['axisF' axSuffix{ax}]), Fterm = T.(['axisF' axSuffix{ax}])(tIND); end + axisSum = P + I + Dterm + Fterm; +end + +thr = []; +if isfield(T, 'setpoint_3_'), thr = T.setpoint_3_(tIND); end + +% scores come from the block list, the pick from the one function that owns +% that rule - two copies of it would drift +w = PSchirpWindows(sinarg_raw, gyro, axisSum, thr, Fs, setupInfo, axisIdx); +[i0, i1] = PSfindChirpWindow(sinarg_raw, gyro); +id.i0 = i0; +id.i1 = i1; +id.nSamp = i1 - i0 + 1; +k = find([w.i0] == i0, 1); +if ~isempty(k) + id.durSec = w(k).durSec; + id.gyroVar = w(k).gyroVar; + id.pidsumLimit = w(k).pidsumLimit; +end + +sinarg_w = sinarg(i0:i1); +sp_filt = PSrotFiltFilt(sp(i0:i1), sinarg_w, Fs); +gyro_filt = PSrotFiltFilt(gyro(i0:i1), sinarg_w, Fs); + +[id.G_track, id.C_track, id.freq, id.nSeg] = ... + PSestimateFreqResponse(sp_filt, gyro_filt, Fs); + +if hasAxisSum + axisSum_filt = PSrotFiltFilt(axisSum(i0:i1), sinarg_w, Fs); + [id.G_uw, id.C_uw, ~] = PSestimateFreqResponse(sp_filt, axisSum_filt, Fs); + id.G_plant = id.G_track ./ (id.G_uw + 1e-12); + id.axisD = Dterm(i0:i1); + Fw = Fterm(i0:i1); + if any(Fw ~= 0) + axisF_filt = PSrotFiltFilt(Fw, sinarg_w, Fs); + [id.G_ff, ~, ~] = PSestimateFreqResponse(sp_filt, axisF_filt, Fs); + end +end + +id.fTrust = PStrustBand(id.freq, id.C_track); + +if ~isempty(setupInfo) + id.fp = PSparseFilterParams(setupInfo); + id.FsPid = id.fp.pid_rate_hz; + if id.FsPid <= 0, id.FsPid = Fs; end + id.gains = PSparsePIDGains(setupInfo, axisIdx); +end + +id.ok = true; + +end + + +function id = blank() + id = struct('ok', false, 'msg', '', ... + 'freq', [], 'G_track', [], 'C_track', [], ... + 'G_plant', [], 'G_uw', [], 'C_uw', [], 'G_ff', [], ... + 'gains', [], 'fp', [], 'FsPid', [], ... + 'axisIdx', [], 'axisName', '', ... + 'i0', [], 'i1', [], 'nSamp', [], 'durSec', NaN, ... + 'gyroVar', NaN, 'nSeg', 0, 'fTrust', NaN, ... + 'axisD', [], 'pidsumLimit', NaN); +end diff --git a/src/plot/PSrunChirpAnalysis.m b/src/plot/PSrunChirpAnalysis.m index 588bde6..3d19737 100644 --- a/src/plot/PSrunChirpAnalysis.m +++ b/src/plot/PSrunChirpAnalysis.m @@ -7,94 +7,28 @@ function PSrunChirpAnalysis(T, setupInfo, debugIdx, Fs, tIND, axisIdx) % tIND - logical time index mask % axisIdx - 0=Roll, 1=Pitch, 2=Yaw -axNames = {'Roll', 'Pitch', 'Yaw'}; -axSuffix = {'_0_', '_1_', '_2_'}; -ax = axisIdx + 1; +id = PSidentifyChirp(T, setupInfo, Fs, tIND, axisIdx); -% check CHIRP debug mode -if ~isfield(T, 'debug_0_') - warndlg('No debug data in log - chirp analysis requires debug_mode = CHIRP'); +if ~id.ok + warndlg(id.msg); return end -sinarg_raw = T.debug_0_(tIND); -sinarg = sinarg_raw / 5000; - -if max(abs(sinarg)) < 0.1 - warndlg('No chirp data found in debug_0_ (sinarg ≈ 0). Set debug_mode = CHIRP in BF.'); - return -end - -% get gyro and setpoint for selected axis -gyro = T.(['gyroADC' axSuffix{ax}])(tIND); -sp = T.(['setpoint' axSuffix{ax}])(tIND); - -% find chirp evaluation window -[i0, i1] = PSfindChirpWindow(sinarg_raw, gyro); -sinarg_w = sinarg(i0:i1); -gyro_w = gyro(i0:i1); -sp_w = sp(i0:i1); - fprintf('Chirp window: samples %d-%d (%d samples, %.1f s)\n', ... - i0, i1, i1-i0+1, (i1-i0+1)/Fs); - -% apply rotating demodulation filter to extract chirp-correlated components -sp_filt = PSrotFiltFilt(sp_w, sinarg_w, Fs); -gyro_filt = PSrotFiltFilt(gyro_w, sinarg_w, Fs); - -% build axisSum and axisSumPI if PID terms available -hasAxisSum = false; -if isfield(T, ['axisP' axSuffix{ax}]) && isfield(T, ['axisI' axSuffix{ax}]) - P = T.(['axisP' axSuffix{ax}])(tIND); - I = T.(['axisI' axSuffix{ax}])(tIND); - D = zeros(size(P)); - F = zeros(size(P)); - if isfield(T, ['axisD' axSuffix{ax}]), D = T.(['axisD' axSuffix{ax}])(tIND); end - if isfield(T, ['axisF' axSuffix{ax}]), F = T.(['axisF' axSuffix{ax}])(tIND); end - - axisSum_w = P(i0:i1) + I(i0:i1) + D(i0:i1) + F(i0:i1); - axisSumPI_w = P(i0:i1) + I(i0:i1); - - axisSum_filt = PSrotFiltFilt(axisSum_w, sinarg_w, Fs); - axisF_filt = PSrotFiltFilt(F(i0:i1), sinarg_w, Fs); - hasAxisSum = true; -end - -% estimate tracking transfer function: T = sp → gyro -[G_track, C_track, freq] = PSestimateFreqResponse(sp_filt, gyro_filt, Fs); - -% estimate plant if PID terms available -G_plant = []; -G_ff = []; -if hasAxisSum - [G_uw, ~, ~] = PSestimateFreqResponse(sp_filt, axisSum_filt, Fs); - % P = T / Guw (transfer function from controller output to gyro) - G_plant = G_track ./ (G_uw + 1e-12); - if any(F(i0:i1) ~= 0) - [G_ff, ~, ~] = PSestimateFreqResponse(sp_filt, axisF_filt, Fs); - end -end + id.i0, id.i1, id.nSamp, id.nSamp / Fs); -% step response from tracking TF stepData = struct(); try - [stepData.t_ms, stepData.step] = PSstepFromFRD(freq, G_track, 300); + [stepData.t_ms, stepData.step] = PSstepFromFRD(id.freq, id.G_track, 300); catch stepData = []; end -% controller model for the prediction sliders - needs the gains and the dterm -% filters from the header, and the PID rate rather than the logging rate pred = []; -if hasAxisSum && ~isempty(setupInfo) - fp = PSparseFilterParams(setupInfo); - FsPid = fp.pid_rate_hz; - if FsPid <= 0, FsPid = Fs; end - pred = struct('gains', PSparsePIDGains(setupInfo, axisIdx), 'fp', fp, ... - 'FsPid', FsPid, 'Fref', G_ff); +if ~isempty(id.G_plant) && ~isempty(id.gains) + pred = struct('gains', id.gains, 'fp', id.fp, 'FsPid', id.FsPid, 'Fref', id.G_ff); end -% plot -PSplotBode(freq, G_track, G_plant, C_track, stepData, axNames{ax}, pred); +PSplotBode(id.freq, id.G_track, id.G_plant, id.C_track, stepData, id.axisName, pred); end diff --git a/tests/test_PSidentifyChirp.m b/tests/test_PSidentifyChirp.m new file mode 100644 index 0000000..c790d7c --- /dev/null +++ b/tests/test_PSidentifyChirp.m @@ -0,0 +1,114 @@ +% test_PSidentifyChirp.m - chirp identification, with the plotting split off + +%!shared T, si, Fs, tIND, kPlant, gPlant +%! % A Betaflight loop run sample by sample against a known plant, driven by a +%! % real 0.5 -> 400 Hz sweep, so the identified plant can be checked against +%! % algebra rather than against a previous run of this same code. +%! Fs = 2000; Ts = 1/Fs; +%! Tc = 20; Nc = Tc*Fs; pad = 2000; N = Nc + 2*pad; +%! f0 = 0.5; f1 = 400; +%! tt = (0:Nc-1)'/Fs; +%! ph = 2*pi*(f0*tt + (f1-f0)/(2*Tc)*tt.^2); +%! sinarg = zeros(N,1); sinarg(pad+1:pad+Nc) = mod(ph, 2*pi); +%! sp = zeros(N,1); sp(pad+1:pad+Nc) = 200*sin(ph); +%! Kp = 45*0.032029; Ki = 60*0.244381; Kd = 20*0.000529; +%! pt1k = @(fc) (1/Fs) / (1/(2*pi*fc) + 1/Fs); +%! kPlant = pt1k(60 * 1.553773974); % pt2 at 60 Hz, as PSbfFilters builds it +%! kDterm = pt1k(80); +%! gPlant = 0.5; +%! y = zeros(N,1); uP = zeros(N,1); uI = zeros(N,1); uD = zeros(N,1); +%! sd = 0; sdPrev = 0; Iacc = 0; s1 = 0; s2 = 0; yNext = 0; +%! for k = 1:N +%! y(k) = yNext; +%! ek = sp(k) - y(k); +%! Iacc = Iacc + Ki*Ts*ek; +%! sd = sd + kDterm*(y(k) - sd); +%! uP(k) = Kp*ek; uI(k) = Iacc; +%! uD(k) = -Kd*(sd - sdPrev)/Ts; sdPrev = sd; +%! s1 = s1 + kPlant*(gPlant*(uP(k)+uI(k)+uD(k)) - s1); +%! s2 = s2 + kPlant*(s1 - s2); +%! yNext = s2; +%! end +%! assert(all(isfinite(y)) && max(abs(y)) < 1e6, 'simulated loop must stay stable'); +%! T = struct(); +%! T.debug_0_ = round(sinarg * 5000); +%! T.gyroADC_0_ = y; T.setpoint_0_ = sp; T.setpoint_3_ = 1500*ones(N,1); +%! T.axisP_0_ = uP; T.axisI_0_ = uI; T.axisD_0_ = uD; T.axisF_0_ = zeros(N,1); +%! si = {'rollPID','45,60,20'; 'ff_weight','0,0,0'; 'd_max','0,0,0'; ... +%! 'looptime','500'; 'pid_process_denom','1'; ... +%! 'dterm_lpf1_type','0'; 'dterm_lpf1_static_hz','80'; ... +%! 'dterm_lpf2_type','0'; 'dterm_lpf2_static_hz','0'; ... +%! 'dterm_notch_hz','0'; 'dterm_notch_cutoff','0'; ... +%! 'chirp_time_seconds','20'; 'pidsum_limit','800'; 'pidsum_limit_yaw','600'}; +%! tIND = true(N,1); + +%!test +%! % The identified plant must match the one the simulation actually contains: +%! % two PT1 sections and the one-sample delay of reading the gyro before the +%! % plant integrates this iteration's output +%! id = PSidentifyChirp(T, si, Fs, tIND, 0); +%! assert(id.ok, 'a clean simulated chirp must identify'); +%! zinv = exp(-2j*pi*id.freq/Fs); +%! H = kPlant ./ (1 - (1-kPlant)*zinv); +%! Pexact = gPlant * H.^2 .* zinv; +%! band = id.freq > 10 & id.freq < 200; +%! err = abs(id.G_plant(band) - Pexact(band)) ./ abs(Pexact(band)); +%! assert(max(err) < 0.02, sprintf('plant off by %.1f%% at worst', 100*max(err))); + +%!test +%! % Both paths carry the same fields, so three axes assemble into one struct +%! % array even when an axis refuses +%! want = {'ok','msg','freq','G_track','C_track','G_plant','G_uw','C_uw', ... +%! 'G_ff','gains','fp','FsPid','axisIdx','axisName','i0','i1', ... +%! 'nSamp','durSec','gyroVar','nSeg','fTrust','axisD','pidsumLimit'}; +%! good = PSidentifyChirp(T, si, Fs, tIND, 0); +%! bad = PSidentifyChirp(struct('gyroADC_0_', zeros(10,1)), si, Fs, true(10,1), 0); +%! assert(isempty(setdiff(want, fieldnames(good))), 'a field the plan names is missing'); +%! assert(isequal(fieldnames(good), fieldnames(bad)), 'both paths must agree on fields'); +%! arr = [good bad]; +%! assert(numel(arr) == 2, 'results must concatenate into a struct array'); + +%!test +%! % Identification must not draw and must not talk - the caller decides how to +%! % report, which is what lets three axes run without opening three windows +%! n0 = numel(findobj('Type', 'figure')); +%! out = evalc('id = PSidentifyChirp(T, si, Fs, tIND, 0);'); +%! assert(numel(findobj('Type', 'figure')) == n0, 'must not open a figure'); +%! assert(isempty(strtrim(out)), 'must not print'); + +%!test +%! % No debug column at all +%! id = PSidentifyChirp(struct('gyroADC_0_', zeros(100,1)), si, Fs, true(100,1), 0); +%! assert(~id.ok); +%! assert(~isempty(strfind(lower(id.msg), 'debug')), 'message must name what is missing'); +%! assert(isempty(id.freq)); + +%!test +%! % Debug column present but no chirp was flown +%! Tz = T; Tz.debug_0_ = zeros(size(T.debug_0_)); +%! id = PSidentifyChirp(Tz, si, Fs, tIND, 0); +%! assert(~id.ok); +%! assert(~isempty(strfind(lower(id.msg), 'chirp'))); + +%!test +%! % Without the PID terms there is no plant, but tracking is still measurable +%! Tn = rmfield(T, {'axisP_0_','axisI_0_','axisD_0_','axisF_0_'}); +%! id = PSidentifyChirp(Tn, si, Fs, tIND, 0); +%! assert(id.ok, 'tracking alone is still a usable measurement'); +%! assert(~isempty(id.G_track)); +%! assert(isempty(id.G_plant), 'no PID terms means no plant'); +%! assert(isempty(id.axisD)); + +%!test +%! % The window and the header land in the result, because the gates and the UI +%! % read them from here rather than re-parsing the log +%! id = PSidentifyChirp(T, si, Fs, tIND, 0); +%! assert(strcmp(id.axisName, 'Roll')); +%! assert(id.axisIdx == 0); +%! assert(id.durSec > 19 && id.durSec < 20.2); +%! assert(id.nSeg > 8, 'too few Welch segments to average'); +%! assert(id.fTrust > 50); +%! assert(id.pidsumLimit == 800); +%! assert(id.gains.P == 45); +%! assert(id.FsPid == 2000); +%! assert(numel(id.axisD) == id.nSamp, 'axisD is the window, ready for the noise budget'); From e80ee3c8b4cddb5ffd13559b54616e2b46dc0351 Mon Sep 17 00:00:00 2001 From: Grzegorz Sterniczuk Date: Sat, 19 Sep 2026 22:07:53 +0200 Subject: [PATCH 06/33] fix(chirp): bound the trust band by the plant coherence, not tracking alone --- src/core/PSidentifyChirp.m | 8 +++++++- src/plot/PSplotBode.m | 5 +++++ src/plot/PSrunChirpAnalysis.m | 3 ++- tests/test_PSidentifyChirp.m | 11 +++++++++++ 4 files changed, 25 insertions(+), 2 deletions(-) diff --git a/src/core/PSidentifyChirp.m b/src/core/PSidentifyChirp.m index d46e0bf..e0a03db 100644 --- a/src/core/PSidentifyChirp.m +++ b/src/core/PSidentifyChirp.m @@ -83,7 +83,13 @@ end end -id.fTrust = PStrustBand(id.freq, id.C_track); +% the plant is the ratio of two estimates, so it is only trustworthy where both +% are +if isempty(id.C_uw) + id.fTrust = PStrustBand(id.freq, id.C_track); +else + id.fTrust = PStrustBand(id.freq, min(id.C_track, id.C_uw)); +end if ~isempty(setupInfo) id.fp = PSparseFilterParams(setupInfo); diff --git a/src/plot/PSplotBode.m b/src/plot/PSplotBode.m index 92251f6..208b5ca 100644 --- a/src/plot/PSplotBode.m +++ b/src/plot/PSplotBode.m @@ -27,7 +27,12 @@ function PSplotBode(freq, G_track, G_plant, C, stepData, titleStr, pred) fPlot = freq(freq > 0); % skip DC for log plot fMask = freq > 0; +% the caller knows the plant coherence as well as the tracking coherence plotted +% here, and the two consumers have to agree on where the band ends fTrust = PStrustBand(freq, C); +if hasPred && isfield(pred, 'fTrust') && ~isempty(pred.fTrust) && isfinite(pred.fTrust) + fTrust = pred.fTrust; +end predMask = fMask & freq <= fTrust; fPred = freq(predMask); diff --git a/src/plot/PSrunChirpAnalysis.m b/src/plot/PSrunChirpAnalysis.m index 3d19737..68f3a6c 100644 --- a/src/plot/PSrunChirpAnalysis.m +++ b/src/plot/PSrunChirpAnalysis.m @@ -26,7 +26,8 @@ function PSrunChirpAnalysis(T, setupInfo, debugIdx, Fs, tIND, axisIdx) pred = []; if ~isempty(id.G_plant) && ~isempty(id.gains) - pred = struct('gains', id.gains, 'fp', id.fp, 'FsPid', id.FsPid, 'Fref', id.G_ff); + pred = struct('gains', id.gains, 'fp', id.fp, 'FsPid', id.FsPid, ... + 'Fref', id.G_ff, 'fTrust', id.fTrust); end PSplotBode(id.freq, id.G_track, id.G_plant, id.C_track, stepData, id.axisName, pred); diff --git a/tests/test_PSidentifyChirp.m b/tests/test_PSidentifyChirp.m index c790d7c..b71a89b 100644 --- a/tests/test_PSidentifyChirp.m +++ b/tests/test_PSidentifyChirp.m @@ -99,6 +99,17 @@ %! assert(isempty(id.G_plant), 'no PID terms means no plant'); %! assert(isempty(id.axisD)); +%!test +%! % The plant is G_track / G_uw, so the band may never be wider than what +%! % either estimate alone supports. On a log where axisSum is an exact function +%! % of the logged signals C_uw sits at 1 and C_track binds, so this guards the +%! % direction rather than a number: widening it, or reading the band off G_uw +%! % alone, would let the prediction run past where the plant was measured. +%! id = PSidentifyChirp(T, si, Fs, tIND, 0); +%! assert(id.fTrust <= PStrustBand(id.freq, id.C_track)); +%! assert(id.fTrust <= PStrustBand(id.freq, id.C_uw)); +%! assert(id.fTrust > 50, 'a clean 400 Hz sweep must leave a usable band'); + %!test %! % The window and the header land in the result, because the gates and the UI %! % read them from here rather than re-parsing the log From dabdfb7856c0dd683628de2b471c79de0c1a3e24 Mon Sep 17 00:00:00 2001 From: Grzegorz Sterniczuk Date: Sat, 19 Sep 2026 23:36:58 +0200 Subject: [PATCH 07/33] feat(chirp): build the controller family once as a basis for the scan --- src/core/PSautotuneBasis.m | 31 ++++++++++++++++ tests/test_PSautotuneBasis.m | 71 ++++++++++++++++++++++++++++++++++++ 2 files changed, 102 insertions(+) create mode 100644 src/core/PSautotuneBasis.m create mode 100644 tests/test_PSautotuneBasis.m diff --git a/src/core/PSautotuneBasis.m b/src/core/PSautotuneBasis.m new file mode 100644 index 0000000..cdbf0db --- /dev/null +++ b/src/core/PSautotuneBasis.m @@ -0,0 +1,31 @@ +function b = PSautotuneBasis(gains, fp, Fs, freq) +%% PSautotuneBasis - the whole controller family as four fixed responses +% gains - only axis and tpa are read; the four gains are set here +% fp - filter params from PSparseFilterParams, or [] for no dterm filtering +% Fs - PID loop rate (Hz) +% freq - frequency vector (Hz) +% +% Every gain enters PSbuildController as a plain scalar multiplier, so two +% calls span the family a scan walks over: +% A = p*b.Ap + i*b.Ai, D = d*b.D1, F = f*b.F1 +% A thousand-cell scan then costs two controller builds instead of a thousand. +% +% b.freq drops DC, where the integrator is Inf and a candidate with I = 0 would +% ask for 0*Inf. b.keep maps b.freq back onto the grid that came in. + +freq = freq(:); +b.keep = freq > 0; +b.freq = freq(b.keep); + +g = struct('axis', 0, 'tpa', []); +if isfield(gains, 'axis') && ~isempty(gains.axis), g.axis = gains.axis; end +if isfield(gains, 'tpa'), g.tpa = gains.tpa; end + +g.P = 1; g.I = 0; g.D = 1; g.F = 1; +[Ap, b.D1, b.F1] = PSbuildController(g, fp, Fs, b.freq); +b.Ap = Ap; + +g.P = 0; g.I = 1; g.D = 0; g.F = 0; +b.Ai = PSbuildController(g, fp, Fs, b.freq); + +end diff --git a/tests/test_PSautotuneBasis.m b/tests/test_PSautotuneBasis.m new file mode 100644 index 0000000..5e5a888 --- /dev/null +++ b/tests/test_PSautotuneBasis.m @@ -0,0 +1,71 @@ +% test_PSautotuneBasis.m - the controller family as four fixed responses + +%!test +%! % The whole scan rests on this: every gain enters PSbuildController as a +%! % plain scalar multiplier, so two calls reproduce any candidate. A nonlinear +%! % term added there later would corrupt every cell of the scan silently, so +%! % pin it against direct builds across axes and across the ends of the range. +%! Fs = 2000; freq = (0:0.4:1000)'; +%! fp = struct('dterm_lpf1_type',0,'dterm_lpf1_hz',80, ... +%! 'dterm_lpf2_type',3,'dterm_lpf2_hz',120, ... +%! 'dterm_notch_hz',350,'dterm_notch_cut',260); +%! tuples = [40 60 25 90; 1 0 0 0; 0 1 0 0; 120 200 55 0; 0 0 0 0]; +%! for axis = 0:2 +%! b = PSautotuneBasis(struct('axis', axis), fp, Fs, freq); +%! for r = 1:rows(tuples) +%! g = struct('axis', axis, 'P', tuples(r,1), 'I', tuples(r,2), ... +%! 'D', tuples(r,3), 'F', tuples(r,4)); +%! [A, D, F] = PSbuildController(g, fp, Fs, b.freq); +%! eA = max(abs(A - (g.P*b.Ap + g.I*b.Ai))) / max(1, max(abs(A))); +%! eD = max(abs(D - g.D*b.D1)) / max(1, max(abs(D))); +%! eF = max(abs(F - g.F*b.F1)) / max(1, max(abs(F))); +%! assert(eA < 1e-12, sprintf('axis %d row %d: A off by %g', axis, r, eA)); +%! assert(eD < 1e-12, sprintf('axis %d row %d: D off by %g', axis, r, eD)); +%! assert(eF < 1e-12, sprintf('axis %d row %d: F off by %g', axis, r, eF)); +%! end +%! end + +%!test +%! % DC is dropped, because there the integrator is Inf and a candidate with +%! % I = 0 would ask for 0*Inf +%! Fs = 2000; freq = (0:0.4:1000)'; +%! b = PSautotuneBasis(struct('axis',0), [], Fs, freq); +%! assert(all(b.freq > 0), 'DC must not reach the basis'); +%! assert(numel(b.freq) == numel(freq) - 1); +%! assert(isequal(freq(b.keep), b.freq), 'keep must map back onto the input grid'); +%! assert(all(isfinite(b.Ap)) && all(isfinite(b.Ai))); +%! assert(all(isfinite(b.D1)) && all(isfinite(b.F1))); +%! assert(all(isfinite(0*b.Ai)), 'an I-free candidate must stay finite'); + +%!test +%! % Yaw runs 2.5x the I gain (pid_init.c:378); losing that in the basis would +%! % make every yaw candidate wrong by the same factor and still look plausible +%! Fs = 2000; freq = (0:0.4:1000)'; +%! bR = PSautotuneBasis(struct('axis',0), [], Fs, freq); +%! bY = PSautotuneBasis(struct('axis',2), [], Fs, freq); +%! assert(max(abs(bY.Ai - 2.5*bR.Ai)) < 1e-12 * max(abs(bR.Ai))); +%! assert(max(abs(bY.Ap - bR.Ap)) < 1e-12 * max(1, max(abs(bR.Ap))), 'P is not scaled'); + +%!test +%! % The dterm filter chain lives inside the basis. A scan that assumed an ideal +%! % Kd*s would promise phase lead the hardware does not have - which is the +%! % whole risk behind moving D at all. +%! Fs = 2000; freq = (0:0.4:1000)'; +%! fp = struct('dterm_lpf1_type',0,'dterm_lpf1_hz',0, ... +%! 'dterm_lpf2_type',3,'dterm_lpf2_hz',120, ... +%! 'dterm_notch_hz',0,'dterm_notch_cut',0); +%! bF = PSautotuneBasis(struct('axis',0), fp, Fs, freq); +%! bN = PSautotuneBasis(struct('axis',0), [], Fs, freq); +%! k = find(bF.freq >= 200, 1); +%! assert(abs(bF.D1(k)) < 0.5 * abs(bN.D1(k)), 'PT3 at 120 Hz must show up at 200 Hz'); +%! j = find(bF.freq >= 5, 1); +%! assert(abs(bF.D1(j)) > 0.95 * abs(bN.D1(j)), 'and must not bite at 5 Hz'); + +%!test +%! % tpa carries through, because it scales P and D but never I +%! Fs = 2000; freq = (0:0.4:1000)'; +%! b1 = PSautotuneBasis(struct('axis',0), [], Fs, freq); +%! b2 = PSautotuneBasis(struct('axis',0,'tpa',0.6), [], Fs, freq); +%! assert(max(abs(b2.Ap - 0.6*b1.Ap)) < 1e-12 * max(1, max(abs(b1.Ap)))); +%! assert(max(abs(b2.D1 - 0.6*b1.D1)) < 1e-12 * max(abs(b1.D1))); +%! assert(max(abs(b2.Ai - b1.Ai)) < 1e-12 * max(abs(b1.Ai)), 'tpa must not touch I'); From 0d6fd350901579b19d0c426c00391fd84704270b Mon Sep 17 00:00:00 2001 From: Grzegorz Sterniczuk Date: Sat, 19 Sep 2026 23:58:29 +0200 Subject: [PATCH 08/33] feat(chirp): scan P and D for the widest loop the margins allow --- src/core/PSautotuneSearch.m | 199 ++++++++++++++++++++++++++++++++++ tests/test_PSautotuneSearch.m | 189 ++++++++++++++++++++++++++++++++ 2 files changed, 388 insertions(+) create mode 100644 src/core/PSautotuneSearch.m create mode 100644 tests/test_PSautotuneSearch.m diff --git a/src/core/PSautotuneSearch.m b/src/core/PSautotuneSearch.m new file mode 100644 index 0000000..285d963 --- /dev/null +++ b/src/core/PSautotuneSearch.m @@ -0,0 +1,199 @@ +function res = PSautotuneSearch(id, opt) +%% PSautotuneSearch - the P and D that widen the loop without spending its margins +% id - from PSidentifyChirp +% opt - thresholds, all exposed so they can be read off logs known to be good +% and known to be bad before anyone freezes them +% +% Maximises the 0 dB crossover under hard limits on phase margin, the +% sensitivity peak and the closed loop peak. The condition is PM >= target, not +% PM == target: on a discrete grid equality is brittle, and maximising +% bandwidth presses PM down onto the target by itself. When something other +% than PM binds, the answer comes out more conservative rather than not coming +% out at all. +% +% res.gains carries P, I, D and F and nothing else. Betaflight issue #5258 was +% a filter cutoff derived from chirp coherence; here there is nowhere to put +% one. + +if nargin < 2 || isempty(opt), opt = struct(); end + +o = struct('pmTarget', 60, 'msMax', 2.0, 'peakMaxDb', 6, ... + 'magAtTrustDb', -6, 'wcpFracTrust', 0.5, 'wcpMaxRatio', 3, ... + 'pClamp', [0.5 2.0], 'pStep', 0.02, ... + 'dClamp', [0.6 1.25], 'dStep', 0.05, 'pidsumFrac', 0.9); +fn = fieldnames(o); +for k = 1:numel(fn) + if isfield(opt, fn{k}) && ~isempty(opt.(fn{k})), o.(fn{k}) = opt.(fn{k}); end +end + +res = blank(); +res.opt = o; +res.notes = {}; + +P0 = id.gains.P; I0 = id.gains.I; D0 = id.gains.D; F0 = id.gains.F; +res.gains = struct('axis', id.gains.axis, 'P', P0, 'I', I0, 'D', D0, 'F', F0); + +% the loop may only be evaluated where the plant was measured +keep = id.freq > 0 & id.freq <= id.fTrust; +if sum(keep) < 16 + res.reason = 'no-candidate'; + res.notes{end+1} = 'trust band too narrow to evaluate a loop in'; + return +end +fk = id.freq(keep); +Pk = id.G_plant(keep); + +b = PSautotuneBasis(id.gains, id.fp, id.FsPid, fk); + +base = evalOne(Pk, fk, P0*b.Ap + I0*b.Ai, D0*b.D1, F0*b.F1); +res.pm0 = base.pm; res.gm0 = base.gm; res.ms0 = base.ms; +res.wcp0 = base.wcp; res.wcg0 = base.wcg; res.peakDb0 = base.peakDb; + +if isnan(base.wcp) + res.reason = 'no-crossover'; + res.notes{end+1} = 'the flown loop never crosses 0 dB inside the trust band'; + return +end + +dHi = o.dClamp(2); +if D0 > 0 && isfield(id.gains, 'dMax') && ~isempty(id.gains.dMax) && id.gains.dMax > D0 + % the log records the floor of a D that moved; the controller model only + % covers the static value, so raising it would be raising something else + dHi = min(dHi, 1.0); + res.notes{end+1} = 'dynamic D was active, so D is held at or below the logged value'; +end +if D0 > 0 && ~isempty(id.axisD) && isfinite(id.pidsumLimit) && id.pidsumLimit > 0 + peakD = max(abs(id.axisD)); + if peakD > 0 + sBudget = o.pidsumFrac * id.pidsumLimit / peakD; + if sBudget < dHi + dHi = max(o.dClamp(1), sBudget); + res.notes{end+1} = sprintf('D capped at %.2fx by the measured pidsum budget', dHi); + end + end +end + +sP = o.pClamp(1):o.pStep:o.pClamp(2); +sD = o.dClamp(1):o.dStep:dHi; +if isempty(sP), sP = 1; end +if isempty(sD), sD = min(1, dHi); end + +nD = numel(sD); nP = numel(sP); +g = struct(); +g.okMask = false(nD, nP); +g.pm = nan(nD, nP); g.ms = nan(nD, nP); g.wcp = nan(nD, nP); +g.gm = nan(nD, nP); g.peakDb = nan(nD, nP); g.nCross = zeros(nD, nP); +g.P = zeros(nD, nP); g.D = zeros(nD, nP); + +for iD = 1:nD + Di = round(sD(iD) * D0); + Dv = Di * b.D1; + for iP = 1:nP + Pi = round(sP(iP) * P0); + c = evalOne(Pk, fk, Pi*b.Ap + I0*b.Ai, Dv, F0*b.F1); + g.P(iD,iP) = Pi; g.D(iD,iP) = Di; + g.pm(iD,iP) = c.pm; g.ms(iD,iP) = c.ms; g.wcp(iD,iP) = c.wcp; + g.gm(iD,iP) = c.gm; g.peakDb(iD,iP) = c.peakDb; + g.nCross(iD,iP) = c.nCross; + g.okMask(iD,iP) = Pi >= 1 && admissible(c, o, id.fTrust, base.wcp); + end +end + +% A cell with no admissible neighbour is a one-point island: rounding to whole +% gains put it there and a neighbouring log would not reproduce it. Requiring +% every neighbour instead would back the answer off by one grid step, which +% would make the proposal depend on how finely the grid was drawn. +sel = g.okMask & (neighbourCount(g.okMask) >= 1 | numel(g.okMask) == 1); +if ~any(sel(:)) + res.reason = 'no-candidate'; + res.notes{end+1} = 'nothing on the grid clears the targets'; + res.grid = g; + return +end + +wcp = g.wcp; wcp(~sel) = -Inf; +best = max(wcp(:)); +tie = sel & (wcp >= best * 0.99); +[iDs, iPs] = find(tie); +kD = sD(iDs); kP = sP(iPs); +[~, pick] = sortrows([kD(:), kP(:)], [1 2]); +iD = iDs(pick(1)); iP = iPs(pick(1)); + +res.iD = iD; res.iP = iP; +res.grid = g; +res.gains.P = g.P(iD,iP); +res.gains.D = g.D(iD,iP); +res.scale = struct('P', ratio(res.gains.P, P0), 'I', 1, ... + 'D', ratio(res.gains.D, D0), 'F', 1); + +% report what the CLI will actually set, not what the scale factor found +fin = evalOne(Pk, fk, res.gains.P*b.Ap + res.gains.I*b.Ai, ... + res.gains.D*b.D1, res.gains.F*b.F1); +res.pm = fin.pm; res.gm = fin.gm; res.ms = fin.ms; +res.wcp = fin.wcp; res.wcg = fin.wcg; res.peakDb = fin.peakDb; + +res.ok = true; +if res.gains.P == P0 && res.gains.D == D0 + res.reason = 'already-tuned'; +else + res.reason = 'ok'; +end + +end + + +function ok = admissible(c, o, fTrust, wcp0) + ok = ~isnan(c.pm) && c.pm >= o.pmTarget ... + && c.ms <= o.msMax ... + && c.peakDb <= o.peakMaxDb ... + && c.nCross == 1 ... + && c.magTrustDb <= o.magAtTrustDb ... + && c.wcp <= o.wcpFracTrust * fTrust ... + && c.wcp <= o.wcpMaxRatio * wcp0 ... + && c.dDom; +end + + +function c = evalOne(P, freq, A, D, F) + [T, L, S] = PSpredictClosedLoop(P, A, D, F); + [c.gm, c.pm, c.wcg, c.wcp] = PSmarginsFromL(freq, L); + c.ms = max(abs(S)); + c.peakDb = 20*log10(max(abs(T)) + 1e-12); + magDb = 20*log10(abs(L) + 1e-12); + c.nCross = sum(diff(sign(magDb)) ~= 0); + c.magTrustDb = magDb(end); + % the crossover has to be held up by the PI part: a loop whose stability + % rests on the derivative rests on the least certain part of the model + c.dDom = true; + if ~isnan(c.wcp) + aW = interp1(freq, abs(A), c.wcp, 'linear'); + dW = interp1(freq, abs(D), c.wcp, 'linear'); + c.dDom = dW <= aW; + end +end + + +function n = neighbourCount(m) + n = zeros(size(m)); + n(2:end,:) = n(2:end,:) + m(1:end-1,:); + n(1:end-1,:) = n(1:end-1,:) + m(2:end,:); + n(:,2:end) = n(:,2:end) + m(:,1:end-1); + n(:,1:end-1) = n(:,1:end-1) + m(:,2:end); +end + + +function r = ratio(new, old) + r = 1; + if old ~= 0, r = new / old; end +end + + +function res = blank() + res = struct('ok', false, 'reason', 'no-candidate', ... + 'gains', [], 'scale', [], 'notes', {{}}, ... + 'pm', NaN, 'gm', NaN, 'ms', NaN, 'wcp', NaN, 'wcg', NaN, ... + 'peakDb', NaN, ... + 'pm0', NaN, 'gm0', NaN, 'ms0', NaN, 'wcp0', NaN, ... + 'wcg0', NaN, 'peakDb0', NaN, ... + 'iD', NaN, 'iP', NaN, 'grid', [], 'opt', []); +end diff --git a/tests/test_PSautotuneSearch.m b/tests/test_PSautotuneSearch.m new file mode 100644 index 0000000..1d403c8 --- /dev/null +++ b/tests/test_PSautotuneSearch.m @@ -0,0 +1,189 @@ +% test_PSautotuneSearch.m - the 2D scan over P and D +% +% Every expected number here comes from phase algebra on the fixture plant, not +% from a previous run of the scan. The fixture is an integrator with delay, +% P(f) = k*exp(-j*w*tau) / (j*w) +% for which proportional-only control gives exactly +% PM = 90 deg - w*tau*(180/pi) +% so a phase margin target fixes the crossover, and the crossover fixes P. + +%!function id = mkfix(varargin) +%! p = struct('k', 180.13977, 'tau', 0.002, 'P', 40, 'I', 0, 'D', 0, 'F', 0, ... +%! 'dMax', 0, 'fTrust', 300, 'fp', [], 'mode', 0, 'modeQ', 3, ... +%! 'axisD', 1, 'notch', 0); +%! for j = 1:2:numel(varargin), p.(varargin{j}) = varargin{j+1}; end +%! Fs = 2000; +%! freq = (0:0.2:600)'; +%! w = 2*pi*freq; s = 1j*w; +%! G = p.k * exp(-1j*w*p.tau) ./ (1j*w); +%! G(1) = 1e9; +%! if p.mode > 0 +%! wn = 2*pi*p.mode; +%! G = G ./ (1 + s/(p.modeQ*wn) + (s/wn).^2); +%! end +%! if p.notch > 0 +%! wz = 2*pi*p.notch; +%! G = G .* (s.^2 + wz^2) ./ (s.^2 + s*wz/3 + wz^2); +%! end +%! id = struct('ok', true, 'msg', '', 'freq', freq, 'G_track', [], ... +%! 'C_track', ones(size(freq)), 'G_plant', G, 'G_uw', [], ... +%! 'C_uw', [], 'G_ff', [], ... +%! 'gains', struct('axis',0,'P',p.P,'I',p.I,'D',p.D,'F',p.F,'dMax',p.dMax), ... +%! 'fp', p.fp, 'FsPid', Fs, 'axisIdx', 0, 'axisName', 'Roll', ... +%! 'i0', 1, 'i1', 40000, 'nSamp', 40000, 'durSec', 20, ... +%! 'gyroVar', 5e5, 'nSeg', 70, 'fTrust', p.fTrust, ... +%! 'axisD', p.axisD*ones(1000,1), 'pidsumLimit', 800); +%!endfunction + +%!test +%! % T1 - the target is hit where the algebra says it is. +%! % tau = 2 ms and a 50 deg target pin w*tau = 40 deg, so w = 349.066 rad/s and +%! % the exact P that lands there is 60.5. k is picked to put it half an integer +%! % from either neighbour, so the scan has to return 60 on the rule and not on +%! % a rounding accident. That P gives w = 346.18 rad/s, f = 55.10 Hz, PM 50.33. +%! % msMax is lifted here so that the phase margin is the only thing binding. +%! res = PSautotuneSearch(mkfix(), struct('pmTarget', 50, 'msMax', 3)); +%! assert(res.ok, res.reason); +%! assert(res.gains.P == 60, sprintf('expected P 60, got %d', res.gains.P)); +%! assert(abs(res.wcp - 55.096) < 0.3, sprintf('crossover %.2f Hz', res.wcp)); +%! assert(res.pm > 50 && res.pm < 50.5, sprintf('pm %.4f', res.pm)); + +%!test +%! % T2 - three targets, three answers, each fixed by the same algebra. The +%! % target sets w*tau, w*tau sets the crossover, and the crossover sets the +%! % exact P: 50 deg -> 40 deg -> 60.50; 60 deg -> 30 deg -> 45.38; +%! % 72.5 deg -> 17.5 deg -> 26.47. Only the integer below each clears. +%! got = zeros(1,3); +%! tgt = [50 60 72.5]; +%! for k = 1:3 +%! r = PSautotuneSearch(mkfix(), struct('pmTarget', tgt(k), 'msMax', 3)); +%! assert(r.ok, r.reason); +%! got(k) = r.gains.P; +%! end +%! assert(isequal(got, [60 45 26]), sprintf('got %d %d %d', got)); +%! assert(all(diff(got) < 0), 'a stricter target must never ask for more gain'); + +%!test +%! % T3 - a lightly damped mode above the crossover bends the Nyquist curve +%! % toward -1 without eating the phase margin, so Ms binds first. The answer +%! % then comes out well inside the phase target instead of spending it, and it +%! % is a different answer from T1 - otherwise this test proves nothing. +%! res = PSautotuneSearch(mkfix('mode', 110, 'modeQ', 3), struct('pmTarget', 50)); +%! assert(res.ok, res.reason); +%! assert(res.ms <= 2.0 + 1e-9, sprintf('Ms %.4f over the hard limit', res.ms)); +%! assert(res.ms > 1.9, sprintf('Ms %.4f is not what bound the answer', res.ms)); +%! assert(res.pm > 60, sprintf('pm %.2f - Ms should have bound well before PM', res.pm)); +%! assert(res.gains.P ~= 60, 'a resonant plant must not land on the T1 answer'); + +%!test +%! % T4 - an over-tuned loop has to be told to come down. Without this the scan +%! % could be written as "only ever raise" and still pass T1 to T3. +%! res = PSautotuneSearch(mkfix('P', 80), struct('pmTarget', 60)); +%! assert(res.ok, res.reason); +%! assert(res.scale.P < 1, sprintf('expected a cut, got scale %.3f', res.scale.P)); +%! assert(res.gains.P == 45, sprintf('expected P 45, got %d', res.gains.P)); + +%!test +%! % T5 - the #5258 barrier. A result that cannot name a filter cannot retune +%! % one. This is structural, so it is checked on every shape of input. +%! cases = {mkfix(), mkfix('P', 80), mkfix('mode', 45), ... +%! mkfix('D', 15, 'tau', 0.006), mkfix('I', 60), mkfix('fTrust', 120)}; +%! for k = 1:numel(cases) +%! r = PSautotuneSearch(cases{k}, struct('pmTarget', 60)); +%! assert(isequal(sort(fieldnames(r.gains)), sort({'axis';'P';'I';'D';'F'})), ... +%! sprintf('case %d: gains must carry exactly axis P I D F', k)); +%! assert(~isfield(r, 'fp'), sprintf('case %d: result must not carry filter params', k)); +%! fn = fieldnames(r); +%! for j = 1:numel(fn) +%! assert(isempty(strfind(lower(fn{j}), 'lpf')), 'no filter field may appear'); +%! assert(isempty(strfind(lower(fn{j}), 'notch')), 'no filter field may appear'); +%! assert(isempty(strfind(lower(fn{j}), 'dterm')), 'no filter field may appear'); +%! assert(isempty(strfind(lower(fn{j}), 'gyro')), 'no filter field may appear'); +%! end +%! end + +%!test +%! % T9 - the D dimension is worth having, and worth knowing by how much. +%! % With tau = 6 ms and P alone, PM = 60 pins w*tau = 30 deg, so the crossover +%! % sits at 13.89 Hz whatever P does: P moves magnitude, never phase. k here +%! % puts the exact P-only answer at 40.5, so the scan must return 40 and 13.72 +%! % Hz. Adding D moves it to 21.98 Hz. +%! % Unlimited lead would give 3.2x rather than 1.6x. It is capped because a +%! % cell is admissible only while |D| <= |A| at the crossover, which holds D's +%! % contribution to at most 45 deg of lead. That cap is the point: lead beyond +%! % it means stability resting on the derivative and its filters, which is the +%! % least certain part of the model. +%! kk = 67.2743; +%! o = struct('pmTarget', 60); +%! rP = PSautotuneSearch(mkfix('k', kk, 'tau', 0.006, 'P', 40, 'D', 0), o); +%! r2 = PSautotuneSearch(mkfix('k', kk, 'tau', 0.006, 'P', 40, 'D', 12), o); +%! assert(rP.ok, rP.reason); assert(r2.ok, r2.reason); +%! assert(abs(rP.wcp - 13.72) < 0.3, sprintf('P-only crossover %.2f Hz', rP.wcp)); +%! assert(r2.wcp > 1.5 * rP.wcp, ... +%! sprintf('2D crossover %.2f vs P-only %.2f Hz', r2.wcp, rP.wcp)); +%! assert(r2.gains.D ~= 12, 'the scan has to actually move D, not just leave it'); + +%!test +%! % T10 - and the D filter takes part of that lead straight back. A scan that +%! % assumed an ideal Kd*s would promise phase lead the hardware does not have, +%! % which is a flyaway derived from a linear model. Measured, strictly ordered: +%! % 13.72 Hz on P alone, 18.10 Hz with a PT1 at 30 Hz, 21.98 Hz unfiltered. +%! kk = 67.2743; +%! fpPT1 = struct('dterm_lpf1_type', 0, 'dterm_lpf1_hz', 30, ... +%! 'dterm_lpf2_type', 0, 'dterm_lpf2_hz', 0, ... +%! 'dterm_notch_hz', 0, 'dterm_notch_cut', 0); +%! o = struct('pmTarget', 60); +%! rP = PSautotuneSearch(mkfix('k',kk,'tau',0.006,'P',40,'D',0), o); +%! rFilt = PSautotuneSearch(mkfix('k',kk,'tau',0.006,'P',40,'D',12,'fp',fpPT1), o); +%! rNone = PSautotuneSearch(mkfix('k',kk,'tau',0.006,'P',40,'D',12), o); +%! assert(rNone.wcp > rFilt.wcp, ... +%! sprintf('unfiltered D must beat PT1-filtered D: %.1f vs %.1f Hz', ... +%! rNone.wcp, rFilt.wcp)); +%! assert(rFilt.wcp > rP.wcp, ... +%! sprintf('filtered D must still beat P alone: %.1f vs %.1f Hz', ... +%! rFilt.wcp, rP.wcp)); + +%!test +%! % T13 - D makes a second 0 dB crossing real, and PSmarginsFromL reports the +%! % first one. A cell with more than one crossing has a phase margin that means +%! % nothing, so it must never be chosen. +%! res = PSautotuneSearch(mkfix('notch', 30), struct('pmTarget', 50)); +%! assert(any(res.grid.nCross(:) > 1), 'this fixture must produce such cells'); +%! if res.ok +%! assert(res.grid.nCross(res.iD, res.iP) == 1, 'the chosen cell must cross once'); +%! end +%! bad = res.grid.nCross > 1; +%! assert(~any(res.grid.okMask(bad)), 'no multi-crossing cell may be admissible'); + +%!test +%! % The D noise budget is measured, not inferred: axisD is in the log and D +%! % scales it linearly, so the headroom against pidsum_limit is arithmetic. +%! % This is the opposite of what went wrong in #5258. +%! fix = mkfix('k', 67.2743, 'tau', 0.006, 'P', 40, 'D', 15, 'axisD', 700); +%! res = PSautotuneSearch(fix, struct('pmTarget', 60)); +%! assert(res.ok, res.reason); +%! assert(res.gains.D <= 15, 'raising D past the pidsum headroom is not allowed'); +%! assert(~isempty(strfind(lower(strjoin(res.notes, ' ')), 'pidsum')), ... +%! 'the budget must say when it bound'); + +%!test +%! % Reason codes have to be usable by the UI without guessing +%! r = PSautotuneSearch(mkfix('P', 40), struct('pmTarget', 60, 'pClamp', [1 1])); +%! assert(strcmp(r.reason, 'already-tuned') || strcmp(r.reason, 'ok')); +%! r2 = PSautotuneSearch(mkfix(), struct('pmTarget', 179)); +%! assert(~r2.ok); +%! assert(strcmp(r2.reason, 'no-candidate'), sprintf('reason was %s', r2.reason)); + +%!test +%! % Margins are recomputed from the integer gains that the CLI will actually +%! % set, not from the scale factor that found them +%! res = PSautotuneSearch(mkfix(), struct('pmTarget', 50)); +%! g = struct('axis', 0, 'P', res.gains.P, 'I', res.gains.I, ... +%! 'D', res.gains.D, 'F', res.gains.F); +%! id = mkfix(); +%! keep = id.freq > 0 & id.freq <= id.fTrust; +%! [A, D, F] = PSbuildController(g, id.fp, id.FsPid, id.freq(keep)); +%! [~, L] = PSpredictClosedLoop(id.G_plant(keep), A, D, F); +%! [gm, pm, wcg, wcp] = PSmarginsFromL(id.freq(keep), L); +%! assert(abs(pm - res.pm) < 1e-9, 'reported PM must come from the integer gains'); +%! assert(abs(wcp - res.wcp) < 1e-9, 'reported crossover likewise'); From 4c382e8011c58c40f860e047aefa970eb19da02d Mon Sep 17 00:00:00 2001 From: Grzegorz Sterniczuk Date: Sun, 20 Sep 2026 00:27:23 +0200 Subject: [PATCH 09/33] fix(chirp): set the D dominance limit from measured tunes, not from a guess --- src/core/PSautotuneSearch.m | 17 ++++++++++++----- 1 file changed, 12 insertions(+), 5 deletions(-) diff --git a/src/core/PSautotuneSearch.m b/src/core/PSautotuneSearch.m index 285d963..8c81782 100644 --- a/src/core/PSautotuneSearch.m +++ b/src/core/PSautotuneSearch.m @@ -19,6 +19,7 @@ o = struct('pmTarget', 60, 'msMax', 2.0, 'peakMaxDb', 6, ... 'magAtTrustDb', -6, 'wcpFracTrust', 0.5, 'wcpMaxRatio', 3, ... + 'dDomRatio', 2.0, ... 'pClamp', [0.5 2.0], 'pStep', 0.02, ... 'dClamp', [0.6 1.25], 'dStep', 0.05, 'pidsumFrac', 0.9); fn = fieldnames(o); @@ -83,6 +84,7 @@ g.okMask = false(nD, nP); g.pm = nan(nD, nP); g.ms = nan(nD, nP); g.wcp = nan(nD, nP); g.gm = nan(nD, nP); g.peakDb = nan(nD, nP); g.nCross = zeros(nD, nP); +g.magTrustDb = nan(nD, nP); g.dRatio = nan(nD, nP); g.P = zeros(nD, nP); g.D = zeros(nD, nP); for iD = 1:nD @@ -95,6 +97,7 @@ g.pm(iD,iP) = c.pm; g.ms(iD,iP) = c.ms; g.wcp(iD,iP) = c.wcp; g.gm(iD,iP) = c.gm; g.peakDb(iD,iP) = c.peakDb; g.nCross(iD,iP) = c.nCross; + g.magTrustDb(iD,iP) = c.magTrustDb; g.dRatio(iD,iP) = c.dRatio; g.okMask(iD,iP) = Pi >= 1 && admissible(c, o, id.fTrust, base.wcp); end end @@ -150,7 +153,7 @@ && c.magTrustDb <= o.magAtTrustDb ... && c.wcp <= o.wcpFracTrust * fTrust ... && c.wcp <= o.wcpMaxRatio * wcp0 ... - && c.dDom; + && c.dRatio <= o.dDomRatio; end @@ -162,13 +165,17 @@ magDb = 20*log10(abs(L) + 1e-12); c.nCross = sum(diff(sign(magDb)) ~= 0); c.magTrustDb = magDb(end); - % the crossover has to be held up by the PI part: a loop whose stability - % rests on the derivative rests on the least certain part of the model - c.dDom = true; + % How far the derivative may outweigh the PI part at the crossover. Lead + % beyond this rests the loop on the modelled D chain instead of on the + % measured plant. Flown Betaflight tunes sit higher here than one might + % guess: across the 18 axes of the pichim corpus the ratio runs 0.10 to + % 1.90, median 1.62, so a limit of 1 would reject every roll and pitch tune + % in it - including the craft of the author of the chirp tooling. + c.dRatio = 0; if ~isnan(c.wcp) aW = interp1(freq, abs(A), c.wcp, 'linear'); dW = interp1(freq, abs(D), c.wcp, 'linear'); - c.dDom = dW <= aW; + c.dRatio = dW / max(aW, 1e-12); end end From 23deb6ed024ce5271cba8152a97dc22e2ba93223 Mon Sep 17 00:00:00 2001 From: Grzegorz Sterniczuk Date: Sun, 20 Sep 2026 00:48:00 +0200 Subject: [PATCH 10/33] fix(chirp): scale I with P so cutting gain does not move the PI corner --- src/core/PSautotuneSearch.m | 18 ++++++++++++------ tests/test_PSautotuneSearch.m | 13 +++++++++++++ 2 files changed, 25 insertions(+), 6 deletions(-) diff --git a/src/core/PSautotuneSearch.m b/src/core/PSautotuneSearch.m index 8c81782..f6b236d 100644 --- a/src/core/PSautotuneSearch.m +++ b/src/core/PSautotuneSearch.m @@ -20,7 +20,7 @@ o = struct('pmTarget', 60, 'msMax', 2.0, 'peakMaxDb', 6, ... 'magAtTrustDb', -6, 'wcpFracTrust', 0.5, 'wcpMaxRatio', 3, ... 'dDomRatio', 2.0, ... - 'pClamp', [0.5 2.0], 'pStep', 0.02, ... + 'pClamp', [0.3 2.0], 'pStep', 0.02, ... 'dClamp', [0.6 1.25], 'dStep', 0.05, 'pidsumFrac', 0.9); fn = fieldnames(o); for k = 1:numel(fn) @@ -85,15 +85,20 @@ g.pm = nan(nD, nP); g.ms = nan(nD, nP); g.wcp = nan(nD, nP); g.gm = nan(nD, nP); g.peakDb = nan(nD, nP); g.nCross = zeros(nD, nP); g.magTrustDb = nan(nD, nP); g.dRatio = nan(nD, nP); -g.P = zeros(nD, nP); g.D = zeros(nD, nP); +g.P = zeros(nD, nP); g.I = zeros(nD, nP); g.D = zeros(nD, nP); for iD = 1:nD Di = round(sD(iD) * D0); Dv = Di * b.D1; for iP = 1:nP Pi = round(sP(iP) * P0); - c = evalOne(Pk, fk, Pi*b.Ap + I0*b.Ai, Dv, F0*b.F1); - g.P(iD,iP) = Pi; g.D(iD,iP) = Di; + % Ki in the firmware is absolute, so holding I while cutting P drags + % the PI corner upwards and the integrator adds lag exactly where the + % scan is trying to buy phase margin. Scaling both keeps the integral + % time the pilot flew. + Ii = round(sP(iP) * I0); + c = evalOne(Pk, fk, Pi*b.Ap + Ii*b.Ai, Dv, F0*b.F1); + g.P(iD,iP) = Pi; g.I(iD,iP) = Ii; g.D(iD,iP) = Di; g.pm(iD,iP) = c.pm; g.ms(iD,iP) = c.ms; g.wcp(iD,iP) = c.wcp; g.gm(iD,iP) = c.gm; g.peakDb(iD,iP) = c.peakDb; g.nCross(iD,iP) = c.nCross; @@ -125,8 +130,9 @@ res.iD = iD; res.iP = iP; res.grid = g; res.gains.P = g.P(iD,iP); +res.gains.I = g.I(iD,iP); res.gains.D = g.D(iD,iP); -res.scale = struct('P', ratio(res.gains.P, P0), 'I', 1, ... +res.scale = struct('P', ratio(res.gains.P, P0), 'I', ratio(res.gains.I, I0), ... 'D', ratio(res.gains.D, D0), 'F', 1); % report what the CLI will actually set, not what the scale factor found @@ -136,7 +142,7 @@ res.wcp = fin.wcp; res.wcg = fin.wcg; res.peakDb = fin.peakDb; res.ok = true; -if res.gains.P == P0 && res.gains.D == D0 +if res.gains.P == P0 && res.gains.I == I0 && res.gains.D == D0 res.reason = 'already-tuned'; else res.reason = 'ok'; diff --git a/tests/test_PSautotuneSearch.m b/tests/test_PSautotuneSearch.m index 1d403c8..4438f4c 100644 --- a/tests/test_PSautotuneSearch.m +++ b/tests/test_PSautotuneSearch.m @@ -166,6 +166,19 @@ %! assert(~isempty(strfind(lower(strjoin(res.notes, ' ')), 'pidsum')), ... %! 'the budget must say when it bound'); +%!test +%! % I moves with P, because Ki in the firmware is absolute. Holding I while +%! % cutting P drags the PI corner upwards, so the integrator contributes more +%! % lag exactly where the scan is trying to buy phase margin. Measured on the +%! % pichim corpus: with I held, one craft tops out at 45.2 deg of reachable +%! % phase margin however far P is cut, and clears 69.9 deg once I follows. +%! res = PSautotuneSearch(mkfix('P', 80, 'I', 120), struct('pmTarget', 60)); +%! assert(res.ok, res.reason); +%! assert(res.gains.I ~= 120, 'I has to move when P does'); +%! assert(abs(res.scale.I - res.scale.P) < 0.02, ... +%! sprintf('I scaled %.3f against P %.3f - the integral time must hold', ... +%! res.scale.I, res.scale.P)); + %!test %! % Reason codes have to be usable by the UI without guessing %! r = PSautotuneSearch(mkfix('P', 40), struct('pmTarget', 60, 'pClamp', [1 1])); From 479ef70e945a7e37057b5ad01ff654fee65a2a50 Mon Sep 17 00:00:00 2001 From: Grzegorz Sterniczuk Date: Sun, 20 Sep 2026 08:18:40 +0200 Subject: [PATCH 11/33] fix(parser): read the log section number from the csv name, not its last two chars --- src/core/PSimport.m | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) diff --git a/src/core/PSimport.m b/src/core/PSimport.m index af9424d..adf41cc 100644 --- a/src/core/PSimport.m +++ b/src/core/PSimport.m @@ -51,7 +51,19 @@ end end -relevantLogNum=str2double(csvFname(end-5:end-4)); +% the decoder names each section .NN.csv, but also drops a .NN.gps.csv +% beside it; reading the number off the last two characters turns that into NaN +[~, csvBase] = fileparts(csvFname); +tok = regexp(csvBase, '\.(\d+)$', 'tokens'); +if isempty(tok) + error('PSimport: cannot tell which log section "%s" belongs to', csvBase); +end +relevantLogNum = str2double(tok{1}{1}); +nSections = min(numel(logStartPoints), numel(logEndPoints)); +if relevantLogNum < 1 || relevantLogNum > nSections + error('PSimport: asked for log section %d but "%s" holds %d', ... + relevantLogNum, BBLFileName, nSections); +end s=c{1}(logStartPoints(relevantLogNum):logEndPoints(relevantLogNum)); n=1; for m=1:size(s,1) From 7dc0133e881cb7544c0a60ad1a349b1f72b4c4c1 Mon Sep 17 00:00:00 2001 From: Grzegorz Sterniczuk Date: Sun, 20 Sep 2026 08:24:13 +0200 Subject: [PATCH 12/33] feat(chirp): carry the sweep, saturation and throttle scores through identification --- src/core/PSidentifyChirp.m | 6 +++++- tests/test_PSidentifyChirp.m | 10 +++++++++- 2 files changed, 14 insertions(+), 2 deletions(-) diff --git a/src/core/PSidentifyChirp.m b/src/core/PSidentifyChirp.m index e0a03db..015ba92 100644 --- a/src/core/PSidentifyChirp.m +++ b/src/core/PSidentifyChirp.m @@ -60,7 +60,10 @@ k = find([w.i0] == i0, 1); if ~isempty(k) id.durSec = w(k).durSec; + id.chirpTime = w(k).chirpTime; id.gyroVar = w(k).gyroVar; + id.satFrac = w(k).satFrac; + id.thrStd = w(k).thrStd; id.pidsumLimit = w(k).pidsumLimit; end @@ -110,6 +113,7 @@ 'gains', [], 'fp', [], 'FsPid', [], ... 'axisIdx', [], 'axisName', '', ... 'i0', [], 'i1', [], 'nSamp', [], 'durSec', NaN, ... - 'gyroVar', NaN, 'nSeg', 0, 'fTrust', NaN, ... + 'chirpTime', NaN, 'gyroVar', NaN, 'satFrac', NaN, ... + 'thrStd', NaN, 'nSeg', 0, 'fTrust', NaN, ... 'axisD', [], 'pidsumLimit', NaN); end diff --git a/tests/test_PSidentifyChirp.m b/tests/test_PSidentifyChirp.m index b71a89b..02b9de0 100644 --- a/tests/test_PSidentifyChirp.m +++ b/tests/test_PSidentifyChirp.m @@ -60,7 +60,8 @@ %! % array even when an axis refuses %! want = {'ok','msg','freq','G_track','C_track','G_plant','G_uw','C_uw', ... %! 'G_ff','gains','fp','FsPid','axisIdx','axisName','i0','i1', ... -%! 'nSamp','durSec','gyroVar','nSeg','fTrust','axisD','pidsumLimit'}; +%! 'nSamp','durSec','chirpTime','gyroVar','satFrac','thrStd', ... +%! 'nSeg','fTrust','axisD','pidsumLimit'}; %! good = PSidentifyChirp(T, si, Fs, tIND, 0); %! bad = PSidentifyChirp(struct('gyroADC_0_', zeros(10,1)), si, Fs, true(10,1), 0); %! assert(isempty(setdiff(want, fieldnames(good))), 'a field the plan names is missing'); @@ -122,4 +123,11 @@ %! assert(id.pidsumLimit == 800); %! assert(id.gains.P == 45); %! assert(id.FsPid == 2000); +%! % the gates judge the measurement, and these three are the measurement: +%! % whether the sweep finished, whether the loop stayed linear, whether the +%! % throttle held still. Recomputing them later would mean finding the window +%! % twice. +%! assert(id.chirpTime == 20); +%! assert(id.satFrac == 0, 'a clean simulation cannot be saturated'); +%! assert(id.thrStd == 0, 'the simulated throttle is constant'); %! assert(numel(id.axisD) == id.nSamp, 'axisD is the window, ready for the noise budget'); From 1b95987056f5429090cca6608985fd6fd71b4b76 Mon Sep 17 00:00:00 2001 From: Grzegorz Sterniczuk Date: Sun, 20 Sep 2026 08:24:13 +0200 Subject: [PATCH 13/33] feat(chirp): refuse logs that cannot be tuned from, with thresholds read off real logs --- src/core/PSautotuneGates.m | 128 +++++++++++++++++++++++++++++++++++ tests/test_PSautotuneGates.m | 125 ++++++++++++++++++++++++++++++++++ 2 files changed, 253 insertions(+) create mode 100644 src/core/PSautotuneGates.m create mode 100644 tests/test_PSautotuneGates.m diff --git a/src/core/PSautotuneGates.m b/src/core/PSautotuneGates.m new file mode 100644 index 0000000..c16691a --- /dev/null +++ b/src/core/PSautotuneGates.m @@ -0,0 +1,128 @@ +function [ok, msgs, info] = PSautotuneGates(id, opt) +%% PSautotuneGates - whether a measurement is fit to tune from +% id - from PSidentifyChirp +% opt - threshold overrides; every threshold is echoed in info +% +% ok - false when the axis must be refused +% msgs - what to tell the pilot; non-empty with ok true means a warning +% info - the thresholds that were applied, plus what the log actually scored +% +% These judge the measurement. The scan's own constraints judge the loop, and +% the two must not be confused: a log can be perfectly measured and still have +% no admissible tune, which is a different answer from "this log cannot be +% read". +% +% The numbers come from the pichim corpus - 18 axes across 6 logs from 3 +% airframes - except where marked structural, meaning the log cannot mean +% anything else. + +if nargin < 2 || isempty(opt), opt = struct(); end + +o = struct('gyroVarMin', 1e4, ... % swept axes score 3.6e5..5.8e5, coupling reaches 1365 + 'sweepFrac', 0.95, ... % structural: a short sweep has no top of band + 'satFracMax', 0.01, ... % structural: saturation makes the plant a fiction + 'nSegMin', 8, ... % the corpus runs 70 on every axis + 'cohMin', 0.78, ... % the corpus sits at 0.999 median + 'fTrustMin', 60, ... % the corpus runs 106..164 Hz + 'FsPidRange', [500 16000]); +fn = fieldnames(o); +for k = 1:numel(fn) + if isfield(opt, fn{k}) && ~isempty(opt.(fn{k})), o.(fn{k}) = opt.(fn{k}); end +end + +info = o; +info.dClampHi = 1.25; +info.gyroVar = getf(id, 'gyroVar'); +info.satFrac = getf(id, 'satFrac'); +info.thrStd = getf(id, 'thrStd'); % carried, not gated: no failing log measured yet +info.nSeg = getf(id, 'nSeg'); +info.fTrust = getf(id, 'fTrust'); +info.cohMedian = NaN; + +msgs = {}; +ok = false; + +if ~isfield(id, 'ok') || ~id.ok + msgs{end+1} = id.msg; + return +end + +if isempty(id.G_plant) + msgs{end+1} = 'The PID terms are not in this log, so the plant cannot be measured'; + return +end + +if ~(id.gains.P > 0 && id.gains.I > 0) + msgs{end+1} = 'P and I must both be non-zero for a gain to be scaled'; + return +end + +if ~(id.FsPid >= o.FsPidRange(1) && id.FsPid <= o.FsPidRange(2)) + msgs{end+1} = sprintf('Loop rate reads %g Hz, which is outside %g..%g', ... + id.FsPid, o.FsPidRange(1), o.FsPidRange(2)); + return +end + +if ~(info.gyroVar > o.gyroVarMin) + msgs{end+1} = sprintf(['This axis was not swept in the run that was picked ' ... + '(gyro variance %.0f, needs %.0f)'], info.gyroVar, o.gyroVarMin); + return +end + +if ~(id.durSec >= o.sweepFrac * id.chirpTime) + msgs{end+1} = sprintf('The sweep stopped after %.1f s of %.1f s, so the top of the band is missing', ... + id.durSec, id.chirpTime); + return +end + +if ~(info.satFrac < o.satFracMax) + msgs{end+1} = sprintf(['The PID sum saturated for %.1f%% of the run, so the loop ' ... + 'was not linear and the plant measured through it is not real'], ... + 100*info.satFrac); + return +end + +if ~(info.nSeg >= o.nSegMin) + msgs{end+1} = sprintf('Only %d segments to average, needs %d', info.nSeg, o.nSegMin); + return +end + +if ~(info.fTrust >= o.fTrustMin) + msgs{end+1} = sprintf('The trusted band ends at %.0f Hz, needs %.0f', ... + info.fTrust, o.fTrustMin); + return +end + +% the plant is G_track/G_uw, so judge it on the worse of the two. The upper edge +% follows fTrust, otherwise this and the trust band gate measure the same thing +% twice and on a 107 Hz log this one becomes unreachable. +hi = min(150, info.fTrust); +band = id.freq > 2 & id.freq <= hi; +if any(band) + C = id.C_track; + if ~isempty(id.C_uw), C = min(id.C_track, id.C_uw); end + info.cohMedian = median(C(band)); +end +if ~(info.cohMedian >= o.cohMin) + msgs{end+1} = sprintf('Plant coherence is %.2f over 2..%.0f Hz, needs %.2f', ... + info.cohMedian, hi, o.cohMin); + return +end + +ok = true; + +% dynamic D is a warning: the log records the floor of a D that moved, and the +% controller model only covers the static value +if isfield(id.gains, 'dMax') && ~isempty(id.gains.dMax) && id.gains.dMax > id.gains.D + info.dClampHi = 1.0; + msgs{end+1} = sprintf(['Dynamic D was active (d_max %g above D %g), so D is held ' ... + 'at or below what was flown'], id.gains.dMax, id.gains.D); +end + +end + + +function v = getf(s, f) + v = NaN; + if isfield(s, f) && ~isempty(s.(f)), v = s.(f); end +end diff --git a/tests/test_PSautotuneGates.m b/tests/test_PSautotuneGates.m new file mode 100644 index 0000000..1d33aa6 --- /dev/null +++ b/tests/test_PSautotuneGates.m @@ -0,0 +1,125 @@ +% test_PSautotuneGates.m - what has to be true of a log before autotune may answer +% +% The gates judge the measurement; the scan's constraints judge the loop. Every +% threshold with a number in it is either structural (the log cannot mean +% anything else) or was read off the pichim corpus - 18 axes over 6 logs from 3 +% airframes - and the reading is written next to it. + +%!function id = mkid(varargin) +%! p = struct('P', 46, 'I', 66, 'D', 32, 'F', 0, 'dMax', 0, ... +%! 'ok', true, 'msg', '', 'nSeg', 70, 'fTrust', 125, ... +%! 'gyroVar', 5e5, 'satFrac', 0, 'durSec', 20, 'chirpTime', 20, ... +%! 'thrStd', 8, 'coh', 0.99, 'FsPid', 2000); +%! for j = 1:2:numel(varargin), p.(varargin{j}) = varargin{j+1}; end +%! freq = (0:0.4:400)'; +%! id = struct('ok', p.ok, 'msg', p.msg, 'freq', freq, ... +%! 'G_track', ones(size(freq)), ... +%! 'C_track', p.coh*ones(size(freq)), ... +%! 'G_plant', ones(size(freq)), 'G_uw', ones(size(freq)), ... +%! 'C_uw', p.coh*ones(size(freq)), 'G_ff', [], ... +%! 'gains', struct('axis',0,'P',p.P,'I',p.I,'D',p.D,'F',p.F,'dMax',p.dMax), ... +%! 'fp', [], 'FsPid', p.FsPid, 'axisIdx', 0, 'axisName', 'Roll', ... +%! 'i0', 1, 'i1', 40000, 'nSamp', 40000, 'durSec', p.durSec, ... +%! 'chirpTime', p.chirpTime, 'gyroVar', p.gyroVar, ... +%! 'satFrac', p.satFrac, 'thrStd', p.thrStd, ... +%! 'nSeg', p.nSeg, 'fTrust', p.fTrust, 'axisD', ones(100,1), ... +%! 'pidsumLimit', 800); +%!endfunction + +%!test +%! % A log like the ones in the corpus passes every gate +%! [ok, msgs] = PSautotuneGates(mkid()); +%! assert(ok, sprintf('a good log was refused: %s', strjoin(msgs, '; '))); +%! assert(isempty(msgs)); + +%!test +%! % Identification having failed is itself a refusal, and its reason carries +%! % through rather than being replaced by a gate's wording +%! [ok, msgs] = PSautotuneGates(mkid('ok', false, 'msg', 'No chirp data found')); +%! assert(~ok); +%! assert(~isempty(strfind(strjoin(msgs, ' '), 'No chirp data found'))); + +%!test +%! % An axis that was not swept. Measured on the corpus: the swept axis scores +%! % 3.6e5 to 5.8e5, while cross-axis coupling during another axis's run reaches +%! % 1365 - so the old floor of 500 sat inside the coupling, not above it. +%! [ok, msgs] = PSautotuneGates(mkid('gyroVar', 1365)); +%! assert(~ok, 'cross-axis coupling must not read as a swept axis'); +%! assert(~isempty(strfind(lower(strjoin(msgs, ' ')), 'swept'))); +%! [ok2, ~] = PSautotuneGates(mkid('gyroVar', 3.6e5)); +%! assert(ok2, 'a genuinely swept axis must pass'); + +%!test +%! % A sweep that was cut short never reached the top of the band, and fTrust +%! % off it still looks perfectly reasonable - which is what makes it dangerous +%! [ok, msgs] = PSautotuneGates(mkid('durSec', 11, 'chirpTime', 20)); +%! assert(~ok); +%! assert(~isempty(strfind(lower(strjoin(msgs, ' ')), 'sweep'))); +%! [ok2, ~] = PSautotuneGates(mkid('durSec', 19.5, 'chirpTime', 20)); +%! assert(ok2, '0.95 of the sweep is enough'); + +%!test +%! % Saturation means the loop was not linear, so the plant measured through it +%! % is a fiction. Every axis in the corpus measured exactly zero, so any real +%! % fraction is already abnormal. +%! [ok, msgs] = PSautotuneGates(mkid('satFrac', 0.02)); +%! assert(~ok); +%! assert(~isempty(strfind(lower(strjoin(msgs, ' ')), 'satur'))); + +%!test +%! % Too few Welch segments to average. The corpus runs 70 on every axis, so 3 +%! % means something went wrong with the window rather than with the flying. +%! [ok, msgs] = PSautotuneGates(mkid('nSeg', 3)); +%! assert(~ok); +%! assert(~isempty(strfind(lower(strjoin(msgs, ' ')), 'average'))); + +%!test +%! % A trust band too narrow to hold a loop. The corpus runs 106 to 164 Hz. +%! [ok, msgs] = PSautotuneGates(mkid('fTrust', 30)); +%! assert(~ok); +%! assert(~isempty(strfind(lower(strjoin(msgs, ' ')), 'band'))); + +%!test +%! % Coherence gate: the corpus sits at 0.999 median, so 0.5 is plainly broken +%! [ok, msgs] = PSautotuneGates(mkid('coh', 0.5)); +%! assert(~ok); +%! assert(~isempty(strfind(lower(strjoin(msgs, ' ')), 'coherence'))); +%! [ok2, ~] = PSautotuneGates(mkid('coh', 0.9)); +%! assert(ok2); + +%!test +%! % Gains that cannot be scaled +%! [okP, mP] = PSautotuneGates(mkid('P', 0)); +%! assert(~okP); +%! [okI, mI] = PSautotuneGates(mkid('I', 0)); +%! assert(~okI); +%! assert(~isempty(strfind(lower(strjoin([mP mI], ' ')), 'gain'))); + +%!test +%! % Dynamic D is a warning, not a refusal - and it has to reach the scan as a +%! % clamp, because the model only covers the static value +%! [ok, msgs, info] = PSautotuneGates(mkid('D', 20, 'dMax', 35)); +%! assert(ok, 'dynamic D must not refuse the axis'); +%! assert(~isempty(msgs), 'but it must warn'); +%! assert(~isempty(strfind(lower(strjoin(msgs, ' ')), 'dynamic'))); +%! assert(info.dClampHi == 1.0, 'and hold D at or below what was flown'); +%! [~, ~, info2] = PSautotuneGates(mkid('D', 20, 'dMax', 0)); +%! assert(info2.dClampHi > 1.0, 'static D keeps the normal headroom'); + +%!test +%! % An implausible loop rate means the header was misread, and everything +%! % derived from it - the whole controller model - is wrong with it +%! [ok, msgs] = PSautotuneGates(mkid('FsPid', 60)); +%! assert(~ok); +%! assert(~isempty(strfind(lower(strjoin(msgs, ' ')), 'loop rate'))); + +%!test +%! % Thresholds are echoed so they can be read back off a log rather than +%! % guessed at again later +%! [~, ~, info] = PSautotuneGates(mkid()); +%! for f = {'gyroVarMin','satFracMax','nSegMin','fTrustMin','cohMin','sweepFrac'} +%! assert(isfield(info, f{1}), sprintf('info must echo %s', f{1})); +%! end +%! % and they can be overridden, so a stricter caller is possible without an edit +%! [ok, ~] = PSautotuneGates(mkid('fTrust', 80), struct('fTrustMin', 100)); +%! assert(~ok, 'a caller-supplied threshold must actually bind'); From 47ea5ee282bb7581d246af1c9b3c05f4479ecd88 Mon Sep 17 00:00:00 2001 From: Grzegorz Sterniczuk Date: Sun, 20 Sep 2026 08:26:58 +0200 Subject: [PATCH 14/33] refactor(filter-sim): hoist the clipboard helpers so other tools can use them --- src/plot/PSfilterSim.m | 41 ++---------------------------------- src/util/PScopyToClipboard.m | 25 ++++++++++++++++++++++ src/util/PSshowCLIDialog.m | 20 ++++++++++++++++++ 3 files changed, 47 insertions(+), 39 deletions(-) create mode 100644 src/util/PScopyToClipboard.m create mode 100644 src/util/PSshowCLIDialog.m diff --git a/src/plot/PSfilterSim.m b/src/plot/PSfilterSim.m index fddef2c..c590370 100644 --- a/src/plot/PSfilterSim.m +++ b/src/plot/PSfilterSim.m @@ -933,11 +933,11 @@ function copyCLI() lines{end+1} = sprintf('set dterm_notch_cutoff = %d', readEdit(h.dn_cut)); lines{end+1} = 'save'; cliText = strjoin(lines, char(10)); - ok = copyToClipboard(cliText); + ok = PScopyToClipboard(cliText); if ok set(h.copyCLI, 'String', 'Copied!'); else - showCLIDialog(cliText); + PSshowCLIDialog(cliText); end end @@ -1209,40 +1209,3 @@ function drawGradientBar(ax, fMax, gradCols, markerFreqs, markerCols, thm) y = sin(phase); end -function ok = copyToClipboard(str) - ok = false; - tmpf = [tempname '.txt']; - fid = fopen(tmpf, 'w'); fprintf(fid, '%s', str); fclose(fid); - if ismac() - [st, ~] = system(sprintf('pbcopy < %s 2>&1', tmpf)); - ok = (st == 0); - elseif ispc() - [st, ~] = system(sprintf('clip < %s 2>&1', tmpf)); - ok = (st == 0); - else - cmds = {'xclip -selection clipboard', 'xsel --clipboard --input', 'wl-copy'}; - for k = 1:numel(cmds) - [st, ~] = system(sprintf('%s < %s 2>&1', cmds{k}, tmpf)); - if st == 0, ok = true; break; end - end - end - delete(tmpf); -end - -function showCLIDialog(cliText) - screensz = get(0, 'ScreenSize'); - dlg = figure('Name', 'BF CLI Commands', 'NumberTitle', 'off', ... - 'Color', [.15 .15 .15], ... - 'Position', round([screensz(3)*.3 screensz(4)*.25 460 380])); - uicontrol(dlg, 'Style', 'text', 'String', 'Select All + Copy:', ... - 'Units', 'normalized', 'Position', [.05 .90 .9 .07], ... - 'FontSize', 11, 'BackgroundColor', [.15 .15 .15], 'ForegroundColor', [.9 .9 .9]); - uicontrol(dlg, 'Style', 'edit', 'Max', 100, 'String', cliText, ... - 'Units', 'normalized', 'Position', [.05 .12 .9 .76], ... - 'HorizontalAlignment', 'left', 'FontName', 'Monospace', 'FontSize', 10, ... - 'BackgroundColor', [.1 .1 .1], 'ForegroundColor', [.9 .9 .9]); - uicontrol(dlg, 'Style', 'pushbutton', 'String', 'Close', ... - 'Units', 'normalized', 'Position', [.35 .02 .3 .08], ... - 'FontSize', 11, 'BackgroundColor', [.3 .3 .3], 'ForegroundColor', [.9 .9 .9], ... - 'Callback', @(~,~) close(dlg)); -end diff --git a/src/util/PScopyToClipboard.m b/src/util/PScopyToClipboard.m new file mode 100644 index 0000000..d5d18b7 --- /dev/null +++ b/src/util/PScopyToClipboard.m @@ -0,0 +1,25 @@ +function ok = PScopyToClipboard(str) +%% PScopyToClipboard - put text on the system clipboard +% Returns false when no clipboard helper is available, which is the normal +% case on a bare X session - the caller is expected to fall back to showing +% the text. + +ok = false; +tmpf = [tempname '.txt']; +fid = fopen(tmpf, 'w'); fprintf(fid, '%s', str); fclose(fid); +if ismac() + [st, ~] = system(sprintf('pbcopy < %s 2>&1', tmpf)); + ok = (st == 0); +elseif ispc() + [st, ~] = system(sprintf('clip < %s 2>&1', tmpf)); + ok = (st == 0); +else + cmds = {'xclip -selection clipboard', 'xsel --clipboard --input', 'wl-copy'}; + for k = 1:numel(cmds) + [st, ~] = system(sprintf('%s < %s 2>&1', cmds{k}, tmpf)); + if st == 0, ok = true; break; end + end +end +delete(tmpf); + +end diff --git a/src/util/PSshowCLIDialog.m b/src/util/PSshowCLIDialog.m new file mode 100644 index 0000000..ae41837 --- /dev/null +++ b/src/util/PSshowCLIDialog.m @@ -0,0 +1,20 @@ +function PSshowCLIDialog(cliText) +%% PSshowCLIDialog - show CLI text for hand copying when the clipboard is not reachable + +screensz = get(0, 'ScreenSize'); +dlg = figure('Name', 'BF CLI Commands', 'NumberTitle', 'off', ... + 'Color', [.15 .15 .15], ... + 'Position', round([screensz(3)*.3 screensz(4)*.25 460 380])); +uicontrol(dlg, 'Style', 'text', 'String', 'Select All + Copy:', ... + 'Units', 'normalized', 'Position', [.05 .90 .9 .07], ... + 'FontSize', 11, 'BackgroundColor', [.15 .15 .15], 'ForegroundColor', [.9 .9 .9]); +uicontrol(dlg, 'Style', 'edit', 'Max', 100, 'String', cliText, ... + 'Units', 'normalized', 'Position', [.05 .12 .9 .76], ... + 'HorizontalAlignment', 'left', 'FontName', 'Monospace', 'FontSize', 10, ... + 'BackgroundColor', [.1 .1 .1], 'ForegroundColor', [.9 .9 .9]); +uicontrol(dlg, 'Style', 'pushbutton', 'String', 'Close', ... + 'Units', 'normalized', 'Position', [.35 .02 .3 .08], ... + 'FontSize', 11, 'BackgroundColor', [.3 .3 .3], 'ForegroundColor', [.9 .9 .9], ... + 'Callback', @(~,~) close(dlg)); + +end From 7b9f594ba5d87e6c6d25233b9bfab9d6d5b00ce6 Mon Sep 17 00:00:00 2001 From: Grzegorz Sterniczuk Date: Sun, 20 Sep 2026 08:28:36 +0200 Subject: [PATCH 15/33] feat(chirp): build the CLI text, with no way to emit a filter setting --- src/util/PSautotuneCLI.m | 40 ++++++++++++++++ tests/test_PSautotuneCLI.m | 94 ++++++++++++++++++++++++++++++++++++++ 2 files changed, 134 insertions(+) create mode 100644 src/util/PSautotuneCLI.m create mode 100644 tests/test_PSautotuneCLI.m diff --git a/src/util/PSautotuneCLI.m b/src/util/PSautotuneCLI.m new file mode 100644 index 0000000..dfe71ac --- /dev/null +++ b/src/util/PSautotuneCLI.m @@ -0,0 +1,40 @@ +function txt = PSautotuneCLI(items, pmTarget) +%% PSautotuneCLI - Betaflight CLI text for a set of autotune results +% items - one struct per axis with .axisName, .ok, .gains (P,I,D,F) and +% .note (why it was refused, when it was) +% pmTarget - the phase margin the proposal was made for, recorded in the text +% +% Only PID gains are ever emitted. A refused axis is named with its reason +% rather than skipped silently, because silence reads as "nothing to change". + +axKeys = struct('Roll', 'roll', 'Pitch', 'pitch', 'Yaw', 'yaw'); + +lines = {}; +lines{end+1} = sprintf('# PIDscope autotune - phase margin target %g deg', pmTarget); +lines{end+1} = '# Verify on a test flight before trusting these in anger.'; + +nSet = 0; +for k = 1:numel(items) + it = items(k); + name = it.axisName; + if ~isfield(axKeys, name), continue; end + key = axKeys.(name); + if ~it.ok + lines{end+1} = sprintf('# %s: refused - %s', name, it.note); + continue + end + lines{end+1} = sprintf('# %s', name); + lines{end+1} = sprintf('set p_%s = %d', key, round(it.gains.P)); + lines{end+1} = sprintf('set i_%s = %d', key, round(it.gains.I)); + lines{end+1} = sprintf('set d_%s = %d', key, round(it.gains.D)); + lines{end+1} = sprintf('set f_%s = %d', key, round(it.gains.F)); + nSet = nSet + 1; +end + +if nSet > 0 + lines{end+1} = 'save'; +end + +txt = strjoin(lines, char(10)); + +end diff --git a/tests/test_PSautotuneCLI.m b/tests/test_PSautotuneCLI.m new file mode 100644 index 0000000..f1b50d6 --- /dev/null +++ b/tests/test_PSautotuneCLI.m @@ -0,0 +1,94 @@ +% test_PSautotuneCLI.m - the CLI text handed to the pilot + +%!function it = mkit(name, ok, P, I, D, F, note) +%! it = struct('axisName', name, 'ok', ok, ... +%! 'gains', struct('P',P,'I',I,'D',D,'F',F), 'note', note); +%!endfunction + +%!test +%! % Three tuned axes produce exactly the lines Betaflight expects, and nothing +%! % else that looks like a setting +%! items = [mkit('Roll', true, 30, 43, 27, 0, ''), ... +%! mkit('Pitch', true, 45, 67, 42, 0, ''), ... +%! mkit('Yaw', true, 59, 128, 2, 0, '')]; +%! txt = PSautotuneCLI(items, 50); +%! lines = strsplit(txt, char(10)); +%! for want = {'set p_roll = 30', 'set i_roll = 43', 'set d_roll = 27', ... +%! 'set f_roll = 0', 'set p_pitch = 45', 'set i_pitch = 67', ... +%! 'set d_pitch = 42', 'set f_pitch = 0', 'set p_yaw = 59', ... +%! 'set i_yaw = 128', 'set d_yaw = 2', 'set f_yaw = 0', 'save'} +%! assert(any(strcmp(lines, want{1})), sprintf('missing line: %s', want{1})); +%! end +%! assert(strcmp(strtrim(lines{end}), 'save'), 'the last line must be save'); + +%!test +%! % Every line is either a comment, one of the twelve settings, or save. A +%! % stray line is a stray command in someone's flight controller. +%! items = [mkit('Roll', true, 30, 43, 27, 0, ''), ... +%! mkit('Pitch', false, 0,0,0,0, 'sweep stopped after 11 s'), ... +%! mkit('Yaw', true, 59, 128, 2, 0, '')]; +%! txt = PSautotuneCLI(items, 60); +%! lines = strsplit(txt, char(10)); +%! for k = 1:numel(lines) +%! L = strtrim(lines{k}); +%! if isempty(L), continue; end +%! okLine = L(1) == '#' || strcmp(L, 'save') || ... +%! ~isempty(regexp(L, '^set [pidf]_(roll|pitch|yaw) = -?\d+$', 'once')); +%! assert(okLine, sprintf('unexpected line: %s', L)); +%! end + +%!test +%! % A refused axis is named, with its reason, and gets no settings at all - +%! % silence would look like "nothing to change" rather than "I could not tell" +%! items = [mkit('Roll', true, 30, 43, 27, 0, ''), ... +%! mkit('Pitch', false, 0,0,0,0, 'sweep stopped after 11 s'), ... +%! mkit('Yaw', false, 0,0,0,0, 'this axis was not swept')]; +%! txt = PSautotuneCLI(items, 60); +%! assert(~isempty(strfind(txt, 'sweep stopped after 11 s'))); +%! assert(~isempty(strfind(txt, 'this axis was not swept'))); +%! lines = strsplit(txt, char(10)); +%! assert(~any(strncmp(lines, 'set p_pitch', 11)), 'a refused axis must not be set'); +%! assert(~any(strncmp(lines, 'set p_yaw', 9)), 'a refused axis must not be set'); +%! assert(any(strncmp(lines, 'set p_roll', 10)), 'the axis that worked still is'); + +%!test +%! % The #5258 barrier, restated where the text is actually produced. The search +%! % has no filter fields to offer; this makes sure nothing invents one on the +%! % way out. Checked over every shape of input this function takes. +%! cases = { ... +%! [mkit('Roll', true, 30, 43, 27, 0, ''), mkit('Pitch', true, 45, 67, 42, 9, ''), mkit('Yaw', true, 59, 128, 2, 0, '')], ... +%! [mkit('Roll', false, 0,0,0,0, 'no candidate'), mkit('Pitch', false, 0,0,0,0, 'not swept'), mkit('Yaw', false, 0,0,0,0, 'saturated')], ... +%! [mkit('Roll', true, 1, 1, 0, 0, ''), mkit('Pitch', false, 0,0,0,0, 'dterm_lpf1_hz was 120'), mkit('Yaw', true, 200, 200, 90, 200, '')] }; +%! for k = 1:numel(cases) +%! txt = PSautotuneCLI(cases{k}, 72.5); +%! lines = strsplit(txt, char(10)); +%! for j = 1:numel(lines) +%! L = strtrim(lines{j}); +%! if isempty(L) || L(1) == '#', continue; end +%! assert(isempty(strfind(L, 'dterm_')), sprintf('case %d emitted: %s', k, L)); +%! assert(isempty(strfind(L, 'gyro_')), sprintf('case %d emitted: %s', k, L)); +%! assert(isempty(strfind(L, 'notch')), sprintf('case %d emitted: %s', k, L)); +%! end +%! end + +%!test +%! % The header says what the numbers are for, because the text outlives the +%! % window it came from +%! items = [mkit('Roll', true, 30, 43, 27, 0, ''), ... +%! mkit('Pitch', true, 45, 67, 42, 0, ''), ... +%! mkit('Yaw', true, 59, 128, 2, 0, '')]; +%! txt = PSautotuneCLI(items, 72.5); +%! assert(~isempty(strfind(txt, 'PIDscope'))); +%! assert(~isempty(strfind(txt, '72.5')), 'the target must be recorded'); +%! assert(~isempty(strfind(lower(txt), 'test flight')), 'and so must the caveat'); + +%!test +%! % Every axis refused still produces usable text rather than an empty string +%! items = [mkit('Roll', false, 0,0,0,0, 'no chirp'), ... +%! mkit('Pitch', false, 0,0,0,0, 'no chirp'), ... +%! mkit('Yaw', false, 0,0,0,0, 'no chirp')]; +%! txt = PSautotuneCLI(items, 60); +%! assert(~isempty(strtrim(txt))); +%! lines = strsplit(txt, char(10)); +%! assert(~any(strncmp(lines, 'set ', 4)), 'nothing to set means nothing set'); +%! assert(~any(strcmp(strtrim(lines), 'save')), 'and nothing to save'); From 88cff9a958852e65005f7d2968de31cb4990220b Mon Sep 17 00:00:00 2001 From: Grzegorz Sterniczuk Date: Sun, 20 Sep 2026 08:29:43 +0200 Subject: [PATCH 16/33] fix(chirp): walk whole gains so the answer stops depending on the grid offset --- src/core/PSautotuneSearch.m | 26 +++++++++++++++----------- tests/test_PSautotuneSearch.m | 21 +++++++++++++++++++++ 2 files changed, 36 insertions(+), 11 deletions(-) diff --git a/src/core/PSautotuneSearch.m b/src/core/PSautotuneSearch.m index f6b236d..7a7f8c9 100644 --- a/src/core/PSautotuneSearch.m +++ b/src/core/PSautotuneSearch.m @@ -20,8 +20,7 @@ o = struct('pmTarget', 60, 'msMax', 2.0, 'peakMaxDb', 6, ... 'magAtTrustDb', -6, 'wcpFracTrust', 0.5, 'wcpMaxRatio', 3, ... 'dDomRatio', 2.0, ... - 'pClamp', [0.3 2.0], 'pStep', 0.02, ... - 'dClamp', [0.6 1.25], 'dStep', 0.05, 'pidsumFrac', 0.9); + 'pClamp', [0.3 2.0], 'dClamp', [0.6 1.25], 'pidsumFrac', 0.9); fn = fieldnames(o); for k = 1:numel(fn) if isfield(opt, fn{k}) && ~isempty(opt.(fn{k})), o.(fn{k}) = opt.(fn{k}); end @@ -74,12 +73,16 @@ end end -sP = o.pClamp(1):o.pStep:o.pClamp(2); -sD = o.dClamp(1):o.dStep:dHi; -if isempty(sP), sP = 1; end -if isempty(sD), sD = min(1, dHi); end +% Walk whole gains rather than scale factors. A scale grid aliases onto the +% integers unevenly, so the same craft answered differently depending only on +% where the grid happened to start - 32 of 54 answers on the pichim corpus +% moved by one count when the lower clamp changed. +Pcand = max(1, round(o.pClamp(1)*P0)) : round(o.pClamp(2)*P0); +Dcand = round(o.dClamp(1)*D0) : round(dHi*D0); +if isempty(Pcand), Pcand = max(1, P0); end +if isempty(Dcand), Dcand = D0; end -nD = numel(sD); nP = numel(sP); +nD = numel(Dcand); nP = numel(Pcand); g = struct(); g.okMask = false(nD, nP); g.pm = nan(nD, nP); g.ms = nan(nD, nP); g.wcp = nan(nD, nP); @@ -88,15 +91,16 @@ g.P = zeros(nD, nP); g.I = zeros(nD, nP); g.D = zeros(nD, nP); for iD = 1:nD - Di = round(sD(iD) * D0); + Di = Dcand(iD); Dv = Di * b.D1; for iP = 1:nP - Pi = round(sP(iP) * P0); + Pi = Pcand(iP); % Ki in the firmware is absolute, so holding I while cutting P drags % the PI corner upwards and the integrator adds lag exactly where the % scan is trying to buy phase margin. Scaling both keeps the integral % time the pilot flew. - Ii = round(sP(iP) * I0); + Ii = I0; + if P0 > 0, Ii = round(Pi / P0 * I0); end c = evalOne(Pk, fk, Pi*b.Ap + Ii*b.Ai, Dv, F0*b.F1); g.P(iD,iP) = Pi; g.I(iD,iP) = Ii; g.D(iD,iP) = Di; g.pm(iD,iP) = c.pm; g.ms(iD,iP) = c.ms; g.wcp(iD,iP) = c.wcp; @@ -123,7 +127,7 @@ best = max(wcp(:)); tie = sel & (wcp >= best * 0.99); [iDs, iPs] = find(tie); -kD = sD(iDs); kP = sP(iPs); +kD = Dcand(iDs); kP = Pcand(iPs); [~, pick] = sortrows([kD(:), kP(:)], [1 2]); iD = iDs(pick(1)); iP = iPs(pick(1)); diff --git a/tests/test_PSautotuneSearch.m b/tests/test_PSautotuneSearch.m index 4438f4c..62e8b72 100644 --- a/tests/test_PSautotuneSearch.m +++ b/tests/test_PSautotuneSearch.m @@ -179,6 +179,27 @@ %! sprintf('I scaled %.3f against P %.3f - the integral time must hold', ... %! res.scale.I, res.scale.P)); +%!test +%! % Opening the lower clamp can only add answers, never change one. The scan +%! % maximises the crossover and the crossover rises with P, so it always takes +%! % the highest admissible gain; gains below the old floor are only ever +%! % considered when nothing above it worked. That is why the floor can sit low +%! % without making any proposal more drastic than it has to be. +%! fix = mkfix(); +%! wide = PSautotuneSearch(fix, struct('pmTarget', 50, 'msMax', 3, 'pClamp', [0.2 2.0])); +%! tight = PSautotuneSearch(fix, struct('pmTarget', 50, 'msMax', 3, 'pClamp', [0.5 2.0])); +%! assert(wide.ok && tight.ok); +%! assert(wide.gains.P == tight.gains.P, ... +%! sprintf('a feasible answer moved: %d vs %d', wide.gains.P, tight.gains.P)); +%! assert(wide.gains.D == tight.gains.D); +%! % and a cut too deep for the tight floor is reachable from the wide one +%! deep = mkfix('P', 120); +%! rT = PSautotuneSearch(deep, struct('pmTarget', 60, 'msMax', 3, 'pClamp', [0.5 2.0])); +%! rW = PSautotuneSearch(deep, struct('pmTarget', 60, 'msMax', 3, 'pClamp', [0.25 2.0])); +%! assert(~rT.ok, 'this fixture needs a cut past half'); +%! assert(rW.ok, 'and the wider floor must reach it'); +%! assert(rW.gains.P == 45, sprintf('expected P 45, got %d', rW.gains.P)); + %!test %! % Reason codes have to be usable by the UI without guessing %! r = PSautotuneSearch(mkfix('P', 40), struct('pmTarget', 60, 'pClamp', [1 1])); From e8a25994dd61e09113ab4a0bd3e8b4782440782e Mon Sep 17 00:00:00 2001 From: Grzegorz Sterniczuk Date: Sun, 20 Sep 2026 08:30:32 +0200 Subject: [PATCH 17/33] feat(chirp): let the aggressiveness setting reach the cut it implies --- src/core/PSautotuneSearch.m | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/core/PSautotuneSearch.m b/src/core/PSautotuneSearch.m index 7a7f8c9..c571b70 100644 --- a/src/core/PSautotuneSearch.m +++ b/src/core/PSautotuneSearch.m @@ -20,7 +20,7 @@ o = struct('pmTarget', 60, 'msMax', 2.0, 'peakMaxDb', 6, ... 'magAtTrustDb', -6, 'wcpFracTrust', 0.5, 'wcpMaxRatio', 3, ... 'dDomRatio', 2.0, ... - 'pClamp', [0.3 2.0], 'dClamp', [0.6 1.25], 'pidsumFrac', 0.9); + 'pClamp', [0.25 2.0], 'dClamp', [0.6 1.25], 'pidsumFrac', 0.9); fn = fieldnames(o); for k = 1:numel(fn) if isfield(opt, fn{k}) && ~isempty(opt.(fn{k})), o.(fn{k}) = opt.(fn{k}); end From 1061244f55f664745011cc1872d191e384901cc5 Mon Sep 17 00:00:00 2001 From: Grzegorz Sterniczuk Date: Sun, 20 Sep 2026 08:46:00 +0200 Subject: [PATCH 18/33] fix(chirp): calibrate the swept-axis floor through the import path, not raw counts --- src/core/PSautotuneGates.m | 2 +- src/core/PSfindChirpWindow.m | 8 ++++---- tests/test_PSautotuneGates.m | 13 +++++++------ tests/test_PSfindChirpWindow.m | 23 +++++++++++++---------- 4 files changed, 25 insertions(+), 21 deletions(-) diff --git a/src/core/PSautotuneGates.m b/src/core/PSautotuneGates.m index c16691a..892ab40 100644 --- a/src/core/PSautotuneGates.m +++ b/src/core/PSautotuneGates.m @@ -18,7 +18,7 @@ if nargin < 2 || isempty(opt), opt = struct(); end -o = struct('gyroVarMin', 1e4, ... % swept axes score 3.6e5..5.8e5, coupling reaches 1365 +o = struct('gyroVarMin', 500, ... % swept axes score 3523..6188, coupling reaches 137 'sweepFrac', 0.95, ... % structural: a short sweep has no top of band 'satFracMax', 0.01, ... % structural: saturation makes the plant a fiction 'nSegMin', 8, ... % the corpus runs 70 on every axis diff --git a/src/core/PSfindChirpWindow.m b/src/core/PSfindChirpWindow.m index a1c169b..cf22aae 100644 --- a/src/core/PSfindChirpWindow.m +++ b/src/core/PSfindChirpWindow.m @@ -13,10 +13,10 @@ return end -% Every run in a log lasts chirp_time_seconds, so run length differs only by -% sampling noise and says nothing about which axis a run swept - the response -% does. Cross-axis coupling on a real log reaches 1e3, which clears varThresh, -% so the threshold only answers "was this axis swept at all". +% Every run lasts chirp_time_seconds, so run length differs only by sampling +% noise and cannot decide between the two runs that swept this axis - the +% response can. varThresh only answers "was this axis swept at all"; measured +% on the pichim corpus a swept run scores 3523..6188 against 137 of coupling. best_var = 0; idx_start = w(1).i0; idx_end = w(1).i1; diff --git a/tests/test_PSautotuneGates.m b/tests/test_PSautotuneGates.m index 1d33aa6..59464b1 100644 --- a/tests/test_PSautotuneGates.m +++ b/tests/test_PSautotuneGates.m @@ -40,14 +40,15 @@ %! assert(~isempty(strfind(strjoin(msgs, ' '), 'No chirp data found'))); %!test -%! % An axis that was not swept. Measured on the corpus: the swept axis scores -%! % 3.6e5 to 5.8e5, while cross-axis coupling during another axis's run reaches -%! % 1365 - so the old floor of 500 sat inside the coupling, not above it. -%! [ok, msgs] = PSautotuneGates(mkid('gyroVar', 1365)); +%! % An axis that was not swept. Measured through the import path, so with +%! % blackbox_high_resolution undone: the swept axis scores 3523 to 6188 +%! % (deg/s)^2 across the corpus, cross-axis coupling reaches 137. The floor +%! % sits between them with 3.6x below and 7x above. +%! [ok, msgs] = PSautotuneGates(mkid('gyroVar', 137)); %! assert(~ok, 'cross-axis coupling must not read as a swept axis'); %! assert(~isempty(strfind(lower(strjoin(msgs, ' ')), 'swept'))); -%! [ok2, ~] = PSautotuneGates(mkid('gyroVar', 3.6e5)); -%! assert(ok2, 'a genuinely swept axis must pass'); +%! [ok2, ~] = PSautotuneGates(mkid('gyroVar', 3523)); +%! assert(ok2, 'the weakest genuinely swept axis in the corpus must pass'); %!test %! % A sweep that was cut short never reached the top of the band, and fTrust diff --git a/tests/test_PSfindChirpWindow.m b/tests/test_PSfindChirpWindow.m index 8f92daf..8185498 100644 --- a/tests/test_PSfindChirpWindow.m +++ b/tests/test_PSfindChirpWindow.m @@ -49,27 +49,30 @@ %! assert(abs(i0 - 19*Fs) < 0.01*Fs, 'must pick the run where the axis responded'); %!test -%! % Every run in a log is the same length by construction, so run length is -%! % noise and cannot decide which axis a run swept. Block sizes and variances -%! % here are the ones measured on 20250907_flipmini_00, where roll answers in -%! % blocks 1 and 4 while the other four still carry 1e3 of cross-axis coupling - -%! % well clear of the 500 floor, and the longest block of the six is #6. +%! % An axis is swept twice in a six-run log, and the two runs last the same +%! % chirp_time_seconds, so their lengths differ only by sampling noise. Sizes +%! % and variances here are measured on 20250907_flipmini_00 through the import +%! % path: roll answers in runs 1 and 4 at 5588 and 5618, the rest carry 2 to 14 +%! % of cross-axis coupling. Picking by length takes run 1 by one sample; +%! % picking by response takes run 4, which is the one that moved the axis more. %! nSamp = [39987 39988 39942 39950 39986 39996]; -%! gvar = [561000 853 211 562000 1365 225]; +%! gvar = [5588 9 2 5618 14 2]; %! randn('state', 31); %! sinarg = []; gyro = []; %! for k = 1:6 %! sinarg = [sinarg; 15000*ones(nSamp(k),1); zeros(2000,1)]; -%! gyro = [gyro; sqrt(gvar(k))*randn(nSamp(k),1); zeros(2000,1)]; +%! g = sqrt(gvar(k))*randn(nSamp(k),1); +%! gyro = [gyro; g/std(g)*sqrt(gvar(k)); zeros(2000,1)]; %! end %! [i0, i1] = PSfindChirpWindow(sinarg, gyro); %! starts = 1 + cumsum([0 nSamp(1:5) + 2000]); %! picked = find(starts == i0); -%! assert(any(picked == [1 4]), 'must pick a run that actually moved this axis'); +%! assert(picked == 4, sprintf('expected the stronger run, got run %d', picked)); %!test -%! % Two longer runs carrying only cross-axis coupling clear the variance floor -%! % but must not outrank the run that was swept +%! % Should a log ever put coupling above the floor - a harder sweep, a softer +%! % airframe - length must still not decide it. The corpus tops out at 137 +%! % against a floor of 500, so this guards a margin rather than an observation. %! randn('state', 20); %! sinarg = [ones(3000,1); zeros(2000,1); ones(9000,1); zeros(2000,1); ones(5000,1)]; %! gyro = [700*randn(3000,1); zeros(2000,1); 30*randn(9000,1); ... From bc3459b4280accd22f6f2063c1e78e9dc129f74f Mon Sep 17 00:00:00 2001 From: Grzegorz Sterniczuk Date: Sun, 20 Sep 2026 08:46:00 +0200 Subject: [PATCH 19/33] feat(ui): add the autotune window and its control panel button --- PIDscope.m | 29 ++++--- src/ui/PSautotuneUI.m | 183 ++++++++++++++++++++++++++++++++++++++++++ src/util/PStheme.m | 1 + 3 files changed, 204 insertions(+), 9 deletions(-) create mode 100644 src/ui/PSautotuneUI.m diff --git a/PIDscope.m b/PIDscope.m index 74a5277..b9b5cdb 100644 --- a/PIDscope.m +++ b/PIDscope.m @@ -201,6 +201,7 @@ posInfo.spectrogramButton = [cpL+cpM vPos-rs*row cpW-2*cpM rh]; row=row+1; posInfo.TuningButton = [cpL+cpM vPos-rs*row cpW-2*cpM rh]; row=row+1; posInfo.PIDsliderButton=[cpL+cpM vPos-rs*row cpW-2*cpM rh]; row=row+1; +posInfo.autotuneButton= [cpL+cpM vPos-rs*row cpW-2*cpM rh]; row=row+1; posInfo.filterSimButton=[cpL+cpM vPos-rs*row cpW/2-cpM rh]; posInfo.testSignalButton=[cpL+cpW/2 vPos-rs*row cpW/2-cpM rh]; row=row+1; posInfo.PIDErrorButton = [cpL+cpM vPos-rs*row cpW/2-cpM rh]; @@ -311,6 +312,15 @@ 'end']); set(guiHandles.PIDsliderButton, 'ForegroundColor', th.btnDash3); +guiHandles.autotuneButton = uicontrol(PSfig,'string','Autotune (chirp)','fontsize',fontsz,... + 'TooltipString','Propose PID gains from a chirp log','units','normalized',... + 'Position',[posInfo.autotuneButton],... + 'callback',['if ~exist(''T'',''var''),warndlg(''Please select file(s)'');' ... + 'else,tmpFcnt=get(guiHandles.FileNum,''Value'');tmpFcnt=tmpFcnt(1);' ... + 'PSautotuneUI(T{tmpFcnt},SetupInfo{tmpFcnt},1000*A_lograte(tmpFcnt),tIND{tmpFcnt});' ... + 'clear tmpFcnt;end']); +set(guiHandles.autotuneButton, 'ForegroundColor', th.btnAutotune); + guiHandles.filterSimButton = uicontrol(PSfig,'string','Filter Simulator','fontsize',fontsz,... 'TooltipString','Simulate BF filter chain (theoretical response)','units','normalized',... 'Position',[posInfo.filterSimButton],... @@ -445,15 +455,16 @@ try set(guiHandles.linewidth, 'Value', defaults.Values(find(strcmp(defaults.Para cpItems{end+1} = struct('h', guiHandles.spectrogramButton, 'type','full', 'row',7, 'col',0, 'nrows',0); cpItems{end+1} = struct('h', guiHandles.TuningButton, 'type','full', 'row',8, 'col',0, 'nrows',0); cpItems{end+1} = struct('h', guiHandles.PIDsliderButton, 'type','full', 'row',9, 'col',0, 'nrows',0); -cpItems{end+1} = struct('h', guiHandles.filterSimButton, 'type','left', 'row',10, 'col',0, 'nrows',0); -cpItems{end+1} = struct('h', guiHandles.testSignalButton, 'type','right', 'row',10, 'col',0, 'nrows',0); -cpItems{end+1} = struct('h', guiHandles.PIDErrorButton, 'type','left', 'row',11, 'col',0, 'nrows',0); -cpItems{end+1} = struct('h', guiHandles.FlightStatsButton, 'type','right', 'row',11, 'col',0, 'nrows',0); -cpItems{end+1} = struct('h', guiHandles.period2Hz, 'type','left', 'row',12, 'col',0, 'nrows',0); -cpItems{end+1} = struct('h', guiHandles.DispInfoButton, 'type','right', 'row',12, 'col',0, 'nrows',0); -cpItems{end+1} = struct('h', guiHandles.saveFig, 'type','left', 'row',13, 'col',0, 'nrows',0); -cpItems{end+1} = struct('h', guiHandles.saveSettings, 'type','right', 'row',13, 'col',0, 'nrows',0); -cpItems{end+1} = struct('h', guiHandles.PIDtuningService, 'type','full', 'row',14, 'col',0, 'nrows',0); +cpItems{end+1} = struct('h', guiHandles.autotuneButton, 'type','full', 'row',10, 'col',0, 'nrows',0); +cpItems{end+1} = struct('h', guiHandles.filterSimButton, 'type','left', 'row',11, 'col',0, 'nrows',0); +cpItems{end+1} = struct('h', guiHandles.testSignalButton, 'type','right', 'row',11, 'col',0, 'nrows',0); +cpItems{end+1} = struct('h', guiHandles.PIDErrorButton, 'type','left', 'row',12, 'col',0, 'nrows',0); +cpItems{end+1} = struct('h', guiHandles.FlightStatsButton, 'type','right', 'row',12, 'col',0, 'nrows',0); +cpItems{end+1} = struct('h', guiHandles.period2Hz, 'type','left', 'row',13, 'col',0, 'nrows',0); +cpItems{end+1} = struct('h', guiHandles.DispInfoButton, 'type','right', 'row',13, 'col',0, 'nrows',0); +cpItems{end+1} = struct('h', guiHandles.saveFig, 'type','left', 'row',14, 'col',0, 'nrows',0); +cpItems{end+1} = struct('h', guiHandles.saveSettings, 'type','right', 'row',14, 'col',0, 'nrows',0); +cpItems{end+1} = struct('h', guiHandles.PIDtuningService, 'type','full', 'row',15, 'col',0, 'nrows',0); nrows = max(cellfun(@(x) x.row, cpItems)); cpItems = [{struct('h', controlpanel, 'type','panel', 'row',0, 'col',0, 'nrows',nrows)}, cpItems]; PSregisterResize(PSfig, cpPx, cpItems, 'rows'); diff --git a/src/ui/PSautotuneUI.m b/src/ui/PSautotuneUI.m new file mode 100644 index 0000000..2c9cbea --- /dev/null +++ b/src/ui/PSautotuneUI.m @@ -0,0 +1,183 @@ +function PSautotuneUI(T, setupInfo, Fs, tIND) +%% PSautotuneUI - propose PID gains from a chirp log, one row per axis +% T, setupInfo, Fs, tIND - as handed to the other tools +% +% The plant is measured once when the window opens; the aggressiveness setting +% only re-runs the search, so there is nothing to press to get an answer. +% Refused axes say why rather than showing blank numbers. + +th = PStheme(); +fontsz = th.fontsz; + +figName = 'Autotune'; +old = findobj('Type', 'figure', 'Name', figName); +if ~isempty(old), close(old); end + +screensz = get(0, 'ScreenSize'); +figW = 770; figH = 430; +fig = figure('Name', figName, 'NumberTitle', 'off', 'Color', th.figBg, ... + 'Position', [round((screensz(3)-figW)/2) round((screensz(4)-figH)/2) figW figH], ... + 'MenuBar', 'none', 'ToolBar', 'none', 'Resize', 'off'); + +axNames = {'Roll', 'Pitch', 'Yaw'}; +targets = [72.5 60 50]; +tgtLabels = {'Conservative 72.5 deg', 'Normal 60 deg', 'Aggressive 50 deg'}; +colLabels = {'Axis', 'P', 'I', 'D', 'FF', 'Phase margin', 'Ms', 'Crossover'}; +colX = [ 20, 85, 170, 255, 340, 425, 545, 620]; +colW = [ 60, 80, 80, 80, 80, 115, 70, 130]; + +uicontrol(fig, 'Style', 'text', 'String', 'Aggressiveness', ... + 'Position', [20 figH-42 110 20], 'FontSize', fontsz, ... + 'HorizontalAlignment', 'left', ... + 'ForegroundColor', th.textPrimary, 'BackgroundColor', th.figBg); +hTarget = uicontrol(fig, 'Style', 'popupmenu', 'String', tgtLabels, 'Value', 2, ... + 'Position', [135 figH-44 190 24], 'FontSize', fontsz, ... + 'Callback', @(~,~) recompute()); + +hStatus = uicontrol(fig, 'Style', 'text', 'String', '', ... + 'Position', [340 figH-42 410 20], 'FontSize', fontsz, ... + 'HorizontalAlignment', 'left', ... + 'ForegroundColor', th.textPrimary, 'BackgroundColor', th.figBg); + +for c = 1:numel(colLabels) + uicontrol(fig, 'Style', 'text', 'String', colLabels{c}, ... + 'Position', [colX(c) figH-82 colW(c) 20], 'FontSize', fontsz, ... + 'HorizontalAlignment', 'left', 'FontWeight', 'bold', ... + 'ForegroundColor', th.textPrimary, 'BackgroundColor', th.figBg); +end + +rowY = [figH-112, figH-142, figH-172]; +hCell = zeros(3, numel(colLabels)); +hNote = zeros(1, 3); +for r = 1:3 + for c = 1:numel(colLabels) + hCell(r,c) = uicontrol(fig, 'Style', 'text', 'String', '', ... + 'Position', [colX(c) rowY(r) colW(c) 20], 'FontSize', fontsz, ... + 'HorizontalAlignment', 'left', ... + 'ForegroundColor', th.textPrimary, 'BackgroundColor', th.figBg); + end + set(hCell(r,1), 'String', axNames{r}, 'FontWeight', 'bold'); + hNote(r) = uicontrol(fig, 'Style', 'text', 'String', '', 'Visible', 'off', ... + 'Position', [colX(2) rowY(r) 660 20], 'FontSize', fontsz, ... + 'HorizontalAlignment', 'left', ... + 'ForegroundColor', th.btnReset, 'BackgroundColor', th.figBg); +end + +hFoot = uicontrol(fig, 'Style', 'text', 'String', '', ... + 'Position', [20 66 730 40], 'FontSize', fontsz, ... + 'HorizontalAlignment', 'left', ... + 'ForegroundColor', th.textPrimary, 'BackgroundColor', th.figBg); + +hCopy = uicontrol(fig, 'Style', 'pushbutton', 'String', 'Copy CLI', ... + 'Position', [20 22 120 30], 'FontSize', fontsz, 'FontWeight', 'bold', ... + 'BackgroundColor', th.btnAutotune, 'ForegroundColor', [0 0 0], ... + 'Callback', @(~,~) copyCLI()); + +ids = []; +res = cell(1, 3); +gateMsg = {'', '', ''}; + +identify(); +recompute(); + + + function identify() + set(hStatus, 'String', 'Measuring the plant on three axes...'); + drawnow; + for a = 0:2 + id = PSidentifyChirp(T, setupInfo, Fs, tIND, a); + if isempty(ids), ids = id; else, ids(a+1) = id; end + end + set(hStatus, 'String', ''); + drawnow; + end + + + function recompute() + tgt = targets(get(hTarget, 'Value')); + set(hStatus, 'String', sprintf('Searching at %.1f deg...', tgt)); + drawnow; + + notes = {}; + for r = 1:3 + id = ids(r); + [gok, gmsgs, ginfo] = PSautotuneGates(id); + gateMsg{r} = strjoin(gmsgs, '; '); + res{r} = []; + if ~gok + showNote(r, gateMsg{r}); + continue + end + opt = struct('pmTarget', tgt, 'dClamp', [0.6 ginfo.dClampHi]); + r_ = PSautotuneSearch(id, opt); + res{r} = r_; + if ~r_.ok + showNote(r, sprintf('No tune clears %.1f deg here (%s)', tgt, r_.reason)); + continue + end + showRow(r, id, r_); + if ~isempty(gmsgs), notes{end+1} = sprintf('%s: %s', id.axisName, gateMsg{r}); end + end + + band = []; + for r = 1:3 + if isfinite(ids(r).fTrust), band(end+1) = ids(r).fTrust; end + end + foot = {}; + if ~isempty(band) + foot{end+1} = sprintf('Plant good to %.0f Hz', min(band)); + end + foot = [foot notes]; + foot{end+1} = 'Verify on a test flight'; + set(hFoot, 'String', strjoin(foot, ' | ')); + set(hStatus, 'String', ''); + drawnow; + end + + + function showNote(r, msg) + for c = 2:size(hCell, 2) + set(hCell(r,c), 'Visible', 'off'); + end + set(hNote(r), 'String', msg, 'Visible', 'on'); + end + + + function showRow(r, id, rr) + set(hNote(r), 'Visible', 'off'); + for c = 2:size(hCell, 2) + set(hCell(r,c), 'Visible', 'on'); + end + set(hCell(r,2), 'String', sprintf('%d -> %d', id.gains.P, rr.gains.P)); + set(hCell(r,3), 'String', sprintf('%d -> %d', id.gains.I, rr.gains.I)); + set(hCell(r,4), 'String', sprintf('%d -> %d', id.gains.D, rr.gains.D)); + set(hCell(r,5), 'String', sprintf('%d -> %d', id.gains.F, rr.gains.F)); + set(hCell(r,6), 'String', sprintf('%.0f -> %.0f deg', rr.pm0, rr.pm)); + set(hCell(r,7), 'String', sprintf('%.2f -> %.2f', rr.ms0, rr.ms)); + set(hCell(r,8), 'String', sprintf('%.0f -> %.0f Hz', rr.wcp0, rr.wcp)); + end + + + function copyCLI() + tgt = targets(get(hTarget, 'Value')); + items = []; + for r = 1:3 + it = struct('axisName', axNames{r}, 'ok', false, ... + 'gains', struct('P',0,'I',0,'D',0,'F',0), 'note', gateMsg{r}); + if ~isempty(res{r}) && res{r}.ok + it.ok = true; + it.gains = res{r}.gains; + elseif isempty(it.note) + it.note = sprintf('no tune clears %.1f deg', tgt); + end + if isempty(items), items = it; else, items(r) = it; end + end + txt = PSautotuneCLI(items, tgt); + if PScopyToClipboard(txt) + set(hCopy, 'String', 'Copied!'); + else + PSshowCLIDialog(txt); + end + end + +end diff --git a/src/util/PStheme.m b/src/util/PStheme.m index 461038c..4914ed4 100644 --- a/src/util/PStheme.m +++ b/src/util/PStheme.m @@ -43,6 +43,7 @@ th.btnDash7 = [.25 .75 .65]; % Flight Stats - vivid teal th.btnMotNoise = [.20 .85 .30]; % Motor Noise - vivid green th.btnChirp = [.70 .40 .95]; % Chirp Analysis - vivid purple +th.btnAutotune = [.55 .55 .98]; % Autotune - indigo th.btnLink = [.95 .60 .15]; % Support PIDscope - amber th.btnPlayer = [.40 .75 1.0]; % Player button - bright sky blue From 554a0f7f2a055a7dbf21fa7676b4cbf98d0e2cff Mon Sep 17 00:00:00 2001 From: Grzegorz Sterniczuk Date: Sun, 20 Sep 2026 08:47:06 +0200 Subject: [PATCH 20/33] docs: credit the Betaflight Configurator autotune as the behavioural model --- README.md | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/README.md b/README.md index c9e77ef..2394e32 100644 --- a/README.md +++ b/README.md @@ -65,6 +65,17 @@ The chirp analysis in PIDscope builds on work by **Michael Peter ([pichim](https - `src/core/PSstepFromFRD.m` - from `calculate_step_response_from_frd.m` - `src/core/PSrotFiltFilt.m` - from `apply_rotfiltfilt.m` +The chirp logs used to validate the autotune come from the same repository and +are flown by the same author. + +The autotune follows the behaviour of the [Betaflight Configurator](https://github.com/betaflight/betaflight-configurator) +autotune (GPL-3.0) - the three aggressiveness levels and their phase margin +targets, and the hard limit on the sensitivity peak. The algorithm here was +written from that description and no code was ported. It differs where it +matters: Betaflight approximates the open loop as `T/(1-T)`, which assumes unity +feedback, while PIDscope measures the plant separately through `axisSum` and so +works with the exact `L = P*(A+D)`. That is what lets it move D as well as P. + Arch Linux packaging is maintained by [FPVogel](https://github.com/FPVogel). --- From 86d77ac0605e1e5e4811c6a9f3864bb7f94d80fd Mon Sep 17 00:00:00 2001 From: Grzegorz Sterniczuk Date: Sun, 20 Sep 2026 10:34:44 +0200 Subject: [PATCH 21/33] feat(ui): show all three autotune settings at once, computed once and cached --- PIDscope.m | 2 +- src/ui/PSautotuneUI.m | 328 ++++++++++++++++++++++++++++-------------- 2 files changed, 218 insertions(+), 112 deletions(-) diff --git a/PIDscope.m b/PIDscope.m index b9b5cdb..3568f54 100644 --- a/PIDscope.m +++ b/PIDscope.m @@ -317,7 +317,7 @@ 'Position',[posInfo.autotuneButton],... 'callback',['if ~exist(''T'',''var''),warndlg(''Please select file(s)'');' ... 'else,tmpFcnt=get(guiHandles.FileNum,''Value'');tmpFcnt=tmpFcnt(1);' ... - 'PSautotuneUI(T{tmpFcnt},SetupInfo{tmpFcnt},1000*A_lograte(tmpFcnt),tIND{tmpFcnt});' ... + 'PSautotuneUI(T{tmpFcnt},SetupInfo{tmpFcnt},1000*A_lograte(tmpFcnt),tIND{tmpFcnt},fnameMaster{tmpFcnt});' ... 'clear tmpFcnt;end']); set(guiHandles.autotuneButton, 'ForegroundColor', th.btnAutotune); diff --git a/src/ui/PSautotuneUI.m b/src/ui/PSautotuneUI.m index 2c9cbea..2989b58 100644 --- a/src/ui/PSautotuneUI.m +++ b/src/ui/PSautotuneUI.m @@ -1,10 +1,14 @@ -function PSautotuneUI(T, setupInfo, Fs, tIND) -%% PSautotuneUI - propose PID gains from a chirp log, one row per axis +function PSautotuneUI(T, setupInfo, Fs, tIND, logName) +%% PSautotuneUI - what to change in your PID, read off a chirp log % T, setupInfo, Fs, tIND - as handed to the other tools +% logName - shown in the header, optional % -% The plant is measured once when the window opens; the aggressiveness setting -% only re-runs the search, so there is nothing to press to get an answer. -% Refused axes say why rather than showing blank numbers. +% All nine answers (three axes, three settings) are worked out once when the +% window opens and then just displayed, so nothing recomputes while you read. +% Stability margins live in the Chirp Analysis window; this one answers the +% only question a pilot has, which is what to type into the CLI. + +if nargin < 5, logName = ''; end th = PStheme(); fontsz = th.fontsz; @@ -13,168 +17,270 @@ function PSautotuneUI(T, setupInfo, Fs, tIND) old = findobj('Type', 'figure', 'Name', figName); if ~isempty(old), close(old); end +figW = 700; figH = 560; screensz = get(0, 'ScreenSize'); -figW = 770; figH = 430; fig = figure('Name', figName, 'NumberTitle', 'off', 'Color', th.figBg, ... 'Position', [round((screensz(3)-figW)/2) round((screensz(4)-figH)/2) figW figH], ... 'MenuBar', 'none', 'ToolBar', 'none', 'Resize', 'off'); axNames = {'Roll', 'Pitch', 'Yaw'}; -targets = [72.5 60 50]; -tgtLabels = {'Conservative 72.5 deg', 'Normal 60 deg', 'Aggressive 50 deg'}; -colLabels = {'Axis', 'P', 'I', 'D', 'FF', 'Phase margin', 'Ms', 'Crossover'}; -colX = [ 20, 85, 170, 255, 340, 425, 545, 620]; -colW = [ 60, 80, 80, 80, 80, 115, 70, 130]; - -uicontrol(fig, 'Style', 'text', 'String', 'Aggressiveness', ... - 'Position', [20 figH-42 110 20], 'FontSize', fontsz, ... +gainNames = {'P', 'I', 'D'}; +setNames = {'CALM', 'NORMAL', 'SHARP'}; +setBlurb = {'most margin', 'recommended', 'least margin'}; +targets = [72.5 60 50]; + +xAxis = 22; wAxis = 64; +xCol = [96 292 488]; wCol = 186; +yTitle = figH - 38; ySummary = figH - 108; ySep = figH - 118; +yHdr = figH - 154; yBlurb = figH - 170; +rowH = 22; grpGap = 12; yTop = figH - 198; +rowY = zeros(3, 3); +for g = 1:3 + for k = 1:3 + rowY(g,k) = yTop - (g-1)*(3*rowH + grpGap) - (k-1)*rowH; + end +end + +% Backdrop: groups the three gains of an axis together and marks the column +% most pilots should take. uicontrols always draw over axes, so this sits +% behind everything without fighting for hit testing. +axBg = axes('Parent', fig, 'Units', 'normalized', 'Position', [0 0 1 1], ... + 'XLim', [0 figW], 'YLim', [0 figH], 'Visible', 'off', 'HitTest', 'off'); +hold(axBg, 'on'); +hiCol = min(1, th.figBg + 0.045); +colBg = {th.figBg, hiCol, th.figBg}; +patch('Parent', axBg, 'FaceColor', hiCol, 'EdgeColor', 'none', 'HitTest', 'off', ... + 'XData', [xCol(2)-10 xCol(2)+wCol-10 xCol(2)+wCol-10 xCol(2)-10], ... + 'YData', [rowY(3,3)-10 rowY(3,3)-10 yHdr+22 yHdr+22]); +line('Parent', axBg, 'XData', [xAxis figW-xAxis], 'YData', [ySep ySep], ... + 'Color', th.gridColor, 'LineWidth', 0.5, 'HitTest', 'off'); +for g = 1:2 + yg = rowY(g,3) - grpGap/2; + line('Parent', axBg, 'XData', [xAxis figW-xAxis], 'YData', [yg yg], ... + 'Color', th.gridColor, 'LineWidth', 0.5, 'HitTest', 'off'); +end +hold(axBg, 'off'); + +uicontrol(fig, 'Style', 'text', 'Units', 'pixels', 'String', 'Autotune', ... + 'Position', [xAxis yTitle 200 24], 'FontSize', fontsz+3, 'FontWeight', 'bold', ... 'HorizontalAlignment', 'left', ... - 'ForegroundColor', th.textPrimary, 'BackgroundColor', th.figBg); -hTarget = uicontrol(fig, 'Style', 'popupmenu', 'String', tgtLabels, 'Value', 2, ... - 'Position', [135 figH-44 190 24], 'FontSize', fontsz, ... - 'Callback', @(~,~) recompute()); + 'ForegroundColor', th.btnAutotune, 'BackgroundColor', th.figBg); +uicontrol(fig, 'Style', 'text', 'Units', 'pixels', 'String', logName, ... + 'Position', [figW-432-22 yTitle+2 432 20], 'FontSize', fontsz, ... + 'HorizontalAlignment', 'right', ... + 'ForegroundColor', th.textSecondary, 'BackgroundColor', th.figBg); -hStatus = uicontrol(fig, 'Style', 'text', 'String', '', ... - 'Position', [340 figH-42 410 20], 'FontSize', fontsz, ... +hSummary = uicontrol(fig, 'Style', 'text', 'Units', 'pixels', 'String', '', ... + 'Position', [xAxis ySummary figW-2*xAxis 56], 'FontSize', fontsz, ... 'HorizontalAlignment', 'left', ... 'ForegroundColor', th.textPrimary, 'BackgroundColor', th.figBg); -for c = 1:numel(colLabels) - uicontrol(fig, 'Style', 'text', 'String', colLabels{c}, ... - 'Position', [colX(c) figH-82 colW(c) 20], 'FontSize', fontsz, ... - 'HorizontalAlignment', 'left', 'FontWeight', 'bold', ... - 'ForegroundColor', th.textPrimary, 'BackgroundColor', th.figBg); -end +% progress sits where the table will be, so hiding it leaves no hole +hBarBg = uicontrol(fig, 'Style', 'text', 'Units', 'pixels', 'String', '', ... + 'Position', [xAxis yTop+6 figW-2*xAxis 10], ... + 'BackgroundColor', th.btnBg, 'ForegroundColor', th.btnBg); +hBar = uicontrol(fig, 'Style', 'text', 'Units', 'pixels', 'String', '', ... + 'Position', [xAxis yTop+6 1 10], ... + 'BackgroundColor', th.btnAutotune, 'ForegroundColor', th.btnAutotune); +hStep = uicontrol(fig, 'Style', 'text', 'Units', 'pixels', 'String', '', ... + 'Position', [xAxis yTop-24 figW-2*xAxis 18], 'FontSize', fontsz, ... + 'HorizontalAlignment', 'left', ... + 'ForegroundColor', th.textSecondary, 'BackgroundColor', th.figBg); -rowY = [figH-112, figH-142, figH-172]; -hCell = zeros(3, numel(colLabels)); +hCell = zeros(3, 3, 3); % axis, setting, gain hNote = zeros(1, 3); -for r = 1:3 - for c = 1:numel(colLabels) - hCell(r,c) = uicontrol(fig, 'Style', 'text', 'String', '', ... - 'Position', [colX(c) rowY(r) colW(c) 20], 'FontSize', fontsz, ... - 'HorizontalAlignment', 'left', ... - 'ForegroundColor', th.textPrimary, 'BackgroundColor', th.figBg); +hHdr = zeros(1, 3); +for g = 1:3 + uicontrol(fig, 'Style', 'text', 'Units', 'pixels', 'String', axNames{g}, ... + 'Position', [xAxis rowY(g,1) wAxis rowH], 'FontSize', fontsz, ... + 'FontWeight', 'bold', 'HorizontalAlignment', 'left', ... + 'ForegroundColor', th.textPrimary, 'BackgroundColor', th.figBg); + hNote(g) = uicontrol(fig, 'Style', 'text', 'Units', 'pixels', 'String', '', ... + 'Visible', 'off', 'Position', [xCol(1) rowY(g,1) figW-xCol(1)-22 rowH], ... + 'FontSize', fontsz, 'HorizontalAlignment', 'left', ... + 'ForegroundColor', th.btnReset, 'BackgroundColor', th.figBg); + for s = 1:3 + for k = 1:3 + hCell(g,s,k) = uicontrol(fig, 'Style', 'text', 'Units', 'pixels', ... + 'String', '', 'Position', [xCol(s) rowY(g,k) wCol rowH], ... + 'FontSize', fontsz, 'FontName', 'Monospace', ... + 'HorizontalAlignment', 'left', ... + 'ForegroundColor', th.textPrimary, 'BackgroundColor', colBg{s}); + end end - set(hCell(r,1), 'String', axNames{r}, 'FontWeight', 'bold'); - hNote(r) = uicontrol(fig, 'Style', 'text', 'String', '', 'Visible', 'off', ... - 'Position', [colX(2) rowY(r) 660 20], 'FontSize', fontsz, ... +end + +for s = 1:3 + hdrCol = th.textPrimary; + if s == 2, hdrCol = th.btnAutotune; end + hHdr(s) = uicontrol(fig, 'Style', 'text', 'Units', 'pixels', 'String', setNames{s}, ... + 'Position', [xCol(s) yHdr wCol 20], 'FontSize', fontsz+1, ... + 'FontWeight', 'bold', 'HorizontalAlignment', 'left', ... + 'ForegroundColor', hdrCol, 'BackgroundColor', colBg{s}); + uicontrol(fig, 'Style', 'text', 'Units', 'pixels', 'String', setBlurb{s}, ... + 'Position', [xCol(s) yBlurb wCol 16], 'FontSize', fontsz-1, ... 'HorizontalAlignment', 'left', ... - 'ForegroundColor', th.btnReset, 'BackgroundColor', th.figBg); + 'ForegroundColor', th.textSecondary, 'BackgroundColor', colBg{s}); end -hFoot = uicontrol(fig, 'Style', 'text', 'String', '', ... - 'Position', [20 66 730 40], 'FontSize', fontsz, ... +hFoot = uicontrol(fig, 'Style', 'text', 'Units', 'pixels', 'String', '', ... + 'Position', [xAxis 96 figW-2*xAxis 40], 'FontSize', fontsz-1, ... 'HorizontalAlignment', 'left', ... - 'ForegroundColor', th.textPrimary, 'BackgroundColor', th.figBg); + 'ForegroundColor', th.textSecondary, 'BackgroundColor', th.figBg); -hCopy = uicontrol(fig, 'Style', 'pushbutton', 'String', 'Copy CLI', ... - 'Position', [20 22 120 30], 'FontSize', fontsz, 'FontWeight', 'bold', ... - 'BackgroundColor', th.btnAutotune, 'ForegroundColor', [0 0 0], ... - 'Callback', @(~,~) copyCLI()); +hBtn = zeros(1, 3); +for s = 1:3 + hBtn(s) = uicontrol(fig, 'Style', 'pushbutton', 'Units', 'pixels', ... + 'String', ['Copy ' setNames{s}], ... + 'Position', [xCol(s) 32 wCol 32], 'FontSize', fontsz, ... + 'FontWeight', 'bold', ... + 'BackgroundColor', th.btnBg, 'ForegroundColor', th.btnAutotune, ... + 'Callback', @(~,~) copyCLI(s)); +end ids = []; -res = cell(1, 3); +res = cell(3, 3); gateMsg = {'', '', ''}; -identify(); -recompute(); +compute(); +render(); - function identify() - set(hStatus, 'String', 'Measuring the plant on three axes...'); - drawnow; - for a = 0:2 - id = PSidentifyChirp(T, setupInfo, Fs, tIND, a); - if isempty(ids), ids = id; else, ids(a+1) = id; end + function compute() + nStep = 12; done = 0; + for a = 1:3 + step(sprintf('Measuring the %s plant...', lower(axNames{a})), done, nStep); + id = PSidentifyChirp(T, setupInfo, Fs, tIND, a-1); + if isempty(ids), ids = id; else, ids(a) = id; end + done = done + 1; end - set(hStatus, 'String', ''); - drawnow; + for a = 1:3 + [gok, gmsgs, ginfo] = PSautotuneGates(ids(a)); + gateMsg{a} = strjoin(gmsgs, '; '); + if ~gok + done = done + 3; + step('', done, nStep); + continue + end + for s = 1:3 + step(sprintf('Working out %s for %s...', setNames{s}, lower(axNames{a})), ... + done, nStep); + res{a,s} = PSautotuneSearch(ids(a), ... + struct('pmTarget', targets(s), 'dClamp', [0.6 ginfo.dClampHi])); + done = done + 1; + end + end + step('', nStep, nStep); + set([hBarBg hBar hStep], 'Visible', 'off'); end - function recompute() - tgt = targets(get(hTarget, 'Value')); - set(hStatus, 'String', sprintf('Searching at %.1f deg...', tgt)); + function step(msg, done, total) + w = max(1, round((figW - 2*xAxis) * done / total)); + set(hBar, 'Position', [xAxis figH-130 w 10]); + if ~isempty(msg), set(hStep, 'String', msg); end drawnow; + end - notes = {}; - for r = 1:3 - id = ids(r); - [gok, gmsgs, ginfo] = PSautotuneGates(id); - gateMsg{r} = strjoin(gmsgs, '; '); - res{r} = []; - if ~gok - showNote(r, gateMsg{r}); + + function render() + hot = {}; soft = {}; refused = {}; + for a = 1:3 + if ~isempty(gateMsg{a}) + set(hNote(a), 'String', gateMsg{a}, 'Visible', 'on'); + for s = 1:3 + for k = 1:3, set(hCell(a,s,k), 'Visible', 'off'); end + end + refused{end+1} = axNames{a}; continue end - opt = struct('pmTarget', tgt, 'dClamp', [0.6 ginfo.dClampHi]); - r_ = PSautotuneSearch(id, opt); - res{r} = r_; - if ~r_.ok - showNote(r, sprintf('No tune clears %.1f deg here (%s)', tgt, r_.reason)); - continue + for s = 1:3 + r = res{a,s}; + for k = 1:3 + if isempty(r) || ~r.ok + txt = ''; + if k == 1, txt = 'out of reach'; end + set(hCell(a,s,k), 'String', txt, ... + 'ForegroundColor', th.textSecondary); + continue + end + was = gainOf(ids(a).gains, k); + now = gainOf(r.gains, k); + col = th.textPrimary; + if now < was, col = th.btnDash2; elseif now > was, col = th.btnRun; end + set(hCell(a,s,k), 'ForegroundColor', col, ... + 'String', sprintf('%s %4d -> %-4d', gainNames{k}, was, now)); + end + end + rn = res{a,2}; + if ~isempty(rn) && rn.ok + if rn.gains.P < ids(a).gains.P, hot{end+1} = axNames{a}; + elseif rn.gains.P > ids(a).gains.P, soft{end+1} = axNames{a}; + end end - showRow(r, id, r_); - if ~isempty(gmsgs), notes{end+1} = sprintf('%s: %s', id.axisName, gateMsg{r}); end end + lines = {}; + if ~isempty(hot) + lines{end+1} = sprintf('%s want less gain - they are flying close to the limit.', ... + strjoin(hot, ' and ')); + end + if ~isempty(soft) + lines{end+1} = sprintf('%s can take more gain than %s flying.', ... + strjoin(soft, ' and '), pickAux(numel(soft))); + end + if ~isempty(refused) + lines{end+1} = sprintf('%s could not be read from this log.', ... + strjoin(refused, ' and ')); + end + if isempty(lines), lines{end+1} = 'Nothing to change on this log.'; end + set(hSummary, 'String', lines); + band = []; - for r = 1:3 - if isfinite(ids(r).fTrust), band(end+1) = ids(r).fTrust; end + for a = 1:3 + if isfinite(ids(a).fTrust), band(end+1) = ids(a).fTrust; end end - foot = {}; + foot = {'Pick a column and paste it into the Betaflight CLI. Feedforward is left as flown.'}; if ~isempty(band) - foot{end+1} = sprintf('Plant good to %.0f Hz', min(band)); + foot{end+1} = sprintf(['Measured from the chirp up to %.0f Hz. ' ... + 'Fly it and check before trusting it.'], min(band)); end - foot = [foot notes]; - foot{end+1} = 'Verify on a test flight'; - set(hFoot, 'String', strjoin(foot, ' | ')); - set(hStatus, 'String', ''); - drawnow; + set(hFoot, 'String', foot); end - function showNote(r, msg) - for c = 2:size(hCell, 2) - set(hCell(r,c), 'Visible', 'off'); + function v = gainOf(g, k) + switch k + case 1, v = g.P; + case 2, v = g.I; + otherwise, v = g.D; end - set(hNote(r), 'String', msg, 'Visible', 'on'); end - function showRow(r, id, rr) - set(hNote(r), 'Visible', 'off'); - for c = 2:size(hCell, 2) - set(hCell(r,c), 'Visible', 'on'); - end - set(hCell(r,2), 'String', sprintf('%d -> %d', id.gains.P, rr.gains.P)); - set(hCell(r,3), 'String', sprintf('%d -> %d', id.gains.I, rr.gains.I)); - set(hCell(r,4), 'String', sprintf('%d -> %d', id.gains.D, rr.gains.D)); - set(hCell(r,5), 'String', sprintf('%d -> %d', id.gains.F, rr.gains.F)); - set(hCell(r,6), 'String', sprintf('%.0f -> %.0f deg', rr.pm0, rr.pm)); - set(hCell(r,7), 'String', sprintf('%.2f -> %.2f', rr.ms0, rr.ms)); - set(hCell(r,8), 'String', sprintf('%.0f -> %.0f Hz', rr.wcp0, rr.wcp)); + function s = pickAux(n) + s = 'it is'; + if n > 1, s = 'they are'; end end - function copyCLI() - tgt = targets(get(hTarget, 'Value')); + function copyCLI(s) items = []; - for r = 1:3 - it = struct('axisName', axNames{r}, 'ok', false, ... - 'gains', struct('P',0,'I',0,'D',0,'F',0), 'note', gateMsg{r}); - if ~isempty(res{r}) && res{r}.ok + for a = 1:3 + it = struct('axisName', axNames{a}, 'ok', false, ... + 'gains', struct('P',0,'I',0,'D',0,'F',0), 'note', gateMsg{a}); + r = res{a,s}; + if ~isempty(r) && r.ok it.ok = true; - it.gains = res{r}.gains; + it.gains = r.gains; elseif isempty(it.note) - it.note = sprintf('no tune clears %.1f deg', tgt); + it.note = sprintf('no tune reaches %s on this axis', setNames{s}); end - if isempty(items), items = it; else, items(r) = it; end + if isempty(items), items = it; else, items(a) = it; end end - txt = PSautotuneCLI(items, tgt); + txt = PSautotuneCLI(items, targets(s)); if PScopyToClipboard(txt) - set(hCopy, 'String', 'Copied!'); + set(hBtn(s), 'String', 'Copied!'); else PSshowCLIDialog(txt); end From 9d7e8c4b799c2050c3d13b134b511c08f2e44aba Mon Sep 17 00:00:00 2001 From: Grzegorz Sterniczuk Date: Sun, 20 Sep 2026 10:54:01 +0200 Subject: [PATCH 22/33] fix(ui): drop the accent colour from the table, print each flown gain once --- src/ui/PSautotuneUI.m | 351 +++++++++++++++++------------------------- 1 file changed, 143 insertions(+), 208 deletions(-) diff --git a/src/ui/PSautotuneUI.m b/src/ui/PSautotuneUI.m index 2989b58..9dd8435 100644 --- a/src/ui/PSautotuneUI.m +++ b/src/ui/PSautotuneUI.m @@ -3,37 +3,73 @@ function PSautotuneUI(T, setupInfo, Fs, tIND, logName) % T, setupInfo, Fs, tIND - as handed to the other tools % logName - shown in the header, optional % -% All nine answers (three axes, three settings) are worked out once when the -% window opens and then just displayed, so nothing recomputes while you read. -% Stability margins live in the Chirp Analysis window; this one answers the -% only question a pilot has, which is what to type into the CLI. +% Everything is worked out before the window opens, so it never appears with +% empty rows. The flown value is printed once per gain and the three settings +% print only what they would change it to. Stability margins live in the Chirp +% Analysis window; this one answers what to type into the CLI. if nargin < 5, logName = ''; end th = PStheme(); fontsz = th.fontsz; +axNames = {'Roll', 'Pitch', 'Yaw'}; +gainNames = {'P', 'I', 'D'}; +setNames = {'CALM', 'NORMAL', 'SHARP'}; +setBlurb = {'most margin', 'recommended', 'least margin'}; +targets = [72.5 60 50]; + +%% --- work it all out first --- +wb = []; +try wb = waitbar(0, 'Measuring the plant...'); catch, end +ids = []; +res = cell(3, 3); +gateMsg = {'', '', ''}; +nStep = 12; done = 0; +for a = 1:3 + tick(done/nStep, sprintf('Measuring the %s plant...', lower(axNames{a}))); + id = PSidentifyChirp(T, setupInfo, Fs, tIND, a-1); + if isempty(ids), ids = id; else, ids(a) = id; end + done = done + 1; +end +for a = 1:3 + [gok, gmsgs, ginfo] = PSautotuneGates(ids(a)); + gateMsg{a} = strjoin(gmsgs, '; '); + if ~gok + done = done + 3; + continue + end + for s = 1:3 + tick(done/nStep, sprintf('Working out %s for %s...', setNames{s}, lower(axNames{a}))); + res{a,s} = PSautotuneSearch(ids(a), ... + struct('pmTarget', targets(s), 'dClamp', [0.6 ginfo.dClampHi])); + done = done + 1; + end +end +try close(wb); catch, end + +%% --- then draw it --- figName = 'Autotune'; old = findobj('Type', 'figure', 'Name', figName); if ~isempty(old), close(old); end -figW = 700; figH = 560; +figW = 604; figH = 520; screensz = get(0, 'ScreenSize'); fig = figure('Name', figName, 'NumberTitle', 'off', 'Color', th.figBg, ... 'Position', [round((screensz(3)-figW)/2) round((screensz(4)-figH)/2) figW figH], ... 'MenuBar', 'none', 'ToolBar', 'none', 'Resize', 'off'); -axNames = {'Roll', 'Pitch', 'Yaw'}; -gainNames = {'P', 'I', 'D'}; -setNames = {'CALM', 'NORMAL', 'SHARP'}; -setBlurb = {'most margin', 'recommended', 'least margin'}; -targets = [72.5 60 50]; +mL = 24; +xAx = mL; wAx = 56; +xGain = 86; wGain = 16; +xNow = 104; wNow = 60; +xCol = [200 330 460]; wCol = 120; +hiCol = min(1, th.figBg + 0.04); +colBg = {th.figBg, hiCol, th.figBg}; -xAxis = 22; wAxis = 64; -xCol = [96 292 488]; wCol = 186; -yTitle = figH - 38; ySummary = figH - 108; ySep = figH - 118; -yHdr = figH - 154; yBlurb = figH - 170; -rowH = 22; grpGap = 12; yTop = figH - 198; +% rows are exactly as tall as they are spaced, so the highlighted column reads +% as one block instead of a stack of stripes +rowH = 18; grpGap = 14; yTop = figH - 204; rowY = zeros(3, 3); for g = 1:3 for k = 1:3 @@ -41,90 +77,98 @@ function PSautotuneUI(T, setupInfo, Fs, tIND, logName) end end -% Backdrop: groups the three gains of an axis together and marks the column -% most pilots should take. uicontrols always draw over axes, so this sits -% behind everything without fighting for hit testing. -axBg = axes('Parent', fig, 'Units', 'normalized', 'Position', [0 0 1 1], ... - 'XLim', [0 figW], 'YLim', [0 figH], 'Visible', 'off', 'HitTest', 'off'); -hold(axBg, 'on'); -hiCol = min(1, th.figBg + 0.045); -colBg = {th.figBg, hiCol, th.figBg}; -patch('Parent', axBg, 'FaceColor', hiCol, 'EdgeColor', 'none', 'HitTest', 'off', ... - 'XData', [xCol(2)-10 xCol(2)+wCol-10 xCol(2)+wCol-10 xCol(2)-10], ... - 'YData', [rowY(3,3)-10 rowY(3,3)-10 yHdr+22 yHdr+22]); -line('Parent', axBg, 'XData', [xAxis figW-xAxis], 'YData', [ySep ySep], ... - 'Color', th.gridColor, 'LineWidth', 0.5, 'HitTest', 'off'); -for g = 1:2 - yg = rowY(g,3) - grpGap/2; - line('Parent', axBg, 'XData', [xAxis figW-xAxis], 'YData', [yg yg], ... - 'Color', th.gridColor, 'LineWidth', 0.5, 'HitTest', 'off'); -end -hold(axBg, 'off'); +txt = @(s, x, y, w, col, align, fs, wt, bg) uicontrol(fig, 'Style', 'text', ... + 'Units', 'pixels', 'String', s, 'Position', [x y w 18], 'FontSize', fs, ... + 'FontWeight', wt, 'HorizontalAlignment', align, ... + 'ForegroundColor', col, 'BackgroundColor', bg); + +rule = @(y) uicontrol(fig, 'Style', 'text', 'Units', 'pixels', 'String', '', ... + 'Position', [mL y figW-2*mL 1], ... + 'BackgroundColor', th.gridColor, 'ForegroundColor', th.gridColor); uicontrol(fig, 'Style', 'text', 'Units', 'pixels', 'String', 'Autotune', ... - 'Position', [xAxis yTitle 200 24], 'FontSize', fontsz+3, 'FontWeight', 'bold', ... + 'Position', [mL figH-42 200 24], 'FontSize', fontsz+3, 'FontWeight', 'bold', ... 'HorizontalAlignment', 'left', ... - 'ForegroundColor', th.btnAutotune, 'BackgroundColor', th.figBg); -uicontrol(fig, 'Style', 'text', 'Units', 'pixels', 'String', logName, ... - 'Position', [figW-432-22 yTitle+2 432 20], 'FontSize', fontsz, ... - 'HorizontalAlignment', 'right', ... - 'ForegroundColor', th.textSecondary, 'BackgroundColor', th.figBg); - -hSummary = uicontrol(fig, 'Style', 'text', 'Units', 'pixels', 'String', '', ... - 'Position', [xAxis ySummary figW-2*xAxis 56], 'FontSize', fontsz, ... + 'ForegroundColor', th.textPrimary, 'BackgroundColor', th.figBg); +txt(logName, figW-mL-380, figH-40, 380, th.textSecondary, 'right', fontsz, 'normal', th.figBg); +rule(figH-54); + +%% --- one plain sentence about what the log says --- +hot = {}; soft = {}; refused = {}; +for a = 1:3 + if ~isempty(gateMsg{a}), refused{end+1} = axNames{a}; continue; end + rn = res{a,2}; + if isempty(rn) || ~rn.ok, continue; end + if rn.gains.P < ids(a).gains.P, hot{end+1} = axNames{a}; + elseif rn.gains.P > ids(a).gains.P, soft{end+1} = axNames{a}; + end +end +lines = {}; +if ~isempty(hot) + lines{end+1} = sprintf('%s want less gain - they are flying close to the limit.', ... + strjoin(hot, ' and ')); +end +if ~isempty(soft) + lines{end+1} = sprintf('%s can take more gain than it is flying.', strjoin(soft, ' and ')); +end +if ~isempty(refused) + lines{end+1} = sprintf('%s could not be read from this log.', strjoin(refused, ' and ')); +end +if isempty(lines), lines{end+1} = 'Nothing to change on this log.'; end +uicontrol(fig, 'Style', 'text', 'Units', 'pixels', 'String', lines, ... + 'Position', [mL figH-112 figW-2*mL 48], 'FontSize', fontsz, ... 'HorizontalAlignment', 'left', ... 'ForegroundColor', th.textPrimary, 'BackgroundColor', th.figBg); +rule(figH-124); -% progress sits where the table will be, so hiding it leaves no hole -hBarBg = uicontrol(fig, 'Style', 'text', 'Units', 'pixels', 'String', '', ... - 'Position', [xAxis yTop+6 figW-2*xAxis 10], ... - 'BackgroundColor', th.btnBg, 'ForegroundColor', th.btnBg); -hBar = uicontrol(fig, 'Style', 'text', 'Units', 'pixels', 'String', '', ... - 'Position', [xAxis yTop+6 1 10], ... - 'BackgroundColor', th.btnAutotune, 'ForegroundColor', th.btnAutotune); -hStep = uicontrol(fig, 'Style', 'text', 'Units', 'pixels', 'String', '', ... - 'Position', [xAxis yTop-24 figW-2*xAxis 18], 'FontSize', fontsz, ... - 'HorizontalAlignment', 'left', ... - 'ForegroundColor', th.textSecondary, 'BackgroundColor', th.figBg); +%% --- header --- +txt('now', xNow, figH-152, wNow, th.textSecondary, 'right', fontsz, 'normal', th.figBg); +for s = 1:3 + txt(setNames{s}, xCol(s), figH-152, wCol, th.textPrimary, 'right', fontsz, 'bold', colBg{s}); + txt(setBlurb{s}, xCol(s), figH-168, wCol, th.textSecondary, 'right', fontsz-1, 'normal', colBg{s}); +end +rule(figH-178); -hCell = zeros(3, 3, 3); % axis, setting, gain -hNote = zeros(1, 3); -hHdr = zeros(1, 3); +%% --- one row per gain, the flown value printed once --- for g = 1:3 - uicontrol(fig, 'Style', 'text', 'Units', 'pixels', 'String', axNames{g}, ... - 'Position', [xAxis rowY(g,1) wAxis rowH], 'FontSize', fontsz, ... - 'FontWeight', 'bold', 'HorizontalAlignment', 'left', ... - 'ForegroundColor', th.textPrimary, 'BackgroundColor', th.figBg); - hNote(g) = uicontrol(fig, 'Style', 'text', 'Units', 'pixels', 'String', '', ... - 'Visible', 'off', 'Position', [xCol(1) rowY(g,1) figW-xCol(1)-22 rowH], ... - 'FontSize', fontsz, 'HorizontalAlignment', 'left', ... - 'ForegroundColor', th.btnReset, 'BackgroundColor', th.figBg); - for s = 1:3 - for k = 1:3 - hCell(g,s,k) = uicontrol(fig, 'Style', 'text', 'Units', 'pixels', ... - 'String', '', 'Position', [xCol(s) rowY(g,k) wCol rowH], ... - 'FontSize', fontsz, 'FontName', 'Monospace', ... - 'HorizontalAlignment', 'left', ... - 'ForegroundColor', th.textPrimary, 'BackgroundColor', colBg{s}); + txt(axNames{g}, xAx, rowY(g,1), wAx, th.textPrimary, 'left', fontsz, 'bold', th.figBg); + if ~isempty(gateMsg{g}) + txt(gateMsg{g}, xGain, rowY(g,1), figW-xGain-mL, th.btnReset, 'left', fontsz, 'normal', th.figBg); + if g < 3, rule(rowY(g,3) - grpGap/2); end + continue + end + for k = 1:3 + was = gainOf(ids(g).gains, k); + txt(gainNames{k}, xGain, rowY(g,k), wGain, th.textSecondary, 'left', fontsz, 'normal', th.figBg); + txt(sprintf('%d', was), xNow, rowY(g,k), wNow, th.textSecondary, 'right', fontsz, 'normal', th.figBg); + for s = 1:3 + r = res{g,s}; + if isempty(r) || ~r.ok + str = ''; col = th.textSecondary; + if k == 1, str = 'out of reach'; end + txt(str, xCol(s), rowY(g,k), wCol, col, 'right', fontsz-1, 'normal', colBg{s}); + else + now = gainOf(r.gains, k); + txt(sprintf('%d', now), xCol(s), rowY(g,k), wCol, th.textPrimary, ... + 'right', fontsz, 'normal', colBg{s}); + end end end + if g < 3, rule(rowY(g,3) - grpGap/2); end end -for s = 1:3 - hdrCol = th.textPrimary; - if s == 2, hdrCol = th.btnAutotune; end - hHdr(s) = uicontrol(fig, 'Style', 'text', 'Units', 'pixels', 'String', setNames{s}, ... - 'Position', [xCol(s) yHdr wCol 20], 'FontSize', fontsz+1, ... - 'FontWeight', 'bold', 'HorizontalAlignment', 'left', ... - 'ForegroundColor', hdrCol, 'BackgroundColor', colBg{s}); - uicontrol(fig, 'Style', 'text', 'Units', 'pixels', 'String', setBlurb{s}, ... - 'Position', [xCol(s) yBlurb wCol 16], 'FontSize', fontsz-1, ... - 'HorizontalAlignment', 'left', ... - 'ForegroundColor', th.textSecondary, 'BackgroundColor', colBg{s}); +%% --- footer and the three copies --- +band = []; +for a = 1:3 + if isfinite(ids(a).fTrust), band(end+1) = ids(a).fTrust; end end - -hFoot = uicontrol(fig, 'Style', 'text', 'Units', 'pixels', 'String', '', ... - 'Position', [xAxis 96 figW-2*xAxis 40], 'FontSize', fontsz-1, ... +foot = {'Pick a column and paste it into the Betaflight CLI. Feedforward is left as flown.'}; +if ~isempty(band) + foot{end+1} = sprintf(['Measured from the chirp up to %.0f Hz. ' ... + 'Fly it and check before trusting it.'], min(band)); +end +uicontrol(fig, 'Style', 'text', 'Units', 'pixels', 'String', foot, ... + 'Position', [mL 82 figW-2*mL 36], 'FontSize', fontsz-1, ... 'HorizontalAlignment', 'left', ... 'ForegroundColor', th.textSecondary, 'BackgroundColor', th.figBg); @@ -132,138 +176,29 @@ function PSautotuneUI(T, setupInfo, Fs, tIND, logName) for s = 1:3 hBtn(s) = uicontrol(fig, 'Style', 'pushbutton', 'Units', 'pixels', ... 'String', ['Copy ' setNames{s}], ... - 'Position', [xCol(s) 32 wCol 32], 'FontSize', fontsz, ... - 'FontWeight', 'bold', ... - 'BackgroundColor', th.btnBg, 'ForegroundColor', th.btnAutotune, ... + 'Position', [xCol(s) 30 wCol 32], 'FontSize', fontsz, 'FontWeight', 'bold', ... + 'BackgroundColor', th.btnBg, 'ForegroundColor', th.textAccent, ... 'Callback', @(~,~) copyCLI(s)); end -ids = []; -res = cell(3, 3); -gateMsg = {'', '', ''}; +PSstyleControls(fig, th); -compute(); -render(); - - function compute() - nStep = 12; done = 0; - for a = 1:3 - step(sprintf('Measuring the %s plant...', lower(axNames{a})), done, nStep); - id = PSidentifyChirp(T, setupInfo, Fs, tIND, a-1); - if isempty(ids), ids = id; else, ids(a) = id; end - done = done + 1; - end - for a = 1:3 - [gok, gmsgs, ginfo] = PSautotuneGates(ids(a)); - gateMsg{a} = strjoin(gmsgs, '; '); - if ~gok - done = done + 3; - step('', done, nStep); - continue - end - for s = 1:3 - step(sprintf('Working out %s for %s...', setNames{s}, lower(axNames{a})), ... - done, nStep); - res{a,s} = PSautotuneSearch(ids(a), ... - struct('pmTarget', targets(s), 'dClamp', [0.6 ginfo.dClampHi])); - done = done + 1; - end - end - step('', nStep, nStep); - set([hBarBg hBar hStep], 'Visible', 'off'); + function tick(frac, msg) + if isempty(wb), return; end + try waitbar(frac, wb, msg); drawnow; catch, end end - function step(msg, done, total) - w = max(1, round((figW - 2*xAxis) * done / total)); - set(hBar, 'Position', [xAxis figH-130 w 10]); - if ~isempty(msg), set(hStep, 'String', msg); end - drawnow; - end - - - function render() - hot = {}; soft = {}; refused = {}; - for a = 1:3 - if ~isempty(gateMsg{a}) - set(hNote(a), 'String', gateMsg{a}, 'Visible', 'on'); - for s = 1:3 - for k = 1:3, set(hCell(a,s,k), 'Visible', 'off'); end - end - refused{end+1} = axNames{a}; - continue - end - for s = 1:3 - r = res{a,s}; - for k = 1:3 - if isempty(r) || ~r.ok - txt = ''; - if k == 1, txt = 'out of reach'; end - set(hCell(a,s,k), 'String', txt, ... - 'ForegroundColor', th.textSecondary); - continue - end - was = gainOf(ids(a).gains, k); - now = gainOf(r.gains, k); - col = th.textPrimary; - if now < was, col = th.btnDash2; elseif now > was, col = th.btnRun; end - set(hCell(a,s,k), 'ForegroundColor', col, ... - 'String', sprintf('%s %4d -> %-4d', gainNames{k}, was, now)); - end - end - rn = res{a,2}; - if ~isempty(rn) && rn.ok - if rn.gains.P < ids(a).gains.P, hot{end+1} = axNames{a}; - elseif rn.gains.P > ids(a).gains.P, soft{end+1} = axNames{a}; - end - end - end - - lines = {}; - if ~isempty(hot) - lines{end+1} = sprintf('%s want less gain - they are flying close to the limit.', ... - strjoin(hot, ' and ')); - end - if ~isempty(soft) - lines{end+1} = sprintf('%s can take more gain than %s flying.', ... - strjoin(soft, ' and '), pickAux(numel(soft))); - end - if ~isempty(refused) - lines{end+1} = sprintf('%s could not be read from this log.', ... - strjoin(refused, ' and ')); - end - if isempty(lines), lines{end+1} = 'Nothing to change on this log.'; end - set(hSummary, 'String', lines); - - band = []; - for a = 1:3 - if isfinite(ids(a).fTrust), band(end+1) = ids(a).fTrust; end - end - foot = {'Pick a column and paste it into the Betaflight CLI. Feedforward is left as flown.'}; - if ~isempty(band) - foot{end+1} = sprintf(['Measured from the chirp up to %.0f Hz. ' ... - 'Fly it and check before trusting it.'], min(band)); - end - set(hFoot, 'String', foot); - end - - - function v = gainOf(g, k) + function v = gainOf(gg, k) switch k - case 1, v = g.P; - case 2, v = g.I; - otherwise, v = g.D; + case 1, v = gg.P; + case 2, v = gg.I; + otherwise, v = gg.D; end end - function s = pickAux(n) - s = 'it is'; - if n > 1, s = 'they are'; end - end - - function copyCLI(s) items = []; for a = 1:3 @@ -278,11 +213,11 @@ function copyCLI(s) end if isempty(items), items = it; else, items(a) = it; end end - txt = PSautotuneCLI(items, targets(s)); - if PScopyToClipboard(txt) + cliText = PSautotuneCLI(items, targets(s)); + if PScopyToClipboard(cliText) set(hBtn(s), 'String', 'Copied!'); else - PSshowCLIDialog(txt); + PSshowCLIDialog(cliText); end end From 6eb07bdb39e641b87a1b641aaff0acbb41d570b8 Mon Sep 17 00:00:00 2001 From: Grzegorz Sterniczuk Date: Sun, 20 Sep 2026 11:03:59 +0200 Subject: [PATCH 23/33] feat(ui): make autotune a full-screen tool with a step response per axis --- src/ui/PSautotuneUI.m | 234 ++++++++++++++++++++++++++---------------- 1 file changed, 147 insertions(+), 87 deletions(-) diff --git a/src/ui/PSautotuneUI.m b/src/ui/PSautotuneUI.m index 9dd8435..e3e967e 100644 --- a/src/ui/PSautotuneUI.m +++ b/src/ui/PSautotuneUI.m @@ -6,7 +6,8 @@ function PSautotuneUI(T, setupInfo, Fs, tIND, logName) % Everything is worked out before the window opens, so it never appears with % empty rows. The flown value is printed once per gain and the three settings % print only what they would change it to. Stability margins live in the Chirp -% Analysis window; this one answers what to type into the CLI. +% Analysis window; this one answers what to type into the CLI, and shows what +% each answer does to the step response. if nargin < 5, logName = ''; end @@ -14,18 +15,22 @@ function PSautotuneUI(T, setupInfo, Fs, tIND, logName) fontsz = th.fontsz; axNames = {'Roll', 'Pitch', 'Yaw'}; +axCols = {th.axisRoll, th.axisPitch, th.axisYaw}; gainNames = {'P', 'I', 'D'}; setNames = {'CALM', 'NORMAL', 'SHARP'}; setBlurb = {'most margin', 'recommended', 'least margin'}; -targets = [72.5 60 50]; +setCols = {th.bodeCoherence, th.textAccent, th.btnReset}; +targets = [72.5 60 50]; %% --- work it all out first --- wb = []; try wb = waitbar(0, 'Measuring the plant...'); catch, end ids = []; res = cell(3, 3); +stepDat = cell(1, 3); gateMsg = {'', '', ''}; nStep = 12; done = 0; + for a = 1:3 tick(done/nStep, sprintf('Measuring the %s plant...', lower(axNames{a}))); id = PSidentifyChirp(T, setupInfo, Fs, tIND, a-1); @@ -45,6 +50,7 @@ function PSautotuneUI(T, setupInfo, Fs, tIND, logName) struct('pmTarget', targets(s), 'dClamp', [0.6 ginfo.dClampHi])); done = done + 1; end + stepDat{a} = stepsFor(ids(a), res(a,:)); end try close(wb); catch, end @@ -53,135 +59,130 @@ function PSautotuneUI(T, setupInfo, Fs, tIND, logName) old = findobj('Type', 'figure', 'Name', figName); if ~isempty(old), close(old); end -figW = 604; figH = 520; screensz = get(0, 'ScreenSize'); -fig = figure('Name', figName, 'NumberTitle', 'off', 'Color', th.figBg, ... - 'Position', [round((screensz(3)-figW)/2) round((screensz(4)-figH)/2) figW figH], ... - 'MenuBar', 'none', 'ToolBar', 'none', 'Resize', 'off'); - -mL = 24; -xAx = mL; wAx = 56; -xGain = 86; wGain = 16; -xNow = 104; wNow = 60; -xCol = [200 330 460]; wCol = 120; -hiCol = min(1, th.figBg + 0.04); -colBg = {th.figBg, hiCol, th.figBg}; +fig = figure('Name', figName, 'NumberTitle', 'off', ... + 'Color', th.figBg, ... + 'Position', round([0 0 screensz(3) screensz(4)])); -% rows are exactly as tall as they are spaced, so the highlighted column reads -% as one block instead of a stack of stripes -rowH = 18; grpGap = 14; yTop = figH - 204; -rowY = zeros(3, 3); -for g = 1:3 - for k = 1:3 - rowY(g,k) = yTop - (g-1)*(3*rowH + grpGap) - (k-1)*rowH; +plotL = 0.05; plotR = 0.66; colW = plotR - plotL; +topMargin = 0.03; botMargin = 0.06; rowGap = 0.045; +plotH = (1 - topMargin - botMargin - 2*rowGap) / 3; +plotB = [1-topMargin-plotH, 0, 0]; +plotB(2) = plotB(1) - plotH - rowGap; +plotB(3) = plotB(2) - plotH - rowGap; + +tShow = 250; % the same window on every axis, so the three can be compared + +for a = 1:3 + ax = axes('Parent', fig, 'Units', 'normalized', ... + 'Position', [plotL plotB(a) colW plotH]); + PSstyleAxes(ax, th); + hold(ax, 'on'); + d = stepDat{a}; + if isempty(d) + text(0.5, 0.5, gateMsg{a}, 'Parent', ax, 'Units', 'normalized', ... + 'HorizontalAlignment', 'center', 'Color', th.btnReset, 'FontSize', fontsz); + set(ax, 'XTick', [], 'YTick', []); + else + line(ax, [0 tShow], [1 1], 'Color', th.bodeRef, 'LineStyle', ':'); + % legend off explicit handles - the reference line would otherwise take + % the first entry and shift every label onto the wrong curve + hs = plot(ax, d.t, d.y0, 'Color', th.textSecondary, 'LineWidth', 1.4, ... + 'LineStyle', '--'); + leg = {'as flown'}; + for s = 1:3 + if isempty(d.y{s}), continue; end + hs(end+1) = plot(ax, d.t, d.y{s}, 'Color', setCols{s}, 'LineWidth', 1.6); + leg{end+1} = setNames{s}; + end + set(ax, 'XLim', [0 tShow], 'YLim', [0 1.6]); + if a == 1 + hl = legend(ax, hs, leg, 'Location', 'southeast'); + try PSstyleLegend(hl, th); catch, end + end + end + set(get(ax, 'YLabel'), 'String', 'Step'); + if a == 3 + set(get(ax, 'XLabel'), 'String', 'Time (ms)'); end + title(ax, axNames{a}, 'Color', axCols{a}); end +%% --- control column --- +cpL = 0.69; +xGain = cpL + 0.030; wGain = 0.014; +xNow = cpL + 0.046; wNow = 0.038; +xSet = cpL + [0.115 0.172 0.229]; wSet = 0.052; +rh = 0.026; grpGap = 0.016; +hiCol = min(1, th.figBg + 0.04); +colBg = {th.figBg, hiCol, th.figBg}; + txt = @(s, x, y, w, col, align, fs, wt, bg) uicontrol(fig, 'Style', 'text', ... - 'Units', 'pixels', 'String', s, 'Position', [x y w 18], 'FontSize', fs, ... + 'Units', 'normalized', 'String', s, 'Position', [x y w rh], 'FontSize', fs, ... 'FontWeight', wt, 'HorizontalAlignment', align, ... 'ForegroundColor', col, 'BackgroundColor', bg); -rule = @(y) uicontrol(fig, 'Style', 'text', 'Units', 'pixels', 'String', '', ... - 'Position', [mL y figW-2*mL 1], ... - 'BackgroundColor', th.gridColor, 'ForegroundColor', th.gridColor); - -uicontrol(fig, 'Style', 'text', 'Units', 'pixels', 'String', 'Autotune', ... - 'Position', [mL figH-42 200 24], 'FontSize', fontsz+3, 'FontWeight', 'bold', ... +uicontrol(fig, 'Style', 'text', 'Units', 'normalized', 'String', 'Autotune', ... + 'Position', [cpL 0.948 0.14 0.032], 'FontSize', fontsz+3, 'FontWeight', 'bold', ... 'HorizontalAlignment', 'left', ... 'ForegroundColor', th.textPrimary, 'BackgroundColor', th.figBg); -txt(logName, figW-mL-380, figH-40, 380, th.textSecondary, 'right', fontsz, 'normal', th.figBg); -rule(figH-54); +txt(logName, cpL, 0.918, 0.281, th.textSecondary, 'left', fontsz, 'normal', th.figBg); -%% --- one plain sentence about what the log says --- -hot = {}; soft = {}; refused = {}; -for a = 1:3 - if ~isempty(gateMsg{a}), refused{end+1} = axNames{a}; continue; end - rn = res{a,2}; - if isempty(rn) || ~rn.ok, continue; end - if rn.gains.P < ids(a).gains.P, hot{end+1} = axNames{a}; - elseif rn.gains.P > ids(a).gains.P, soft{end+1} = axNames{a}; - end -end -lines = {}; -if ~isempty(hot) - lines{end+1} = sprintf('%s want less gain - they are flying close to the limit.', ... - strjoin(hot, ' and ')); -end -if ~isempty(soft) - lines{end+1} = sprintf('%s can take more gain than it is flying.', strjoin(soft, ' and ')); -end -if ~isempty(refused) - lines{end+1} = sprintf('%s could not be read from this log.', strjoin(refused, ' and ')); -end -if isempty(lines), lines{end+1} = 'Nothing to change on this log.'; end -uicontrol(fig, 'Style', 'text', 'Units', 'pixels', 'String', lines, ... - 'Position', [mL figH-112 figW-2*mL 48], 'FontSize', fontsz, ... +uicontrol(fig, 'Style', 'text', 'Units', 'normalized', 'String', summaryLines(), ... + 'Position', [cpL 0.828 0.281 0.075], 'FontSize', fontsz, ... 'HorizontalAlignment', 'left', ... 'ForegroundColor', th.textPrimary, 'BackgroundColor', th.figBg); -rule(figH-124); -%% --- header --- -txt('now', xNow, figH-152, wNow, th.textSecondary, 'right', fontsz, 'normal', th.figBg); +txt('now', xNow, 0.782, wNow, th.textSecondary, 'right', fontsz, 'normal', th.figBg); for s = 1:3 - txt(setNames{s}, xCol(s), figH-152, wCol, th.textPrimary, 'right', fontsz, 'bold', colBg{s}); - txt(setBlurb{s}, xCol(s), figH-168, wCol, th.textSecondary, 'right', fontsz-1, 'normal', colBg{s}); + txt(setNames{s}, xSet(s), 0.782, wSet, th.textPrimary, 'right', fontsz, 'bold', colBg{s}); + txt(setBlurb{s}, xSet(s), 0.759, wSet, th.textSecondary, 'right', fontsz-2, 'normal', colBg{s}); end -rule(figH-178); -%% --- one row per gain, the flown value printed once --- +yTop = 0.700; for g = 1:3 - txt(axNames{g}, xAx, rowY(g,1), wAx, th.textPrimary, 'left', fontsz, 'bold', th.figBg); + yG = yTop - (g-1)*(3*rh + grpGap); + txt(axNames{g}, cpL, yG, 0.030, axCols{g}, 'left', fontsz, 'bold', th.figBg); if ~isempty(gateMsg{g}) - txt(gateMsg{g}, xGain, rowY(g,1), figW-xGain-mL, th.btnReset, 'left', fontsz, 'normal', th.figBg); - if g < 3, rule(rowY(g,3) - grpGap/2); end + txt('not usable', xGain, yG, 0.10, th.btnReset, 'left', fontsz, 'normal', th.figBg); continue end for k = 1:3 + yR = yG - (k-1)*rh; was = gainOf(ids(g).gains, k); - txt(gainNames{k}, xGain, rowY(g,k), wGain, th.textSecondary, 'left', fontsz, 'normal', th.figBg); - txt(sprintf('%d', was), xNow, rowY(g,k), wNow, th.textSecondary, 'right', fontsz, 'normal', th.figBg); + txt(gainNames{k}, xGain, yR, wGain, th.textSecondary, 'left', fontsz, 'normal', th.figBg); + txt(sprintf('%d', was), xNow, yR, wNow, th.textSecondary, 'right', fontsz, 'normal', th.figBg); for s = 1:3 r = res{g,s}; if isempty(r) || ~r.ok - str = ''; col = th.textSecondary; - if k == 1, str = 'out of reach'; end - txt(str, xCol(s), rowY(g,k), wCol, col, 'right', fontsz-1, 'normal', colBg{s}); + str = ''; + if k == 1, str = 'n/a'; end + txt(str, xSet(s), yR, wSet, th.textSecondary, 'right', fontsz, 'normal', colBg{s}); else - now = gainOf(r.gains, k); - txt(sprintf('%d', now), xCol(s), rowY(g,k), wCol, th.textPrimary, ... - 'right', fontsz, 'normal', colBg{s}); + txt(sprintf('%d', gainOf(r.gains, k)), xSet(s), yR, wSet, ... + th.textPrimary, 'right', fontsz, 'normal', colBg{s}); end end end - if g < 3, rule(rowY(g,3) - grpGap/2); end end -%% --- footer and the three copies --- -band = []; -for a = 1:3 - if isfinite(ids(a).fTrust), band(end+1) = ids(a).fTrust; end -end -foot = {'Pick a column and paste it into the Betaflight CLI. Feedforward is left as flown.'}; -if ~isempty(band) - foot{end+1} = sprintf(['Measured from the chirp up to %.0f Hz. ' ... - 'Fly it and check before trusting it.'], min(band)); -end -uicontrol(fig, 'Style', 'text', 'Units', 'pixels', 'String', foot, ... - 'Position', [mL 82 figW-2*mL 36], 'FontSize', fontsz-1, ... +uicontrol(fig, 'Style', 'text', 'Units', 'normalized', 'String', footLines(), ... + 'Position', [cpL 0.385 0.281 0.060], 'FontSize', fontsz-1, ... 'HorizontalAlignment', 'left', ... 'ForegroundColor', th.textSecondary, 'BackgroundColor', th.figBg); hBtn = zeros(1, 3); for s = 1:3 - hBtn(s) = uicontrol(fig, 'Style', 'pushbutton', 'Units', 'pixels', ... + hBtn(s) = uicontrol(fig, 'Style', 'pushbutton', 'Units', 'normalized', ... 'String', ['Copy ' setNames{s}], ... - 'Position', [xCol(s) 30 wCol 32], 'FontSize', fontsz, 'FontWeight', 'bold', ... + 'Position', [xSet(s)-0.004 0.330 wSet+0.008 0.038], ... + 'FontSize', fontsz, 'FontWeight', 'bold', ... 'BackgroundColor', th.btnBg, 'ForegroundColor', th.textAccent, ... 'Callback', @(~,~) copyCLI(s)); end PSstyleControls(fig, th); +PSdatatipSetup(fig); function tick(frac, msg) @@ -190,6 +191,65 @@ function tick(frac, msg) end + function d = stepsFor(id, rrow) + d = []; + keep = id.freq > 0 & id.freq <= id.fTrust; + if sum(keep) < 16, return; end + fk = id.freq(keep); + Pk = id.G_plant(keep); + fmax = min(id.fTrust, 300); + [A, D, F] = PSbuildController(id.gains, id.fp, id.FsPid, fk); + Tp = PSpredictClosedLoop(Pk, A, D, F); + [d.t, d.y0] = PSstepFromFRD(fk, Tp, fmax); + d.y = cell(1, 3); + for s = 1:3 + r = rrow{s}; + if isempty(r) || ~r.ok, continue; end + [A, D, F] = PSbuildController(r.gains, id.fp, id.FsPid, fk); + Tp = PSpredictClosedLoop(Pk, A, D, F); + [~, d.y{s}] = PSstepFromFRD(fk, Tp, fmax); + end + end + + + function lines = summaryLines() + hot = {}; soft = {}; bad = {}; + for a = 1:3 + if ~isempty(gateMsg{a}), bad{end+1} = axNames{a}; continue; end + rn = res{a,2}; + if isempty(rn) || ~rn.ok, continue; end + if rn.gains.P < ids(a).gains.P, hot{end+1} = axNames{a}; + elseif rn.gains.P > ids(a).gains.P, soft{end+1} = axNames{a}; + end + end + lines = {}; + if ~isempty(hot) + lines{end+1} = sprintf('%s want less gain - flying close to the limit.', ... + strjoin(hot, ' and ')); + end + if ~isempty(soft) + lines{end+1} = sprintf('%s can take more gain.', strjoin(soft, ' and ')); + end + if ~isempty(bad) + lines{end+1} = sprintf('%s could not be read from this log.', strjoin(bad, ' and ')); + end + if isempty(lines), lines{end+1} = 'Nothing to change on this log.'; end + end + + + function foot = footLines() + band = []; + for a = 1:3 + if isfinite(ids(a).fTrust), band(end+1) = ids(a).fTrust; end + end + foot = {'Pick a column, paste it into the Betaflight CLI.', ... + 'Feedforward is left as flown.'}; + if ~isempty(band) + foot{end+1} = sprintf('Measured to %.0f Hz. Fly it and check.', min(band)); + end + end + + function v = gainOf(gg, k) switch k case 1, v = gg.P; From cc0c3197beff7da19d7338dcb70f50eee1920367 Mon Sep 17 00:00:00 2001 From: Grzegorz Sterniczuk Date: Sun, 20 Sep 2026 11:53:31 +0200 Subject: [PATCH 24/33] fix(ui): band the table rows, colour each value by direction, one colour per copy button --- src/ui/PSautotuneUI.m | 53 ++++++++++++++++++++++++++----------------- 1 file changed, 32 insertions(+), 21 deletions(-) diff --git a/src/ui/PSautotuneUI.m b/src/ui/PSautotuneUI.m index e3e967e..0dd7b96 100644 --- a/src/ui/PSautotuneUI.m +++ b/src/ui/PSautotuneUI.m @@ -96,10 +96,9 @@ function PSautotuneUI(T, setupInfo, Fs, tIND, logName) leg{end+1} = setNames{s}; end set(ax, 'XLim', [0 tShow], 'YLim', [0 1.6]); - if a == 1 - hl = legend(ax, hs, leg, 'Location', 'southeast'); - try PSstyleLegend(hl, th); catch, end - end + hl = legend(ax, hs, leg, 'Location', 'southeast'); + try PSstyleLegend(hl, th); catch, end + try set(hl, 'FontSize', fontsz+1); catch, end end set(get(ax, 'YLabel'), 'String', 'Step'); if a == 3 @@ -110,12 +109,14 @@ function PSautotuneUI(T, setupInfo, Fs, tIND, logName) %% --- control column --- cpL = 0.69; -xGain = cpL + 0.030; wGain = 0.014; -xNow = cpL + 0.046; wNow = 0.038; -xSet = cpL + [0.115 0.172 0.229]; wSet = 0.052; +xGain = cpL + 0.032; wGain = 0.014; +xNow = cpL + 0.048; wNow = 0.042; +xSet = cpL + [0.095 0.155 0.215]; wSet = 0.055; rh = 0.026; grpGap = 0.016; -hiCol = min(1, th.figBg + 0.04); -colBg = {th.figBg, hiCol, th.figBg}; +% banding so the eye keeps the row across the four numbers, with the +% recommended column a shade lighter still +band = {th.figBg, min(1, th.figBg + 0.022)}; +cellBg = @(k, s) min(1, band{mod(k,2)+1} + 0.035*(s == 2)); txt = @(s, x, y, w, col, align, fs, wt, bg) uicontrol(fig, 'Style', 'text', ... 'Units', 'normalized', 'String', s, 'Position', [x y w rh], 'FontSize', fs, ... @@ -133,10 +134,11 @@ function PSautotuneUI(T, setupInfo, Fs, tIND, logName) 'HorizontalAlignment', 'left', ... 'ForegroundColor', th.textPrimary, 'BackgroundColor', th.figBg); -txt('now', xNow, 0.782, wNow, th.textSecondary, 'right', fontsz, 'normal', th.figBg); +txt('now', xNow, 0.782, wNow, th.textSecondary, 'right', fontsz, 'bold', th.figBg); for s = 1:3 - txt(setNames{s}, xSet(s), 0.782, wSet, th.textPrimary, 'right', fontsz, 'bold', colBg{s}); - txt(setBlurb{s}, xSet(s), 0.759, wSet, th.textSecondary, 'right', fontsz-2, 'normal', colBg{s}); + hdrBg = min(1, th.figBg + 0.035*(s == 2)); + txt(setNames{s}, xSet(s), 0.782, wSet, setCols{s}, 'right', fontsz+1, 'bold', hdrBg); + txt(setBlurb{s}, xSet(s), 0.759, wSet, th.textSecondary, 'right', fontsz-2, 'normal', hdrBg); end yTop = 0.700; @@ -150,17 +152,22 @@ function PSautotuneUI(T, setupInfo, Fs, tIND, logName) for k = 1:3 yR = yG - (k-1)*rh; was = gainOf(ids(g).gains, k); - txt(gainNames{k}, xGain, yR, wGain, th.textSecondary, 'left', fontsz, 'normal', th.figBg); - txt(sprintf('%d', was), xNow, yR, wNow, th.textSecondary, 'right', fontsz, 'normal', th.figBg); + txt(gainNames{k}, xGain, yR, wGain, th.textSecondary, 'left', fontsz, 'bold', cellBg(k,0)); + txt(sprintf('%d', was), xNow, yR, wNow, th.textPrimary, 'right', fontsz+1, 'normal', cellBg(k,0)); for s = 1:3 r = res{g,s}; if isempty(r) || ~r.ok str = ''; if k == 1, str = 'n/a'; end - txt(str, xSet(s), yR, wSet, th.textSecondary, 'right', fontsz, 'normal', colBg{s}); + txt(str, xSet(s), yR, wSet, th.textSecondary, 'right', fontsz, 'normal', cellBg(k,s)); else - txt(sprintf('%d', gainOf(r.gains, k)), xSet(s), yR, wSet, ... - th.textPrimary, 'right', fontsz, 'normal', colBg{s}); + now = gainOf(r.gains, k); + % colour says the direction, so a glance along the row reads as + % a change rather than four unrelated numbers + col = th.textSecondary; + if now < was, col = th.btnReset; elseif now > was, col = th.bodeCoherence; end + txt(sprintf('%d', now), xSet(s), yR, wSet, col, 'right', fontsz+1, ... + 'normal', cellBg(k,s)); end end end @@ -171,15 +178,19 @@ function PSautotuneUI(T, setupInfo, Fs, tIND, logName) 'HorizontalAlignment', 'left', ... 'ForegroundColor', th.textSecondary, 'BackgroundColor', th.figBg); +% one button per column, in that column's colour, so the button and the curve +% it belongs to read as the same thing hBtn = zeros(1, 3); for s = 1:3 hBtn(s) = uicontrol(fig, 'Style', 'pushbutton', 'Units', 'normalized', ... - 'String', ['Copy ' setNames{s}], ... - 'Position', [xSet(s)-0.004 0.330 wSet+0.008 0.038], ... - 'FontSize', fontsz, 'FontWeight', 'bold', ... - 'BackgroundColor', th.btnBg, 'ForegroundColor', th.textAccent, ... + 'String', setNames{s}, ... + 'Position', [xSet(s) 0.318 wSet 0.046], ... + 'FontSize', fontsz+1, 'FontWeight', 'bold', ... + 'HorizontalAlignment', 'center', ... + 'BackgroundColor', th.btnBg, 'ForegroundColor', setCols{s}, ... 'Callback', @(~,~) copyCLI(s)); end +txt('Copy to CLI:', cpL, 0.328, 0.075, th.textSecondary, 'left', fontsz, 'normal', th.figBg); PSstyleControls(fig, th); PSdatatipSetup(fig); From 4a18ecedd0bc26ac45f275f7e5044eb43a825f9a Mon Sep 17 00:00:00 2001 From: Grzegorz Sterniczuk Date: Sun, 20 Sep 2026 12:38:00 +0200 Subject: [PATCH 25/33] fix(chirp): tune I against the step instead of scaling it blindly with P --- src/core/PSautotuneSearch.m | 54 +++++++++++++++++++++++++++++++++-- tests/test_PSautotuneSearch.m | 14 +++++++++ 2 files changed, 66 insertions(+), 2 deletions(-) diff --git a/src/core/PSautotuneSearch.m b/src/core/PSautotuneSearch.m index c571b70..88bfda4 100644 --- a/src/core/PSautotuneSearch.m +++ b/src/core/PSautotuneSearch.m @@ -20,7 +20,8 @@ o = struct('pmTarget', 60, 'msMax', 2.0, 'peakMaxDb', 6, ... 'magAtTrustDb', -6, 'wcpFracTrust', 0.5, 'wcpMaxRatio', 3, ... 'dDomRatio', 2.0, ... - 'pClamp', [0.25 2.0], 'dClamp', [0.6 1.25], 'pidsumFrac', 0.9); + 'pClamp', [0.25 2.0], 'dClamp', [0.6 1.25], 'pidsumFrac', 0.9, ... + 'peakStepMin', 1.05); fn = fieldnames(o); for k = 1:numel(fn) if isfield(opt, fn{k}) && ~isempty(opt.(fn{k})), o.(fn{k}) = opt.(fn{k}); end @@ -136,6 +137,19 @@ res.gains.P = g.P(iD,iP); res.gains.I = g.I(iD,iP); res.gains.D = g.D(iD,iP); + +% D acts on the gyro only, so the tracking response is P*A/(1+P*(A+D)) and the +% PI zero at Ki/Kp sits in the numerator where the margins never see it. +% Measured on the pichim corpus: at a fixed cell, I moved the step overshoot +% from 1.11 to 1.17 while the phase margin moved 0.6 deg and Ms not at all. +% So take the most I that does not make the step worse than it already flies - +% I is what rejects disturbances, and it is nearly free of the margins. +[res.gains.I, res.peak, res.peak0] = shapeI(id, res.gains, I0, o, base.wcp); +if isfinite(res.peak) && isfinite(res.peak0) && res.peak > max(res.peak0, o.peakStepMin) + 1e-6 + res.notes{end+1} = sprintf(['step overshoot still %.0f%% against %.0f%% flown - ' ... + 'the margins would not allow less I'], ... + 100*(res.peak-1), 100*(res.peak0-1)); +end res.scale = struct('P', ratio(res.gains.P, P0), 'I', ratio(res.gains.I, I0), ... 'D', ratio(res.gains.D, D0), 'F', 1); @@ -155,6 +169,42 @@ end +function [Ibest, pk, pk0] = shapeI(id, gains, I0, o, wcp0) + Ibest = gains.I; + pk0 = stepPeak(id, id.gains); + pk = stepPeak(id, gains); + if I0 <= 0 || ~isfinite(pk0) || ~isfinite(pk), return; end + limit = max(pk0, o.peakStepMin); + if pk <= limit, return; end + + keep = id.freq > 0 & id.freq <= id.fTrust; + fk = id.freq(keep); + Pk = id.G_plant(keep); + + % walk I down, but only over values that still clear every constraint - an + % I that fixes the step and loses the margin is not an answer + for Ii = (gains.I-1):-1:max(1, round(0.2*I0)) + trial = gains; trial.I = Ii; + [A, D, F] = PSbuildController(trial, id.fp, id.FsPid, fk); + if ~admissible(evalOne(Pk, fk, A, D, F), o, id.fTrust, wcp0), continue; end + p = stepPeak(id, trial); + if p < pk, Ibest = Ii; pk = p; end + if p <= limit, break; end + end +end + + +function pk = stepPeak(id, gains) + pk = NaN; + if isempty(id.G_plant), return; end + [A, D, F] = PSbuildController(gains, id.fp, id.FsPid, id.freq); + Tp = PSpredictClosedLoop(id.G_plant, A, D, F); + [~, s] = PSstepFromFRD(id.freq, Tp, min(id.fTrust, 300)); + if isempty(s), return; end + pk = max(s); +end + + function ok = admissible(c, o, fTrust, wcp0) ok = ~isnan(c.pm) && c.pm >= o.pmTarget ... && c.ms <= o.msMax ... @@ -209,7 +259,7 @@ res = struct('ok', false, 'reason', 'no-candidate', ... 'gains', [], 'scale', [], 'notes', {{}}, ... 'pm', NaN, 'gm', NaN, 'ms', NaN, 'wcp', NaN, 'wcg', NaN, ... - 'peakDb', NaN, ... + 'peakDb', NaN, 'peak', NaN, 'peak0', NaN, ... 'pm0', NaN, 'gm0', NaN, 'ms0', NaN, 'wcp0', NaN, ... 'wcg0', NaN, 'peakDb0', NaN, ... 'iD', NaN, 'iP', NaN, 'grid', [], 'opt', []); diff --git a/tests/test_PSautotuneSearch.m b/tests/test_PSautotuneSearch.m index 62e8b72..9acae57 100644 --- a/tests/test_PSautotuneSearch.m +++ b/tests/test_PSautotuneSearch.m @@ -200,6 +200,20 @@ %! assert(rW.ok, 'and the wider floor must reach it'); %! assert(rW.gains.P == 45, sprintf('expected P 45, got %d', rW.gains.P)); +%!test +%! % I is shaped against the step, not scaled blindly with P. D acts on the +%! % gyro only, so the tracking response is P*A/(1+P*(A+D)) and the PI zero at +%! % Ki/Kp sits in the numerator where the margins never see it: on the pichim +%! % corpus, at a fixed cell, I moved the overshoot from 1.11 to 1.17 while the +%! % phase margin moved 0.6 deg. So the proposal must not hand back a worse step +%! % than the one being flown. +%! res = PSautotuneSearch(mkfix('P', 80, 'I', 120), struct('pmTarget', 60)); +%! assert(res.ok, res.reason); +%! assert(isfinite(res.peak) && isfinite(res.peak0), 'both peaks must be reported'); +%! assert(res.peak <= max(res.peak0, 1.05) + 1e-9, ... +%! sprintf('step got worse: %.3f against %.3f flown', res.peak, res.peak0)); +%! assert(res.pm >= 60 - 1e-6, sprintf('shaping I cost phase margin: %.2f', res.pm)); + %!test %! % Reason codes have to be usable by the UI without guessing %! r = PSautotuneSearch(mkfix('P', 40), struct('pmTarget', 60, 'pClamp', [1 1])); From bc743df23d934479644cbba028a790bce5a34dd0 Mon Sep 17 00:00:00 2001 From: Grzegorz Sterniczuk Date: Sun, 20 Sep 2026 12:38:00 +0200 Subject: [PATCH 26/33] fix(ui): reconstruct the step on the full grid, name overlapping curves, key the colours --- src/ui/PSautotuneUI.m | 45 +++++++++++++++++++++++++++++-------------- 1 file changed, 31 insertions(+), 14 deletions(-) diff --git a/src/ui/PSautotuneUI.m b/src/ui/PSautotuneUI.m index 0dd7b96..4a5090e 100644 --- a/src/ui/PSautotuneUI.m +++ b/src/ui/PSautotuneUI.m @@ -90,10 +90,23 @@ function PSautotuneUI(T, setupInfo, Fs, tIND, logName) hs = plot(ax, d.t, d.y0, 'Color', th.textSecondary, 'LineWidth', 1.4, ... 'LineStyle', '--'); leg = {'as flown'}; + % two settings can land on the same gains - on yaw Ms binds before the + % phase margin does, so NORMAL and SHARP come out identical. Drawing + % both would hide one curve completely under the other. for s = 1:3 if isempty(d.y{s}), continue; end + same = {}; + for s2 = (s+1):3 + if ~isempty(res{a,s}) && ~isempty(res{a,s2}) && ... + isequal(res{a,s}.gains, res{a,s2}.gains) + same{end+1} = setNames{s2}; + d.y{s2} = []; + end + end + nm = setNames{s}; + if ~isempty(same), nm = strjoin([{nm} same], ' = '); end hs(end+1) = plot(ax, d.t, d.y{s}, 'Color', setCols{s}, 'LineWidth', 1.6); - leg{end+1} = setNames{s}; + leg{end+1} = nm; end set(ax, 'XLim', [0 tShow], 'YLim', [0 1.6]); hl = legend(ax, hs, leg, 'Location', 'southeast'); @@ -173,8 +186,12 @@ function PSautotuneUI(T, setupInfo, Fs, tIND, logName) end end +% the colours in the table mean something, so say what +txt('lower than now', xNow, 0.420, 0.075, th.btnReset, 'left', fontsz-1, 'normal', th.figBg); +txt('higher', xNow+0.080, 0.420, 0.050, th.bodeCoherence, 'left', fontsz-1, 'normal', th.figBg); + uicontrol(fig, 'Style', 'text', 'Units', 'normalized', 'String', footLines(), ... - 'Position', [cpL 0.385 0.281 0.060], 'FontSize', fontsz-1, ... + 'Position', [cpL 0.345 0.281 0.060], 'FontSize', fontsz-1, ... 'HorizontalAlignment', 'left', ... 'ForegroundColor', th.textSecondary, 'BackgroundColor', th.figBg); @@ -184,13 +201,13 @@ function PSautotuneUI(T, setupInfo, Fs, tIND, logName) for s = 1:3 hBtn(s) = uicontrol(fig, 'Style', 'pushbutton', 'Units', 'normalized', ... 'String', setNames{s}, ... - 'Position', [xSet(s) 0.318 wSet 0.046], ... + 'Position', [xSet(s) 0.272 wSet 0.046], ... 'FontSize', fontsz+1, 'FontWeight', 'bold', ... 'HorizontalAlignment', 'center', ... 'BackgroundColor', th.btnBg, 'ForegroundColor', setCols{s}, ... 'Callback', @(~,~) copyCLI(s)); end -txt('Copy to CLI:', cpL, 0.328, 0.075, th.textSecondary, 'left', fontsz, 'normal', th.figBg); +txt('Copy to CLI:', cpL, 0.282, 0.075, th.textSecondary, 'left', fontsz, 'normal', th.figBg); PSstyleControls(fig, th); PSdatatipSetup(fig); @@ -204,21 +221,21 @@ function tick(frac, msg) function d = stepsFor(id, rrow) d = []; - keep = id.freq > 0 & id.freq <= id.fTrust; - if sum(keep) < 16, return; end - fk = id.freq(keep); - Pk = id.G_plant(keep); + if isempty(id.G_plant) || numel(id.freq) < 16, return; end + % the full grid, DC included: PSstepFromFRD reads bin 1 as DC and + % normalises by it, so handing it a band-limited grid shifts every + % frequency by one bin and the step never settles to 1 fmax = min(id.fTrust, 300); - [A, D, F] = PSbuildController(id.gains, id.fp, id.FsPid, fk); - Tp = PSpredictClosedLoop(Pk, A, D, F); - [d.t, d.y0] = PSstepFromFRD(fk, Tp, fmax); + [A, D, F] = PSbuildController(id.gains, id.fp, id.FsPid, id.freq); + Tp = PSpredictClosedLoop(id.G_plant, A, D, F); + [d.t, d.y0] = PSstepFromFRD(id.freq, Tp, fmax); d.y = cell(1, 3); for s = 1:3 r = rrow{s}; if isempty(r) || ~r.ok, continue; end - [A, D, F] = PSbuildController(r.gains, id.fp, id.FsPid, fk); - Tp = PSpredictClosedLoop(Pk, A, D, F); - [~, d.y{s}] = PSstepFromFRD(fk, Tp, fmax); + [A, D, F] = PSbuildController(r.gains, id.fp, id.FsPid, id.freq); + Tp = PSpredictClosedLoop(id.G_plant, A, D, F); + [~, d.y{s}] = PSstepFromFRD(id.freq, Tp, fmax); end end From d5e0bd31873de762a5a618b4901b5cf416cf7a00 Mon Sep 17 00:00:00 2001 From: Grzegorz Sterniczuk Date: Sun, 20 Sep 2026 13:05:37 +0200 Subject: [PATCH 27/33] feat(ui): add an editable column that draws the closed loop from your own gains --- src/ui/PSautotuneUI.m | 104 ++++++++++++++++++++++++++++++++++-------- 1 file changed, 84 insertions(+), 20 deletions(-) diff --git a/src/ui/PSautotuneUI.m b/src/ui/PSautotuneUI.m index 4a5090e..e1365b3 100644 --- a/src/ui/PSautotuneUI.m +++ b/src/ui/PSautotuneUI.m @@ -16,7 +16,7 @@ function PSautotuneUI(T, setupInfo, Fs, tIND, logName) axNames = {'Roll', 'Pitch', 'Yaw'}; axCols = {th.axisRoll, th.axisPitch, th.axisYaw}; -gainNames = {'P', 'I', 'D'}; +gainNames = {'P', 'I', 'D', 'FF'}; setNames = {'CALM', 'NORMAL', 'SHARP'}; setBlurb = {'most margin', 'recommended', 'least margin'}; setCols = {th.bodeCoherence, th.textAccent, th.btnReset}; @@ -64,7 +64,7 @@ function PSautotuneUI(T, setupInfo, Fs, tIND, logName) 'Color', th.figBg, ... 'Position', round([0 0 screensz(3) screensz(4)])); -plotL = 0.05; plotR = 0.66; colW = plotR - plotL; +plotL = 0.04; plotR = 0.585; colW = plotR - plotL; topMargin = 0.03; botMargin = 0.06; rowGap = 0.045; plotH = (1 - topMargin - botMargin - 2*rowGap) / 3; plotB = [1-topMargin-plotH, 0, 0]; @@ -72,6 +72,11 @@ function PSautotuneUI(T, setupInfo, Fs, tIND, logName) plotB(3) = plotB(2) - plotH - rowGap; tShow = 250; % the same window on every axis, so the three can be compared +customCol = th.btnDash6; +hCustom = zeros(1, 3); +axCustom = zeros(1, 3); +hEdit = zeros(3, 4); +hCustomInfo = []; for a = 1:3 ax = axes('Parent', fig, 'Units', 'normalized', ... @@ -108,6 +113,11 @@ function PSautotuneUI(T, setupInfo, Fs, tIND, logName) hs(end+1) = plot(ax, d.t, d.y{s}, 'Color', setCols{s}, 'LineWidth', 1.6); leg{end+1} = nm; end + % the try-it-yourself curve, empty until something is typed + hCustom(a) = plot(ax, nan, nan, 'Color', customCol, 'LineWidth', 1.8); + hs(end+1) = hCustom(a); + leg{end+1} = 'custom'; + axCustom(a) = ax; set(ax, 'XLim', [0 tShow], 'YLim', [0 1.6]); hl = legend(ax, hs, leg, 'Location', 'southeast'); try PSstyleLegend(hl, th); catch, end @@ -121,11 +131,12 @@ function PSautotuneUI(T, setupInfo, Fs, tIND, logName) end %% --- control column --- -cpL = 0.69; -xGain = cpL + 0.032; wGain = 0.014; -xNow = cpL + 0.048; wNow = 0.042; -xSet = cpL + [0.095 0.155 0.215]; wSet = 0.055; -rh = 0.026; grpGap = 0.016; +cpL = 0.615; +xGain = cpL + 0.028; wGain = 0.014; +xNow = cpL + 0.044; wNow = 0.040; +xSet = cpL + [0.090 0.146 0.202]; wSet = 0.052; +xCust = cpL + 0.262; wCust = 0.058; +rh = 0.024; grpGap = 0.014; nGain = 4; % banding so the eye keeps the row across the four numbers, with the % recommended column a shade lighter still band = {th.figBg, min(1, th.figBg + 0.022)}; @@ -140,10 +151,10 @@ function PSautotuneUI(T, setupInfo, Fs, tIND, logName) 'Position', [cpL 0.948 0.14 0.032], 'FontSize', fontsz+3, 'FontWeight', 'bold', ... 'HorizontalAlignment', 'left', ... 'ForegroundColor', th.textPrimary, 'BackgroundColor', th.figBg); -txt(logName, cpL, 0.918, 0.281, th.textSecondary, 'left', fontsz, 'normal', th.figBg); +txt(logName, cpL, 0.918, 0.340, th.textSecondary, 'left', fontsz, 'normal', th.figBg); uicontrol(fig, 'Style', 'text', 'Units', 'normalized', 'String', summaryLines(), ... - 'Position', [cpL 0.828 0.281 0.075], 'FontSize', fontsz, ... + 'Position', [cpL 0.828 0.340 0.075], 'FontSize', fontsz, ... 'HorizontalAlignment', 'left', ... 'ForegroundColor', th.textPrimary, 'BackgroundColor', th.figBg); @@ -153,25 +164,29 @@ function PSautotuneUI(T, setupInfo, Fs, tIND, logName) txt(setNames{s}, xSet(s), 0.782, wSet, setCols{s}, 'right', fontsz+1, 'bold', hdrBg); txt(setBlurb{s}, xSet(s), 0.759, wSet, th.textSecondary, 'right', fontsz-2, 'normal', hdrBg); end +txt('CUSTOM', xCust, 0.782, wCust, customCol, 'left', fontsz+1, 'bold', th.figBg); +txt('type and see', xCust, 0.759, wCust, th.textSecondary, 'left', fontsz-2, 'normal', th.figBg); yTop = 0.700; for g = 1:3 - yG = yTop - (g-1)*(3*rh + grpGap); - txt(axNames{g}, cpL, yG, 0.030, axCols{g}, 'left', fontsz, 'bold', th.figBg); + yG = yTop - (g-1)*(nGain*rh + grpGap); + txt(axNames{g}, cpL, yG, 0.028, axCols{g}, 'left', fontsz, 'bold', th.figBg); if ~isempty(gateMsg{g}) txt('not usable', xGain, yG, 0.10, th.btnReset, 'left', fontsz, 'normal', th.figBg); continue end - for k = 1:3 + for k = 1:nGain yR = yG - (k-1)*rh; was = gainOf(ids(g).gains, k); txt(gainNames{k}, xGain, yR, wGain, th.textSecondary, 'left', fontsz, 'bold', cellBg(k,0)); txt(sprintf('%d', was), xNow, yR, wNow, th.textPrimary, 'right', fontsz+1, 'normal', cellBg(k,0)); for s = 1:3 r = res{g,s}; - if isempty(r) || ~r.ok + % feedforward is not proposed - showing the flown number again in + % all three columns would be noise, so the row stays blank there + if k == 4 || isempty(r) || ~r.ok str = ''; - if k == 1, str = 'n/a'; end + if k == 1 && (isempty(r) || ~r.ok), str = 'n/a'; end txt(str, xSet(s), yR, wSet, th.textSecondary, 'right', fontsz, 'normal', cellBg(k,s)); else now = gainOf(r.gains, k); @@ -183,15 +198,28 @@ function PSautotuneUI(T, setupInfo, Fs, tIND, logName) 'normal', cellBg(k,s)); end end + start = was; + r = res{g,2}; + if ~isempty(r) && r.ok && k < 4, start = gainOf(r.gains, k); end + hEdit(g,k) = uicontrol(fig, 'Style', 'edit', 'Units', 'normalized', ... + 'String', sprintf('%d', start), ... + 'Position', [xCust yR wCust rh], 'FontSize', fontsz, ... + 'HorizontalAlignment', 'center', ... + 'Callback', @(~,~) onCustom(g)); end end % the colours in the table mean something, so say what -txt('lower than now', xNow, 0.420, 0.075, th.btnReset, 'left', fontsz-1, 'normal', th.figBg); -txt('higher', xNow+0.080, 0.420, 0.050, th.bodeCoherence, 'left', fontsz-1, 'normal', th.figBg); +txt('lower than now', xNow, 0.372, 0.075, th.btnReset, 'left', fontsz-1, 'normal', th.figBg); +txt('higher', xNow+0.080, 0.372, 0.050, th.bodeCoherence, 'left', fontsz-1, 'normal', th.figBg); + +hCustomInfo = uicontrol(fig, 'Style', 'text', 'Units', 'normalized', 'String', '', ... + 'Position', [cpL 0.336 0.340 rh], 'FontSize', fontsz-1, ... + 'HorizontalAlignment', 'left', ... + 'ForegroundColor', th.textSecondary, 'BackgroundColor', th.figBg); uicontrol(fig, 'Style', 'text', 'Units', 'normalized', 'String', footLines(), ... - 'Position', [cpL 0.345 0.281 0.060], 'FontSize', fontsz-1, ... + 'Position', [cpL 0.258 0.340 0.060], 'FontSize', fontsz-1, ... 'HorizontalAlignment', 'left', ... 'ForegroundColor', th.textSecondary, 'BackgroundColor', th.figBg); @@ -201,13 +229,13 @@ function PSautotuneUI(T, setupInfo, Fs, tIND, logName) for s = 1:3 hBtn(s) = uicontrol(fig, 'Style', 'pushbutton', 'Units', 'normalized', ... 'String', setNames{s}, ... - 'Position', [xSet(s) 0.272 wSet 0.046], ... + 'Position', [xSet(s) 0.196 wSet 0.046], ... 'FontSize', fontsz+1, 'FontWeight', 'bold', ... 'HorizontalAlignment', 'center', ... 'BackgroundColor', th.btnBg, 'ForegroundColor', setCols{s}, ... 'Callback', @(~,~) copyCLI(s)); end -txt('Copy to CLI:', cpL, 0.282, 0.075, th.textSecondary, 'left', fontsz, 'normal', th.figBg); +txt('Copy to CLI:', cpL, 0.206, 0.075, th.textSecondary, 'left', fontsz, 'normal', th.figBg); PSstyleControls(fig, th); PSdatatipSetup(fig); @@ -282,7 +310,43 @@ function tick(frac, msg) switch k case 1, v = gg.P; case 2, v = gg.I; - otherwise, v = gg.D; + case 3, v = gg.D; + otherwise, v = gg.F; + end + end + + + function onCustom(g) + id = ids(g); + if isempty(id.G_plant) || hCustom(g) == 0, return; end + v = zeros(1, 4); + for k = 1:4 + v(k) = round(str2double(get(hEdit(g,k), 'String'))); + if ~isfinite(v(k)) || v(k) < 0, v(k) = 0; end + set(hEdit(g,k), 'String', sprintf('%d', v(k))); + end + gg = struct('axis', id.axisIdx, 'P', v(1), 'I', v(2), 'D', v(3), 'F', v(4)); + [A, D, F] = PSbuildController(gg, id.fp, id.FsPid, id.freq); + Tp = PSpredictClosedLoop(id.G_plant, A, D, F); + [t, y] = PSstepFromFRD(id.freq, Tp, min(id.fTrust, 300)); + set(hCustom(g), 'XData', t, 'YData', y); + % feedforward never enters L = P*(A+D), so it cannot destabilise the + % loop - the margins below answer for P, I and D only + keep = id.freq > 0 & id.freq <= id.fTrust; + [A, D, F] = PSbuildController(gg, id.fp, id.FsPid, id.freq(keep)); + [~, L, S] = PSpredictClosedLoop(id.G_plant(keep), A, D, F); + [~, pm] = PSmarginsFromL(id.freq(keep), L); + if isnan(pm) + s = sprintf('%s custom: no 0 dB crossing in band', axNames{g}); + else + s = sprintf('%s custom: PM %.0f deg, Ms %.2f, overshoot %.0f%%', ... + axNames{g}, pm, max(abs(S)), 100*(max(y)-1)); + end + set(hCustomInfo, 'String', s); + if pm < 30 || max(abs(S)) > 2.5 + set(hCustomInfo, 'ForegroundColor', th.btnReset); + else + set(hCustomInfo, 'ForegroundColor', th.textSecondary); end end From 0a4797534d2200f13d015198142bf82d39de02a6 Mon Sep 17 00:00:00 2001 From: Grzegorz Sterniczuk Date: Sun, 20 Sep 2026 14:30:40 +0200 Subject: [PATCH 28/33] feat(chirp): judge whether the flown tune needs changing at all --- src/core/PSautotuneVerdict.m | 85 ++++++++++++++++++++++++++++++++++ src/ui/PSautotuneUI.m | 26 ++++++----- tests/test_PSautotuneVerdict.m | 70 ++++++++++++++++++++++++++++ 3 files changed, 169 insertions(+), 12 deletions(-) create mode 100644 src/core/PSautotuneVerdict.m create mode 100644 tests/test_PSautotuneVerdict.m diff --git a/src/core/PSautotuneVerdict.m b/src/core/PSautotuneVerdict.m new file mode 100644 index 0000000..17f9951 --- /dev/null +++ b/src/core/PSautotuneVerdict.m @@ -0,0 +1,85 @@ +function [needsWork, msgs, info] = PSautotuneVerdict(id, opt) +%% PSautotuneVerdict - does the tune being flown actually need changing? +% id - from PSidentifyChirp +% opt - threshold overrides; every threshold is echoed in info +% +% needsWork - true when the flown loop breaks a limit worth acting on +% msgs - what to tell the pilot, empty when there is nothing to say +% +% Separate from the search, which answers "what else is reachable". A loop can +% be perfectly healthy and still have a faster tune available; that is an +% option, not advice. +% +% Thresholds come from the pichim corpus, 18 axes over 6 logs, of which two +% (20250918_aosmini_01 roll and pitch) are plainly mistuned: +% +% 16 good axes the 2 bad ones +% phase margin 38 .. 82 deg 25, 29 deg +% closed loop pk 0.5 .. 1.4 dB 5.2, 6.5 dB +% step overshoot 6 .. 15 % 47, 50 % +% Ms 1.39 .. 2.23 2.12, 2.37 +% +% Ms is the one that does not separate them, so it is a backstop for something +% extreme rather than the test. Betaflight's own limit of 2.0 is drawn against +% a different quantity - their open loop is approximated as T/(1-T), which +% reads about 0.74x our sensitivity peak and 25 deg more phase margin on an +% axis with real D - so it cannot be carried over unchanged. + +if nargin < 2 || isempty(opt), opt = struct(); end + +o = struct('pmFloor', 35, 'peakDbMax', 3.0, 'overshootMax', 0.25, 'msMax', 2.7); +fn = fieldnames(o); +for k = 1:numel(fn) + if isfield(opt, fn{k}) && ~isempty(opt.(fn{k})), o.(fn{k}) = opt.(fn{k}); end +end + +info = o; +info.pm = NaN; info.ms = NaN; info.peakDb = NaN; info.overshoot = NaN; +msgs = {}; +needsWork = false; + +if ~isfield(id, 'ok') || ~id.ok || isempty(id.G_plant) + msgs{end+1} = 'The plant was not measured, so this tune cannot be judged'; + return +end + +keep = id.freq > 0 & id.freq <= id.fTrust; +if sum(keep) < 16 + msgs{end+1} = 'The trusted band is too narrow to judge this tune'; + return +end +fk = id.freq(keep); + +[A, D, F] = PSbuildController(id.gains, id.fp, id.FsPid, fk); +[Tc, L, S] = PSpredictClosedLoop(id.G_plant(keep), A, D, F); +[~, info.pm] = PSmarginsFromL(fk, L); +info.ms = max(abs(S)); +info.peakDb = 20*log10(max(abs(Tc)) + 1e-12); + +[A, D, F] = PSbuildController(id.gains, id.fp, id.FsPid, id.freq); +Tf = PSpredictClosedLoop(id.G_plant, A, D, F); +[~, s] = PSstepFromFRD(id.freq, Tf, min(id.fTrust, 300)); +if ~isempty(s), info.overshoot = max(s) - 1; end + +if ~isnan(info.pm) && info.pm < o.pmFloor + needsWork = true; + msgs{end+1} = sprintf(['Phase margin is %.0f deg, below the %.0f deg this ' ... + 'tool treats as the floor'], info.pm, o.pmFloor); +end +if info.peakDb > o.peakDbMax + needsWork = true; + msgs{end+1} = sprintf('The loop peaks %.1f dB above the setpoint, over %.1f dB', ... + info.peakDb, o.peakDbMax); +end +if isfinite(info.overshoot) && info.overshoot > o.overshootMax + needsWork = true; + msgs{end+1} = sprintf('A step overshoots by %.0f%%, over %.0f%%', ... + 100*info.overshoot, 100*o.overshootMax); +end +if info.ms > o.msMax + needsWork = true; + msgs{end+1} = sprintf('Disturbances are amplified %.1fx at worst, over %.1fx', ... + info.ms, o.msMax); +end + +end diff --git a/src/ui/PSautotuneUI.m b/src/ui/PSautotuneUI.m index e1365b3..81125a2 100644 --- a/src/ui/PSautotuneUI.m +++ b/src/ui/PSautotuneUI.m @@ -269,27 +269,29 @@ function tick(frac, msg) function lines = summaryLines() - hot = {}; soft = {}; bad = {}; + % the headline answers "does anything need changing", which is not the + % same question as "what else is reachable" - a healthy loop can still + % have a faster tune available, and that is an option, not advice + bad = {}; flagged = {}; why = {}; for a = 1:3 if ~isempty(gateMsg{a}), bad{end+1} = axNames{a}; continue; end - rn = res{a,2}; - if isempty(rn) || ~rn.ok, continue; end - if rn.gains.P < ids(a).gains.P, hot{end+1} = axNames{a}; - elseif rn.gains.P > ids(a).gains.P, soft{end+1} = axNames{a}; + [needs, vmsgs] = PSautotuneVerdict(ids(a)); + if needs + flagged{end+1} = axNames{a}; + if isempty(why), why = vmsgs(1); end end end lines = {}; - if ~isempty(hot) - lines{end+1} = sprintf('%s want less gain - flying close to the limit.', ... - strjoin(hot, ' and ')); - end - if ~isempty(soft) - lines{end+1} = sprintf('%s can take more gain.', strjoin(soft, ' and ')); + if ~isempty(flagged) + lines{end+1} = sprintf('%s needs attention.', strjoin(flagged, ' and ')); + if ~isempty(why), lines{end+1} = why{1}; end + else + lines{end+1} = 'Your tune is within limits - nothing here needs changing.'; + lines{end+1} = 'The columns are options, not advice.'; end if ~isempty(bad) lines{end+1} = sprintf('%s could not be read from this log.', strjoin(bad, ' and ')); end - if isempty(lines), lines{end+1} = 'Nothing to change on this log.'; end end diff --git a/tests/test_PSautotuneVerdict.m b/tests/test_PSautotuneVerdict.m new file mode 100644 index 0000000..b2a7176 --- /dev/null +++ b/tests/test_PSautotuneVerdict.m @@ -0,0 +1,70 @@ +% test_PSautotuneVerdict.m - does this tune need changing at all? +% +% Thresholds are read off the pichim corpus, 18 axes over 6 logs. Sixteen of +% them are well flown and two (20250918_aosmini_01 roll and pitch) plainly are +% not. Any limit worth having must separate those two groups; Ms does not, +% because its range overlaps them completely. + +%!function id = mkid(varargin) +%! p = struct('tau', 0.002, 'k', 180.13977, 'P', 46, 'I', 66, 'D', 0, 'F', 0, ... +%! 'dMax', 0, 'fTrust', 300); +%! for j = 1:2:numel(varargin), p.(varargin{j}) = varargin{j+1}; end +%! freq = (0:0.2:600)'; +%! w = 2*pi*freq; +%! G = p.k * exp(-1j*w*p.tau) ./ (1j*w); +%! G(1) = 1e9; +%! id = struct('ok', true, 'msg', '', 'freq', freq, 'G_plant', G, ... +%! 'G_track', [], 'C_track', ones(size(freq)), 'G_uw', [], ... +%! 'C_uw', [], 'G_ff', [], 'fp', [], 'FsPid', 2000, ... +%! 'gains', struct('axis',0,'P',p.P,'I',p.I,'D',p.D,'F',p.F,'dMax',p.dMax), ... +%! 'axisIdx', 0, 'axisName', 'Roll', 'fTrust', p.fTrust, ... +%! 'i0', 1, 'i1', 100, 'nSamp', 100, 'durSec', 20, 'chirpTime', 20, ... +%! 'gyroVar', 5e3, 'satFrac', 0, 'thrStd', 8, 'nSeg', 70, ... +%! 'axisD', ones(10,1), 'pidsumLimit', 800); +%!endfunction + +%!test +%! % A loop with plenty of margin needs nothing said about it +%! [needs, msgs] = PSautotuneVerdict(mkid('P', 26)); +%! assert(~needs, sprintf('a healthy tune was flagged: %s', strjoin(msgs, '; '))); +%! assert(isempty(msgs)); + +%!test +%! % Phase margin is what actually separated the corpus: the sixteen good axes +%! % run 38 to 82 deg, the two bad ones 25 and 29 +%! [needs, msgs] = PSautotuneVerdict(mkid('P', 90)); +%! assert(needs, 'a loop this tight must be flagged'); +%! assert(~isempty(strfind(lower(strjoin(msgs, ' ')), 'phase margin'))); + +%!test +%! % The floor is where it was measured to be, and it is settable +%! healthy = mkid('P', 26); +%! [a, ~, info] = PSautotuneVerdict(healthy); +%! assert(info.pmFloor == 35, 'the measured floor must be the default'); +%! assert(~a, 'this fixture is the healthy one'); +%! [b, msgs] = PSautotuneVerdict(healthy, struct('pmFloor', info.pm + 5)); +%! assert(b, 'a floor above the measured margin must flag it'); +%! assert(~isempty(strfind(lower(strjoin(msgs, ' ')), 'phase margin'))); + +%!test +%! % Ms alone is not a verdict. On the corpus a well flown axis reaches 2.23 and +%! % a badly flown one sits at 2.12, so no Ms line separates them - it stays a +%! % backstop for the extreme, not the headline. +%! [~, ~, info] = PSautotuneVerdict(mkid()); +%! assert(info.msMax >= 2.5, 'Ms must not be the tight test any more'); +%! assert(isfield(info, 'ms') && isfinite(info.ms), 'but it is still reported'); + +%!test +%! % Everything measured is handed back so the window can say why +%! [~, ~, info] = PSautotuneVerdict(mkid()); +%! for f = {'pm', 'ms', 'peakDb', 'overshoot', 'pmFloor', 'peakDbMax', 'overshootMax'} +%! assert(isfield(info, f{1}), sprintf('info must carry %s', f{1})); +%! end + +%!test +%! % No plant means no verdict, and it must say so rather than guess +%! bad = mkid(); +%! bad.G_plant = []; +%! [needs, msgs] = PSautotuneVerdict(bad); +%! assert(~needs); +%! assert(~isempty(msgs), 'it must explain why it cannot judge'); From ec2e070d6fd7ab0ee30086def62733b9069037b9 Mon Sep 17 00:00:00 2001 From: Grzegorz Sterniczuk Date: Sun, 20 Sep 2026 14:47:15 +0200 Subject: [PATCH 29/33] feat(ui): copy the custom column too, and tint it like the rest --- src/ui/PSautotuneUI.m | 69 +++++++++++++++++++++++++++++++--------- src/util/PSautotuneCLI.m | 6 +++- 2 files changed, 59 insertions(+), 16 deletions(-) diff --git a/src/ui/PSautotuneUI.m b/src/ui/PSautotuneUI.m index 81125a2..dd42930 100644 --- a/src/ui/PSautotuneUI.m +++ b/src/ui/PSautotuneUI.m @@ -164,8 +164,8 @@ function PSautotuneUI(T, setupInfo, Fs, tIND, logName) txt(setNames{s}, xSet(s), 0.782, wSet, setCols{s}, 'right', fontsz+1, 'bold', hdrBg); txt(setBlurb{s}, xSet(s), 0.759, wSet, th.textSecondary, 'right', fontsz-2, 'normal', hdrBg); end -txt('CUSTOM', xCust, 0.782, wCust, customCol, 'left', fontsz+1, 'bold', th.figBg); -txt('type and see', xCust, 0.759, wCust, th.textSecondary, 'left', fontsz-2, 'normal', th.figBg); +txt('CUSTOM', xCust, 0.782, wCust, customCol, 'right', fontsz+1, 'bold', th.figBg); +txt('type and see', xCust, 0.759, wCust, th.textSecondary, 'right', fontsz-2, 'normal', th.figBg); yTop = 0.700; for g = 1:3 @@ -204,7 +204,7 @@ function PSautotuneUI(T, setupInfo, Fs, tIND, logName) hEdit(g,k) = uicontrol(fig, 'Style', 'edit', 'Units', 'normalized', ... 'String', sprintf('%d', start), ... 'Position', [xCust yR wCust rh], 'FontSize', fontsz, ... - 'HorizontalAlignment', 'center', ... + 'HorizontalAlignment', 'right', ... 'Callback', @(~,~) onCustom(g)); end end @@ -225,19 +225,22 @@ function PSautotuneUI(T, setupInfo, Fs, tIND, logName) % one button per column, in that column's colour, so the button and the curve % it belongs to read as the same thing -hBtn = zeros(1, 3); -for s = 1:3 +hBtn = zeros(1, 4); +btnX = [xSet xCust]; btnW = [wSet wSet wSet wCust]; +btnName = [setNames {'CUSTOM'}]; btnCol = [setCols {customCol}]; +for s = 1:4 hBtn(s) = uicontrol(fig, 'Style', 'pushbutton', 'Units', 'normalized', ... - 'String', setNames{s}, ... - 'Position', [xSet(s) 0.196 wSet 0.046], ... + 'String', btnName{s}, ... + 'Position', [btnX(s) 0.196 btnW(s) 0.046], ... 'FontSize', fontsz+1, 'FontWeight', 'bold', ... 'HorizontalAlignment', 'center', ... - 'BackgroundColor', th.btnBg, 'ForegroundColor', setCols{s}, ... + 'BackgroundColor', th.btnBg, 'ForegroundColor', btnCol{s}, ... 'Callback', @(~,~) copyCLI(s)); end txt('Copy to CLI:', cpL, 0.206, 0.075, th.textSecondary, 'left', fontsz, 'normal', th.figBg); PSstyleControls(fig, th); +tintBoxes(); % after the theme pass, which would otherwise repaint them PSdatatipSetup(fig); @@ -308,6 +311,28 @@ function tick(frac, msg) end + function tintBoxes() + % same reading as the proposed columns: what you typed against what you + % are flying, so the custom column is scanned the same way as the rest + for a = 1:3 + if hEdit(a,1) == 0, continue; end + for k = 1:4 + was = gainOf(ids(a).gains, k); + v = boxVal(a, k); + col = th.inputFg; + if v < was, col = th.btnReset; elseif v > was, col = th.bodeCoherence; end + set(hEdit(a,k), 'ForegroundColor', col); + end + end + end + + + function v = boxVal(a, k) + v = round(str2double(get(hEdit(a,k), 'String'))); + if ~isfinite(v) || v < 0, v = 0; end + end + + function v = gainOf(gg, k) switch k case 1, v = gg.P; @@ -344,6 +369,7 @@ function onCustom(g) s = sprintf('%s custom: PM %.0f deg, Ms %.2f, overshoot %.0f%%', ... axNames{g}, pm, max(abs(S)), 100*(max(y)-1)); end + tintBoxes(); set(hCustomInfo, 'String', s); if pm < 30 || max(abs(S)) > 2.5 set(hCustomInfo, 'ForegroundColor', th.btnReset); @@ -358,16 +384,29 @@ function copyCLI(s) for a = 1:3 it = struct('axisName', axNames{a}, 'ok', false, ... 'gains', struct('P',0,'I',0,'D',0,'F',0), 'note', gateMsg{a}); - r = res{a,s}; - if ~isempty(r) && r.ok - it.ok = true; - it.gains = r.gains; - elseif isempty(it.note) - it.note = sprintf('no tune reaches %s on this axis', setNames{s}); + if s == 4 + % whatever is in the boxes, even if it was never proposed + if hEdit(a,1) ~= 0 + it.ok = true; + it.gains = struct('P', boxVal(a,1), 'I', boxVal(a,2), ... + 'D', boxVal(a,3), 'F', boxVal(a,4)); + elseif isempty(it.note) + it.note = 'nothing typed for this axis'; + end + else + r = res{a,s}; + if ~isempty(r) && r.ok + it.ok = true; + it.gains = r.gains; + elseif isempty(it.note) + it.note = sprintf('no tune reaches %s on this axis', setNames{s}); + end end if isempty(items), items = it; else, items(a) = it; end end - cliText = PSautotuneCLI(items, targets(s)); + tgt = NaN; + if s <= 3, tgt = targets(s); end + cliText = PSautotuneCLI(items, tgt); if PScopyToClipboard(cliText) set(hBtn(s), 'String', 'Copied!'); else diff --git a/src/util/PSautotuneCLI.m b/src/util/PSautotuneCLI.m index dfe71ac..12dadad 100644 --- a/src/util/PSautotuneCLI.m +++ b/src/util/PSautotuneCLI.m @@ -10,7 +10,11 @@ axKeys = struct('Roll', 'roll', 'Pitch', 'pitch', 'Yaw', 'yaw'); lines = {}; -lines{end+1} = sprintf('# PIDscope autotune - phase margin target %g deg', pmTarget); +if isfinite(pmTarget) + lines{end+1} = sprintf('# PIDscope autotune - phase margin target %g deg', pmTarget); +else + lines{end+1} = '# PIDscope autotune - gains typed by hand, not proposed'; +end lines{end+1} = '# Verify on a test flight before trusting these in anger.'; nSet = 0; From 5f8a7882a4df0eeba76dc742ea9bb5e54f95cdfe Mon Sep 17 00:00:00 2001 From: Grzegorz Sterniczuk Date: Sun, 20 Sep 2026 15:17:34 +0200 Subject: [PATCH 30/33] fix(chirp): put the overshoot limit inside the gap the logs actually show --- src/core/PSautotuneVerdict.m | 28 +++++++++++++++------------- tests/test_PSautotuneVerdict.m | 16 +++++++++++----- 2 files changed, 26 insertions(+), 18 deletions(-) diff --git a/src/core/PSautotuneVerdict.m b/src/core/PSautotuneVerdict.m index 17f9951..66b90f5 100644 --- a/src/core/PSautotuneVerdict.m +++ b/src/core/PSautotuneVerdict.m @@ -10,24 +10,26 @@ % be perfectly healthy and still have a faster tune available; that is an % option, not advice. % -% Thresholds come from the pichim corpus, 18 axes over 6 logs, of which two -% (20250918_aosmini_01 roll and pitch) are plainly mistuned: +% Thresholds come from every chirp log we have: 24 usable axes over 8 logs, +% six airframes, four pilots, three firmware versions and two MCU families. +% Four axes are plainly mistuned (20250918_aosmini_01 roll and pitch, galina_8 +% roll and pitch), the other twenty fly well. % -% 16 good axes the 2 bad ones -% phase margin 38 .. 82 deg 25, 29 deg -% closed loop pk 0.5 .. 1.4 dB 5.2, 6.5 dB -% step overshoot 6 .. 15 % 47, 50 % -% Ms 1.39 .. 2.23 2.12, 2.37 +% 20 good axes the 4 bad ones separates? +% phase margin 38.1 .. 86.5 23.2 .. 29.0 yes, clean gap +% step overshoot 1 .. 15 % 22 .. 50 % yes, gap 15-22 +% closed loop pk 0.36 .. 1.79 dB 1.76 .. 6.51 dB no, they overlap +% Ms 1.28 .. 2.23 2.12 .. 3.44 no, they overlap % -% Ms is the one that does not separate them, so it is a backstop for something -% extreme rather than the test. Betaflight's own limit of 2.0 is drawn against -% a different quantity - their open loop is approximated as T/(1-T), which -% reads about 0.74x our sensitivity peak and 25 deg more phase margin on an -% axis with real D - so it cannot be carried over unchanged. +% So the verdict rests on phase margin and overshoot; the peak and Ms are kept +% only to catch something extreme. Betaflight's own limit of Ms <= 2.0 is drawn +% against a different quantity - their open loop is approximated as T/(1-T), +% which reads about 0.74x our sensitivity peak and 25 deg more phase margin on +% an axis with real D - so it cannot be carried over unchanged. if nargin < 2 || isempty(opt), opt = struct(); end -o = struct('pmFloor', 35, 'peakDbMax', 3.0, 'overshootMax', 0.25, 'msMax', 2.7); +o = struct('pmFloor', 35, 'peakDbMax', 3.0, 'overshootMax', 0.18, 'msMax', 2.7); fn = fieldnames(o); for k = 1:numel(fn) if isfield(opt, fn{k}) && ~isempty(opt.(fn{k})), o.(fn{k}) = opt.(fn{k}); end diff --git a/tests/test_PSautotuneVerdict.m b/tests/test_PSautotuneVerdict.m index b2a7176..30e2a0a 100644 --- a/tests/test_PSautotuneVerdict.m +++ b/tests/test_PSautotuneVerdict.m @@ -47,12 +47,18 @@ %! assert(~isempty(strfind(lower(strjoin(msgs, ' ')), 'phase margin'))); %!test -%! % Ms alone is not a verdict. On the corpus a well flown axis reaches 2.23 and -%! % a badly flown one sits at 2.12, so no Ms line separates them - it stays a -%! % backstop for the extreme, not the headline. +%! % Neither Ms nor the closed loop peak separates good tunes from bad ones: on +%! % 24 measured axes the good range 1.28..2.23 overlaps the bad 2.12..3.44, and +%! % the peak overlaps too. Both stay as backstops, so neither may be set tight +%! % enough to become the headline test. %! [~, ~, info] = PSautotuneVerdict(mkid()); -%! assert(info.msMax >= 2.5, 'Ms must not be the tight test any more'); -%! assert(isfield(info, 'ms') && isfinite(info.ms), 'but it is still reported'); +%! assert(info.msMax >= 2.5, 'Ms must not be the tight test'); +%! assert(info.peakDbMax >= 2.0, 'nor the closed loop peak'); +%! assert(isfield(info, 'ms') && isfinite(info.ms), 'but both are still reported'); +%! % the two that do separate sit inside their measured gaps +%! assert(info.pmFloor > 29 && info.pmFloor < 38.1, 'PM floor must sit in the gap'); +%! assert(info.overshootMax > 0.15 && info.overshootMax < 0.22, ... +%! 'overshoot limit must sit in the gap'); %!test %! % Everything measured is handed back so the window can say why From 862d6088b5e88a107c89c64d49afbc9cd8024587 Mon Sep 17 00:00:00 2001 From: Grzegorz Sterniczuk Date: Sun, 20 Sep 2026 16:33:55 +0200 Subject: [PATCH 31/33] fix(chirp): stop judging a tune by a step the firmware never runs --- src/core/PSautotuneVerdict.m | 49 ++++++++++++++++++---------------- tests/test_PSautotuneVerdict.m | 42 ++++++++++++++++++++--------- 2 files changed, 56 insertions(+), 35 deletions(-) diff --git a/src/core/PSautotuneVerdict.m b/src/core/PSautotuneVerdict.m index 66b90f5..35d44a1 100644 --- a/src/core/PSautotuneVerdict.m +++ b/src/core/PSautotuneVerdict.m @@ -10,26 +10,39 @@ % be perfectly healthy and still have a faster tune available; that is an % option, not advice. % -% Thresholds come from every chirp log we have: 24 usable axes over 8 logs, -% six airframes, four pilots, three firmware versions and two MCU families. +% Thresholds come from every chirp log we have: 41 usable axes over 11 logs, +% eight airframes, four pilots, three firmware versions and two MCU families. % Four axes are plainly mistuned (20250918_aosmini_01 roll and pitch, galina_8 -% roll and pitch), the other twenty fly well. +% roll and pitch), the other thirty-seven fly well. % -% 20 good axes the 4 bad ones separates? -% phase margin 38.1 .. 86.5 23.2 .. 29.0 yes, clean gap -% step overshoot 1 .. 15 % 22 .. 50 % yes, gap 15-22 -% closed loop pk 0.36 .. 1.79 dB 1.76 .. 6.51 dB no, they overlap +% 37 good axes the 4 bad ones separates? +% phase margin 37.7 .. 86.5 23.2 .. 29.0 yes, clean gap % Ms 1.28 .. 2.23 2.12 .. 3.44 no, they overlap % -% So the verdict rests on phase margin and overshoot; the peak and Ms are kept -% only to catch something extreme. Betaflight's own limit of Ms <= 2.0 is drawn -% against a different quantity - their open loop is approximated as T/(1-T), -% which reads about 0.74x our sensitivity peak and 25 deg more phase margin on -% an axis with real D - so it cannot be carried over unchanged. +% Step overshoot and the closed loop peak are measured and reported, but they +% decide nothing. Both come from T = P*(A+F)*S, which carries the modelled +% integrator, and a chirp cannot see that integrator on roll or pitch: +% iterm_relax is on by default for exactly those two axes, and a sweep drives +% the setpoint past its cutoff for the whole run. Rebuilding every logged PID +% term across all 15 chirp logs put the running integrator at 6..74% of the +% integral of the error on roll and pitch, correlating as weakly as 0.29, while +% yaw - which the mechanism does not touch - reconstructed at 1.00 with +% correlation 1.000. Gating on those two flagged 19 of 41 axes; phase margin and +% Ms alone flag the same four known-bad axes and nothing else. +% +% A setpoint step is itself a fast stick move, so iterm_relax acts during one +% too and the full-I step is a run the craft never makes. A gust does see the +% full integrator, but a gust enters at the plant output, so S describes it - +% and Ms already measures S. +% +% Betaflight's own limit of Ms <= 2.0 is drawn against a different quantity - +% their open loop is approximated as T/(1-T), which reads about 0.74x our +% sensitivity peak and 25 deg more phase margin on an axis with real D - so it +% cannot be carried over unchanged. if nargin < 2 || isempty(opt), opt = struct(); end -o = struct('pmFloor', 35, 'peakDbMax', 3.0, 'overshootMax', 0.18, 'msMax', 2.7); +o = struct('pmFloor', 35, 'msMax', 2.7); fn = fieldnames(o); for k = 1:numel(fn) if isfield(opt, fn{k}) && ~isempty(opt.(fn{k})), o.(fn{k}) = opt.(fn{k}); end @@ -68,16 +81,6 @@ msgs{end+1} = sprintf(['Phase margin is %.0f deg, below the %.0f deg this ' ... 'tool treats as the floor'], info.pm, o.pmFloor); end -if info.peakDb > o.peakDbMax - needsWork = true; - msgs{end+1} = sprintf('The loop peaks %.1f dB above the setpoint, over %.1f dB', ... - info.peakDb, o.peakDbMax); -end -if isfinite(info.overshoot) && info.overshoot > o.overshootMax - needsWork = true; - msgs{end+1} = sprintf('A step overshoots by %.0f%%, over %.0f%%', ... - 100*info.overshoot, 100*o.overshootMax); -end if info.ms > o.msMax needsWork = true; msgs{end+1} = sprintf('Disturbances are amplified %.1fx at worst, over %.1fx', ... diff --git a/tests/test_PSautotuneVerdict.m b/tests/test_PSautotuneVerdict.m index 30e2a0a..0045d6f 100644 --- a/tests/test_PSautotuneVerdict.m +++ b/tests/test_PSautotuneVerdict.m @@ -47,25 +47,43 @@ %! assert(~isempty(strfind(lower(strjoin(msgs, ' ')), 'phase margin'))); %!test -%! % Neither Ms nor the closed loop peak separates good tunes from bad ones: on -%! % 24 measured axes the good range 1.28..2.23 overlaps the bad 2.12..3.44, and -%! % the peak overlaps too. Both stay as backstops, so neither may be set tight -%! % enough to become the headline test. +%! % Ms does not separate good tunes from bad ones: on 24 measured axes the good +%! % range 1.28..2.23 overlaps the bad 2.12..3.44. It stays as a backstop, so it +%! % may not be set tight enough to become the headline test. %! [~, ~, info] = PSautotuneVerdict(mkid()); %! assert(info.msMax >= 2.5, 'Ms must not be the tight test'); -%! assert(info.peakDbMax >= 2.0, 'nor the closed loop peak'); -%! assert(isfield(info, 'ms') && isfinite(info.ms), 'but both are still reported'); -%! % the two that do separate sit inside their measured gaps -%! assert(info.pmFloor > 29 && info.pmFloor < 38.1, 'PM floor must sit in the gap'); -%! assert(info.overshootMax > 0.15 && info.overshootMax < 0.22, ... -%! 'overshoot limit must sit in the gap'); +%! assert(isfield(info, 'ms') && isfinite(info.ms), 'but it is still reported'); +%! assert(info.pmFloor > 29 && info.pmFloor < 37.7, 'PM floor must sit in the gap'); %!test -%! % Everything measured is handed back so the window can say why +%! % Step overshoot and the closed loop peak both come from T = P*(A+F)*S, which +%! % carries the modelled integrator. Rebuilding every logged PID term across all +%! % 15 chirp logs showed iterm_relax gates that integrator off on roll and pitch +%! % for the whole sweep - the logged axisI runs 6..74% of the integral of the +%! % error, and correlates with it as weakly as 0.29 - while yaw, which the +%! % mechanism does not touch, reconstructs at 1.00 with correlation 1.000. +%! % So neither number may decide anything. This loop has margin to spare and +%! % would still overshoot 28% on paper. +%! id = mkid('P', 20, 'I', 160); +%! [needs, msgs, info] = PSautotuneVerdict(id); +%! assert(info.pm > 38, sprintf('fixture must have margin, has %.1f', info.pm)); +%! assert(info.ms < 2.3, sprintf('fixture must have a calm Ms, has %.2f', info.ms)); +%! assert(info.overshoot > 0.25, ... +%! sprintf('fixture must overshoot on paper, does %.0f%%', 100*info.overshoot)); +%! assert(~needs, sprintf('a modelled overshoot must not flag a tune: %s', ... +%! strjoin(msgs, '; '))); + +%!test +%! % The two contaminated numbers are still measured and handed to the window - +%! % they are worth showing - but there is no threshold left to compare them to %! [~, ~, info] = PSautotuneVerdict(mkid()); -%! for f = {'pm', 'ms', 'peakDb', 'overshoot', 'pmFloor', 'peakDbMax', 'overshootMax'} +%! for f = {'pm', 'ms', 'peakDb', 'overshoot', 'pmFloor', 'msMax'} %! assert(isfield(info, f{1}), sprintf('info must carry %s', f{1})); %! end +%! for f = {'peakDbMax', 'overshootMax'} +%! assert(~isfield(info, f{1}), ... +%! sprintf('%s still exists, so something still gates on it', f{1})); +%! end %!test %! % No plant means no verdict, and it must say so rather than guess From 92c3bbccaeefcb59e70eb75f9114d4eba0ea05d5 Mon Sep 17 00:00:00 2001 From: Grzegorz Sterniczuk Date: Sun, 20 Sep 2026 16:33:58 +0200 Subject: [PATCH 32/33] fix(chirp): settle I inside the scan so the judged cell is the reported one --- src/core/PSautotuneSearch.m | 103 +++++++++++++++++++++------------- tests/test_PSautotuneSearch.m | 72 +++++++++++++++++------- 2 files changed, 118 insertions(+), 57 deletions(-) diff --git a/src/core/PSautotuneSearch.m b/src/core/PSautotuneSearch.m index 88bfda4..fd81a69 100644 --- a/src/core/PSautotuneSearch.m +++ b/src/core/PSautotuneSearch.m @@ -11,6 +11,12 @@ % than PM binds, the answer comes out more conservative rather than not coming % out at all. % +% I is settled inside the scan, not after it, so the cell that is judged is the +% cell that is reported. Choosing it afterwards let the grid hold one +% integrator and the answer another, which on a real yaw axis left the margin +% 14 deg above the target and turned a looser request into a more aggressive +% tune. +% % res.gains carries P, I, D and F and nothing else. Betaflight issue #5258 was % a filter cutoff derived from chirp coherence; here there is nowhere to put % one. @@ -20,8 +26,8 @@ o = struct('pmTarget', 60, 'msMax', 2.0, 'peakMaxDb', 6, ... 'magAtTrustDb', -6, 'wcpFracTrust', 0.5, 'wcpMaxRatio', 3, ... 'dDomRatio', 2.0, ... - 'pClamp', [0.25 2.0], 'dClamp', [0.6 1.25], 'pidsumFrac', 0.9, ... - 'peakStepMin', 1.05); + 'pClamp', [0.25 2.0], 'dClamp', [0.6 1.25], 'iClamp', [0.25 2.0], ... + 'pidsumFrac', 0.9, 'zeroRatio', []); fn = fieldnames(o); for k = 1:numel(fn) if isfield(opt, fn{k}) && ~isempty(opt.(fn{k})), o.(fn{k}) = opt.(fn{k}); end @@ -82,6 +88,20 @@ Dcand = round(o.dClamp(1)*D0) : round(dHi*D0); if isempty(Pcand), Pcand = max(1, P0); end if isempty(Dcand), Dcand = D0; end +iLo = max(1, round(o.iClamp(1)*I0)); +iHi = max(iLo, round(o.iClamp(2)*I0)); + +% Where the flown tune put the PI corner relative to its own crossover. Holding +% that fraction is the scale-invariant choice: a slower loop has a smaller phase +% budget, and an integrator left at the old corner eats more of it. There is no +% textbook value to impose instead - across the 37 well flown axes we hold the +% fraction runs 1.96 to 14.34 and splits by axis, yaw sitting at 2.0..3.1 +% because the firmware runs 2.5x the I gain there. +zRatio = o.zeroRatio; +if isempty(zRatio), zRatio = zeroRatio(b, P0, I0, base.wcp); end +if ~isfinite(zRatio) + res.notes{end+1} = 'no PI corner in the flown tune, so I follows P'; +end nD = numel(Dcand); nP = numel(Pcand); g = struct(); @@ -98,11 +118,19 @@ Pi = Pcand(iP); % Ki in the firmware is absolute, so holding I while cutting P drags % the PI corner upwards and the integrator adds lag exactly where the - % scan is trying to buy phase margin. Scaling both keeps the integral - % time the pilot flew. + % scan is trying to buy phase margin. Scale it with P to find where this + % cell crosses over, then put the corner back at the flown fraction of + % that crossover and judge the cell on the I it will actually be given. Ii = I0; if P0 > 0, Ii = round(Pi / P0 * I0); end c = evalOne(Pk, fk, Pi*b.Ap + Ii*b.Ai, Dv, F0*b.F1); + if isfinite(zRatio) && ~isnan(c.wcp) + Ij = min(max(iFromZero(b, Pi, c.wcp / zRatio), iLo), iHi); + if Ij ~= Ii + Ii = Ij; + c = evalOne(Pk, fk, Pi*b.Ap + Ii*b.Ai, Dv, F0*b.F1); + end + end g.P(iD,iP) = Pi; g.I(iD,iP) = Ii; g.D(iD,iP) = Di; g.pm(iD,iP) = c.pm; g.ms(iD,iP) = c.ms; g.wcp(iD,iP) = c.wcp; g.gm(iD,iP) = c.gm; g.peakDb(iD,iP) = c.peakDb; @@ -138,18 +166,14 @@ res.gains.I = g.I(iD,iP); res.gains.D = g.D(iD,iP); -% D acts on the gyro only, so the tracking response is P*A/(1+P*(A+D)) and the -% PI zero at Ki/Kp sits in the numerator where the margins never see it. -% Measured on the pichim corpus: at a fixed cell, I moved the step overshoot -% from 1.11 to 1.17 while the phase margin moved 0.6 deg and Ms not at all. -% So take the most I that does not make the step worse than it already flies - -% I is what rejects disturbances, and it is nearly free of the margins. -[res.gains.I, res.peak, res.peak0] = shapeI(id, res.gains, I0, o, base.wcp); -if isfinite(res.peak) && isfinite(res.peak0) && res.peak > max(res.peak0, o.peakStepMin) + 1e-6 - res.notes{end+1} = sprintf(['step overshoot still %.0f%% against %.0f%% flown - ' ... - 'the margins would not allow less I'], ... - 100*(res.peak-1), 100*(res.peak0-1)); -end +% Reported, never optimised against. A step is a fast setpoint move, and +% iterm_relax gates the integrator off during one on roll and pitch - measured +% across every chirp log we hold, the running integrator is 6..74% of the +% integral of the error there, against 100% on yaw, which the mechanism does not +% touch. So the modelled step is not a quantity to tune I by. +res.peak0 = stepPeak(id, id.gains); +res.peak = stepPeak(id, res.gains); + res.scale = struct('P', ratio(res.gains.P, P0), 'I', ratio(res.gains.I, I0), ... 'D', ratio(res.gains.D, D0), 'F', 1); @@ -169,28 +193,31 @@ end -function [Ibest, pk, pk0] = shapeI(id, gains, I0, o, wcp0) - Ibest = gains.I; - pk0 = stepPeak(id, id.gains); - pk = stepPeak(id, gains); - if I0 <= 0 || ~isfinite(pk0) || ~isfinite(pk), return; end - limit = max(pk0, o.peakStepMin); - if pk <= limit, return; end - - keep = id.freq > 0 & id.freq <= id.fTrust; - fk = id.freq(keep); - Pk = id.G_plant(keep); - - % walk I down, but only over values that still clear every constraint - an - % I that fixes the step and loses the margin is not an answer - for Ii = (gains.I-1):-1:max(1, round(0.2*I0)) - trial = gains; trial.I = Ii; - [A, D, F] = PSbuildController(trial, id.fp, id.FsPid, fk); - if ~admissible(evalOne(Pk, fk, A, D, F), o, id.fTrust, wcp0), continue; end - p = stepPeak(id, trial); - if p < pk, Ibest = Ii; pk = p; end - if p <= limit, break; end - end +function fz = zeroFreq(b, P, I) + % The PI corner is where the two halves of A are equal in magnitude. Reading + % it off the basis rather than from Ki/Kp keeps the Betaflight scaling + % constants, and the 2.5x the firmware puts on yaw, in the one file that + % owns them. + fz = NaN; + if P <= 0 || I <= 0, return; end + d = abs(P * b.Ap) - abs(I * b.Ai); + k = find(d(1:end-1) < 0 & d(2:end) >= 0, 1); + if isempty(k), return; end + fz = interp1(d(k:k+1), b.freq(k:k+1), 0, 'linear'); +end + + +function r = zeroRatio(b, P, I, wcp) + r = NaN; + fz = zeroFreq(b, P, I); + if isfinite(fz) && fz > 0 && isfinite(wcp), r = wcp / fz; end +end + + +function Ii = iFromZero(b, P, fz) + ap = interp1(b.freq, abs(b.Ap), fz, 'linear', 'extrap'); + ai = interp1(b.freq, abs(b.Ai), fz, 'linear', 'extrap'); + Ii = max(1, round(P * ap / max(ai, 1e-12))); end diff --git a/tests/test_PSautotuneSearch.m b/tests/test_PSautotuneSearch.m index 9acae57..b1b404c 100644 --- a/tests/test_PSautotuneSearch.m +++ b/tests/test_PSautotuneSearch.m @@ -35,6 +35,17 @@ %! 'axisD', p.axisD*ones(1000,1), 'pidsumLimit', 800); %!endfunction +%!function fz = picorner(gains, fp, Fs, freq) +%! % where the P half and the I half of A are equal in magnitude +%! f = freq(freq > 0); +%! gp = gains; gp.I = 0; +%! gi = gains; gi.P = 0; +%! d = abs(PSbuildController(gp, fp, Fs, f)) - abs(PSbuildController(gi, fp, Fs, f)); +%! k = find(d(1:end-1) < 0 & d(2:end) >= 0, 1); +%! fz = NaN; +%! if ~isempty(k), fz = interp1(d(k:k+1), f(k:k+1), 0, 'linear'); end +%!endfunction + %!test %! % T1 - the target is hit where the algebra says it is. %! % tau = 2 ms and a 50 deg target pin w*tau = 40 deg, so w = 349.066 rad/s and @@ -167,17 +178,23 @@ %! 'the budget must say when it bound'); %!test -%! % I moves with P, because Ki in the firmware is absolute. Holding I while -%! % cutting P drags the PI corner upwards, so the integrator contributes more -%! % lag exactly where the scan is trying to buy phase margin. Measured on the -%! % pichim corpus: with I held, one craft tops out at 45.2 deg of reachable -%! % phase margin however far P is cut, and clears 69.9 deg once I follows. -%! res = PSautotuneSearch(mkfix('P', 80, 'I', 120), struct('pmTarget', 60)); +%! % I follows the loop, not just P. Ki in the firmware is absolute, so holding +%! % I while cutting P drags the PI corner upwards and the integrator adds lag +%! % exactly where the scan is trying to buy phase margin. Scaling it with P +%! % instead holds the corner still in Hz, which is not the same thing either: a +%! % slower loop has a smaller phase budget, so the corner has to come down with +%! % the crossover. What is held is the ratio between them - measured at 1.96 to +%! % 14.34 across the 37 well flown axes we hold, splitting by axis because the +%! % firmware runs 2.5x the I gain on yaw. +%! fix = mkfix('P', 80, 'I', 120); +%! res = PSautotuneSearch(fix, struct('pmTarget', 60)); %! assert(res.ok, res.reason); -%! assert(res.gains.I ~= 120, 'I has to move when P does'); -%! assert(abs(res.scale.I - res.scale.P) < 0.02, ... -%! sprintf('I scaled %.3f against P %.3f - the integral time must hold', ... -%! res.scale.I, res.scale.P)); +%! assert(res.gains.I ~= 120, 'I has to move when the loop does'); +%! before = res.wcp0 / picorner(fix.gains, fix.fp, fix.FsPid, fix.freq); +%! after = res.wcp / picorner(res.gains, fix.fp, fix.FsPid, fix.freq); +%! assert(abs(after - before) / before < 0.1, ... +%! sprintf('the PI corner moved relative to crossover: %.2f -> %.2f', ... +%! before, after)); %!test %! % Opening the lower clamp can only add answers, never change one. The scan @@ -201,18 +218,35 @@ %! assert(rW.gains.P == 45, sprintf('expected P 45, got %d', rW.gains.P)); %!test -%! % I is shaped against the step, not scaled blindly with P. D acts on the -%! % gyro only, so the tracking response is P*A/(1+P*(A+D)) and the PI zero at -%! % Ki/Kp sits in the numerator where the margins never see it: on the pichim -%! % corpus, at a fixed cell, I moved the overshoot from 1.11 to 1.17 while the -%! % phase margin moved 0.6 deg. So the proposal must not hand back a worse step -%! % than the one being flown. +%! % The modelled step is reported and never tuned against. Both peaks come back +%! % so the window can draw them, but no proposal is rejected for having a worse +%! % one: T = P*(A+F)*S carries the modelled integrator, and iterm_relax gates +%! % that integrator off on roll and pitch through any fast setpoint move - a +%! % step included. Rebuilding every logged PID term across all 15 chirp logs put +%! % the running integrator at 6..74% of the integral of the error on those axes +%! % and at 100% on yaw, which the mechanism does not touch. %! res = PSautotuneSearch(mkfix('P', 80, 'I', 120), struct('pmTarget', 60)); %! assert(res.ok, res.reason); %! assert(isfinite(res.peak) && isfinite(res.peak0), 'both peaks must be reported'); -%! assert(res.peak <= max(res.peak0, 1.05) + 1e-9, ... -%! sprintf('step got worse: %.3f against %.3f flown', res.peak, res.peak0)); -%! assert(res.pm >= 60 - 1e-6, sprintf('shaping I cost phase margin: %.2f', res.pm)); +%! assert(res.pm >= 60 - 1e-6, sprintf('phase margin missed: %.2f', res.pm)); +%! assert(~isfield(res.opt, 'peakStepMin'), ... +%! 'a step threshold in the options means something still tunes to it'); + +%!test +%! % The cell the scan picked has to be the cell it reports. I used to be chosen +%! % after the grid had already judged every cell admissible or not, so the grid +%! % held one integrator and the answer another. On a real yaw axis that left the +%! % reported margin 14 deg above the target it was asked for, and made the +%! % proposal jump from P 36 to P 75 when the target was relaxed from 72.5 to 60 +%! % - a looser request returning a more aggressive tune. +%! res = PSautotuneSearch(mkfix('P', 30, 'I', 80), struct('pmTarget', 50)); +%! assert(res.ok, res.reason); +%! assert(res.grid.I(res.iD, res.iP) == res.gains.I, ... +%! sprintf('grid judged I %d, answer sets I %d', ... +%! res.grid.I(res.iD, res.iP), res.gains.I)); +%! assert(abs(res.grid.pm(res.iD, res.iP) - res.pm) < 1e-6, ... +%! sprintf('grid judged PM %.2f, answer reports %.2f', ... +%! res.grid.pm(res.iD, res.iP), res.pm)); %!test %! % Reason codes have to be usable by the UI without guessing From 9dab53da63d9e9e3fec87d8d50038607544afe0b Mon Sep 17 00:00:00 2001 From: Grzegorz Sterniczuk Date: Sun, 20 Sep 2026 19:36:43 +0200 Subject: [PATCH 33/33] fix(ui): a gate warning is not a refusal, plus verdict banner, colour key and custom steppers --- src/ui/PSautotuneUI.m | 105 +++++++++++++++++++++++++++++++++++------- src/util/PStheme.m | 4 ++ 2 files changed, 92 insertions(+), 17 deletions(-) diff --git a/src/ui/PSautotuneUI.m b/src/ui/PSautotuneUI.m index dd42930..c2e67bc 100644 --- a/src/ui/PSautotuneUI.m +++ b/src/ui/PSautotuneUI.m @@ -29,6 +29,8 @@ function PSautotuneUI(T, setupInfo, Fs, tIND, logName) res = cell(3, 3); stepDat = cell(1, 3); gateMsg = {'', '', ''}; +gateWarn = {'', '', ''}; +gateOk = false(1, 3); nStep = 12; done = 0; for a = 1:3 @@ -39,8 +41,15 @@ function PSautotuneUI(T, setupInfo, Fs, tIND, logName) end for a = 1:3 [gok, gmsgs, ginfo] = PSautotuneGates(ids(a)); - gateMsg{a} = strjoin(gmsgs, '; '); - if ~gok + % a passing axis can still carry a message - dynamic D is a warning, not a + % refusal. Conflating the two marked a usable axis unusable and, worse, kept + % it out of the verdict entirely, so a craft with 23 deg of phase margin was + % told nothing needed changing. + gateOk(a) = gok; + if gok + gateWarn{a} = strjoin(gmsgs, '; '); + else + gateMsg{a} = strjoin(gmsgs, '; '); done = done + 3; continue end @@ -136,6 +145,10 @@ function PSautotuneUI(T, setupInfo, Fs, tIND, logName) xNow = cpL + 0.044; wNow = 0.040; xSet = cpL + [0.090 0.146 0.202]; wSet = 0.052; xCust = cpL + 0.262; wCust = 0.058; +wStep = 0.014; +xStep = xCust + wCust + [0.003 0.019]; +stepLbl = {'-', '+'}; +stepDir = [-1 1]; rh = 0.024; grpGap = 0.014; nGain = 4; % banding so the eye keeps the row across the four numbers, with the % recommended column a shade lighter still @@ -153,10 +166,24 @@ function PSautotuneUI(T, setupInfo, Fs, tIND, logName) 'ForegroundColor', th.textPrimary, 'BackgroundColor', th.figBg); txt(logName, cpL, 0.918, 0.340, th.textSecondary, 'left', fontsz, 'normal', th.figBg); -uicontrol(fig, 'Style', 'text', 'Units', 'normalized', 'String', summaryLines(), ... - 'Position', [cpL 0.828 0.340 0.075], 'FontSize', fontsz, ... +% The headline answers the only question most people open this window with, so +% it gets a band of its own - the previous version said it in body text the same +% size and colour as everything else, and it read as a caption. +[headline, bodyLines, allWell] = summaryLines(); +bannerBg = th.bannerWarn; bannerFg = th.btnReset; +if allWell, bannerBg = th.bannerOk; bannerFg = th.bodeCoherence; end +% runs to the right edge of the table below it, steppers included, so the band +% reads as belonging to the column rather than stopping short of it +bannerW = 0.262 + 0.058 + 0.019 + 0.014; +uicontrol(fig, 'Style', 'text', 'Units', 'normalized', 'String', [' ' headline], ... + 'Position', [cpL 0.862 bannerW 0.038], 'FontSize', fontsz+3, 'FontWeight', 'bold', ... 'HorizontalAlignment', 'left', ... - 'ForegroundColor', th.textPrimary, 'BackgroundColor', th.figBg); + 'ForegroundColor', bannerFg, 'BackgroundColor', bannerBg); +uicontrol(fig, 'Style', 'text', 'Units', 'normalized', ... + 'String', cellfun(@(s) [' ' s], bodyLines, 'UniformOutput', false), ... + 'Position', [cpL 0.806 bannerW 0.056], 'FontSize', fontsz, ... + 'HorizontalAlignment', 'left', ... + 'ForegroundColor', th.textPrimary, 'BackgroundColor', bannerBg); txt('now', xNow, 0.782, wNow, th.textSecondary, 'right', fontsz, 'bold', th.figBg); for s = 1:3 @@ -171,7 +198,7 @@ function PSautotuneUI(T, setupInfo, Fs, tIND, logName) for g = 1:3 yG = yTop - (g-1)*(nGain*rh + grpGap); txt(axNames{g}, cpL, yG, 0.028, axCols{g}, 'left', fontsz, 'bold', th.figBg); - if ~isempty(gateMsg{g}) + if ~gateOk(g) txt('not usable', xGain, yG, 0.10, th.btnReset, 'left', fontsz, 'normal', th.figBg); continue end @@ -206,12 +233,34 @@ function PSautotuneUI(T, setupInfo, Fs, tIND, logName) 'Position', [xCust yR wCust rh], 'FontSize', fontsz, ... 'HorizontalAlignment', 'right', ... 'Callback', @(~,~) onCustom(g)); + % nudging one count at a time is how a gain is actually explored; typing + % still works, so an exact value is never more than a keystroke away + for d = 1:2 + uicontrol(fig, 'Style', 'pushbutton', 'Units', 'normalized', ... + 'String', stepLbl{d}, 'Tag', 'autotuneStep', ... + 'Position', [xStep(d) yR wStep rh], ... + 'FontSize', fontsz, 'FontWeight', 'bold', ... + 'ForegroundColor', customCol, 'BackgroundColor', th.btnBg, ... + 'Callback', @(~,~) bump(g, k, stepDir(d))); + end end end -% the colours in the table mean something, so say what -txt('lower than now', xNow, 0.372, 0.075, th.btnReset, 'left', fontsz-1, 'normal', th.figBg); -txt('higher', xNow+0.080, 0.372, 0.050, th.bodeCoherence, 'left', fontsz-1, 'normal', th.figBg); +% The colours in the table mean something, so say what. The swatch is what makes +% this read as a key - two coloured words on their own look like stray labels. +yKey = 0.372; wSwatch = 0.010; +txt('Colour key', cpL, yKey, 0.055, th.textSecondary, 'left', fontsz-1, 'normal', th.figBg); +keyX = cpL + 0.062; +keyCols = {th.btnReset, th.bodeCoherence}; +keyText = {'lower than now', 'higher than now'}; +for k = 1:2 + uicontrol(fig, 'Style', 'text', 'Units', 'normalized', 'String', '', ... + 'Position', [keyX yKey+0.005 wSwatch rh-0.010], ... + 'BackgroundColor', keyCols{k}); + txt(keyText{k}, keyX+wSwatch+0.006, yKey, 0.085, keyCols{k}, 'left', ... + fontsz-1, 'normal', th.figBg); + keyX = keyX + wSwatch + 0.100; +end hCustomInfo = uicontrol(fig, 'Style', 'text', 'Units', 'normalized', 'String', '', ... 'Position', [cpL 0.336 0.340 rh], 'FontSize', fontsz-1, ... @@ -219,7 +268,7 @@ function PSautotuneUI(T, setupInfo, Fs, tIND, logName) 'ForegroundColor', th.textSecondary, 'BackgroundColor', th.figBg); uicontrol(fig, 'Style', 'text', 'Units', 'normalized', 'String', footLines(), ... - 'Position', [cpL 0.258 0.340 0.060], 'FontSize', fontsz-1, ... + 'Position', [cpL 0.244 bannerW 0.076], 'FontSize', fontsz-1, ... 'HorizontalAlignment', 'left', ... 'ForegroundColor', th.textSecondary, 'BackgroundColor', th.figBg); @@ -230,7 +279,7 @@ function PSautotuneUI(T, setupInfo, Fs, tIND, logName) btnName = [setNames {'CUSTOM'}]; btnCol = [setCols {customCol}]; for s = 1:4 hBtn(s) = uicontrol(fig, 'Style', 'pushbutton', 'Units', 'normalized', ... - 'String', btnName{s}, ... + 'String', btnName{s}, 'Tag', 'autotuneCopy', ... 'Position', [btnX(s) 0.196 btnW(s) 0.046], ... 'FontSize', fontsz+1, 'FontWeight', 'bold', ... 'HorizontalAlignment', 'center', ... @@ -271,13 +320,13 @@ function tick(frac, msg) end - function lines = summaryLines() + function [headline, lines, allWell] = summaryLines() % the headline answers "does anything need changing", which is not the % same question as "what else is reachable" - a healthy loop can still % have a faster tune available, and that is an option, not advice bad = {}; flagged = {}; why = {}; for a = 1:3 - if ~isempty(gateMsg{a}), bad{end+1} = axNames{a}; continue; end + if ~gateOk(a), bad{end+1} = axNames{a}; continue; end [needs, vmsgs] = PSautotuneVerdict(ids(a)); if needs flagged{end+1} = axNames{a}; @@ -285,12 +334,16 @@ function tick(frac, msg) end end lines = {}; - if ~isempty(flagged) - lines{end+1} = sprintf('%s needs attention.', strjoin(flagged, ' and ')); + allWell = isempty(flagged); + if ~allWell + verb = 'NEEDS'; + if numel(flagged) > 1, verb = 'NEED'; end + headline = sprintf('%s %s ATTENTION', upper(strjoin(flagged, ' AND ')), verb); if ~isempty(why), lines{end+1} = why{1}; end else - lines{end+1} = 'Your tune is within limits - nothing here needs changing.'; - lines{end+1} = 'The columns are options, not advice.'; + headline = 'NOTHING NEEDS CHANGING'; + lines{end+1} = 'Your tune is within limits. The columns below are'; + lines{end+1} = 'options, not advice.'; end if ~isempty(bad) lines{end+1} = sprintf('%s could not be read from this log.', strjoin(bad, ' and ')); @@ -308,6 +361,16 @@ function tick(frac, msg) if ~isempty(band) foot{end+1} = sprintf('Measured to %.0f Hz. Fly it and check.', min(band)); end + % a warning is worth saying once, naming the axes it applies to, rather + % than repeating the same sentence three times + warned = {}; + for a = 1:3 + if gateOk(a) && ~isempty(gateWarn{a}), warned{end+1} = axNames{a}; end + end + if ~isempty(warned) + foot{end+1} = sprintf('Dynamic D on %s, so D is held at or below what was flown.', ... + strjoin(warned, ' and ')); + end end @@ -327,6 +390,14 @@ function tintBoxes() end + function bump(g, k, d) + if hEdit(g,k) == 0, return; end + v = max(0, boxVal(g, k) + d); + set(hEdit(g,k), 'String', sprintf('%d', v)); + onCustom(g); + end + + function v = boxVal(a, k) v = round(str2double(get(hEdit(a,k), 'String'))); if ~isfinite(v) || v < 0, v = 0; end diff --git a/src/util/PStheme.m b/src/util/PStheme.m index 4914ed4..34df5e7 100644 --- a/src/util/PStheme.m +++ b/src/util/PStheme.m @@ -70,6 +70,10 @@ % diff highlight (Setup Info) th.diffBg = [.30 .30 .32]; +% verdict banner (Autotune) - dark enough to read light text on +th.bannerOk = [.13 .25 .15]; +th.bannerWarn = [.30 .21 .10]; + % checkbox / input bg th.checkBg = [.18 .18 .20]; th.inputBg = [.14 .14 .16];