fix(ardu): fix major angle issues

utils/loadardu.m

-function arduData = loadardu(arduFile)
+function [arduShort, arduData] = loadardu(arduFile)
     % LOADARDU Load the Arduino data and clean it.
     %   This function loads the Arduino data. Then it:
     %       - removes the unneeded columns
@@ -38,7 +38,6 @@ function arduData = loadardu(arduFile)
         arduTable = loadcsv(['arduinoFiles/', arduFile, '.csv']);
     end
 
-    % Time conversion
     arduTable.time = timetosec(arduTable.time);
 
     % Remove data with duplicate timecode
@@ -47,16 +46,22 @@ function arduData = loadardu(arduFile)
     arduTable = arduTable(idx, :);
 
     % Convert into table
-    arduData = table2array(arduTable);
+    arduRaw = table2array(arduTable);
+    time = arduRaw(:, 1);
 
     % Calculate power, remove tension and current
-    power = arduData(:, [6, 8]) .* arduData(:, [7, 9]);
-    arduData(:, 6:end) = [];
-    arduData = [arduData, power];
+    power = arduRaw(:, [6, 8]) .* arduRaw(:, [7, 9]);
 
     % Recalculate angles properly from wing positions recoreded
-    arduData(:, 2:5) = fixangles(arduData(:, 2:5));
+    goodAngles = fixangles(arduRaw(:, 2:5));
 
+    % Take mean angles for each set of wings (diff is almost negligible)
+    meanAnglesFront = mean(goodAngles(:, 1:2), 2);
+    meanAnglesAft = mean(goodAngles(:, 3:4), 2);
+
+    % Output a cleaned up tables
+    arduData = [time, goodAngles, power]; % time, FL, FR, AL, AR, PowF, PowA
+    arduShort = [time, meanAnglesFront, meanAnglesAft, power];
 end
 
 function rawTable = loadcsv(file)
@@ -96,11 +101,15 @@ end
 function angles = fixangles(wingPos)
     % FIXANGLES Fix the angle values to avoid issues with the 12 bit overflow
     %   Note: 3rd sensor had an encoding issue. This issue is fixed here automatically
+    %   Note: Aft sensors enocde in the wrong direction (Angles should be opposite)
+    %   Note: Sometimes error with front left wing stuck at 0 or 4095 when not flapping. If stuck,
+    %   just copy right wing
 
     % Defaults and constants
     ENC_PRECISION = 12; % 12-bits encoders
     MAX_WING_ANGLE = 38.07; % Determined by geometry of the setup
-    CALIB_OFFSET = [100.8984  115.7520  199.5996  131.7480]; % Maximum angle recored BEFORE offset
+    CALIB_OFFSET = [100.8984  115.7520  211.4648  278.6133]; % Maximum angle recored BEFORE offset
+    N_VAL_STATIC = 10; % Number of different positions under which it wing is supposed static
 
     % Abbreviations
     nBits = 2^ENC_PRECISION;
@@ -110,9 +119,28 @@ function angles = fixangles(wingPos)
     % [FIXME] Once this is fixed in the controller, add a switch to correct or not the data
     wingPos(:, 3) = mod(wingPos(:, 3) + nBits / 2, nBits16);
 
+    % Aft sensors are in the wrong direction
+    % [FIXME] Once this is fixed in the controller, add a switch to correct or not the data
+    wingPos(:, 3:4) = nBits - wingPos(:, 3:4);
+
     % Convert position data into actual degree
     angles = wingPos * 360 / nBits;
 
+    % maxangles = max(angles)
     % Calibrate values so max angle recored matches MAX_WING_ANGLE
     angles = angles - CALIB_OFFSET + MAX_WING_ANGLE;
+
+    % Fix for front wing stuck in 0 or 4095
+    % For some reason the first wing get sometimes stuck in strange values when there is no motion.
+    % It then returns a position oscillating between 0 and 4095 (or a bit more than that). This can
+    % be detected by simply looking if some diff in position are greater than nBist/2. If it is the
+    % case, either we have an abrupt change of +180° (impossible), or there is an overflow issue.
+
+    if (any(diff(wingPos(:, 1)) > nBits / 2) || ...
+        all(wingPos(:, 1) == nBits - 1) || ...
+        all(wingPos(:, 1) == 0)) && ...
+        ~any(diff(wingPos(:, 2)) > nBits / 2)
+        angles(:, 1) = angles(:, 2);
+    end
+
 end