From 8653bf7349fcd98e7a38db4af9a73bda83e8e186 Mon Sep 17 00:00:00 2001 From: "Bredemeier, Fabian (TD-M)" Date: Tue, 17 Jan 2023 10:01:41 +0100 Subject: [PATCH] Tools: Add missing functions for Simulink model Fixes #22643 --- .../arducopter/functions/dcmFromEuler.m | 27 +++++++++++++++++ .../arducopter/functions/fromAxisAngle.m | 29 +++++++++++++++++++ .../simulink/arducopter/functions/quatMult.m | 10 +++++++ .../arducopter/sid_controller_validation.m | 24 ++++++++------- 4 files changed, 80 insertions(+), 10 deletions(-) create mode 100644 Tools/simulink/arducopter/functions/dcmFromEuler.m create mode 100644 Tools/simulink/arducopter/functions/fromAxisAngle.m create mode 100644 Tools/simulink/arducopter/functions/quatMult.m diff --git a/Tools/simulink/arducopter/functions/dcmFromEuler.m b/Tools/simulink/arducopter/functions/dcmFromEuler.m new file mode 100644 index 0000000000..a8f5c41e8b --- /dev/null +++ b/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::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 \ No newline at end of file diff --git a/Tools/simulink/arducopter/functions/fromAxisAngle.m b/Tools/simulink/arducopter/functions/fromAxisAngle.m new file mode 100644 index 0000000000..dd88133b22 --- /dev/null +++ b/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::from_axis_angle(Vector3) + +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::from_axis_angle(const Vector3 &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 \ No newline at end of file diff --git a/Tools/simulink/arducopter/functions/quatMult.m b/Tools/simulink/arducopter/functions/quatMult.m new file mode 100644 index 0000000000..767e70a0aa --- /dev/null +++ b/Tools/simulink/arducopter/functions/quatMult.m @@ -0,0 +1,10 @@ +function qRes = quatMult(q1, q2) +% Quaternion multiplication. Code based on QuaternionT::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 \ No newline at end of file diff --git a/Tools/simulink/arducopter/sid_controller_validation.m b/Tools/simulink/arducopter/sid_controller_validation.m index 014ea31c24..1c22aca734 100644 --- a/Tools/simulink/arducopter/sid_controller_validation.m +++ b/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');