Tools: Add missing functions for Simulink model

Fixes #22643

Tools/simulink/arducopter/functions/dcmFromEuler.m

@@ -0,0 +1,27 @@
+function dcm = dcmFromEuler(roll, pitch, yaw)
+% Function to calculate the DCM from Euler Angles.
+% Euler Angles are given in radian.
+% Taken from Matrix3<T>::from_euler()
+sr = sin(roll);
+cr = cos(roll);
+sp = sin(pitch);
+cp = cos(pitch);
+sy = sin(yaw);
+cy = cos(yaw);
+
+dcm = zeros(3,3);
+% First row
+dcm(1,1) = cp * cy;
+dcm(1,2) = (sr * sp * cy) - (cr * sy);
+dcm(1,3) = (cr * sp * cy) + (sr * sy);
+% Second row
+dcm(2,1) = cp * sy;
+dcm(2,2) = (sr * sp * sy) + (cr * cy);
+dcm(2,3) = (cr * sp * sy) - (sr * cy);
+% Third row
+dcm(3,1) = -sp;
+dcm(3,2) = sr * cp;
+dcm(3,3) = cr * cp;
+
+
+end

Tools/simulink/arducopter/functions/fromAxisAngle.m

@@ -0,0 +1,29 @@
+function q = fromAxisAngle(vec)
+% Create a quaternion from its axis-angle representation. Just the rotation
+% vector is given as input.
+% Based on QuaternionT<T>::from_axis_angle(Vector3<T>)
+
+q = zeros(4,1);
+angle = single(sqrt(vec(1)^2 + vec(2)^2 + vec(3)^2));
+
+if angle == 0
+    q(1) = single(1);
+    q(2) = single(0);
+    q(3) = single(0);
+    q(4) = single(0);
+else
+    axis = vec ./ angle;
+    % The following lines are based on QuaternionT<T>::from_axis_angle(const Vector3<T> &axis, T theta)
+    if angle == 0
+        q(1) = single(1);
+        q(2) = single(0);
+        q(3) = single(0);
+        q(4) = single(0);
+    else
+        st2 = single(sin(0.5*angle));
+        q(1) = single(cos(0.5*angle));
+        q(2) = axis(1) * st2;
+        q(3) = axis(2) * st2;
+        q(4) = axis(3) * st2;
+    end
+end

Tools/simulink/arducopter/functions/quatMult.m

@@ -0,0 +1,10 @@
+function qRes = quatMult(q1, q2)
+% Quaternion multiplication. Code based on QuaternionT<T>::operator*() in
+% quaternion.cpp
+
+qRes = single(zeros(4,1));
+qRes(1) = q1(1)*q2(1) - q1(2)*q2(2) - q1(3)*q2(3) - q1(4)*q2(4);
+qRes(2) = q1(1)*q2(2) + q1(2)*q2(1) + q1(3)*q2(4) - q1(4)*q2(3);
+qRes(3) = q1(1)*q2(3) - q1(2)*q2(4) + q1(3)*q2(1) + q1(4)*q2(2);
+qRes(4) = q1(1)*q2(4) + q1(2)*q2(3) - q1(3)*q2(2) + q1(4)*q2(1);
+end

Tools/simulink/arducopter/sid_controller_validation.m

@@ -14,7 +14,7 @@ curPath = pwd;
 
 %% Define log file index and controller type to be validated
 % Log file index used for validation
-log_idx = 2;
+log_idx = 5;
 
 % Check validity of log_idx
 if log_idx > numel(sidLogs)
@@ -25,35 +25,35 @@ end
 % validate
 figure % Attitude controller signals
 subplot(311)
-plot(sidLogs(log_idx).data.RATE.TimeS, sidLogs(log_idx).data.RATE.RDes, 'LineWidth', 1.4);
+plot(sidLogs(log_idx).data.ATT.TimeS, sidLogs(log_idx).data.ATT.DesRoll, 'LineWidth', 1.4);
 hold on
-plot(sidLogs(log_idx).data.RATE.TimeS, sidLogs(log_idx).data.RATE.R, 'LineWidth', 1.4);
+plot(sidLogs(log_idx).data.ATT.TimeS, sidLogs(log_idx).data.ATT.Roll, 'LineWidth', 1.4);
 hold off
 grid on
 legend('Desired', 'Actual');
 xlabel('Time (s)');
 ylabel('Roll angle (deg)');
-xlim([0 max(sidLogs(log_idx).data.RATE.TimeS)]);
+xlim([0 max(sidLogs(log_idx).data.ATT.TimeS)]);
 subplot(312)
-plot(sidLogs(log_idx).data.RATE.TimeS, sidLogs(log_idx).data.RATE.PDes, 'LineWidth', 1.4);
+plot(sidLogs(log_idx).data.ATT.TimeS, sidLogs(log_idx).data.ATT.DesPitch, 'LineWidth', 1.4);
 hold on
-plot(sidLogs(log_idx).data.RATE.TimeS, sidLogs(log_idx).data.RATE.P, 'LineWidth', 1.4);
+plot(sidLogs(log_idx).data.ATT.TimeS, sidLogs(log_idx).data.ATT.Pitch, 'LineWidth', 1.4);
 hold off
 grid on
 legend('Desired', 'Actual');
 xlabel('Time (s)');
 ylabel('Pitch angle (deg)');
-xlim([0 max(sidLogs(log_idx).data.RATE.TimeS)]);
+xlim([0 max(sidLogs(log_idx).data.ATT.TimeS)]);
 subplot(313)
-plot(sidLogs(log_idx).data.RATE.TimeS, sidLogs(log_idx).data.RATE.YDes, 'LineWidth', 1.4);
+plot(sidLogs(log_idx).data.ATT.TimeS, sidLogs(log_idx).data.ATT.DesYaw, 'LineWidth', 1.4);
 hold on
-plot(sidLogs(log_idx).data.RATE.TimeS, sidLogs(log_idx).data.RATE.Y, 'LineWidth', 1.4);
+plot(sidLogs(log_idx).data.ATT.TimeS, sidLogs(log_idx).data.ATT.Yaw, 'LineWidth', 1.4);
 hold off
 grid on
 legend('Desired', 'Actual');
 xlabel('Time (s)');
 ylabel('Yaw angle (deg)');
-xlim([0 max(sidLogs(log_idx).data.RATE.TimeS)]);
+xlim([0 max(sidLogs(log_idx).data.ATT.TimeS)]);
 
 % Plot z Controller signals if Althold flight mode was active
 if sidLogs(log_idx).data.MODE.Mode == 2
@@ -80,6 +80,10 @@ switch sidLogs(log_idx).data.MODE.Mode
         fprintf(' (Stabilize)\n');
     case 2
         fprintf(' (Althold)\n');
+    otherwise
+        fprintf(['\n No available flight mode for the validation could be found. ' ...
+            'Please use the Stabilize or Althold flight mode!\n']);
+        return;
 end
 fprintf('Available controllers that can be validated: \n');
 fprintf('1 = Rate controller\n');