feat: get ranges for different phase diff

resultsanalysis.m

@@ -39,6 +39,16 @@ function ResData = resultsanalysis(matFiles)
         ResData = ResData_;
     end
 
-    idx = findwingphase(ResData(1, 1), 0);
+    for i = 1:size(ResData, 1)
+        for j = 1:size(ResData, 2)
+            fprintf('\nFinding phase for element (%d,%d): %0.2fm, %0.2fHz, %0.1fm/s\n', ...
+                    i, j, ResData(i, j).dx, ResData(i, j).freq, ResData(i, j).airspeed);
+            offsetVals = [0, 30, 60, 90, 180];
+            for k = 1:length(offsetVals)
+                ResData(i, j).Phase(k).phaseVal = offsetVals(k);
+                ResData(i, j).Phase(k).idx = findwingphase(ResData(i, j), offsetVals(k));
+            end
+        end
+    end
 
 end

utils/analysis/findwingphase.m

-function idx = findwingphase(ResData, phaseDeg)
-    % FINDWINDPHASE Return the indexes where the wings are at a certain phase
+function peakRanges = findwingphase(ResData, phaseDeg)
+    % FINDWINDPHASE Return the indexes where the wings have a certain phase difference
     %   Todo
 
     % ----------------------------------------------------------------------------------------------
@@ -10,8 +10,7 @@ function idx = findwingphase(ResData, phaseDeg)
     %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
     % Constants and defautls
-    PHASE_TOL = 5e-2; % Tolerance on phase
-    DEBUG = true;
+    N_PHASE_PEAKS = 5; % Number of peaks in phase to return ideally
 
     % Abbreviations
     sampling = 1 / diff(ResData.front.arduShort(1:2, 1));
@@ -22,17 +21,18 @@ function idx = findwingphase(ResData, phaseDeg)
     period = 1 ./ [trueFreq(1), trueFreq(1)]; % Signal perdio, [s]
     nPeriodIdx = period * sampling; % Number of indexes in a period
 
-    % Determine period offset that match phase
-    % TODO: Some sort of average instead of based on first wing only
-    periodOffsetIdx = round((phaseDeg / 360) * nPeriodIdx(1));
-
-    syncedPeaksLocs = findsyncedpeaks(ResData.front.arduShort(:, 2), ...
-                                      ResData.aft.arduShort(:, 2), ...
-                                      nPeriodIdx(1), periodOffsetIdx, PHASE_TOL);
+    printColor = 1;
+    if any(abs(trueFreq - ResData.freq) > 0.25 * ResData.freq)
+        printColor = 2;
+    end
 
-    % Find indexes with proper phase difference
+    fprintf(printColor, '    TrueFreq = %0.2f/%0.2fHz, theory = %0.2fHz\n', ...
+            trueFreq(1), trueFreq(2), ResData.freq);
 
-    idx = 0; % FIXME, proper return
+    % Determine the ranges when the two angles have the proper phase difference
+    peakRanges = findsyncedpeaks(ResData.front.arduShort(:, 2), ...
+                                 ResData.aft.arduShort(:, 2), ...
+                                 nPeriodIdx(1), phaseDeg, N_PHASE_PEAKS);
 
 end
 
@@ -58,37 +58,45 @@ function trueFreq = getmainfrequency(time, signal)
     freq = sampling * (0:(nts / 2)) / nts; % Frequency domain
 
     % Get main frequency
-    [~, trueFreqIdx] = max(fT1);
-    trueFreq = freq(trueFreqIdx);
+    [~, trueFreqIdx] = max(fT1(2:end));
+    trueFreq = freq(1 + trueFreqIdx);
 
     if DEBUG
+        figure;
+        plot(time, signal);
+        grid on;
+
         figure;
         plot(freq, fT1);
         title("Single-Sided Amplitude Spectrum of S(t)");
         xlabel("f (Hz)");
         ylabel("|P1(f)|");
+        grid on;
     end
 
 end
 
-function peakLocs = findallpeaks(signal)
-    % FINDANGLEPEAKS Find peaks in the angle signal
+function peakLocs = findallpeaks(signal, nPeriodIdx)
+    % FINDANGLEPEAKS Find all true peaks in the signal
 
     maxAngle = max(signal);
 
