feat(loadardu): true angle calculation

utils/loadardu.m

@@ -54,8 +54,8 @@ function arduData = loadardu(arduFile)
     arduData(:, 6:end) = [];
     arduData = [arduData, power];
 
-    % Recalculate angles properly
-
+    % Recalculate angles properly from wing positions recoreded
+    arduData(:, 2:5) = fixangles(arduData(:, 2:5));
 end
 
 function rawTable = loadcsv(file)
@@ -78,7 +78,6 @@ function rawTable = loadcsv(file)
     rawTable.Properties.VariableNames = {'time', 'angleFrontLeft', 'angleFrontRight', ...
                                          'angleAftLeft', 'angleAftRight', ...
                                          'tensionF', 'currentF', 'tensionA', 'currentA'};
-
 end
 
 function timeMilli = timetomilli(time)
@@ -89,5 +88,28 @@ function timeMilli = timetomilli(time)
 
     % Converts into difference from the first time
     timeMilli = milliseconds(time - time(1)) + 1; % Shift of 1 so first time is 1 millisecond
+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
+
+    % Defaults and constants
+    ENC_PRECISION = 12; % 12-bits encoders
+    MAX_WING_ANGLE = 38.07; % Determined by geometry of the setup
+
+    % Abbreviations
+    nBits = 2^ENC_PRECISION;
+    nBits16 = 2^16;
+
+    % Fix issue with third sensor (aft left wing) where it was converted in a 16-bits int
+    % 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);
+
+    % Convert position data into actual degree
+    angles = wingPos * 360 / nBits;
 
+    % Calibrate values so max aligns with MAX_WING_ANGLE
+    maxAng = max(angles);
+    angles = angles - maxAng + MAX_WING_ANGLE;
 end