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

Copter: Simulink Model and init scripts 

- arducopter.slx: Simulates ArduCopter Stabilize and Althold controller and optional plant model
- sid_pre.m: Loads *.bin files to Matlab structs
- sid_sim_init.m: Loads signals and parameters from Matlab structure into Simulink model
- sid_controller_validation.m: Validation of the flight controller model with the flight data loaded to the Matlab workspace.
This commit is contained in:
Bredemeier, Fabian (TD-M) 2022-12-19 17:49:35 +01:00 committed by Peter Barker
parent dd539f8ec9
commit 2abfb1bec8
14 changed files with 2455 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
Tools/simulink/arducopter/README.md Normal file
View File

@ -0,0 +1,19 @@
# ArduCopter Simulink Model
The Simulink file [arducopter.slx](https://github.com/fbredeme/ardupilot_simulink/blob/master/arducopter.slx) contains a model of the ArduCopter Stabilize and Althold controllers. The plant models of the copter itself can be generated by [system identification](https://ardupilot.org/copter/docs/systemid-mode.html).
![](simulink_model.PNG?raw=true "Simulink screenshot")
To run the simulation, the relevant data from a *.bin log file needs to be loaded to the MATLAB workspace. This can be done with the two scripts that are provided:
1) **sim_pre.m**: Loads flightdata from dataflash logs
Opens a dialog to select the flightdata logs for data extraction with Ardupilog. Then, subflights from the log can be selected in the command line. A subflight represents a time segment with one active flight mode. Therefore, flight mode changes divide the log in several subflights. After subflight selection, a array called "sidLogs" is created. Each element of the array represents one selected subflight.
2) **sid_sim_init.m**: Loads relevant data for the simulation of a subflight from the sidLogs array
At first, the index of the subflight in the sidLogs array has to be configured in the first line of the script. The subflight can be cropped in its length by configuring start and stop time ("tStart" and "tEnd"). Setting these to "0" and "inf" respectively will load the whole subflight into the workspace. Finally, the variable "simMode" can be used to test different parts of the whole Simulink model. Transfer function models of the copter, if available, can be simulated by setting the variable to 0. In any other configuration, dummy models will be used. The other modes 2, 3 and 4 can be used to validate the implemented controllers against measured data. In these simulation modes, the logged target and actual values are given to the respective controller, ignoring the computed signals of the model. The correct control behaviour can be validated by comparing the computed output signals to the measured output signals with the "Data Inspector" of Simulink.
For the validation of the implemented controller models, one can use the script **sid_controller_validation.m** after the flight data has been loaded to the workspace with **sim_pre.m**. In this case, only the index of the loaded log file ("log_idx") has to be specified by the user. The script then asks the user for the controller type to be validated and runs the simulation automatically. Afterwards, the simulated outputs of the respective controller are plotted together with the measured outputs and the fit between the corresponding signals is calculated. Both the figures and the calculated fits are saved in the "results" folder.
## Prerequesites
- [Ardupilog](https://github.com/Georacer/ardupilog) (add local repo to path in Matlab)
- A dataflash log of a test flight with the following LOG_BITMASK: 135923

BIN
Tools/simulink/arducopter/arducopter.slx Normal file
View File

Binary file not shown.

204
Tools/simulink/arducopter/functions/ModeConfig.m Normal file
View File

@ -0,0 +1,204 @@
% This class provides important setting information for the flight modes
% that have been chosen for the SID process.
%
% Fabian Bredemeier - IAV GmbH
% License: GPL v3
classdef ModeConfig
properties
name = '';
mode_num;
thr_man;
filter_msgs = {};
end
methods
% Constructor
function obj = ModeConfig(mode_num)
props = ModeConfig.getModeProps(mode_num);
obj.name = props.name;
obj.mode_num = mode_num;
obj.thr_man = props.thr_man;
obj.filter_msgs = props.filter_msgs;
end
% Function that returns index of mode properties that corresponds to
% the mode number
function modeIndex = getIndexFromModeNum(obj, modeNum)
for i=1:length(obj.names)
if (obj.mode_num_list(i) == modeNum)
modeIndex = i;
break;
end
end
end
function filter_msgs = get_filter_msgs(obj)
filter_msgs = obj.filter_msgs;
end
function thr_man = get_thr_man(obj)
thr_man = obj.thr_man;
end
function name = get_name(obj)
name = obj.name;
end
% Function to compare properties to another ModeConfig object
function identical = compare_to(obj, obj2)
% Name
if strcmp(obj.name, obj2.get_name()) ~= 1
identical = false;
return;
end
% Throttle Man
if obj.thr_man ~= obj2.get_thr_man
identical = false;
return;
end
% Filter message list
if numel(obj.filter_msgs) ~= numel(obj2.get_filter_msgs())
identical = false;
return;
end
for i=1:numel(obj.filter_msgs)
obj2_filter_msgs = obj2.get_filter_msgs();
if strcmp(obj.filter_msgs{i}, obj2_filter_msgs{i}) ~= 1
identical = false;
return;
end
end
identical = true;
end
end
methods (Static)
% Function that stores all mode properties
function propsStruct = getModeProps(modeNum)
% Define basic filter message cell array which is necessary for
% only the inner loop or the part that is used in SYSID
% respectively. Used for modes whose relevant msgs are not yet
% clear.
filter_msgs_sysid = {'FMT', 'UNIT', 'FMTU', 'MULT', 'PARM', 'MODE', 'SIDD', ...
'SIDS', 'ATT', 'CTRL', 'RATE', 'PIDR', 'PIDP', 'PIDY', 'CTUN', ...
'IMU','BAT', 'BARO', 'MOTB'};
% Filter message cell array which is necessary to run simulation
% in loiter mode
filter_msgs_loiter = {'FMT', 'UNIT', 'FMTU', 'MULT', 'PARM', 'MODE', ...
'ATT', 'CTRL', 'RATE', 'PIDR', 'PIDP', 'PIDY', 'PIDA', 'CTUN', ...
'IMU', 'BAT', 'BARO', 'MOTB', 'PSCD', 'PSCE', 'PSCN', 'RCIN'};
% Stabilize
filter_msgs_stabilize = {'FMT', 'UNIT', 'FMTU', 'MULT', 'PARM', 'MODE', ...
'ATT', 'CTRL', 'RATE', 'PIDR', 'PIDP', 'PIDY', 'PIDA', 'CTUN', ...
'IMU', 'BAT', 'BARO', 'MOTB', 'GYR', 'ACC', 'RCIN'};
% Acro
filter_msgs_acro = {'FMT', 'UNIT', 'FMTU', 'MULT', 'PARM', 'MODE', ...
'ATT', 'CTRL', 'RATE', 'PIDR', 'PIDP', 'PIDY', 'PIDA', 'CTUN', ...
'IMU', 'BAT', 'BARO', 'MOTB', 'GYR', 'ACC'};
% AltHold
filter_msgs_althold = {'FMT', 'UNIT', 'FMTU', 'MULT', 'PARM', 'MODE', ...
'ATT', 'CTRL', 'RATE', 'PIDR', 'PIDP', 'PIDY', 'PIDA', 'CTUN', ...
'IMU', 'BAT', 'BARO', 'MOTB', 'PSCD', 'GYR', 'ACC', 'RCIN'};
switch modeNum
case 0 % Stabilize
propsStruct.name = 'Stabilize';
propsStruct.thr_man = true;
propsStruct.filter_msgs = filter_msgs_stabilize;
case 1 % Acro
propsStruct.name = 'Acro';
propsStruct.thr_man = true;
propsStruct.filter_msgs = filter_msgs_acro;
case 2 % Alt_Hold
propsStruct.name = 'Alt_Hold';
propsStruct.thr_man = false;
propsStruct.filter_msgs = filter_msgs_althold;
case 3 % Auto
propsStruct.name = 'Auto';
propsStruct.thr_man = false;
propsStruct.filter_msgs = filter_msgs_loiter;
case 4 % Guided
propsStruct.name = 'Guided';
propsStruct.thr_man = false;
propsStruct.filter_msgs = filter_msgs_loiter;
case 5 % Loiter
propsStruct.name = 'Loiter';
propsStruct.thr_man = false;
propsStruct.filter_msgs = filter_msgs_loiter;
case 6 % RTL
propsStruct.name = 'RTL';
propsStruct.thr_man = false;
propsStruct.filter_msgs = filter_msgs_sysid;
case 7 % Circle
propsStruct.name = 'Circle';
propsStruct.thr_man = false;
propsStruct.filter_msgs = filter_msgs_sysid;
case 9 % Land
propsStruct.name = 'Land';
propsStruct.thr_man = false;
propsStruct.filter_msgs = filter_msgs_sysid;
case 11 % Drift
propsStruct.name = 'Drift';
propsStruct.thr_man = false;
propsStruct.filter_msgs = filter_msgs_sysid;
case 13 % Sport
propsStruct.name = 'Sport';
propsStruct.thr_man = false;
propsStruct.filter_msgs = filter_msgs_sysid;
case 14 % Flip
propsStruct.name = 'Flip';
propsStruct.thr_man = false;
propsStruct.filter_msgs = filter_msgs_sysid;
case 15 % Autotune
propsStruct.name = 'Autotune';
propsStruct.thr_man = false;
propsStruct.filter_msgs = filter_msgs_stabilize;
case 16 % Poshold
propsStruct.name = 'Poshold';
propsStruct.thr_man = false;
propsStruct.filter_msgs = filter_msgs_sysid;
case 17 % Brake
propsStruct.name = 'Brake';
propsStruct.thr_man = false;
propsStruct.filter_msgs = filter_msgs_sysid;
case 18 % Throw
propsStruct.name = 'Throw';
propsStruct.thr_man = false;
propsStruct.filter_msgs = filter_msgs_sysid;
case 19 % Avoid_ADSB
propsStruct.name = 'Avoid_ADSB';
propsStruct.thr_man = false;
propsStruct.filter_msgs = filter_msgs_sysid;
case 20 % Guided_NoGPS
propsStruct.name = 'Guided_NoGPS';
propsStruct.thr_man = false;
propsStruct.filter_msgs = filter_msgs_sysid;
case 21 % SmartRTL
propsStruct.name = 'SmartRTL';
propsStruct.thr_man = false;
propsStruct.filter_msgs = filter_msgs_sysid;
case 22 % Flowhold
propsStruct.name = 'Flowhold';
propsStruct.thr_man = false;
propsStruct.filter_msgs = filter_msgs_sysid;
case 23 % Follow
propsStruct.name = 'Follow';
propsStruct.thr_man = false;
propsStruct.filter_msgs = filter_msgs_sysid;
case 24 % Zigzag
propsStruct.name = 'Zigzag';
propsStruct.thr_man = false;
propsStruct.filter_msgs = filter_msgs_sysid;
case 25 % Systemid
propsStruct.name = 'Systemid';
propsStruct.thr_man = true;
propsStruct.filter_msgs = filter_msgs_sysid;
case 26 % Autorotate
propsStruct.name = 'Autorotate';
propsStruct.thr_man = false;
propsStruct.filter_msgs = filter_msgs_sysid;
end
end
end
end

41
Tools/simulink/arducopter/functions/demodYawAngle.m Normal file
View File

@ -0,0 +1,41 @@
function [sigOut] = demodYawAngle(sigIn)
% Inverse the modulo of 360° operation used on the yaw angle signals
%
% Fabian Bredemeier - IAV GmbH
% License: GPL v3
% Output signal
sigOut = zeros(length(sigIn),1);
% Calcualte the differences between samples
sigDiff = diff(sigIn);
% If difference is larger than 2*pi, safe last sample as offset for
% following signals
offset = 0;
skip = 0; % Var to skip the signal sample directly after abrupt angle change
for i = 1:length(sigIn)
sigOut(i) = offset + sigIn(i+skip);
if i < length(sigDiff) && i > 1
% Reset prevent Adding
if abs(sigDiff(i)) < 90
skip = 0;
% Add to offset, if switch happened from 2pi to 0
elseif sigDiff(i) <= -90 && ~skip
offset = offset + 360;%(sigIn(i-2)+sigIn(i-1))/2; % Smooth signal values since they are oscillating at jumps
skip = 1;
% Subtract from offset, if switch happened from 0 tp 2 pi
elseif sigDiff(i) > 90 && ~skip
offset = offset - 360;%(sigIn(i+2)+sigIn(i+1))/2; % Smooth signal values since they are oscillating at jumps
skip = 1;
end
end
end
% Apply moving average filter to smooth out sample value jumps
steps = 10;
coeffFilt = ones(1, steps)/steps;
% sigOut = filter(coeffFilt, 1, sigOut);
sigOut = movmean(sigOut, 9);
end

126
Tools/simulink/arducopter/functions/getMotorMixerFactors.m Normal file
View File

@ -0,0 +1,126 @@
% Function to get the copters number of motors and there configuration to
% calculate the motor mixer factors for roll, pitch and yaw
%
% Fabian Bredemeier - IAV GmbH
% License: GPL v3
function [num_motors, axis_facs_motors] = getMotorMixerFactors(frame_class, frame_type)
d2r = pi/180;
axis_facs_motors = struct;
num_motors = 0;
% Preliminary cell array for the motors axis factors consisting of the
% roll/pitch factor in degree (first element of cells) and the yaw
% factor
axis_facs_motors_pre = cell(0,0);
%% Get configuration
switch frame_class
case 1 % Quad
num_motors = 4;
axis_facs_motors_pre = cell(4,1);
switch frame_type
case 0 % Plus
axis_facs_motors_pre = { [90, 1]; [-90, 1]; [0, -1]; [180, -1] };
case 1 % X
axis_facs_motors_pre = { [45, 1]; [-135, 1]; [-45, -1]; [135, -1] };
case 2 % V
axis_facs_motors_pre = { [45, 0.7981]; [-135, 1.0]; [-45, -0.7981]; [135, -1.0] };
case 3 % H
axis_facs_motors_pre = { [45, -1]; [-135, -1]; [-45, 1]; [135, 1] };
case 13 % DJIX
axis_facs_motors_pre = { [45, 1]; [-45, -1]; [-135, 1]; [135, -1] };
case 14 % Clockwise ordering
axis_facs_motors_pre = { [45, 1]; [135, -1]; [-135, 1]; [-45, -1] };
end
case 2 % Hexa
num_motors = 6;
case {3, 4} % Octa and OctaQuad
num_motors = 8;
axis_facs_motors_pre = cell(8,1);
switch frame_type
case 0 % Plus
axis_facs_motors_pre = { [0, -1]; [180, -1]; [45, 1]; [135, 1]; ...
[-45, 1]; [-135, 1]; [-90, -1]; [90, -1] };
case 1 % X
axis_facs_motors_pre = { [22.5, -1]; [-157.5, -1]; [67.5, 1]; [157.5, 1]; ...
[-22.5, 1]; [-122.5, 1]; [-67.5, -1]; [112.5, -1] };
case 2 % V
axis_facs_motors_pre = { [0.83, 0.34, -1]; [-0.67, -0.32, -1]; [0.67, -0.32, 1]; [-0.50, -1.00, 1]; ...
[1.00, 1.00, 1]; [-0.83, 0.34, 1]; [-1.00, 1.00, -1]; [0.5, -1.00, -1] };
end
case 5 % Y6
num_motors = 6;
case 12 % DodecaHexa
num_motors = 12;
case 14 % Deca
num_motors = 14;
end
%% Check if configuration is defined
% Leave function if frame class is not configured yet
if num_motors == 0
disp("Frame class unknown. Please add the configuration to the function.");
return;
end
% Leave function if axis factors are not yet defined for frame
if isempty(axis_facs_motors_pre)
disp("Motor axis factors are not yet defined for frame class!");
disp("The factors can be found in the setup_motors function within AP_MotorsMatrix.cpp.");
return;
end
%% Calculate axis factors (roll/pitch) from preliminary array
% Create the output struct that stores the factors for the different
% axis in separate arrays. Each column of the subarrays represents one motor.
axis_facs_motors = struct('roll', zeros(1,num_motors), ...
'pitch', zeros(1,num_motors), ...
'yaw', zeros(1,num_motors) , ...
'throttle', zeros(1, num_motors) );
for i=1:num_motors
% Check if factors for roll and pitch are defined separately and
% therefore in radian -> dimension of a cell would be 3
if ( length(axis_facs_motors_pre{i}) >= 3 )
axis_facs_motors.roll(1,i) = axis_facs_motors_pre{i}(1);
axis_facs_motors.pitch(1,i) = axis_facs_motors_pre{i}(2);
else
axis_facs_motors.roll(1,i) = cos( (axis_facs_motors_pre{i}(1)+90)*d2r );
axis_facs_motors.pitch(1,i) = cos( (axis_facs_motors_pre{i}(1))*d2r );
end
% The factors for yaw can be acquired directly from the preliminary
% array
axis_facs_motors.yaw(1,i) = axis_facs_motors_pre{i}(2);
% The factors for throttle are 1.0 by default (see AP_MotorsMatrix.h,
% line 87)
axis_facs_motors.throttle(1,i) = 1.0;
end
%% Normalization of factors (AP_MotorsMatrix.cpp, line 1218)
roll_fac_max = 0.0;
pitch_fac_max = 0.0;
yaw_fac_max = 0.0;
throttle_fac_max = 0.0;
% Find maximum factor
for i=1:num_motors
roll_fac_max = max(roll_fac_max, axis_facs_motors.roll(1,i));
pitch_fac_max = max(pitch_fac_max, axis_facs_motors.pitch(1,i));
yaw_fac_max = max(yaw_fac_max, axis_facs_motors.yaw(1,i));
throttle_fac_max = max(throttle_fac_max, axis_facs_motors.throttle(1,i));
end
% Scale factors back to -0.5 to +0.5 for each axis
for i=1:num_motors
if(roll_fac_max ~= 0)
axis_facs_motors.roll(1,i) = 0.5 * axis_facs_motors.roll(1,i) / roll_fac_max;
end
if(pitch_fac_max ~= 0)
axis_facs_motors.pitch(1,i) = 0.5 * axis_facs_motors.pitch(1,i) / pitch_fac_max;
end
if(yaw_fac_max ~= 0)
axis_facs_motors.yaw(1,i) = 0.5 * axis_facs_motors.yaw(1,i) / yaw_fac_max;
end
if(throttle_fac_max ~= 0)
axis_facs_motors.throttle(1,i) = 0.5 * axis_facs_motors.throttle(1,i) / throttle_fac_max;
end
end
end

39
Tools/simulink/arducopter/functions/getParamVal.m Normal file
View File

@ -0,0 +1,39 @@
function paramVals = getParamVal(obj,paramName)
% Return the value for the parameters defined in the cell array
% paramName of the sid object obj
%
% Fabian Bredemeier - IAV GmbH
% License: GPL v3
% Get list of parameter names
param_names = cell(length(obj.PARM.Name),1);
for i=1:length(obj.PARM.Name)
param_names{i,1} = deblank(obj.PARM.Name(i,1:16));
end
% Translate parameter name to cell array if only one name is provided
if ~iscell(paramName)
desParamNames{1} = paramName;
else
desParamNames = paramName;
end
% Read parameter(s) with boolean indexing
paramVals = single(zeros(numel(desParamNames),1));
for i = 1:length(desParamNames)
paramIdx = strcmp(param_names, desParamNames{i});
paramVal = single(obj.PARM.Value(paramIdx));
if isempty(paramVal)
warning('Parameter could not be found!');
paramVal = [];
return;
end
paramVals(i,1) = paramVal(1); % Prevent nultidimensional parameters
if isempty(paramVals)
error(['Parameter ' desParam{i} ' could not be found.']);
end
end
end

47
Tools/simulink/arducopter/functions/getTimeZero.m Normal file
View File

@ -0,0 +1,47 @@
function timeZero = getTimeZero(log, msgs, msgs2Ex)
% Get the earliest time stamp in the log
%
% Fabian Bredemeier - IAV GmbH
% License: GPL v3
% Create dummy zero timestamp
if isprop(log, 'RATE')
timeZero = log.RATE.TimeS(1);
else
error('Message RATE not contained in log.');
end
% Define messages that should not be included in search for time stamp
if nargin < 3
msgs2Exclude = {'FMT', 'FMTU', 'MODE', 'MULT', 'UNIT', 'PARM'};
else
msgs2Exclude = msgs2Ex;
end
for m = msgs
msg = m{1};
% Skip messages to exclude
if any(strcmp(msgs2Exclude, msg))
continue;
end
% Output warning if message is not included in log
if ~any(strcmp(fieldnames(log), msg))
warning(['Message ' msg ' not included in log!']);
end
% Skip message if message is deleted handle
if ~isvalid(log.(msg))
continue;
end
% Compare first time stamps and get lower start time
firstTime = log.(msg).TimeS(1);
if abs(timeZero - firstTime) > 2
error(['Time vector of ' msg ' invalid. The first time stamp is too far off. Aborting.']);
elseif firstTime < timeZero
timeZero = firstTime;
end
end
end

98
Tools/simulink/arducopter/functions/plausibilizeTime.m Normal file
View File

@ -0,0 +1,98 @@
function [timePlaus, signalPlaus] = plausibilizeTime(time,signal)
% Checks if time vector has a major tracking error and delete respective
% element from time and signal vector
%
% Fabian Bredemeier - IAV GmbH
% License: GPL v3
% Get indices for time values that are too high
validIdx = ~(time > 1e6);
if time(1) > time(2) % First time stamp is invalid if greater than second
validIdx(1) = 0;
end
% Calculate average sample time
Ts_avg = sum(diff(time(validIdx)))/length(time);
% Only plausibilize time vector
if nargin == 1
% Set first element
if time(1) > 1e6 && time(2) < 1e6 || time(1) > time(2)
timePlaus = time(2)-Ts_avg;
else
timePlaus = time(1);
end
idxDiff = 0; % Tracking of removed elements
skipNext = 0; % Flag to skip next element
for i = 2:length(time)-1
if skipNext
skipNext = 0;
continue;
end
if(time(i+1)<time(i))
% Check if current or next element is incorrect
if abs((time(i)-time(i-1))-Ts_avg) > abs((time(i+1)-time(i-1))-Ts_avg) && time(i+1)-time(i-1) > 0
% Current element is wrong since deviation to average
% sample time is larger - skip current and do nothing
else
% Next element is wrong, so add current to corrected vector
timePlaus(i-idxDiff,1) = time(i);
% Make sure next one is skipped
skipNext = 1;
end
idxDiff = idxDiff + 1;
else
timePlaus(i-idxDiff,1) = time(i);
end
end
% Add last element
len = length(time);
timePlaus(len-idxDiff,1) = time(end);
return;
% Also plausibilize signal vector
elseif nargin == 2
% Set first element
if time(1) > 1e6 && time(2) < 1e6 || time(1) > time(2)
timePlaus = time(2)-Ts_avg;
signalPlaus = signal(2);
else
timePlaus = time(1);
signalPlaus = signal(1);
end
idxDiff = 0; % Tracking of removed elements
skipNext = 0; % Flag to skip next element
for i = 2:length(time)-1
if skipNext
skipNext = 0;
continue;
end
if(time(i+1)<time(i))
% Check if current or next element is incorrect
if abs((time(i)-time(i-1))-Ts_avg) > abs((time(i+1)-time(i-1))-Ts_avg) && time(i+1)-time(i-1) > 0
% Current element is wrong since deviation to average
% sample time is larger - skip current and do nothing
else
% Next element is wrong, so add current to corrected vector
timePlaus(i-idxDiff,1) = time(i);
signalPlaus(i-idxDiff,1) = signal(i);
% Make sure next one is skipped
skipNext = 1;
end
idxDiff = idxDiff + 1;
else
timePlaus(i-idxDiff,1) = time(i);
signalPlaus(i-idxDiff,1) = signal(i);
end
end
% Add last element
len = length(time);
timePlaus(len-idxDiff,1) = time(end);
signalPlaus(len-idxDiff,1) = signal(end);
end
end

166
Tools/simulink/arducopter/functions/resampleLog.m Normal file
View File

@ -0,0 +1,166 @@
function [logResampled] = resampleLog(log,resampling,method)
% Resamples log if resample is set to true, so that all included messages
% have the same first time stamp and the same amount of elements
% (if they have the same sample time)
% If resample is set to false, the Time vector of all messages is just
% adapted to the first time stamp of the log
%
% Fabian Bredemeier - IAV GmbH
% License: GPL v3
headerProps2Copy = {'fileName', 'filePathName', 'platform', 'version', ...
'commit', 'bootTimeUTC', 'msgsContained', 'totalLogMsgs', 'msgFilter', ...
'numMsgs'};
% Define messages that are to be excluded from resampling, but still copied
msgs2Exclude = {'FMT', 'FMTU', 'MODE', 'MULT', 'UNIT', 'PARM', 'SIDS'};
% Messages with multiple instances
msgWithMultInstances = {'ARSP', 'BARO', 'BAT', 'BCL', 'BCL2', 'ESC', ...
'GPA', 'GPS', 'IMU', 'MAG', 'PRX', 'RFND'};
% Define message header props that need to be copied to new log
msgProps2Copy = {'typeNumID', 'fieldUnits', 'fieldMultipliers', 'name', ...
'MsgInstance'};
% Define message props that need to be excluded in new log
msgProps2Exclude = {'TimeS', 'LineNo', 'DatenumUTC', 'TimeUS'};
% Define sample times that are used for logging
TsVec = [0.00025 0.001 0.0025 0.01 0.02 0.04 0.1 0.2 1];
% Allowed jitter percentage of sample time
TsJitPerc = 0.5;
% Get first timestamp of log
timeZero = getTimeZero(log, log.msgsContained, msgs2Exclude);
% Convert LogMsgGroup object into structs
log = log.getStruct();
for prop = string(fieldnames(log))'
propObj = log.(prop);
% Just copy property if contained in cell array
if any(strcmp(prop, headerProps2Copy))
logResampled.(prop) = propObj;
continue;
end
% Skip message if it shall be excluded
if any(strcmp(prop, msgs2Exclude))
logResampled.(prop) = propObj;
continue;
end
% Skip message if it is a deleted handle
if isempty(propObj)
continue;
end
% Handle messages with multiple instances (like BAT)
if any(strcmp(msgWithMultInstances, prop))
% Create idx vector for certain instance
if isfield(propObj, 'Instance') % Check message name of instance
idxVec = propObj.Instance == 0;
elseif isfield(propObj, 'I')
idxVec = propObj.I == 0;
elseif isfield(propObj, 'C') % XKF messages
idxVec = propObj.C == 0;
end
else % Message without multiple instances
idxVec = logical(ones(length(propObj.TimeS),1));
end
% Get plausibilized time vector
timePlaus = plausibilizeTime(log.(prop).TimeS(idxVec));
% Determine sample time of message
TsRaw = mean(diff(timePlaus));
Ts = 0;
for TsElem = TsVec
if TsRaw > TsElem*(1-TsJitPerc) && TsRaw < TsElem*(1+TsJitPerc)
Ts = TsElem;
break;
end
end
% Throw error if Ts could not be determined
if Ts == 0
error(['Sample time of message ' char(prop) ' could not be determined. Mean: ' num2str(TsRaw) 's']);
end
% Copy properties to new object. Resample signals
msgProps = string(fieldnames(log.(prop)))';
for msgProp = msgProps(1:end)
% Copy property if included in msgProps2Copy
if any(strcmp(msgProp, msgProps2Copy))
logResampled.(prop).(msgProp) = propObj.(msgProp);
continue;
end
% Skip property if included in msgProps2Exclude
if any(strcmp(msgProp, msgProps2Exclude))
continue;
end
% Get signal values for the correct indices
signal = propObj.(msgProp)(idxVec);
% Define time starting from first time stamp
time = propObj.TimeS(idxVec) - timeZero;
% Plausibilize time and signal vector
[time, signal] = plausibilizeTime(time, signal);
% If any time sample is still larger than the final time,
% plausibilize the time vector again
while(any(time > time(end)))
[time, signal] = plausibilizeTime(time, signal);
end
% Define old time vector. Insert zero at beginning for the signals
% to start at 0s. Insert first signal value in value vector.
if time(1) > 1e-3
timeOld = zeros(length(time)+1,1);
signalOld = zeros(length(signal)+1,1);
timeOld(2:length(time)+1, 1) = time;
timeOld(1) = 0;
signalOld(2:length(signal)+1, 1) = signal;
signalOld(1) = signal(1);
else
timeOld = time;
signalOld = signal;
end
% Catch nan in signal
if any(isnan(signalOld))
signalOld = zeros(length(signalOld),1);
warning(['Detected nan in ' char(prop) '.' char(msgProp)])
end
if resampling
% Resample signal
if any(strcmp(method, 'makima')) || any(strcmp(method, 'nearest'))
len = round(timeOld(end) / Ts, 0); % Get amount of time vec elements
timeNew = Ts * (0:len)';
signalNew = interp1(timeOld, signalOld, timeNew, method, 'extrap');
else
[signalNew, timeNew] = resample(signalOld, timeOld, 1/Ts, method);
end
else
signalNew = signal;
timeNew = Ts*(0:length(signal)-1)';
end
% Insert property
logResampled.(prop).(msgProp) = signalNew;
% Add new property sampling time Ts
logResampled.(prop).Ts = Ts;
end
% Add Time Property
logResampled.(prop).TimeS = timeNew;
end
end

237
Tools/simulink/arducopter/sid_controller_validation.m Normal file
View File

@ -0,0 +1,237 @@
% Validation of the controller model
%
% The script initializes and runs the Simulink model in controller
% validation mode and validates the controller model.
%
% The user has to specify the controller that is to be validated (simMode)
% and the index of the logfile in "sidLogs" (log_idx) that is used for the
% validation.
%
% Fabian Bredemeier - IAV GmbH
% License: GPL v3
curPath = pwd;
%% Define log file index and controller type to be validated
% Log file index used for validation
log_idx = 2;
% Check validity of log_idx
if log_idx > numel(sidLogs)
error(['Defined log_idx exceeds number of elements in sidLogs (' num2str(numel(sidLogs)) ')!']);
end
% Show figure with flight paths for user to decide which controller to
% validate
figure % Attitude controller signals
subplot(311)
plot(sidLogs(log_idx).data.RATE.TimeS, sidLogs(log_idx).data.RATE.RDes, 'LineWidth', 1.4);
hold on
plot(sidLogs(log_idx).data.RATE.TimeS, sidLogs(log_idx).data.RATE.R, '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)]);
subplot(312)
plot(sidLogs(log_idx).data.RATE.TimeS, sidLogs(log_idx).data.RATE.PDes, 'LineWidth', 1.4);
hold on
plot(sidLogs(log_idx).data.RATE.TimeS, sidLogs(log_idx).data.RATE.P, '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)]);
subplot(313)
plot(sidLogs(log_idx).data.RATE.TimeS, sidLogs(log_idx).data.RATE.YDes, 'LineWidth', 1.4);
hold on
plot(sidLogs(log_idx).data.RATE.TimeS, sidLogs(log_idx).data.RATE.Y, '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)]);
% Plot z Controller signals if Althold flight mode was active
if sidLogs(log_idx).data.MODE.Mode == 2
figure
plot(sidLogs(log_idx).data.PSCD.TimeS, -1*(sidLogs(log_idx).data.PSCD.TPD), 'LineWidth', 1.4); % -1 to yield upwards position
hold on
plot(sidLogs(log_idx).data.PSCD.TimeS, -1*(sidLogs(log_idx).data.PSCD.PD), 'LineWidth', 1.4);
hold off
grid on
legend('Desired', 'Actual');
xlabel('Time (s)');
ylabel('z Position (m)');
xlim([0 max(sidLogs(log_idx).data.RATE.TimeS)]);
end
% Ask user for controller to validate
% 1 = Rate controller
% 2 = Attitude controller
% 3 = Position controller
fprintf('Flight mode in log file: ');
fprintf('%d', sidLogs(log_idx).data.MODE.Mode);
switch sidLogs(log_idx).data.MODE.Mode
case 0
fprintf(' (Stabilize)\n');
case 2
fprintf(' (Althold)\n');
end
fprintf('Available controllers that can be validated: \n');
fprintf('1 = Rate controller\n');
fprintf('2 = Attitude controller\n');
fprintf('3 = z Position controller\n');
fprintf('0 = Abort.\n');
simMode = input(['Enter controller to be validated: ']);
switch simMode
case 0
fprintf('Aborting...\n');
close all
return;
case 1
ctrlName = 'rateCtrl';
case 2
ctrlName = 'attCtrl';
case 3
ctrlName = 'zPosCtrl';
end
% Define title of validation for documentation purposes
valTitle = 'Arducopter-4.3';
%% Configuration of validation - Get signal names for comparison
% Declaration of temporary evaluation structs
val_out_sig_names = {};
val_out_sig_sim = {};
val_out_sig_meas = {};
val_in_sig_names = {};
val_in_sig = {};
% Call the config script to read the necessary signals for the evaluation
% and the names of their counterparts in the Ardupilot code
sid_controller_validation_cfg
%% Run simulation
% Sim init script
sid_sim_init
% Run simulation
simOut = sim("arducopter");
%% Store results
% Create directory for storing the results for the controller validation
% if we are not currently in the result folder...
if isempty(regexp(curPath, 'results'))
% ...AND if the folder is not already existing on the current path
if ~exist('results', 'file')
mkdir results
cd results
else
cd results
end
end
if isempty(regexp(curPath, 'ctrlVal'))
if ~exist('ctrlVal', 'file')
mkdir ctrlVal
cd ctrlVal
else
cd ctrlVal
end
end
% Get number of existing subdirectories with today's date and create new
% subdirectory with the validation title and controller that is validated
date = datestr(now, 'yy_mm_dd');
numFolders = 0;
while true
if numFolders < 10
dir_name = [date '_0' num2str(numFolders) '_' valTitle '_' ctrlName];
if ~(isfolder(dir_name))
mkdir(dir_name);
break;
end
else
dir_name = [date '_' num2str(numFolders) '_' valTitle '_' ctrlName];
if ~(isfolder(dir_name))
mkdir(dir_name);
break;
end
end
numFolders = numFolders + 1;
end
% Open or create log
log_id = fopen('result_log.txt','a+');
fprintf(log_id, 'Validation %s\n', dir_name); % Print directory to safe figures
fprintf(log_id, 'Flight data path: %s\n', sidLogs(log_idx).data.filePathName); % Print flight data path
fprintf(log_id, 'Flight data file: %s\n', sidLogs(log_idx).data.fileName); % Print flight data file name
fprintf(log_id, 'Subflight: %d\n', loadedDataConfig.subflights(log_idx));
header_mark = '---------------------------------------------------------------\n';
fprintf(log_id, header_mark);
table_header = '%s\t\t\t|%s\n';
fprintf(log_id, table_header, 'Signal', 'Fit between measured and simulated signal (in %)');
fprintf(log_id, header_mark);
fclose(log_id);
% Calculate evaluation struct & save results
for i=1:length(val_out_sig_names)
% Get SimulationData object
out_sim = simOut.logsout.get(val_out_sig_sim{i});
out_meas = simOut.logsout.get(val_out_sig_meas{i});
out_name = val_out_sig_names{i};
in_test = simOut.logsout.get(val_in_sig{i});
% Create figure of results
figure('Name', out_name);
plot(out_sim.Values.time, out_sim.Values.Data);
hold on
plot(out_meas.Values.time, out_meas.Values.Data);
hold off
ylabel(out_name)
yyaxis right
plot(in_test.Values.time, in_test.Values.Data);
yAxR = gca;
ylabel(val_in_sig_names{i});
yAxR.YLim = [min(in_test.Values.Data)-min(in_test.Values.Data)*0.05, max(in_test.Values.Data)+max(in_test.Values.Data)*0.05];
% Figure settings
xlim([0 out_sim.Values.time(end)]); % Limit time
grid on % Activate grid of axes
title([out_name ': Comparison between simulated and measured controller outputs.'], 'Interpreter', 'none');
legend('Simulated Output', 'Measured Output', 'Input');
% Save figure
cd(dir_name)
savefig([out_name '.fig'])
cd ..
close all
% Calculate coefficient of determination to determine fit between
% measured and simulated signal
sigFit = 1 - sum((out_meas.Values.Data - out_sim.Values.Data).^2)/sum(out_meas.Values.Data.^2);
% Write log
log_line = '%s\t\t| %6.3f \n';
log_id = fopen('result_log.txt','a');
fprintf(log_id, log_line, out_name, sigFit*100);
fclose(log_id);
end
log_id = fopen('result_log.txt','a');
fprintf(log_id, '\n');
fclose(log_id);
% Return to main directory
cd(curPath);
clear val_in_sig val_in_sig_names val_out_sig_meas val_out_sig_sim val_out_sig_names
clear valTitle sigFit sig_name out_name sig_meas sig_test test_sig test_sig_names
clear table_header f_name num_folders file_struct header header_mark
clear log_id log_line num_folders sig_name_ap sig_sim sig_real signal_log underscore
clear date i dir_name out out_meas out_sim in_test yAxR ctrlName

27
Tools/simulink/arducopter/sid_controller_validation_cfg.m Normal file
View File

@ -0,0 +1,27 @@
% Configuration script for the control model validation
% Determines simulated and measured signals that are compared to validate
% the control model
%
% Fabian Bredemeier - IAV GmbH
% License: GPL v3
switch simMode
case 1 % Rate Controller
val_out_sig_names = {'RATE.ROut', 'RATE.POut', 'RATE.YOut'}; % Names of signals in the Ardupilot dataflash logs
val_out_sig_sim = {'rollRateCtrlOut', 'pitchRateCtrlOut', 'yawRateCtrlOut'}; % Names of the simulated signals
val_out_sig_meas = {'rollRateTotMeas', 'pitchRateTotMeas', 'yawRateTotMeas'}; % Names of the measured signals
val_in_sig_names = {'RATE.RDes', 'RATE.PDes', 'RATE.YDes'};
val_in_sig = {'<rollRateTar>', '<pitchRateTar>', '<yawRateTar>'};
case 2 % Attitude controller
val_out_sig_names = {'RATE.RDes','RATE.PDes', 'RATE.YDes'};
val_out_sig_sim = {'rollRateTar', 'pitchRateTar', 'yawRateTar'};
val_out_sig_meas = {'rollRateTarMeas','pitchRateTarMeas', 'yawRateTarMeas'};
val_in_sig_names = {'RCIN.C1', 'RCIN.C2', 'RCIN.C4'};
val_in_sig = {'rcinRoll', 'rcinPitch', 'rcinYaw'};
case 3 % z position controller
val_out_sig_names = {'CTUN.ThI', 'PSCD.TAD', 'PSCD.TVD'};
val_out_sig_sim = {'thrOut', 'zAccTar', 'zVelTar'};
val_out_sig_meas = {'thrInMeas', 'zAccTarMeas', 'zVelTarMeas'};
val_in_sig_names = {'PSCD.TAD', 'PSCD.TVD', 'PSCD.TPD'};
val_in_sig = {'zAccTar', 'zVelTar', 'zPosTar'};
end

536
Tools/simulink/arducopter/sid_pre.m Normal file
View File

@ -0,0 +1,536 @@
% Reads ArduPilot's system identification dataflash logs and extracts the SID
% subsections of the fligts into .mat file, discarding all irrelevant data
%
%
% Fabian Bredemeier - IAV GmbH
% License: GPL v3
close all;
%% File selection by user
% ---------------------------------------------- %
% %
% File selection %
% %
% ---------------------------------------------- %
% 1) Ask user if next data should be added to existing data set
if exist('sidLogs', 'var')
% Abort if config data for existing data is missing
if ~exist('loadedDataConfig', 'var')
error('No configuration of the existing data could be found. Aborting...');
end
addData = input('Add next data to existing data set (0: no, 1: yes)?');
if addData > 1 && ~isnumeric(addData)
error('Input not valid. Only 0 or 1 are allowed.')
end
else
addData = 0;
end
% Delete old data of new data will not be added. Otherwise get number of
% existing elements
if ~addData
clear sidLogs idData
numOldSubflights = 0;
numOldSysidflights = 0;
else
numOldSubflights = numel(sidLogs);
if exist('idData', 'var')
numOldSysidflights = numel(idData);
end
end
% 2) User chooses new flight data from explorer
mainPath = pwd;
flightDataSearchPath = [mainPath];
[files,flightDataPath] = uigetfile({'*.*'},...
'Select One or More Files', ...
'MultiSelect', 'on', ...
flightDataSearchPath);
% If only one file is selected, convert char array to cell array
if ~iscell(files)
files = cellstr(files);
numFiles = 1;
else
numFiles = size(files,2);
end
% 3) Get file extensions - .BIN oder .mat
fileExt = cell(numFiles,1);
for i=1:numFiles
fileExt{i} = files{i}(end-2:end);
end
% 4) Create cell array with file names
fileNames = cell(numFiles,1);
for i=1:numFiles
fileNames{i} = [flightDataPath files{i}];
end
% 5) Load .mat files - loadedDataConfig is struct, so load fileName field
chooseSubflights = false;
for i=1:numFiles
if strcmp(fileExt{i}, 'mat')
load(fileNames{i});
fileNames = loadedDataConfig.fileNames;
% Fill chooseSubflights variable with false for amount of
% subflights
for j=1:length(loadedDataConfig.subflights)
chooseSubflights(j) = false;
end
else
% Activate subflight choosing if BIN files were selected
chooseSubflights(i) = true;
end
end
%% Load complete log data and show tested modes
% ---------------------------------------------- %
% %
% Loading Dataflash Logs %
% %
% ---------------------------------------------- %
fprintf('\n-------------------------------------------\nLoading Flash Logs...\n')
fprintf('-------------------------------------------\n');
% Variables to store subflight numbers,amount of subflights and modes
subflights = 0;
fileIndices = 0;
numSubflights = 0;
modes = 0;
for fileIdx = 1:length(fileNames)
fprintf('Processing file %d ...\n', fileIdx);
% Throw error if files cannot be found
if ~isfile(fileNames{fileIdx})
error(['File ' + string(fileNames{fileIdx}) + ' could not be found.']);
end
logs(fileIdx) = Ardupilog(char(fileNames{fileIdx}));
% Plot and Output for sysid mode
if any(logs(fileIdx).MODE.Mode == 25)
fig = figure;
set(fig, 'defaultAxesColorOrder', [[0 0 0]; [0 0 0]]); % Set axes color to black
subplot(2, 1, 1);
plot(logs(fileIdx).MODE.TimeS, logs(fileIdx).MODE.ModeNum, 'k*');
ylabel(logs(fileIdx).getLabel('MODE/ModeNum'));
if ~issorted(logs(fileIdx).MODE.TimeS, 'strictascend')
disp([char(fileNames{fileIdx}) ' MODE.TimeS is not monotonic!'])
end
title(['Sysid Mode' fileNames{fileIdx}], 'Interpreter', 'none');
% Plot tested SID axes
hold on;
yyaxis right;
plot(logs(fileIdx).SIDS.TimeS, logs(fileIdx).SIDS.Ax, 'r*');
ylabel(logs(fileIdx).getLabel('SIDS/Ax'));
if ~issorted(logs(fileIdx).SIDS.TimeS, 'strictascend')
disp([char(fileNames{fileIdx}) ' SIDS.TimeS is not monotonic!'])
end
xlim([logs(fileIdx).MODE.TimeS(1) logs(fileIdx).PIDR.TimeS(end)]);
hold off;
% Plot frequency of SID tests
subplot(2, 1, 2);
plot(logs(fileIdx).SIDD.TimeS, logs(fileIdx).SIDD.F, 'b.');
ylabel(logs(fileIdx).getLabel('SIDD/F'));
if ~issorted(logs(fileIdx).SIDD.TimeS, 'strictascend')
disp([char(fileNames{fileIdx}) ' SIDD.TimeS is not monotonic!'])
end
hold on;
% Plot subflight numbers
yyaxis right;
TimeS = logs(fileIdx).MODE.TimeS;
subflightNr = 1:length(logs(fileIdx).MODE.Mode);
plot(TimeS, subflightNr, 'k*');
ylabel('subflight');
if ~issorted(logs(fileIdx).SIDS.TimeS, 'strictascend')
disp([char(fileNames{fileIdx}) ' SIDS.TimeS is not monotonic!'])
end
xlim([logs(fileIdx).MODE.TimeS(1) logs(fileIdx).PIDR.TimeS(end)]);
hold off;
end
% Plot for z position controller
if ~isempty(logs(fileIdx).PSCD.TimeS)
fig = figure;
set(fig, 'defaultAxesColorOrder', [[0 0 0]; [0 0 0]]); % Set axes color to black
% Plot mode numbers
subplot(2, 1, 1);
plot(logs(fileIdx).MODE.TimeS, logs(fileIdx).MODE.ModeNum, 'k*');
ylabel(logs(fileIdx).getLabel('MODE/ModeNum'));
if ~issorted(logs(fileIdx).MODE.TimeS, 'strictascend')
disp([char(fileNames{fileIdx}) ' MODE.TimeS is not monotonic!'])
end
xlim([logs(fileIdx).MODE.TimeS(1) logs(fileIdx).PIDR.TimeS(end)]);
ylim([min(logs(fileIdx).MODE.ModeNum)-1 max(logs(fileIdx).MODE.ModeNum)+1]);
title(['Other Modes' fileNames{fileIdx}], 'Interpreter', 'none');
% Plot measured z position
subplot(2,1,2);
plot(logs(fileIdx).PSCD.TimeS, -1*(logs(fileIdx).PSCD.PD), 'b.'); % -1 because of inverted logging of PSCD.PD
ylabel('z-Position (m)');
if ~issorted(logs(fileIdx).PSCD.TimeS, 'strictascend')
disp([char(fileNames{fileIdx}) ' PSCD.TimeS is not monotonic!'])
end
% Plot subflight numbers
hold on;
yyaxis right;
TimeS = logs(fileIdx).MODE.TimeS;
subflightNr = 1:length(logs(fileIdx).MODE.Mode);
plot(TimeS, subflightNr, 'k*');
ylabel('subflight');
xlim([logs(fileIdx).MODE.TimeS(1) logs(fileIdx).PIDR.TimeS(end)]);
ylim([0 subflightNr(end)+1]);
hold off;
end
% Plot for x-y position controller
if ~isempty(logs(fileIdx).PSCN.TimeS) && ~isempty(logs(fileIdx).PSCE.TimeS)
fig = figure;
set(fig, 'defaultAxesColorOrder', [[0 0 0]; [0 0 0]]); % Set axes color to black
% Plot mode numbers
subplot(2, 1, 1);
plot(logs(fileIdx).MODE.TimeS, logs(fileIdx).MODE.ModeNum, 'k*');
ylabel(logs(fileIdx).getLabel('MODE/ModeNum'));
if ~issorted(logs(fileIdx).MODE.TimeS, 'strictascend')
disp([char(fileNames{fileIdx}) ' MODE.TimeS is not monotonic!'])
end
xlim([logs(fileIdx).MODE.TimeS(1) logs(fileIdx).PIDR.TimeS(end)]);
ylim([min(logs(fileIdx).MODE.ModeNum)-1 max(logs(fileIdx).MODE.ModeNum)+1]);
title(['Other Modes' fileNames{fileIdx}], 'Interpreter', 'none');
% Plot measured x and y position
subplot(2,1,2);
plot(logs(fileIdx).PSCN.TimeS, logs(fileIdx).PSCN.PN, 'r.');
ylabel('Position (m)');
if ~issorted(logs(fileIdx).PSCN.TimeS, 'strictascend')
disp([char(fileNames{fileIdx}) ' PSCN.TimeS is not monotonic!'])
end
hold on;
plot(logs(fileIdx).PSCE.TimeS, logs(fileIdx).PSCE.PE', 'b.');
if ~issorted(logs(fileIdx).PSCE.TimeS, 'strictascend')
disp([char(fileNames{fileIdx}) ' PSCE.TimeS is not monotonic!'])
end
% Plot subflight numbers
yyaxis right;
TimeS = logs(fileIdx).MODE.TimeS;
subflightNr = 1:length(logs(fileIdx).MODE.Mode);
plot(TimeS, subflightNr, 'k*');
ylabel('subflight');
xlim([logs(fileIdx).MODE.TimeS(1) logs(fileIdx).PIDR.TimeS(end)]);
ylim([0 subflightNr(end)+1]);
hold off;
legend('x-Position', 'y-Position', 'Subflight') % PSCN is x, PSCE is y
end
% This aids the users configuring the subflights
disp('Tested Flight Modes:');
disp(' flight filename');
disp([' ' num2str(fileIdx) ' ' char(fileNames{fileIdx})]);
fprintf('Subflight\tMode\tSID_AXIS\n');
j = 1;
for i=1:length(logs(fileIdx).MODE.Mode)
if logs(fileIdx).MODE.Mode(i) == 25
% If mode is sysid, print tested axis
% Decrement sysid mode index j if the start time of
% sysid mode was not tracked (and does not equal the mode
% time stamps as a result). If it was not tracked, the
% settings are identical to the sysid mode before and were
% not changed.
if abs(logs(fileIdx).MODE.TimeS(i)-logs(fileIdx).SIDS.TimeS(j)) > 0.01 && j > 1
j = j-1;
end
fprintf('\t%d\t\t%d\t\t%d\n', i, logs(fileIdx).MODE.Mode(i), logs(fileIdx).SIDS.Ax(j));
% Increment sysid mode index j only if the start time was
% tracked (and equals the mode time stamps as a result).
% Otherwise, the tracked sysid information belongs to the
% next sysid flight and is equal to the current, so that
% the index should not be changed to derive the same infos.
if abs(logs(fileIdx).MODE.TimeS(i)-logs(fileIdx).SIDS.TimeS(j)) < 0.01
j = j+1;
end
else
% If not, just print mode
fprintf('\t%d\t\t%d\t\t-\n', i, logs(fileIdx).MODE.Mode(i));
end
end
% Let the user select the subflight if BIN files are selected
iSubflights = 1;
while(true)
if chooseSubflights(fileIdx)
nextSubflight = input('Insert number of subflight to be saved (skip with 0): ');
% Repeat loop if input is empty
if isempty(nextSubflight)
continue;
end
% Leave loop when nextSubflight is zero
if nextSubflight == 0
fprintf(['Subflight selection for flight ' num2str(fileIdx) ' done.\n']);
break;
end
% Give warnings when given number exceeds number of
% subflights...
if nextSubflight > length(logs(fileIdx).MODE.Mode)
warning('Given number exceeds number of subflights!');
continue
end
% ...or when subflight was selected twice for the same flight
if any(nextSubflight == subflights(numSubflights+1:end))
warning('Subflight already selected!');
continue
end
else
% Break if number of added subflights equals the total number
% of loaded subflights for that file (represented by appearence
% of the respective file index in fileIndices)
if numel(subflights)-numSubflights >= sum(loadedDataConfig.fileIndices == fileIdx) && subflights ~= 0
break;
end
% mat file was chosen - read subflight from config
nextSubflight = loadedDataConfig.subflights(numSubflights+iSubflights);
end
% Add subflight, file index and mode
subflights(numSubflights+iSubflights) = nextSubflight;
fileIndices(numSubflights+iSubflights) = fileIdx;
modes(numSubflights+iSubflights) = logs(fileIdx).MODE.Mode(nextSubflight);
modesConfig(numSubflights+iSubflights) = ModeConfig(modes(end));
iSubflights = iSubflights + 1; % Update subflight index
% Break loop if number of selected subflights equals number of
% modes
if length(logs(fileIdx).MODE.Mode) < iSubflights
break;
end
end
% Update amount of subflights only if 0 was not chosen as subflight
if ~any(subflights == 0)
numSubflights = length(subflights);
end
end
clear file ax TimeS subflightNr fig nextSubflight iSubflights numFiles fileIdx fileExt
clear flightDataSearchPath
%% Slice logs to desired subflights
% Time increment that is used to extend the slicing time interval to
% prevent signals (that are only defined at the first time step) to be
% excluded from the sliced log because of floating point precision
dt_ext = 0.001;
% Create list for axis that were tested (axis=0 if sysid was not used)
axisList = 0;
% ---------------------------------------------- %
% %
% Extract relevant log messages and slice logs %
% %
% ---------------------------------------------- %
% slice the system identification flight subsection(s)
fprintf('\n-------------------------------------------\n')
fprintf('Extracting messages and slicing logs...\n');
fprintf('-------------------------------------------\n');
% Delete sid in case of reloading for new config
clear sid
for i = 1:length(fileIndices)
fprintf(['Subflight Nr. ' num2str(i) '\n']);
% Check if current subflight number doesn't exceed actual subflights
if (subflights(i) > length(logs(fileIndices(i)).MODE.Mode) )
error(['Defined subflight (index ' num2str(i) ') exceeds amount of actual subflights!']);
end
% Filter log messages
logFilterMsgs = {};
logFilterMsgs = modesConfig(i).filter_msgs;
% Check if log_filter_msgs is not empty
if ~iscellstr(logFilterMsgs)
error('logFilterMsgs variable is not a string cell array');
end
% Check if message is included in flight log
for msg = logFilterMsgs
% Skip message if it was not defined - delete it from array
if ~isprop(logs(fileIndices(i)), msg{1})
warning(['Message ' msg{1} ' not included in subflight ' ...
num2str(subflights(i)) ' of logfile ' ...
fileNames{fileIndices(i)} '!'])
elseif isempty(logs(fileIndices(i)).(msg{1}).LineNo)
warning(['Message ' msg{1} ' not included in subflight ' ...
num2str(subflights(i)) ' of logfile ' ...
fileNames{fileIndices(i)} '!']);
logFilterMsgs(strcmp(logFilterMsgs, msg{1})) = [];
end
end
log = logs(fileIndices(i)).filterMsgs(logFilterMsgs);
% Slice log to desired time section
if subflights(i) < length(logs(fileIndices(i)).MODE.Mode)
% Current subflight is not last subflight of log
start_time = logs(fileIndices(i)).MODE.TimeUS(subflights(i))/1e6;
% end time: Subtract 2*dt_ext to prevent first time step of
% next flight mode to be part of sliced log
end_time = logs(fileIndices(i)).MODE.TimeUS(subflights(i)+1)/1e6-2*dt_ext;
else % use end time of TimeUS message in case of the last subflight
start_time = logs(fileIndices(i)).MODE.TimeUS(subflights(i))/1e6;
end_time = logs(fileIndices(i)).RATE.TimeUS(end)/1e6;
end
log = log.getSlice( [start_time-dt_ext end_time+dt_ext], 'TimeS');
% Check if flight mode of log corresponds to defined mode in config
if modes(i) ~= log.MODE.Mode
error_msg = ['Defined Mode (' num2str(modes(i)) ') in ' ...
'mode_list with index ' num2str(i) ' does not correspond '...
'to mode of log (' num2str(log.MODE.Mode) ')!'];
error(error_msg);
end
% preserve some messages that are outside of the slice time interval
log.FMT = logs(fileIndices(i)).FMT;
log.UNIT = logs(fileIndices(i)).UNIT;
log.FMTU = logs(fileIndices(i)).FMTU;
log.MULT = logs(fileIndices(i)).MULT;
log.PARM = logs(fileIndices(i)).PARM;
% Update the number of actual included messages
log.numMsgs = 0;
for msg = log.msgsContained
msgName = char(msg);
% Check if message has been sliced off log
if ~isvalid(log.(msgName))
warning(['Message ' msg{1} ' in log ' ...
num2str(i) ' was sliced off message!']);
else
log.numMsgs = log.numMsgs + length(log.(msgName).LineNo);
end
end
% Get axis from flight. Set to 0 if sysid mode is not active.
axis = get_axis(log);
axis = axis(1);
% Add new axis.
axisList(i) = axis;
% Resample log and save it to sid array
sidLogs(numOldSubflights + i).data = resampleLog(log, true, 'linear');
end
% Save identification data in IdData objects
% Index to save IO data of SID runs
j = 1;
for i=1:length(sidLogs)-numOldSubflights
if sidLogs(numOldSubflights + i).data.MODE.Mode == 25
% Let user insert minimum and maximum frequencies for spectral
% analysis
disp('Insert minimum and maximum frequency for spectral analysis (enter no value to choose sweep values):')
fminSweep = sidLogs(numOldSubflights + i).data.SIDS.FSt;
fmaxSweep = sidLogs(numOldSubflights + i).data.SIDS.FSp;
fmin = input(['Minimum frequency (sweep = ' num2str(fminSweep) '): fmin = ']);
fmax = input(['Maximum frequency (sweep = ' num2str(fmaxSweep) '): fmax = ']);
if isempty(fmin) fmin = fminSweep; end
if isempty(fmax) fmax = fmaxSweep; end
idData(numOldSysidflights + j) = IdData(sidLogs(numOldSubflights + i).data, subflights(i), i, fmin, fmax);
% Increment IO data index for later saving
j = j+1;
end
end
% Print modes and/or tested sid axis in log array
fprintf('\nChosen test flights:\n')
fprintf('Dataset\t|File\t|Subflight\t|Mode\t|Axis\t|Id Quality\n')
for i=1:length(sidLogs)-numOldSubflights
if (isfield(sidLogs(numOldSubflights + i).data, 'SIDS'))
% Find correct idData object for data quality
idDataObj = findobj(idData, '-function', ...
@(obj) strcmp(obj.fileName, fileNames{fileIndices(i)}) && obj.dataIndex == i);
% When identical objects are contained in idData vector, use latest
idDataObj = idDataObj(end);
fprintf('sid(%d)\t|%d\t\t|%d\t\t\t|%d\t\t|%d\t\t|%.2f\t\n', int16(i+numOldSubflights), ...
fileIndices(i), subflights(i), sidLogs(numOldSubflights + i).data.MODE.Mode(1), ...
sidLogs(numOldSubflights + i).data.SIDS.Ax, idDataObj.dataQuality);
else
fprintf('sid(%d)\t|%d\t\t|%d\t\t\t|%d\t\t|-\t\t|-\n', int16(i+numOldSubflights), ...
fileIndices(i), subflights(i), sidLogs(numOldSubflights + i).data.MODE.Mode(1));
end
end
% Save configuration of loaded data to struct and to repo
if numOldSubflights == 0
% No configuration in workspace yet
loadedDataConfig.fileNames = fileNames;
loadedDataConfig.fileIndices = fileIndices;
loadedDataConfig.subflights = subflights;
loadedDataConfig.modes = modes;
loadedDataConfig.modesConfig = modesConfig;
loadedDataConfig.axisList = axisList;
else
% Add new configuration to existing configuration array
for i=1:length(fileNames)
loadedDataConfig.fileNames{end+1} = fileNames{i};
end
for i=1:length(fileIndices)
% File indices must be adjusted to index of file in existing config
loadedDataConfig.fileIndices(numOldSubflights+i) = length(loadedDataConfig.fileNames);
loadedDataConfig.subflights(numOldSubflights+i) = subflights(i);
loadedDataConfig.modes(numOldSubflights+i) = modes(i);
loadedDataConfig.modesConfig(numOldSubflights+i) = modesConfig(i);
loadedDataConfig.axisList(numOldSubflights+i) = axisList(i);
end
end
loadedDataConfigFile = [flightDataPath 'dataLoadConfig.mat'];
save(loadedDataConfigFile, 'loadedDataConfig');
% Save new flightdata to repo
newDataFile = 'sid.mat';
newDataFile = [flightDataPath newDataFile];
if exist(newDataFile, 'file')
[filename, path] = uiputfile({'*.mat','Mat file (*.mat)';'*.*','All files (*.*)'}, 'Save File Name', ...
newDataFile);
% Set user-specified file name
newDataFile = [flightDataPath filename];
end
if newDataFile ~= 0
if exist('idData','var')
save(newDataFile, 'sidLogs', 'idData', 'loadedDataConfig');
else
save(newDataFile, 'sidLogs', 'loadedDataConfig');
end
end
clear msg msgName filename path in_dat out_dat len idd delta_T k
clear start_time end_time sid idDataObj minNumElem
clear newDataFile logPathName numOldSubflights numOldSysidflights
clear fmin fminSweep fmax fmaxSweep addData logs
clear i j dt_ext log_filenames axis axisList files chooseSubflights
clear numSubflights storedFile logFilterMsgs fileNames fileIndices subflights
clear dataLoadConfigFile dataStored loadedDataConfigFile modes modesConfig
% Interact with user to continue
uiwait(msgbox(['Data loaded. Press ok to close figures and continue.'],'Data loaded','none','nonmodal'));
close all
%% Functions
% Function to read tested axis from log
function axis = get_axis(obj)
if isprop(obj, 'SIDS')
axis = obj.SIDS.Ax;
else
% Set to zero if sysid mode is not active
axis = 0;
end
end

915
Tools/simulink/arducopter/sid_sim_init.m Normal file
View File

@ -0,0 +1,915 @@
% Initialization of the Simulink model. The script loads all important
% signals and parameter to the workspace.
%
% Fabian Bredemeier - IAV GmbH
% License: GPL v3
%% Define log index and start + stop time
if ~exist('log_idx', 'var')
log_idx = 2;
end
tStart = 0;
tEnd = inf;
%% Setting simMode
% 0 = Model validation - Identified models are used and full control loop
% is active
% 1 = Rate Controller validation - No models used, measured input signals
% of rate controller are used
% 2 = Attitude Controller validation - No models used, measured input
% signals of attitude controller are used
% 3 = Altitude Controller validation - No models used, measured input
% signals of attitude controller are used
% 4 = Optimization of controller - Not implemented yet
if ~exist('simMode', 'var')
simMode = 0;
end
%% Check for existance of data
% Check if sid data is existing
if exist('sidLogs', 'var') % in Workspace
disp('.mat file already loaded. Reading parameter and signals...');
elseif exist('sid.mat', 'file') % in a .mat file with all other axis tests
load('sid.mat');
disp('Loaded data from sid.mat');
elseif ~exist(loadedDataConfig)
error('Could not find configuration struct. Aborting script.')
else
error('Could not find data. Aborting script.')
end
%% Config and variable declaration
% Sample rate of simInulation
Fs = 400;
dt = 0.0025;
simIn.param.dt = 0.0025;
% Load data
sid = sidLogs(log_idx).data;
% Check if relevant messages are defined
if (numel(sid) == 0)
error('Ardupilog object sid is empty. Aborting.');
end
% Get mode of current test run
mode = loadedDataConfig.modes(log_idx);
% Get manual throttle flag for current test run
thr_man = loadedDataConfig.modesConfig(log_idx).thr_man;
% Get filter messages of current test run
msgs = loadedDataConfig.modesConfig(log_idx).filter_msgs;
% Set axis
axis = loadedDataConfig.axisList(log_idx);
% Check if config parameters are complete
if (isempty(mode) || isempty(thr_man) || isempty(msgs))
error('Config variables (mode, thr_man, msgs) not complete. Aborting.');
end
% Define validation and optimization flag of simInulation
simIn.param.mdlVal = simMode == 0;
simIn.param.rateCtrlVal = simMode == 1;
simIn.param.attCtrlVal = simMode == 2 || simMode == 3;
simIn.param.altCtrlVal = simMode == 3 || mode == 2;
simIn.param.optCtrl = simMode == 4;
%% General Copter settings
simIn.param.LOOP_RATE = getParamVal(sid, 'SCHED_LOOP_RATE');
simIn.param.dt = round(double(1 / simIn.param.LOOP_RATE), 4); % Sample time resulting from loop rate. Round to four decimal digits
simIn.param.FRAME_CLASS = getParamVal(sid, 'FRAME_CLASS');
simIn.param.FRAME_TYPE = getParamVal(sid, 'FRAME_TYPE');
[simIn.param.NUM_MOTORS, simIn.param.AXIS_FAC_MOTORS] = getMotorMixerFactors(simIn.param.FRAME_CLASS, simIn.param.FRAME_TYPE);
simIn.param.EKF_TYPE = getParamVal(sid, 'AHRS_EKF_TYPE');
simIn.param.GRAVITY_MSS = single(9.80665);
simIn.param.dt_THR_LOOP = 1 / 50; % Sample time of throttle loop in Copter.cpp. Defined in Copter.cpp, line 94
simIn.param.ANGLE_MAX = getParamVal(sid, 'ANGLE_MAX'); % Maximum lean angle in all flight modes
simIn.param.PILOT_Y_EXPO = getParamVal(sid, 'PILOT_Y_EXPO'); % Acro yaw expo to allow faster rotation when stick at edges
simIn.param.PILOT_Y_RATE = getParamVal(sid, 'PILOT_Y_RATE');
simIn.param.PILOT_Y_RATE_TC = getParamVal(sid, 'PILOT_Y_RATE_TC'); % Pilot yaw rate control input time constant
simIn.param.MODE.ModeNr = mode; % Flight mode
% Abort script if frame configuration is not defined yet
if (isempty(simIn.param.AXIS_FAC_MOTORS) || simIn.param.NUM_MOTORS == 0)
error("Body configuration of copter is not defined yet!");
end
%% Simulation duration
% Define end time of simInulation
if simIn.param.mdlVal || simIn.param.rateCtrlVal || simIn.param.attCtrlVal || simIn.param.altCtrlVal
if isinf(tEnd) % Run full data
simIn.param.timeEnd = sid.RATE.TimeS(end);
else % Only run part of data
duration = tEnd-tStart;
iEnd = int16(duration/dt);
simIn.param.timeEnd = sid.RATE.TimeS(iEnd);
clear duration iEnd
end
elseif simIn.param.optCtrl
simIn.param.timeEnd = OPT.tEndOpt;
end
%% Inertial Sensor
simIn.param.INS.GYRO_FILTER = getParamVal(sid, 'INS_GYRO_FILTER'); % Filter cutoff frequency for gyro lowpass filter
simIn.param.INS.GYRO_RATE = 400; % Gyro sampling rate. Actually define by INS_GYRO_RATE, but simInulation is run with 400 Hz
%% AHRS
trim_x = getParamVal(sid, 'AHRS_TRIM_X');
trim_y = getParamVal(sid, 'AHRS_TRIM_Y');
simIn.param.AHRS.FC2BF = single(dcmFromEuler(trim_x, trim_y, 0)); % Rotation matrix from FC to Body Frame based on AP_AHRS constructor
simIn.param.AHRS.BF2FC = simIn.param.AHRS.FC2BF';
clear trim_x trim_y
%% RCIN - Radio Input and parameters for Attitude Control
rollCh = getParamVal(sid, 'RCMAP_ROLL');
pitchCh = getParamVal(sid, 'RCMAP_PITCH');
yawCh = getParamVal(sid, 'RCMAP_YAW');
thrCh = getParamVal(sid, 'RCMAP_THROTTLE');
simIn.param.RCIN.DZ_RLL = getParamVal(sid, ['RC' num2str(rollCh) '_DZ']);
simIn.param.RCIN.MIN_RLL = getParamVal(sid, ['RC' num2str(rollCh) '_MIN']);
simIn.param.RCIN.MAX_RLL = getParamVal(sid, ['RC' num2str(rollCh) '_MAX']);
simIn.param.RCIN.TRIM_RLL = getParamVal(sid, ['RC' num2str(rollCh) '_TRIM']);
simIn.param.RCIN.REVERSED_RLL = getParamVal(sid, ['RC' num2str(rollCh) '_REVERSED']);
simIn.param.RCIN.DZ_PIT = getParamVal(sid, ['RC' num2str(pitchCh) '_DZ']);
simIn.param.RCIN.MIN_PIT = getParamVal(sid, ['RC' num2str(pitchCh) '_MIN']);
simIn.param.RCIN.MAX_PIT = getParamVal(sid, ['RC' num2str(pitchCh) '_MAX']);
simIn.param.RCIN.TRIM_PIT = getParamVal(sid, ['RC' num2str(pitchCh) '_TRIM']);
simIn.param.RCIN.REVERSED_PIT = getParamVal(sid, ['RC' num2str(pitchCh) '_REVERSED']);
simIn.param.RCIN.DZ_YAW = getParamVal(sid, ['RC' num2str(yawCh) '_DZ']);
simIn.param.RCIN.MIN_YAW = getParamVal(sid, ['RC' num2str(yawCh) '_MIN']);
simIn.param.RCIN.MAX_YAW = getParamVal(sid, ['RC' num2str(yawCh) '_MAX']);
simIn.param.RCIN.TRIM_YAW = getParamVal(sid, ['RC' num2str(yawCh) '_TRIM']);
simIn.param.RCIN.REVERSED_YAW = getParamVal(sid, ['RC' num2str(yawCh) '_REVERSED']);
simIn.param.RCIN.DZ_THR = getParamVal(sid, ['RC' num2str(thrCh) '_DZ']);
simIn.param.RCIN.MIN_THR = getParamVal(sid, ['RC' num2str(thrCh) '_MIN']);
simIn.param.RCIN.MAX_THR = getParamVal(sid, ['RC' num2str(thrCh) '_MAX']);
simIn.param.RCIN.TRIM_THR = getParamVal(sid, ['RC' num2str(thrCh) '_TRIM']);
simIn.param.RCIN.REVERSED_THR = getParamVal(sid, ['RC' num2str(thrCh) '_REVERSED']);
simIn.param.RCIN.ROLL_PITCH_YAW_INPUT_MAX = 4500; % Yaw, roll, pitch input range, defined in config.h
% Check if RCIN log is available
if simIn.param.mdlVal || simIn.param.attCtrlVal || simIn.param.altCtrlVal
if isfield(sid, 'RCIN')
iVec = (sid.RCIN.TimeS >= tStart & sid.RCIN.TimeS <= tEnd);
simIn.signals.RCIN.Time = single(sid.RCIN.TimeS(iVec)-tStart);
simIn.signals.RCIN.RollInput = eval(['single(sid.RCIN.C' num2str(rollCh) '(iVec))']);
simIn.signals.RCIN.PitchInput = eval(['single(sid.RCIN.C' num2str(pitchCh) '(iVec))']);
simIn.signals.RCIN.YawInput = eval(['single(sid.RCIN.C' num2str(yawCh) '(iVec))']);
simIn.signals.RCIN.ThrottleInput = eval(['single(sid.RCIN.C' num2str(thrCh) '(iVec))']);
else
if ~isfield(sid, 'RCIN') && axis == 13
warning(['RCIN message cannot be found in log ' num2str(log_idx) '! Validating vertical motion model.']);
else
warning(['RCIN message cannot be found in log ' num2str(log_idx) '! Ommiting attitude controller and models in simInulation. Only rate controller is validated']);
simIn.param.rateCtrlVal = 1;
simIn.param.mdlVal = 0;
end
% Set RCIN signals to zero
simIn.signals.RCIN.Time = single(0:0.1:simIn.param.timeEnd);
simIn.signals.RCIN.RollInput = single(zeros(length(simIn.signals.RCIN.Time),1));
simIn.signals.RCIN.PitchInput = single(zeros(length(simIn.signals.RCIN.Time),1));
simIn.signals.RCIN.YawInput = single(zeros(length(simIn.signals.RCIN.Time),1));
simIn.signals.RCIN.ThrottleInput = single(zeros(length(simIn.signals.RCIN.Time),1));
end
end
%% Attitude
% Yaw Angle signals are a modulo of 360°, so this operation has to be
% inverted
iVec = (sid.ATT.TimeS >= tStart & sid.ATT.TimeS <= tEnd);
simIn.signals.ATT.Time = single(sid.ATT.TimeS(iVec)-tStart);
simIn.signals.ATT.Roll = single(sid.ATT.Roll(iVec));
simIn.signals.ATT.Pitch = single(sid.ATT.Pitch(iVec));
simIn.signals.ATT.Yaw = single(demodYawAngle(sid.ATT.Yaw(iVec)));
simIn.signals.ATT.DesRoll = single(sid.ATT.DesRoll(iVec));
simIn.signals.ATT.DesPitch = single(sid.ATT.DesPitch(iVec));
simIn.signals.ATT.DesYaw = single(demodYawAngle(sid.ATT.DesYaw(iVec)));
% The actual yaw angle is calculated from the DCM with
% Matrix3<T>::to_euler() with the four-quadrant arcus tangens atan2(), for
% example in AP_AHRS_View::update(), so the controller can only handle
% values in the range [-pi, pi].
simIn.signals.ATT.Yaw = simIn.signals.ATT.Yaw - 360 * (simIn.signals.ATT.Yaw > 180);
simIn.signals.ATT.DesYaw = simIn.signals.ATT.DesYaw - 360 * (simIn.signals.ATT.DesYaw > 180);
% Initial DCM matrix based on initial Euler angles
% Calculation based on Matrix3<T>::from_euler()
for i=1:length(iVec)
if iVec(i) == 1
iInit = i;
break;
end
end
rollAngInit = single(sid.ATT.Roll(iInit))*pi/180;
pitchAngInit = single(sid.ATT.Pitch(iInit))*pi/180;
yawAngInit = single(simIn.signals.ATT.Yaw(1))*pi/180;
simIn.init.ATT.Roll = rollAngInit;
simIn.init.ATT.Pitch = pitchAngInit;
simIn.init.ATT.Yaw = yawAngInit;
simIn.init.ATT.DCM = dcmFromEuler(rollAngInit, pitchAngInit, yawAngInit);
% Initialize attitudeTarget according to
% AC_AttitudeControl::reset_yaw_target_and_rate(), which is called in
% ModeStabilize::run() when copter is landed
attitudeTargetUpdate = fromAxisAngle([0;0;yawAngInit]);
simIn.init.ATT.attitudeTarget = quatMult(attitudeTargetUpdate, [1;0;0;0]);
simIn.init.ATT.eulerRateTar = [0;0;0];
% Test signals in SITL
% Attitude Controller Targets
% simIn.signals.ATT.attTarMeasW = single(sid.ATAR.attW);
% simIn.signals.ATT.attTarMeasX = single(sid.ATAR.attX);
% simIn.signals.ATT.attTarMeasY = single(sid.ATAR.attY);
% simIn.signals.ATT.attTarMeasZ = single(sid.ATAR.attZ);
% simIn.signals.ATT.eulerAngTarMeasX = single(sid.ATAR.angX);
% simIn.signals.ATT.eulerAngTarMeasY = single(sid.ATAR.angY);
% simIn.signals.ATT.eulerAngTarMeasZ = single(sid.ATAR.angZ);
% simIn.signals.ATT.eulerRatTarMeasX = single(sid.ATAR.ratX);
% simIn.signals.ATT.eulerRatTarMeasY = single(sid.ATAR.ratY);
% simIn.signals.ATT.eulerRatTarMeasZ = single(sid.ATAR.ratZ);
%
% simIn.signals.ATT.CtrlRollIn = single(sid.CTIN.rllCtrl);
% simIn.signals.ATT.CtrlPitchIn = single(sid.CTIN.pitCtrl);
% simIn.signals.ATT.CtrlYawIn = single(sid.CTIN.yawCtrl);
% simIn.signals.ATT.yawRateDesMeas = single(sid.ATIN.yawDes);
clear rollAngInit pitchAngInit yawAngInit attitudeTargetUpdate iInit
%% Attitude Controller General
simIn.param.ATC.RATE_FF_ENAB = getParamVal(sid, 'ATC_RATE_FF_ENAB');
simIn.param.ATC.INPUT_TC = getParamVal(sid, 'ATC_INPUT_TC'); % Attitude control input time constant
simIn.param.ATC.ANGLE_LIMIT_MIN = single(10); % Min lean angle so that vehicle can maintain limited control, defined in AC_AttitudeControl.cpp as AC_ATTITUDE_CONTROL_ANGLE_LIMIT_MIN
simIn.param.ATC.ANGLE_LIMIT_THROTTLE_MAX = single(0.8); % Max throttle used to limit lean angle so that vehicle does not lose altitude, defined in AC_AttitudeControl.h as AC_ATTITUDE_CONTROL_ANGLE_LIMIT_THROTTLE_MAX
simIn.param.ATC.ANG_LIM_TC = getParamVal(sid, 'ATC_ANG_LIM_TC'); % Angle Limit (to maintain altitude) Time Constant
% Roll Angle Controller
simIn.param.ATC.ANG_RLL_P = getParamVal(sid, 'ATC_ANG_RLL_P');
simIn.param.ATC.ACCEL_R_MAX = getParamVal(sid, 'ATC_ACCEL_R_MAX');
simIn.param.ATC.RATE_R_MAX = getParamVal(sid, 'ATC_RATE_R_MAX');
simIn.param.ATC.ACCEL_RP_CONTROLLER_MIN_RADSS = 40*pi/180; % Maximum body-frame acceleration limit for the stability controller, defined in AC_AttitudeControl.h
simIn.param.ATC.ACCEL_RP_CONTROLLER_MAX_RADSS = 720*pi/180;
% Pitch Angle Controller
simIn.param.ATC.ANG_PIT_P = getParamVal(sid, 'ATC_ANG_PIT_P');
simIn.param.ATC.ACCEL_P_MAX = getParamVal(sid, 'ATC_ACCEL_P_MAX');
simIn.param.ATC.RATE_P_MAX = getParamVal(sid, 'ATC_RATE_P_MAX');
% Yaw Angle Controller
simIn.param.ATC.ANG_YAW_P = getParamVal(sid, 'ATC_ANG_YAW_P');
simIn.param.ATC.ACCEL_Y_MAX = getParamVal(sid, 'ATC_ACCEL_Y_MAX');
simIn.param.ATC.RATE_Y_MAX = getParamVal(sid, 'ATC_RATE_Y_MAX');
simIn.param.ATC.ACCEL_Y_CONTROLLER_MAX_RADSS = single(120*pi/180); % Maximum body-frame acceleration limit for the stability controller, defined in AC_AttitudeControl.h
simIn.param.ATC.ACCEL_Y_CONTROLLER_MIN_RADSS = single(10*pi/180);
simIn.param.ATC.THRUST_ERROR_ANGLE = single(30*pi/180); % Thrust angle error above which yaw corrections are limited. Defined in AC_AttitudeControl.h
% Thrust and throttle calculation parameters
simIn.param.MODE.THR_MAN_FLG = thr_man; % Flag for manual throttle, which depends on the current mode.
if thr_man
simIn.param.ATC.THR_FILT_CUTOFF = getParamVal(sid, 'PILOT_THR_FILT'); % Cutoff frequency of throttle filter for modes with manual throttle. parameter is given to function call of AC_AttitudeControl_Multi::set_throttle_out().
else
simIn.param.ATC.THR_FILT_CUTOFF = 2.0; % Cutoff frequency of throttle filter for modes with z position control (Defined in AC_PosControl.h, line 33)
end
simIn.param.ATC.ANGLE_BOOST = getParamVal(sid, 'ATC_ANGLE_BOOST'); % Enabling of angle boost to increase output throttle. Used in AC_AttitudeConrtol_Multi::get_throttle_boosted()
simIn.param.ATC.THR_MIX_MAN = getParamVal(sid, 'ATC_THR_MIX_MAN'); % Throttle vs. attitude priorisation during manual flight
simIn.param.ATC.THR_MIX_MIN = getParamVal(sid, 'ATC_THR_MIX_MIN'); % Throttle vs. attitude priorisation used when landing
simIn.param.ATC.THR_MIX_MAX = getParamVal(sid, 'ATC_THR_MIX_MAX'); % Throttle vs. attitude priorisation during active flights
simIn.param.ATC.THR_MIX_DFLT = single(0.5); % Default value for the priorization of throttle use between attitude control and throttle demand by pilot (or autopilot). Defined in AC_AttitudeControl.h, line 44
if thr_man
simIn.init.ATC.THR_MIX = simIn.param.ATC.THR_MIX_MAN;
else
simIn.init.ATC.THR_MIX = simIn.param.ATC.THR_MIX_DFLT;
end
simIn.param.MOT.HOVER_LEARN = getParamVal(sid, 'MOT_HOVER_LEARN'); % Enable/Disable automatic learning of hover throttle (0=Disabled, 1=Learn, 2=Learn and Save
simIn.param.MOT.THST_HOVER = getParamVal(sid, 'MOT_THST_HOVER'); % Motor thrust needed to hover. Default value of _throttle_hover.
simIn.param.MOT.THST_HOVER = simIn.param.MOT.THST_HOVER(1); % Assign first value of array to default value which is the correct value in the case, that the parameter is defined twice in the param file.
simIn.param.MOT.THST_HOVER_TC = single(10.0); % Time constant used to update estimated hover throttle, found as AP_MOTORS_THST_HOVER_TC in AP_MotorsMulticopter.h
simIn.param.MOT.THST_HOVER_MIN = single(0.125); % Minimum possible hover throttle, found as AP_MOTORS_THST_HOVER_MIN in AP_MotorsMulticopter.h
simIn.param.MOT.THST_HOVER_MAX = single(0.6875); % Maximum possible hover throttle, found as AP_MOTORS_THST_HOVER_MAX in AP_MotorsMulticopter.h
simIn.param.MOT.BAT_VOLT_MAX = getParamVal(sid, 'MOT_BAT_VOLT_MAX'); % Maximum voltage above which no additional scaling on thrust is performed
simIn.param.MOT.BAT_VOLT_MIN = getParamVal(sid, 'MOT_BAT_VOLT_MIN'); % Minimum voltage below which no additional scaling on thrust is performed
simIn.param.MOT.BAT_CURR_MAX = getParamVal(sid, 'MOT_BAT_CURR_MAX'); % Maximum current over which maximum throttle is limited (and no further scaling is performed)
simIn.param.MOT.THST_EXPO = getParamVal(sid, 'MOT_THST_EXPO'); % Motor thrust curve exponent (0.0 for linear to 1.0 for second order curve)
simIn.param.MOT.BAT_CURR_TC = getParamVal(sid, 'MOT_BAT_CURR_TC'); % Time constant used to limit the maximum current
simIn.param.MOT.YAW_HEADROOM = getParamVal(sid, 'MOT_YAW_HEADROOM'); % Yaw control is goven at least this PWM in microseconds range
% Throttle inputs
iVec = (sid.CTUN.TimeS >= tStart & sid.CTUN.TimeS <= tEnd);
simIn.signals.CTUN.Time = single(sid.CTUN.TimeS(iVec)--tStart);
simIn.signals.CTUN.ThrIn = single(sid.CTUN.ThI(iVec)); % Throttle In, pilots input to Attitude Control (attitude_control->get_throttle_in())
simIn.signals.CTUN.ThrOut = single(sid.CTUN.ThO(iVec)); % Throttle Out, throttle given to motor mixer after filtering (motors->get_throttle())
simIn.signals.CTUN.AngBst = single(sid.CTUN.ABst(iVec)); % Extra amount of throttle, added to throttle_in in AC_AttitudeControl_Multi::get_throttle_boosted()
simIn.signals.CTUN.ThrHov = single(sid.CTUN.ThH(iVec)); % Estimated throttle required to hover throttle in range 0-1 (AP_MotorsMulticopter::get_throttle_hover())
% Throttle inital values
if simIn.param.rateCtrlVal || simIn.param.attCtrlVal || simIn.param.mdlVal || simIn.param.altCtrlVal
simIn.init.CTUN.ThrOut = single(sid.CTUN.ThO(1));
simIn.init.CTUN.ThrHov = single(sid.CTUN.ThH(1));
else
simIn.init.CTUN.ThrOut = single(0);
simIn.init.CTUN.ThrHov = single(simIn.param.MOT.THST_HOVER);
end
% Altitude signals
simIn.signals.CTUN.DSAlt = single(sid.CTUN.DSAlt(iVec)); % Desired rangefinder altitude
simIn.signals.CTUN.DAlt = single(sid.CTUN.DAlt(iVec)); % Desired altitude
simIn.signals.CTUN.Alt = single(sid.CTUN.Alt(iVec)); % Achieved altitude (inav)
simIn.signals.CTUN.SAlt = single(sid.CTUN.SAlt(iVec)); % Achieved rangefinder altitude
simIn.signals.CTUN.TAlt = single(sid.CTUN.TAlt(iVec)); % Terrain altitude
simIn.signals.CTUN.BAlt = single(sid.CTUN.BAlt(iVec)); % Barometric altitude
simIn.signals.CTUN.CRt = single(sid.CTUN.CRt(iVec)); % Climb rate inav
simIn.signals.CTUN.DCRt = single(sid.CTUN.DCRt(iVec)); % Desired climb rate
% Altitude inital values
simIn.init.CTUN.Alt = single(sid.CTUN.Alt(1));
simIn.init.CTUN.CRt = single(sid.CTUN.CRt(1)*0.01);
% Intermediate throttle calculation quantities - only read if message
% exists
if isfield(sid, 'MOTQ')
iVec = (sid.MOTQ.TimeS >= tStart & sid.MOTQ.TimeS <= tEnd);
simIn.signals.MOTQ.Time = single(sid.MOTQ.TimeS(iVec)-tStart);
simIn.signals.MOTQ.ThrAvgMax = single(sid.MOTQ.ThAvgMax(iVec));
simIn.signals.MOTQ.ThrThstMaxMeas = single(sid.MOTQ.ThThstMax(iVec));
simIn.signals.MOTQ.CompGain = single(sid.MOTQ.CompGain(iVec));
simIn.signals.MOTQ.AirDensityRatio = single(sid.MOTQ.AirDensRat(iVec));
simIn.signals.MOTQ.ThrMixOut = single(sid.MOTQ.ThrOut(iVec));
else
iVec = (sid.CTUN.TimeS >= tStart & sid.CTUN.TimeS <= tEnd);
simIn.signals.MOTQ.Time = single(sid.CTUN.TimeS(iVec)-tStart);
simIn.signals.MOTQ.ThrAvgMax = single(zeros(length(simIn.signals.MOTQ.Time),1));
simIn.signals.MOTQ.ThrThstMaxMeas = single(zeros(length(simIn.signals.MOTQ.Time),1));
simIn.signals.MOTQ.CompGain = single(zeros(length(simIn.signals.MOTQ.Time),1));
simIn.signals.MOTQ.AirDensityRatio = single(zeros(length(simIn.signals.MOTQ.Time),1));
simIn.signals.MOTQ.ThrMixOut = single(zeros(length(simIn.signals.MOTQ.Time),1));
end
% Battery inputs
iVec = (sid.BAT.TimeS >= tStart & sid.BAT.TimeS <= tEnd);
simIn.signals.BAT.Time = single(sid.BAT.TimeS(iVec)-tStart);
simIn.signals.BAT.RestVoltEst = single(sid.BAT.VoltR(iVec)); % Estimated resting voltage of the battery
simIn.signals.BAT.Curr = single(sid.BAT.Curr(iVec)); % Measured battery current
simIn.signals.BAT.Res = single(sid.BAT.Res(iVec)); % Estimated battery resistance
simIn.signals.BAT.Volt = single(sid.BAT.Volt(iVec)); % Measured voltage resistance
% Battery inital values
if simIn.param.rateCtrlVal || simIn.param.attCtrlVal || simIn.param.mdlVal || simIn.param.altCtrlVal
simIn.init.BAT.RestVoltEst = single(sid.BAT.VoltR(1));
else
simIn.init.BAT.RestVoltEst = single(0);
end
% Baro inputs
iVec = (sid.BARO.TimeS >= tStart & sid.BARO.TimeS <= tEnd);
simIn.signals.BARO.Time = single(sid.BARO.TimeS(iVec)-tStart);
simIn.signals.BARO.Alt = single(sid.BARO.Alt(iVec)); % Calculated altitude by the barometer
simIn.signals.BARO.AirPress = single(sid.BARO.Press(iVec)); % Measured atmospheric pressure by the barometer
simIn.signals.BARO.GndTemp = single(sid.BARO.GndTemp(iVec)); % Temperature on ground, specified by parameter or measured while on ground
% Attitude controller outputs
iVec = (sid.MOTB.TimeS >= tStart & sid.MOTB.TimeS <= tEnd);
simIn.signals.MOTB.Time = single(sid.MOTB.TimeS(iVec)-tStart);
simIn.signals.MOTB.LiftMax = single(sid.MOTB.LiftMax(iVec)); % Maximum motor compensation gain, calculated in AP_MotorsMulticopter::update_lift_max_from_batt_voltage()
simIn.signals.MOTB.ThrLimit = single(sid.MOTB.ThLimit(iVec)); % Throttle limit set due to battery current limitations, calculated in AP_MotorsMulticopter::get_current_limit_max_throttle()
simIn.signals.MOTB.ThrAvMx = single(sid.MOTB.ThrAvMx(iVec)); % Throttle average max
% SID Inputs
% Create array for sid signals, each element containing the signal struct
if (mode == 25)
iVec = (sid.SIDD.TimeS >= tStart & sid.SIDD.TimeS <= tEnd);
for i=1:13
simIn.signals.SIDD.Time{i} = single(sid.SIDD.TimeS(iVec)-tStart);
if (i == axis)
simIn.signals.SIDD.Chirp{i} = single(sid.SIDD.Targ(iVec)); % SID test signal
else
simIn.signals.SIDD.Chirp{i} = single(zeros(length(simIn.signals.SIDD.Time{i}),1));
end
end
simIn.signals.SIDD.AccX = single(sid.SIDD.Ax(iVec));
simIn.signals.SIDD.AccY = single(sid.SIDD.Ay(iVec));
simIn.signals.SIDD.AccZ = single(sid.SIDD.Az(iVec));
simIn.signals.SIDD.GyrX = single(sid.SIDD.Gx(iVec));
simIn.signals.SIDD.GyrY = single(sid.SIDD.Gy(iVec));
simIn.signals.SIDD.GyrZ = single(sid.SIDD.Gz(iVec));
else
iVec = (sid.RATE.TimeS >= tStart & sid.RATE.TimeS <= tEnd);
for i=1:13
simIn.signals.SIDD.Time{i} = single(sid.RATE.TimeS(iVec)-tStart);
simIn.signals.SIDD.Chirp{i} = single(zeros(length(simIn.signals.SIDD.Time{i}),1));
end
simIn.signals.SIDD.AccX = single(zeros(length(simIn.signals.SIDD.Time{1}),1));
simIn.signals.SIDD.AccY = single(zeros(length(simIn.signals.SIDD.Time{1}),1));
simIn.signals.SIDD.AccZ = single(zeros(length(simIn.signals.SIDD.Time{1}),1));
simIn.signals.SIDD.GyrX = single(zeros(length(simIn.signals.SIDD.Time{1}),1));
simIn.signals.SIDD.GyrY = single(zeros(length(simIn.signals.SIDD.Time{1}),1));
simIn.signals.SIDD.GyrZ = single(zeros(length(simIn.signals.SIDD.Time{1}),1));
end
%% Roll Rate Controller
iVec = (sid.PIDR.TimeS >= tStart & sid.PIDR.TimeS <= tEnd);
simIn.signals.PIDR.Time = single(sid.PIDR.TimeS(iVec)-tStart);
% Inputs
simIn.signals.PIDR.Tar = single(sid.PIDR.Tar(iVec)); % target values filtered
simIn.signals.PIDR.Act = single(sid.PIDR.Act(iVec)); % actual values
simIn.signals.PIDR.Err = single(sid.PIDR.Err(iVec)); % error values
if simIn.param.rateCtrlVal || simIn.param.attCtrlVal || simIn.param.mdlVal
% Clock of Slew Limiter in ms
% Use tracked logging time of PID message in validation modes
simIn.signals.PIDR.ClockDMod = single(1000*simIn.signals.PIDR.Time);
else
simIn.signals.PIDR.ClockDMod = single(1000 * simIn.param.dt * (0:length(simIn.signals.PIDR.Time)-1)');
end
% Outputs
simIn.signals.PIDR.FF = single(sid.PIDR.FF(iVec));
simIn.signals.PIDR.P = single(sid.PIDR.P(iVec));
simIn.signals.PIDR.I = single(sid.PIDR.I(iVec));
simIn.signals.PIDR.D = single(sid.PIDR.D(iVec));
simIn.signals.PIDR.DMod = single(sid.PIDR.Dmod(iVec));
simIn.signals.PIDR.ILimit = single(sid.PIDR.Limit(iVec));
simIn.signals.PIDR.SRate = single(sid.PIDR.SRate(iVec)); % Output slew rate of the slew limiter, stored in _output_slew_rate in AC_PID.cpp, line 161
% parameters - Read from PARM.Value cell array with logical indexing
simIn.param.PIDR.TC = getParamVal(sid, 'ATC_INPUT_TC');
simIn.param.PIDR.FLTT_f = getParamVal(sid, 'ATC_RAT_RLL_FLTT');
simIn.param.PIDR.FLTE_f = getParamVal(sid, 'ATC_RAT_RLL_FLTE');
simIn.param.PIDR.FLTD_f = getParamVal(sid, 'ATC_RAT_RLL_FLTD');
simIn.param.PIDR.P = getParamVal(sid, 'ATC_RAT_RLL_P');
simIn.param.PIDR.I = getParamVal(sid, 'ATC_RAT_RLL_I');
simIn.param.PIDR.D = getParamVal(sid, 'ATC_RAT_RLL_D');
simIn.param.PIDR.IMAX = getParamVal(sid, 'ATC_RAT_RLL_IMAX');
simIn.param.PIDR.FF = getParamVal(sid, 'ATC_RAT_RLL_FF'); %0.05;
simIn.param.PIDR.SR_MAX = getParamVal(sid, 'ATC_RAT_RLL_SMAX'); %5.0;
simIn.param.PIDR.SR_TAU = single(1.0); % Slew Rate Tau - not yet available as a parameter of the copter. Set to 1.0 by default (AC_PID.h, line 24).
simIn.param.PIDR.SR_FLT_f = single(25.0); % Slew Rate lowpass filter cutoff frequency. Defined in SlewLimiter.h, line 14.
% Inital inputs
if simIn.param.rateCtrlVal || simIn.param.attCtrlVal || simIn.param.mdlVal
simIn.init.PIDR.P = single(sid.PIDR.P(1));
simIn.init.PIDR.I = single(sid.PIDR.I(1));
simIn.init.PIDR.D = single(sid.PIDR.D(1));
simIn.init.PIDR.Tar = single(sid.RATE.RDes(1)*pi/180); % Convert to radian due to conversion to degree for logging (AP_AHRS_View::Write_Rate)
simIn.init.PIDR.TarFilt = single(sid.PIDR.Tar(1));
simIn.init.PIDR.ErrFilt = single(sid.PIDR.Err(1));
simIn.init.PIDR.SROut = single(sid.PIDR.SRate(1)); % Initial value of the slew rate determined by the slew limiter. Used for both modifier_slew_rate and output_slew_rate.
if (sid.PIDR.D(1) ~= 0) % Prevent division by zero
simIn.init.PIDR.DerFilt = single(sid.PIDR.D(1))/simIn.param.PIDR.D;
else
simIn.init.PIDR.DerFilt = single(0);
end
else % Set to zero if we do not want to validate model with test run
simIn.init.PIDR.P = single(0);
simIn.init.PIDR.I = single(0);
simIn.init.PIDR.D = single(0);
simIn.init.PIDR.Tar = single(0);
simIn.init.PIDR.TarFilt = single(0);
simIn.init.PIDR.ErrFilt = single(0);
simIn.init.PIDR.DerFilt = single(0);
simIn.init.PIDR.SROut = single(0);
end
%% Pitch Rate Controller
iVec = (sid.PIDP.TimeS >= tStart & sid.PIDP.TimeS <= tEnd);
simIn.signals.PIDP.Time = single(sid.PIDP.TimeS(iVec)-tStart);
% Inputs
simIn.signals.PIDP.Tar = single(sid.PIDP.Tar(iVec)); % target values filtered
simIn.signals.PIDP.Act = single(sid.PIDP.Act(iVec)); % actual values
simIn.signals.PIDP.Err = single(sid.PIDP.Err(iVec)); % error values
if simIn.param.rateCtrlVal || simIn.param.attCtrlVal || simIn.param.mdlVal
% Clock of Slew Limiter
% Use tracked logging time of PID message in validation modes
simIn.signals.PIDP.ClockDMod = single(1000 * simIn.signals.PIDP.Time);
else
simIn.signals.PIDP.ClockDMod = single(1000 * simIn.param.dt * (0:length(simIn.signals.PIDP.Time)-1)');
end
% Outputs
simIn.signals.PIDP.FF = single(sid.PIDP.FF(iVec));
simIn.signals.PIDP.P = single(sid.PIDP.P(iVec));
simIn.signals.PIDP.I = single(sid.PIDP.I(iVec));
simIn.signals.PIDP.D = single(sid.PIDP.D(iVec));
simIn.signals.PIDP.DMod = single(sid.PIDP.Dmod(iVec));
simIn.signals.PIDP.ILimit = single(sid.PIDP.Limit(iVec));
simIn.signals.PIDP.SRate = single(sid.PIDP.SRate(iVec)); % Output slew rate of the slew limiter, stored in _output_slew_rate in AC_PID.cpp, line 161
% Parameters - Read from PARM.Value cell array with logical indexing
simIn.param.PIDP.TC = getParamVal(sid, 'ATC_INPUT_TC');
simIn.param.PIDP.FLTT_f = getParamVal(sid, 'ATC_RAT_PIT_FLTT');
simIn.param.PIDP.FLTE_f = getParamVal(sid, 'ATC_RAT_PIT_FLTE');
simIn.param.PIDP.FLTD_f = getParamVal(sid, 'ATC_RAT_PIT_FLTD');
simIn.param.PIDP.P = getParamVal(sid, 'ATC_RAT_PIT_P');
simIn.param.PIDP.I = getParamVal(sid, 'ATC_RAT_PIT_I');
simIn.param.PIDP.D = getParamVal(sid, 'ATC_RAT_PIT_D');
simIn.param.PIDP.IMAX = getParamVal(sid, 'ATC_RAT_PIT_IMAX');
simIn.param.PIDP.FF = getParamVal(sid, 'ATC_RAT_PIT_FF');
simIn.param.PIDP.SR_MAX = getParamVal(sid, 'ATC_RAT_PIT_SMAX');
simIn.param.PIDP.SR_TAU = single(1.0); % Slew Rate Tau - not yet available as a parameter of the copter. Set to 1.0 by default (AC_PID.h, line 24).
simIn.param.PIDP.SR_FLT_f = single(25.0); % Slew Rate lowpass filter cutoff frequency. Defined in SlewLimiter.h, line 14.
% Inital inputs
if simIn.param.rateCtrlVal || simIn.param.attCtrlVal || simIn.param.mdlVal
simIn.init.PIDP.P = single(sid.PIDP.P(1));
simIn.init.PIDP.I = single(sid.PIDP.I(1));
simIn.init.PIDP.D = single(sid.PIDP.D(1));
simIn.init.PIDP.Tar = single(sid.RATE.PDes(1)*pi/180); % Convert to radian due to conversion to degree for logging (AP_AHRS_View::Write_Rate)
simIn.init.PIDP.TarFilt = single(sid.PIDP.Tar(1));
simIn.init.PIDP.ErrFilt = single(sid.PIDP.Err(1));
simIn.init.PIDP.SrOut = single(sid.PIDP.SRate(1)); % Initial value of the slew rate determined by the slew limiter. Used for both modifier_slew_rate and output_slew_rate.
if (simIn.param.PIDP.D ~= 0)
simIn.init.PIDP.DerFilt = single(sid.PIDP.D(1))/simIn.param.PIDP.D;
else
simIn.init.PIDP.DerFilt = single(0);
end
else
simIn.init.PIDP.P = single(0);
simIn.init.PIDP.I = single(0);
simIn.init.PIDP.D = single(0);
simIn.init.PIDP.Tar = single(0);
simIn.init.PIDP.TarFilt = single(0);
simIn.init.PIDP.ErrFilt = single(0);
simIn.init.PIDP.DerFilt = single(0);
simIn.init.PIDP.SrOut = single(0);
end
%% Yaw Rate Controller
iVec = (sid.PIDY.TimeS >= tStart & sid.PIDY.TimeS <= tEnd);
simIn.signals.PIDY.Time = single(sid.PIDY.TimeS(iVec)-tStart);
% Inputs
simIn.signals.PIDY.Tar = single(sid.PIDY.Tar(iVec)); % target values filtered
simIn.signals.PIDY.Act = single(sid.PIDY.Act(iVec)); % actual values
simIn.signals.PIDY.Err = single(sid.PIDY.Err(iVec)); % error values
if simIn.param.rateCtrlVal || simIn.param.attCtrlVal || simIn.param.mdlVal
% Clock of Slew Limiter
% Use tracked logging time of PID message in validation modes
simIn.signals.PIDY.ClockDMod = single(1000 * simIn.signals.PIDY.Time);
else
simIn.signals.PIDY.ClockDMod = single(1000 * simIn.param.dt * (0:length(simIn.signals.PIDY.Time)-1)');
end
% Outputs
simIn.signals.PIDY.FF = single(sid.PIDY.FF(iVec));
simIn.signals.PIDY.P = single(sid.PIDY.P(iVec));
simIn.signals.PIDY.I = single(sid.PIDY.I(iVec));
simIn.signals.PIDY.D = single(sid.PIDY.D(iVec));
simIn.signals.PIDY.DMod = single(sid.PIDY.Dmod(iVec));
simIn.signals.PIDY.ILimit = single(sid.PIDY.Limit(iVec));
simIn.signals.PIDY.SRate = single(sid.PIDY.SRate(iVec)); % Output slew rate of the slew limiter, stored in _output_slew_rate in AC_PID.cpp, line 161
% Parameters - Read from PARM.Value cell array with logical indexing
simIn.param.PIDY.TC = getParamVal(sid, 'ATC_INPUT_TC');
simIn.param.PIDY.FLTT_f = getParamVal(sid, 'ATC_RAT_YAW_FLTT');
simIn.param.PIDY.FLTE_f = getParamVal(sid, 'ATC_RAT_YAW_FLTE');
simIn.param.PIDY.FLTD_f = getParamVal(sid, 'ATC_RAT_YAW_FLTD');
simIn.param.PIDY.P = getParamVal(sid, 'ATC_RAT_YAW_P');
simIn.param.PIDY.I = getParamVal(sid, 'ATC_RAT_YAW_I');
simIn.param.PIDY.D = getParamVal(sid, 'ATC_RAT_YAW_D');
simIn.param.PIDY.IMAX = getParamVal(sid, 'ATC_RAT_YAW_IMAX');
simIn.param.PIDY.FF = getParamVal(sid, 'ATC_RAT_YAW_FF');
simIn.param.PIDY.SR_MAX = getParamVal(sid, 'ATC_RAT_YAW_SMAX');
simIn.param.PIDY.SR_TAU = single(1.0); % Slew Rate Tau - not yet available as a parameter of the copter. Set to 1.0 by default (AC_PID.h, line 24).
simIn.param.PIDY.SR_FLT_f = single(25.0); % Slew Rate lowpass filter cutoff frequency. Defined in SlewLimiter.h, line 14.
% Inital inputs
if simIn.param.rateCtrlVal || simIn.param.attCtrlVal || simIn.param.mdlVal
simIn.init.PIDY.P = single(sid.PIDY.P(1));
simIn.init.PIDY.I = single(sid.PIDY.I(1));
simIn.init.PIDY.D = single(sid.PIDY.D(1));
simIn.init.PIDY.Tar = single(sid.RATE.PDes(1)*pi/180); % Convert to radian due to conversion to degree for logging (AP_AHRS_View::Write_Rate)
simIn.init.PIDY.TarFilt = single(sid.PIDY.Tar(1));
simIn.init.PIDY.ErrFilt = single(sid.PIDY.Err(1));
simIn.init.PIDY.SrOut = single(sid.PIDY.SRate(1)); % Initial value of the slew rate determined by the slew limiter. Used for both modifier_slew_rate and output_slew_rate.
if (simIn.param.PIDY.D ~= 0)
simIn.init.PIDY.DerFilt = single(sid.PIDY.D(1))/simIn.param.PIDY.D;
else
simIn.init.PIDY.DerFilt = single(0);
end
else
simIn.init.PIDY.P = single(0);
simIn.init.PIDY.I = single(0);
simIn.init.PIDY.D = single(0);
simIn.init.PIDY.Tar = single(0);
simIn.init.PIDY.TarFilt = single(0);
simIn.init.PIDY.ErrFilt = single(0);
simIn.init.PIDY.DerFilt = single(0);
simIn.init.PIDY.SrOut = single(0);
end
%% RATE message
iVec = (sid.RATE.TimeS >= tStart & sid.RATE.TimeS <= tEnd);
simIn.signals.RATE.Time = single(sid.RATE.TimeS(iVec)-tStart);
% Roll
simIn.signals.RATE.ROut = single(sid.RATE.ROut(iVec)); % Rate Controller total output (except FF)
% Rate Controller target (deg/s): Logged signal in val phase, step in opt phase.
if simIn.param.rateCtrlVal || simIn.param.attCtrlVal || simIn.param.mdlVal
simIn.signals.RATE.RDes = single(sid.RATE.RDes(iVec));
elseif simIn.param.optCtrl && axis == 10
simIn.signals.RATE.RDes = single((simIn.signals.RATE.Time > OPT.stepTime)'*OPT.stepMag); % Step at 5s
else
simIn.signals.RATE.RDes = single(zeros(numel(simIn.signals.RATE.Time), 1)); % Set to zero if axis not optimized
end
% Pitch
simIn.signals.RATE.POut = single(sid.RATE.POut(iVec));
% Rate Controller target (deg/s): Logged signal in val phase, step in opt phase
if simIn.param.rateCtrlVal || simIn.param.attCtrlVal || simIn.param.mdlVal
simIn.signals.RATE.PDes = single(sid.RATE.PDes(iVec));
elseif simIn.param.optCtrl && axis == 11
simIn.signals.RATE.PDes = single((simIn.signals.RATE.Time > OPT.stepTime)'*OPT.stepMag); % Step at 5s
else
simIn.signals.RATE.PDes = single(zeros(numel(simIn.signals.RATE.Time), 1)); % Set to zero if axis not optimized
end
% Yaw
simIn.signals.RATE.YOut = single(sid.RATE.YOut(iVec));
% Rate Controller target (deg/s): Logged signal in val phase, step in opt phase
if simIn.param.rateCtrlVal || simIn.param.attCtrlVal || simIn.param.mdlVal
simIn.signals.RATE.YDes = single(sid.RATE.YDes(iVec));
elseif simIn.param.optCtrl && axis == 12
simIn.signals.RATE.YDes = single((simIn.signals.RATE.Time > OPT.stepTime)'*OPT.stepMag); % Step at 5s
else
simIn.signals.RATE.YDes = single(zeros(numel(simIn.signals.RATE.Time), 1)); % Set to zero if axis not optimized
end
% Throttle
simIn.signals.RATE.AOut = single(sid.RATE.AOut(iVec)); % Throttle command to motors
simIn.signals.RATE.A = single(sid.RATE.A(iVec)); % Vertical acceleration in earth frame
%% Position Controller
% Default definitions
simIn.param.PSCD.THR_DZ = getParamVal(sid, 'THR_DZ'); % Deadzone above and below mid throttle in PWM microseconds. Used in Althold, Loiter, PosHold. Defined in Parameters.cpp
simIn.param.PSCD.VEL_MAX_DOWN_DFLT = single(-150); % Default descent rate in cm/s, defined in AC_PosControl.h, line 27
simIn.param.PSCD.VEL_MAX_UP_DFLT = single(250); % Default climb rate in cm/s, defined in AC_PosControl.h, line 28
simIn.param.PSCD.ACC_MAX_Z_DFLT = single(250); % Default vertical acceleration cm/s/s, defined in AC_PosControl.h, line 30
simIn.param.PSCD.JERK_MAX_Z_DFLT = single(500); % Default vertical jerk, defined as m/s/s/s in AC_PosControl.h, line 31. Converted to cm/s/s/s in AC_PosControl.cpp, line 318.
simIn.param.PSCD.VEL_MAX_DN = getParamVal(sid, 'PILOT_SPEED_DN'); % Maximum vertical descending velocity in cm/s, defined in parameters.cpp, line 906
simIn.param.PSCD.VEL_MAX_UP = getParamVal(sid, 'PILOT_SPEED_UP'); % Maximum vertical ascending velocity in cm/s, defined in parameters.cpp, line 223
simIn.param.PSCD.ACC_MAX_Z = getParamVal(sid, 'PILOT_ACCEL_Z'); % Maximum vertical acceleration used when pilot is controlling the altitude, parameters.cpp, line 232
simIn.param.PSCD.JERK_MAX_Z = getParamVal(sid, 'PSC_JERK_Z'); % Jerk limit of vertical kinematic path generation in m/s^3. Determines how quickly aircraft changes acceleration target
simIn.param.PSCD.OVERSPEED_GAIN_Z = single(2); % gain controlling rate at which z-axis speed is brought back within SPEED_UP and SPEED_DOWN range, defined in AC_PosControl.h
%% Position Controller z
% Parameters z position controller
simIn.param.PSCD.P_POS = getParamVal(sid, 'PSC_POSZ_P'); % P gain of the vertical position controller
% Parameters z velocity controller
simIn.param.PSCD.P_VEL = getParamVal(sid, 'PSC_VELZ_P'); % P gain of the vertical velocity controller
simIn.param.PSCD.I_VEL = getParamVal(sid, 'PSC_VELZ_I'); % I gain of the vertical velocity controller
simIn.param.PSCD.D_VEL = getParamVal(sid, 'PSC_VELZ_D'); % D gain of the vertical velocity controller
simIn.param.PSCD.IMAX_VEL = getParamVal(sid, 'PSC_VELZ_IMAX'); % I gain maximu vertical velocity controller
simIn.param.PSCD.FF_VEL = getParamVal(sid, 'PSC_VELZ_FF'); % Feed Forward gain of the vertical velocity controller
simIn.param.PSCD.FLTE_VEL = getParamVal(sid, 'PSC_VELZ_FLTE'); % Cutoff frequency of the error filter (in Hz)
simIn.param.PSCD.FLTD_VEL = getParamVal(sid, 'PSC_VELZ_FLTD'); % Cutoff frequency of the D term filter (in Hz)
simIn.param.PSCD.CTRL_RATIO_INIT = single(2.0); % Initial value of _vel_z_control_ratio, defined in PosControl.h, line 456
% Parameters z acceleration controller
simIn.param.PIDA.P = getParamVal(sid, 'PSC_ACCZ_P'); % P gain of the vertical acceleration controller
simIn.param.PIDA.I = getParamVal(sid, 'PSC_ACCZ_I'); % I gain of the vertical acceleration controller
simIn.param.PIDA.D = getParamVal(sid, 'PSC_ACCZ_D'); % D gain of the vertical acceleration controller
simIn.param.PIDA.IMAX = getParamVal(sid, 'PSC_ACCZ_IMAX'); % I gain maximu vertical acceleration controller
simIn.param.PIDA.FF = getParamVal(sid, 'PSC_ACCZ_FF'); % Feed Forward gain of the vertical acceleration controller
simIn.param.PIDA.FLTE_f = getParamVal(sid, 'PSC_ACCZ_FLTE'); % Cutoff frequency of the error filter (in Hz)
simIn.param.PIDA.FLTD_f = getParamVal(sid, 'PSC_ACCZ_FLTD'); % Cutoff frequency of the D term filter (in Hz)
simIn.param.PIDA.FLTT_f = getParamVal(sid, 'PSC_ACCZ_FLTT'); % Cutoff frequency of the target filter (in Hz)
simIn.param.PIDA.SR_MAX = getParamVal(sid, 'PSC_ACCZ_SMAX'); % Upper limit of the slew rate produced by combined P and D gains
simIn.param.PIDA.SR_TAU = single(1.0); % Slew Rate Tau - not yet available as a parameter of the copter. Set to 1.0 by default (AC_PID.h, line 24).
simIn.param.PIDA.SR_FLT_f = single(25.0); % Slew Rate lowpass filter cutoff frequency. Defined in SlewLimiter.h, line 14.
% Read signals for z position controller if PSCD message was logged
if isfield(sid, 'PSCD')
% Signals of PSCD message
iVec = (sid.PSCD.TimeS >= tStart & sid.PSCD.TimeS <= tEnd);
simIn.signals.PSCD.Time = single(sid.PSCD.TimeS(iVec)-tStart);
% Inputs - Multiply by 100 to get cm as used in controller
% Inversion of signals necessary due to inverted logging
simIn.signals.PSCD.zPosTar = single(-sid.PSCD.TPD(iVec)*100); % Target z-Position, calculated in set_pos_target_z_from_climb_rate_cm()
simIn.signals.PSCD.zPosAct = single(-sid.PSCD.PD(iVec)*100); % Actual z-Position, obtained from INAV
simIn.signals.PSCD.zVelDes = single(-sid.PSCD.DVD(iVec)*100); % Desired z-Velocity, calculated in set_pos_target_z_from_climb_rate_cm()
simIn.signals.PSCD.zVelAct = single(-sid.PSCD.VD(iVec)*100); % Actual z-Velocity, obtained from INAV
simIn.signals.PSCD.zVelTar = single(-sid.PSCD.TVD(iVec)*100); % Target z-Velocity, calculated in update_z_controller()
simIn.signals.PSCD.zAccDes = single(-sid.PSCD.DAD(iVec)*100); % Desired z-Acceleration, calculated in set_pos_target_z_from_climb_rate_cm()
simIn.signals.PSCD.zAccAct = single(-sid.PSCD.AD(iVec)*100); % Actual z-Acceleration, obtained from AHRS
simIn.signals.PSCD.zAccTar = single(-sid.PSCD.TAD(iVec)*100); % Target z-Acceleration, calculated in update_z_controller()
% Signals of PIDA message
iVec = (sid.PIDA.TimeS >= tStart & sid.PIDA.TimeS <= tEnd);
simIn.signals.PIDA.Time = single(sid.PIDA.TimeS(iVec)-tStart);
% Inputs PID z acceleration
simIn.signals.PIDA.Err = single(sid.PIDA.Err(iVec)); % Error between target and actual z-acceleration
simIn.signals.PIDA.Tar = single(sid.PIDA.Tar(iVec));
simIn.signals.PIDA.Act = single(sid.PIDA.Act(iVec));
if simIn.param.rateCtrlVal || simIn.param.attCtrlVal || simIn.param.mdlVal || simIn.param.altCtrlVal
% Clock of Slew Limiter
% Use tracked logging time of PID message in validation modes
simIn.signals.PIDA.ClockDMod = single(1000 * simIn.signals.PIDA.Time);
else
simIn.signals.PIDA.ClockDMod = single(1000 * simIn.param.dt * (0:length(simIn.signals.PIDA.Time)-1)');
end
% Outputs PID z acceleration
simIn.signals.PIDA.FF = single(sid.PIDA.FF(iVec));
simIn.signals.PIDA.P = single(sid.PIDA.P(iVec));
simIn.signals.PIDA.I = single(sid.PIDA.I(iVec));
simIn.signals.PIDA.D = single(sid.PIDA.D(iVec));
simIn.signals.PIDA.DMod = single(sid.PIDA.Dmod(iVec));
simIn.signals.PIDA.ILimit = single(sid.PIDA.Limit(iVec));
simIn.signals.PIDA.SRate = single(sid.PIDA.SRate(iVec));
% Inital inputs
if simIn.param.rateCtrlVal || simIn.param.attCtrlVal || simIn.param.mdlVal || simIn.param.altCtrlVal
% Initial actual values - in meter
simIn.init.PSCD.posAct = single(-sid.PSCD.PD(1));
simIn.init.PSCD.velAct = single(-sid.PSCD.VD(1));
simIn.init.PSCD.accAct = single(-sid.PSCD.AD(1));
% Init the position controller to the current position, velocity
% and acceleration (from AC_PosControl::init_z()) in cm
simIn.init.PIDA.posTar = single(-sid.PSCD.TPD(1)*100);
simIn.init.PIDA.velDes = single(-sid.PSCD.DVD(1)*100);
simIn.init.PIDA.accDes = single(min(max(-sid.PSCD.DAD(1)*100, -simIn.param.PSCD.ACC_MAX_Z_DFLT) ,simIn.param.PSCD.ACC_MAX_Z_DFLT));
simIn.init.PIDA.P = single(sid.PIDA.P(1));
simIn.init.PIDA.I = single(simIn.signals.CTUN.ThrIn(1) - simIn.signals.CTUN.ThrHov(1)) * 1000 ...
- simIn.param.PIDA.P * (-sid.PSCD.TAD(1)*100 - (-sid.PSCD.AD(1))*100) ...
- simIn.param.PIDA.FF * sid.PIDA.Tar(1); % Integrator init. according to AC_PosControl::init_z_controller()
simIn.init.PIDA.D = single(sid.PIDA.D(1));
simIn.init.PIDA.TarFilt = single(sid.PIDA.Tar(1));
simIn.init.PIDA.ErrFilt = single(sid.PIDA.Err(1));
simIn.init.PIDA.SROut = single(sid.PIDA.SRate(1)); % Initial value of the slew rate determined by the slew limiter. Used for both modifier_slew_rate and output_slew_rate.
if (simIn.param.PIDA.D ~= 0)
simIn.init.PIDA.DerFilt = single(sid.PIDA.D(1))/PSC_ACC_Z.param.D; % Divide through D gain to obtain inital D input
else
simIn.init.PIDA.DerFilt = single(0);
end
else
% Initial actual values
simIn.init.PSCD.posAct = single(0);
simIn.init.PSCD.velAct = single(0);
simIn.init.PSCD.accAct = single(0);
simIn.init.PIDA.posTar = single(0);
simIn.init.PIDA.velDes = single(0);
simIn.init.PIDA.accDes = single(0);
simIn.init.PIDA.P = single(0);
simIn.init.PIDA.I = single(0);
simIn.init.PIDA.D = single(0);
simIn.init.PIDA.TarFilt = single(0);
simIn.init.PIDA.ErrFilt = single(0);
simIn.init.PIDA.DerFilt = single(0);
simIn.init.PIDA.SROut = single(0);
end
else
% Set all signals to zero, if PSCD message was not logged and z
% controller has been deactivated
iVec = (sid.RATE.TimeS >= tStart & sid.RATE.TimeS <= tEnd);
simIn.signals.PSCD.Time = single(sid.RATE.TimeS(iVec)-tStart);
simIn.signals.PSCD.zPosTar = single(zeros(length(simIn.signals.PSCD.Time),1)); % Target z-Position, calculated in set_pos_target_z_from_climb_rate_cm()
simIn.signals.PSCD.zPosAct = single(zeros(length(simIn.signals.PSCD.Time),1)); % Actual z-Position, obtained from INAV
simIn.signals.PSCD.zVelDes = single(zeros(length(simIn.signals.PSCD.Time),1)); % Desired z-Velocity, calculated in set_pos_target_z_from_climb_rate_cm()
simIn.signals.PSCD.zVelAct = single(zeros(length(simIn.signals.PSCD.Time),1)); % Actual z-Velocity, obtained from INAV
simIn.signals.PSCD.zVelTar = single(zeros(length(simIn.signals.PSCD.Time),1)); % Target z-Velocity, calculated in update_z_controller()
simIn.signals.PSCD.zAccDes = single(zeros(length(simIn.signals.PSCD.Time),1)); % Desired z-Acceleration, calculated in set_pos_target_z_from_climb_rate_cm()
simIn.signals.PSCD.zAccAct = single(zeros(length(simIn.signals.PSCD.Time),1)); % Actual z-Acceleration, obtained from AHRS
simIn.signals.PSCD.zAccTar = single(zeros(length(simIn.signals.PSCD.Time),1)); % Target z-Acceleration, calculated in update_z_controller()
% Signals of PIDA message
simIn.signals.PIDA.Time = single(simIn.signals.RATE.Time)-tStart;
% Inputs PID z acceleration
simIn.signals.PIDA.Err = single(zeros(length(simIn.signals.PIDA.Time),1)); % Error between target and actual z-acceleration
simIn.signals.PIDA.ClockDMod = single(zeros(length(simIn.signals.PIDA.Time),1)); % Clock of Slew Limiter
% Outputs PID z acceleration
simIn.signals.PIDA.FF = single(zeros(length(simIn.signals.PIDA.Time),1));
simIn.signals.PIDA.P = single(zeros(length(simIn.signals.PIDA.Time),1));
simIn.signals.PIDA.I = single(zeros(length(simIn.signals.PIDA.Time),1));
simIn.signals.PIDA.D = single(zeros(length(simIn.signals.PIDA.Time),1));
simIn.signals.PIDA.DMod = single(zeros(length(simIn.signals.PIDA.Time),1));
simIn.signals.PIDA.ILimit = single(zeros(length(simIn.signals.PIDA.Time),1));
simIn.signals.PIDA.SRate = single(zeros(length(simIn.signals.PIDA.Time),1));
simIn.signals.PIDA.Act = single(zeros(length(simIn.signals.PIDA.Time),1));
simIn.signals.PIDA.Tar = single(zeros(length(simIn.signals.PIDA.Time),1));
% Initial inputs
% Initial actual values
simIn.init.PSCD.posAct = single(0);
simIn.init.PSCD.velAct = single(0);
simIn.init.PSCD.accAct = single(0);
simIn.init.PIDA.posTar = single(0);
simIn.init.PIDA.velDes = single(0);
simIn.init.PIDA.accDes = single(0);
simIn.init.PIDA.P = single(0);
simIn.init.PIDA.I = single(0);
simIn.init.PIDA.D = single(0);
simIn.init.PIDA.TarFilt = single(0);
simIn.init.PIDA.ErrFilt = single(0);
simIn.init.PIDA.DerFilt = single(0);
simIn.init.PIDA.SROut = single(0);
end
% Real Flight Signals of PID z velocity - only load if log messages
% exists
if isfield(sid, 'PCVZ')
iVec = (sid.PCVZ.TimeS >= tStart & sid.PCVZ.TimeS <= tEnd);
simIn.signals.PCVZ.Time = single(sid.PCVZ.TimeS(iVec)-tStart);
simIn.signals.PCVZ.Err = single(sid.PCVZ.Err(iVec));
simIn.signals.PCVZ.P = single(sid.PCVZ.P(iVec));
simIn.signals.PCVZ.I = single(sid.PCVZ.I(iVec));
simIn.signals.PCVZ.D = single(sid.PCVZ.D(iVec));
simIn.signals.PCVZ.FF = single(sid.PCVZ.FF(iVec));
else % Set to zero otherwise
iVec = (sid.RATE.TimeS >= tStart & sid.RATE.TimeS <= tEnd);
simIn.signals.PCVZ.Time = single(sid.RATE.TimeS(iVec)-tStart);
simIn.signals.PCVZ.Err = single(zeros(length(simIn.signals.PCVZ.Time),1));
simIn.signals.PCVZ.P = single(zeros(length(simIn.signals.PCVZ.Time),1));
simIn.signals.PCVZ.I = single(zeros(length(simIn.signals.PCVZ.Time),1));
simIn.signals.PCVZ.D = single(zeros(length(simIn.signals.PCVZ.Time),1));
simIn.signals.PCVZ.FF = single(zeros(length(simIn.signals.PCVZ.Time),1));
end
%% Load identified plant models data
if simIn.param.optCtrl || simIn.param.mdlVal && exist('idModel', 'var')
% Roll
if isempty(findobj(idModel, 'axis', 'RLL'))
simIn.models.RollAxis = idpoly(tf(1, [1 1], simIn.param.dt));
else
model = findobj(idModel, 'axis', 'RLL');
% If more than one model is found, let user choose
if length(model) > 1
mdlIdx = input('More than one model found for the roll axis. Input index of desired model: ');
else
mdlIdx = 1;
end
simIn.models.RollAxis = idss(c2d(model(mdlIdx).tfModel, simIn.param.dt));
% Decrease delay to account for the additional feedback delay
simIn.models.RollAxis.InputDelay = simIn.models.RollAxis.InputDelay-1;
end
% Pitch
if isempty(findobj(idModel, 'axis', 'PIT'))
simIn.models.PitchAxis = idpoly(tf(1, [1 1], simIn.param.dt));
else
model = findobj(idModel, 'axis', 'PIT');
% If more than one model is found, let user choose
if length(model) > 1
mdlIdx = input('More than one model found for the pitch axis. Input index of desired model: ');
else
mdlIdx = 1;
end
simIn.models.PitchAxis = idss(c2d(model(mdlIdx).tfModel, simIn.param.dt));
simIn.models.PitchAxis.InputDelay = simIn.models.PitchAxis.InputDelay-1;
end
% Yaw
if isempty(findobj(idModel, 'axis', 'YAW'))
simIn.models.YawAxis = idpoly(tf(1, [1 1], simIn.param.dt));
else
model = findobj(idModel, 'axis', 'YAW');
% If more than one model is found, let user choose
if length(model) > 1
mdlIdx = input('More than one model found for the yaw axis. Input index of desired model: ');
else
mdlIdx = 1;
end
simIn.models.YawAxis = idss(c2d(model(mdlIdx).tfModel, simIn.param.dt));
simIn.models.YawAxis.InputDelay = simIn.models.YawAxis.InputDelay-1;
end
% Vertical Motion - Throttle
if isempty(findobj(idModel, 'axis', 'THR'))
simIn.models.RollAxis = idpoly(tf(1, [1 1], simIn.param.dt));
else
model = findobj(idModel, 'axis', 'THR');
% If more than one model is found, let user choose
if length(model) > 1
mdlIdx = input('More than one model found for the vertical axis. Input index of desired model: ');
else
mdlIdx = 1;
end
simIn.models.VerticalAxis = idss(c2d(model(mdlIdx).tfModel, simIn.param.dt));
% Decrease delay to account for the additional feedback delay
simIn.models.VerticalAxis.InputDelay = simIn.models.VerticalAxis.InputDelay-1;
end
else % Create dummy models of optimization is not active
simIn.models.RollAxis = idpoly(tf(1, [1 1], simIn.param.dt));
simIn.models.PitchAxis = idpoly(tf(1, [1 1], simIn.param.dt));
simIn.models.YawAxis = idpoly(tf(1, [1 1], simIn.param.dt));
simIn.models.VerticalAxis = idpoly(tf(1, [1 1], simIn.param.dt));
end
% Initial values
if simIn.param.mdlVal && mode == 25
simIn.models.RollInit = single(sid.SIDD.Gx(1)*pi/180);
simIn.models.PitchInit = single(sid.SIDD.Gy(1)*pi/180);
simIn.models.YawInit = single(sid.SIDD.Gz(1)*pi/180);
simIn.models.VerticalOutInit = single(sid.SIDD.Az(1));
simIn.models.VerticalInInit = single(sid.RATE.AOut(1));
else
simIn.models.RollInit = single(0);
simIn.models.PitchInit = single(0);
simIn.models.YawInit = single(0);
simIn.models.VerticalOutInit = single(-9.81);
simIn.models.VerticalInInit = single(sid.RATE.AOut(1));
end
%% Delete all irrelevant data
clear i dist start thr_man rel_msg mode param_names msgs tEnd tStart simMode
clear phase_des runOutsideLoop mdlIdx pitchCh rollCh yawCh iVec Fs axis dt
clear simMode thrCh

BIN
Tools/simulink/arducopter/simulink_model.PNG Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 166 KiB