-    [~, peakLocs] = findpeaks(signal, 'MinPeakHeight', maxAngle / 2);
-
+    [~, peakLocs] = findpeaks(signal, ...
+                              'MinPeakProminence', maxAngle / 2, ...
+                              'MinPeakDistance', nPeriodIdx / 2);
 end
 
-function peakLocs = findsyncedpeaks(signal1, signal2, nPeriodIdx, offset, tol)
-    % FINDANGLEPEAKS Find peaks in the angle signal
+function signalRange = findsyncedpeaks(signal1, signal2, nPeriodIdx, offset, nPeaks)
+    % FINDANGLEPEAKS Find peaks in the signals that are synced with respect to the offset desired
 
     % Constants and defaults
-    DEBUG = true;
+    DEBUG = false;
+    OFFSET_TOL = 2e-2;
+    DISABLE_WARNING = true;
 
     % Find peaks in the wing angles
-    peakLocs1 = findallpeaks(signal1);
-    peakLocs2 = findallpeaks(signal2);
+    peakLocs1 = findallpeaks(signal1, nPeriodIdx);
+    peakLocs2 = findallpeaks(signal2, nPeriodIdx);
     npeaks = min(length(peakLocs1), length(peakLocs2));
 
     % Trim to same number of peaks
@@ -96,16 +104,40 @@ function peakLocs = findsyncedpeaks(signal1, signal2, nPeriodIdx, offset, tol)
     peakLocs2 = peakLocs2(1:npeaks);
 
     % Peaks where offset is good
-    diffPeakVals = mod(peakLocs1 - peakLocs2, nPeriodIdx) - offset; % Difference in peaks %FIXME
-    peakLocs1 = peakLocs1(abs(diffPeakVals) <= tol * nPeriodIdx); % FIXME
+    offsets = getphaseoffsets(peakLocs1, peakLocs2, nPeriodIdx);
+    [offVal, offsetIdx] = mink(abs(offsets - offset), nPeaks);
+    % Remove if we start or end by a peak
+    limitsIdx = ismember(offsetIdx, [1, length(peakLocs1)]);
+    offVal(limitsIdx) = [];
+    offsetIdx(limitsIdx) = [];
+
+    if any(offVal > OFFSET_TOL * 360)
+        offsetIdx = offsetIdx(offVal <= OFFSET_TOL * 360);
+        if ~DISABLE_WARNING
+            warning('findwingphase:findsyncedpeaks:wrongPhaseOffset', ...
+                    ['Some peaks have a phase difference larger than %0.2f° from what was '...
+                     'requested. '...
+                     'Reducing the number of peaks to match the tolerance criterion.\n'...
+                     'New number of peaks considered = %d.\n'], OFFSET_TOL * 360, ...
+                    length(offsetIdx));
+        end
+        if isempty(offsetIdx)
+            fprintf(2, '      Phase difference of %0.2f not found!\n', offset);
+        end
+
+    end
+
+    % Signal ranges where it is properly synchronized
+    signalRange = peakLocs1([offsetIdx - 1, offsetIdx + 1]);
 
     if DEBUG
+        peakLocs = peakLocs1(offsetIdx); % Location of the peaks
         figure;
         hold on;
         plot(signal1);
         plot(signal2);
-        for i = 1:length(peakLocs1)
-            plot([peakLocs1(i), peakLocs1(i)], ylim, '-k', 'Linewidth', 1.2);
+        for i = 1:length(peakLocs)
+            plot([peakLocs(i), peakLocs(i)], ylim, '-k', 'Linewidth', 1.2);
         end
         hold off;
         grid on;
@@ -113,3 +145,13 @@ function peakLocs = findsyncedpeaks(signal1, signal2, nPeriodIdx, offset, tol)
     end
 
 end
+
+function offsets = getphaseoffsets(peaks1, peaks2, nPeriodIdx)
+    % GETPHASEOFFSETS Calculate phase offsets between every contiguous peaks
+    %   Note: Offset is positive when peak2 is AFTER peak1
+
+    offsets = peaks2 - peaks1;
+
+    offsets = offsets * 360 / nPeriodIdx;
+
+end