Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Tools: Add missing functions for Simulink model 

Fixes #22643
This commit is contained in:
Bredemeier, Fabian (TD-M) 2023-01-17 10:01:41 +01:00 committed by Andrew Tridgell
parent db60b2586e
commit 8653bf7349
4 changed files with 80 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
Tools/simulink/arducopter/functions/dcmFromEuler.m Normal file
View File

@ -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

29
Tools/simulink/arducopter/functions/fromAxisAngle.m Normal file
View File

@ -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

10
Tools/simulink/arducopter/functions/quatMult.m Normal file
View File

@ -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

24
Tools/simulink/arducopter/sid_controller_validation.m
View File

@ -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');