CoCalc -- sid_sim

Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.

GitHub Repository: Ardupilot/ardupilot
Path: blob/master/Tools/simulink/arducopter/sid_sim_init.m
Views: ¹⁷⁹⁹
1
% Initialization of the Simulink model. The script loads all important
2
% signals and parameter to the workspace.
3
%
4
% Fabian Bredemeier - IAV GmbH
5
% License: GPL v3
6
%% Define log index and start + stop time
7
if ~exist('log_idx', 'var')
8
    log_idx = 2;
9
end
10
tStart = 0;
11
tEnd = inf;
12

13
%% Setting simMode
14
% 0 = Model validation - Identified models are used and full control loop
15
% is active
16
% 1 = Rate Controller validation - No models used, measured input signals
17
% of rate controller are used
18
% 2 = Attitude Controller validation - No models used, measured input
19
% signals of attitude controller are used
20
% 3 = Altitude Controller validation - No models used, measured input
21
% signals of attitude controller are used
22
% 4 = Optimization of controller - Not implemented yet
23
if ~exist('simMode', 'var')
24
    simMode = 0;
25
end
26

27
%% Check for existance of data
28

29
% Check if sid data is existing
30
if exist('sidLogs', 'var') % in Workspace
31
    disp('.mat file already loaded. Reading parameter and signals...');
32
elseif exist('sid.mat', 'file') % in a .mat file with all other axis tests
33
    load('sid.mat');
34
    disp('Loaded data from sid.mat');
35
elseif ~exist(loadedDataConfig)
36
    error('Could not find configuration struct. Aborting script.')
37
else
38
    error('Could not find data. Aborting script.')
39
end
40

41
%% Config and variable declaration
42

43
% Sample rate of simInulation
44
Fs = 400;
45
dt = 0.0025;
46
simIn.param.dt = 0.0025;
47

48
% Load data
49
sid = sidLogs(log_idx).data;
50
% Check if relevant messages are defined
51
if (numel(sid) == 0)
52
    error('Ardupilog object sid is empty. Aborting.');
53
end
54

55
% Get mode of current test run
56
mode = loadedDataConfig.modes(log_idx);
57
% Get manual throttle flag for current test run
58
thr_man = loadedDataConfig.modesConfig(log_idx).thr_man;
59
% Get filter messages of current test run
60
msgs = loadedDataConfig.modesConfig(log_idx).filter_msgs;
61
% Set axis
62
axis = loadedDataConfig.axisList(log_idx);
63
% Check if config parameters are complete
64
if (isempty(mode) || isempty(thr_man) || isempty(msgs))
65
   error('Config variables (mode, thr_man, msgs) not complete. Aborting.'); 
66
end
67

68
% Define validation and optimization flag of simInulation
69
simIn.param.mdlVal = simMode == 0;
70
simIn.param.rateCtrlVal = simMode == 1;
71
simIn.param.attCtrlVal = simMode == 2 || simMode == 3;
72
simIn.param.altCtrlVal = simMode == 3 || mode == 2;
73
simIn.param.optCtrl = simMode == 4;
74

75
%% General Copter settings
76
simIn.param.LOOP_RATE = getParamVal(sid, 'SCHED_LOOP_RATE');
77
simIn.param.dt = round(double(1 / simIn.param.LOOP_RATE), 4); % Sample time resulting from loop rate. Round to four decimal digits
78
simIn.param.FRAME_CLASS = getParamVal(sid, 'FRAME_CLASS');
79
simIn.param.FRAME_TYPE = getParamVal(sid, 'FRAME_TYPE');
80
[simIn.param.NUM_MOTORS, simIn.param.AXIS_FAC_MOTORS] = getMotorMixerFactors(simIn.param.FRAME_CLASS, simIn.param.FRAME_TYPE);
81
simIn.param.EKF_TYPE = getParamVal(sid, 'AHRS_EKF_TYPE');
82
simIn.param.GRAVITY_MSS = single(9.80665);
83
simIn.param.dt_THR_LOOP = 1 / 50; % Sample time of throttle loop in Copter.cpp. Defined in Copter.cpp, line 94
84
simIn.param.ANGLE_MAX = getParamVal(sid, 'ANGLE_MAX'); % Maximum lean angle in all flight modes
85
simIn.param.PILOT_Y_EXPO = getParamVal(sid, 'PILOT_Y_EXPO'); % Acro yaw expo to allow faster rotation when stick at edges
86
simIn.param.PILOT_Y_RATE = getParamVal(sid, 'PILOT_Y_RATE');
87
simIn.param.PILOT_Y_RATE_TC = getParamVal(sid, 'PILOT_Y_RATE_TC'); % Pilot yaw rate control input time constant
88
simIn.param.MODE.ModeNr = mode; % Flight mode
89

90
% Abort script if frame configuration is not defined yet
91
if (isempty(simIn.param.AXIS_FAC_MOTORS) || simIn.param.NUM_MOTORS == 0)
92
    error("Body configuration of copter is not defined yet!");
93
end
94

95
%% Simulation duration
96
% Define end time of simInulation
97
if simIn.param.mdlVal || simIn.param.rateCtrlVal || simIn.param.attCtrlVal || simIn.param.altCtrlVal
98
    if isinf(tEnd) % Run full data
99
        simIn.param.timeEnd = sid.RATE.TimeS(end);
100
    else % Only run part of data
101
        duration = tEnd-tStart;
102
        iEnd = int16(duration/dt);
103
        simIn.param.timeEnd = sid.RATE.TimeS(iEnd);
104
        clear duration iEnd
105
    end
106
elseif simIn.param.optCtrl
107
    simIn.param.timeEnd = OPT.tEndOpt;
108
end
109

110
%% Inertial Sensor
111
simIn.param.INS.GYRO_FILTER = getParamVal(sid, 'INS_GYRO_FILTER'); % Filter cutoff frequency for gyro lowpass filter
112
simIn.param.INS.GYRO_RATE = 400; % Gyro sampling rate. Actually define by INS_GYRO_RATE, but simInulation is run with 400 Hz
113

114
%% AHRS
115
trim_x = getParamVal(sid, 'AHRS_TRIM_X');
116
trim_y = getParamVal(sid, 'AHRS_TRIM_Y');
117
simIn.param.AHRS.FC2BF = single(dcmFromEuler(trim_x, trim_y, 0)); % Rotation matrix from FC to Body Frame based on AP_AHRS constructor
118
simIn.param.AHRS.BF2FC = simIn.param.AHRS.FC2BF';
119

120
clear trim_x trim_y
121
%% RCIN - Radio Input and parameters for Attitude Control
122
rollCh = getParamVal(sid, 'RCMAP_ROLL');
123
pitchCh = getParamVal(sid, 'RCMAP_PITCH');
124
yawCh = getParamVal(sid, 'RCMAP_YAW');
125
thrCh = getParamVal(sid, 'RCMAP_THROTTLE');
126

127
simIn.param.RCIN.DZ_RLL = getParamVal(sid, ['RC' num2str(rollCh) '_DZ']);
128
simIn.param.RCIN.MIN_RLL = getParamVal(sid, ['RC' num2str(rollCh) '_MIN']);
129
simIn.param.RCIN.MAX_RLL = getParamVal(sid, ['RC' num2str(rollCh) '_MAX']);
130
simIn.param.RCIN.TRIM_RLL = getParamVal(sid, ['RC' num2str(rollCh) '_TRIM']);
131
simIn.param.RCIN.REVERSED_RLL = getParamVal(sid, ['RC' num2str(rollCh) '_REVERSED']);
132
simIn.param.RCIN.DZ_PIT = getParamVal(sid, ['RC' num2str(pitchCh) '_DZ']);
133
simIn.param.RCIN.MIN_PIT = getParamVal(sid, ['RC' num2str(pitchCh) '_MIN']);
134
simIn.param.RCIN.MAX_PIT = getParamVal(sid, ['RC' num2str(pitchCh) '_MAX']);
135
simIn.param.RCIN.TRIM_PIT = getParamVal(sid, ['RC' num2str(pitchCh) '_TRIM']);
136
simIn.param.RCIN.REVERSED_PIT = getParamVal(sid, ['RC' num2str(pitchCh) '_REVERSED']);
137
simIn.param.RCIN.DZ_YAW = getParamVal(sid, ['RC' num2str(yawCh) '_DZ']);
138
simIn.param.RCIN.MIN_YAW = getParamVal(sid, ['RC' num2str(yawCh) '_MIN']);
139
simIn.param.RCIN.MAX_YAW = getParamVal(sid, ['RC' num2str(yawCh) '_MAX']);
140
simIn.param.RCIN.TRIM_YAW = getParamVal(sid, ['RC' num2str(yawCh) '_TRIM']);
141
simIn.param.RCIN.REVERSED_YAW = getParamVal(sid, ['RC' num2str(yawCh) '_REVERSED']);
142
simIn.param.RCIN.DZ_THR = getParamVal(sid, ['RC' num2str(thrCh) '_DZ']);
143
simIn.param.RCIN.MIN_THR = getParamVal(sid, ['RC' num2str(thrCh) '_MIN']);
144
simIn.param.RCIN.MAX_THR = getParamVal(sid, ['RC' num2str(thrCh) '_MAX']);
145
simIn.param.RCIN.TRIM_THR = getParamVal(sid, ['RC' num2str(thrCh) '_TRIM']);
146
simIn.param.RCIN.REVERSED_THR = getParamVal(sid, ['RC' num2str(thrCh) '_REVERSED']);
147

148
simIn.param.RCIN.ROLL_PITCH_YAW_INPUT_MAX = 4500; % Yaw, roll, pitch input range, defined in config.h
149

150
% Check if RCIN log is available
151
if simIn.param.mdlVal || simIn.param.attCtrlVal || simIn.param.altCtrlVal
152
    if isfield(sid, 'RCIN')
153
        iVec = (sid.RCIN.TimeS >= tStart & sid.RCIN.TimeS <= tEnd);
154
        simIn.signals.RCIN.Time = single(sid.RCIN.TimeS(iVec)-tStart);
155
        simIn.signals.RCIN.RollInput = eval(['single(sid.RCIN.C' num2str(rollCh) '(iVec))']);
156
        simIn.signals.RCIN.PitchInput = eval(['single(sid.RCIN.C' num2str(pitchCh) '(iVec))']);
157
        simIn.signals.RCIN.YawInput = eval(['single(sid.RCIN.C' num2str(yawCh) '(iVec))']);
158
        simIn.signals.RCIN.ThrottleInput = eval(['single(sid.RCIN.C' num2str(thrCh) '(iVec))']);
159
    else
160
        if ~isfield(sid, 'RCIN') && axis == 13 
161
            warning(['RCIN message cannot be found in log ' num2str(log_idx) '! Validating vertical motion model.']);
162
        else
163
            warning(['RCIN message cannot be found in log ' num2str(log_idx) '! Ommiting attitude controller and models in simInulation. Only rate controller is validated']);
164
            simIn.param.rateCtrlVal = 1;
165
            simIn.param.mdlVal = 0;
166
        end
167
        % Set RCIN signals to zero
168
        simIn.signals.RCIN.Time = single(0:0.1:simIn.param.timeEnd);
169
        simIn.signals.RCIN.RollInput = single(zeros(length(simIn.signals.RCIN.Time),1));
170
        simIn.signals.RCIN.PitchInput = single(zeros(length(simIn.signals.RCIN.Time),1));
171
        simIn.signals.RCIN.YawInput = single(zeros(length(simIn.signals.RCIN.Time),1));
172
        simIn.signals.RCIN.ThrottleInput = single(zeros(length(simIn.signals.RCIN.Time),1));
173
    end
174
end
175

176
%% Attitude
177
% Yaw Angle signals are a modulo of 360°, so this operation has to be
178
% inverted
179
iVec = (sid.ATT.TimeS >= tStart & sid.ATT.TimeS <= tEnd);
180
simIn.signals.ATT.Time = single(sid.ATT.TimeS(iVec)-tStart);
181
simIn.signals.ATT.Roll = single(sid.ATT.Roll(iVec));
182
simIn.signals.ATT.Pitch = single(sid.ATT.Pitch(iVec));
183
simIn.signals.ATT.Yaw = single(demodYawAngle(sid.ATT.Yaw(iVec)));
184
simIn.signals.ATT.DesRoll = single(sid.ATT.DesRoll(iVec));
185
simIn.signals.ATT.DesPitch = single(sid.ATT.DesPitch(iVec));
186
simIn.signals.ATT.DesYaw = single(demodYawAngle(sid.ATT.DesYaw(iVec)));
187

188
% The actual yaw angle is calculated from the DCM with
189
% Matrix3<T>::to_euler() with the four-quadrant arcus tangens atan2(), for 
190
% example in AP_AHRS_View::update(), so the controller can only handle 
191
% values in the range [-pi, pi]. 
192
simIn.signals.ATT.Yaw = simIn.signals.ATT.Yaw - 360 * (simIn.signals.ATT.Yaw > 180);
193
simIn.signals.ATT.DesYaw = simIn.signals.ATT.DesYaw - 360 * (simIn.signals.ATT.DesYaw > 180);
194

195
% Initial DCM matrix based on initial Euler angles
196
% Calculation based on Matrix3<T>::from_euler()
197
for i=1:length(iVec)
198
    if iVec(i) == 1
199
        iInit = i;
200
        break;
201
    end 
202
end
203
rollAngInit = single(sid.ATT.Roll(iInit))*pi/180;
204
pitchAngInit = single(sid.ATT.Pitch(iInit))*pi/180;
205
yawAngInit = single(simIn.signals.ATT.Yaw(1))*pi/180;
206
simIn.init.ATT.Roll = rollAngInit;
207
simIn.init.ATT.Pitch = pitchAngInit;
208
simIn.init.ATT.Yaw = yawAngInit;
209
simIn.init.ATT.DCM = dcmFromEuler(rollAngInit, pitchAngInit, yawAngInit);
210

211
% Initialize attitudeTarget according to
212
% AC_AttitudeControl::reset_yaw_target_and_rate(), which is called in
213
% ModeStabilize::run() when copter is landed
214
attitudeTargetUpdate = fromAxisAngle([0;0;yawAngInit]);
215
simIn.init.ATT.attitudeTarget = quatMult(attitudeTargetUpdate, [1;0;0;0]);
216
simIn.init.ATT.eulerRateTar = [0;0;0];
217

218
% Test signals in SITL
219
% Attitude Controller Targets
220
% simIn.signals.ATT.attTarMeasW = single(sid.ATAR.attW);
221
% simIn.signals.ATT.attTarMeasX = single(sid.ATAR.attX);
222
% simIn.signals.ATT.attTarMeasY = single(sid.ATAR.attY);
223
% simIn.signals.ATT.attTarMeasZ = single(sid.ATAR.attZ);
224
% simIn.signals.ATT.eulerAngTarMeasX = single(sid.ATAR.angX);
225
% simIn.signals.ATT.eulerAngTarMeasY = single(sid.ATAR.angY);
226
% simIn.signals.ATT.eulerAngTarMeasZ = single(sid.ATAR.angZ);
227
% simIn.signals.ATT.eulerRatTarMeasX = single(sid.ATAR.ratX);
228
% simIn.signals.ATT.eulerRatTarMeasY = single(sid.ATAR.ratY);
229
% simIn.signals.ATT.eulerRatTarMeasZ = single(sid.ATAR.ratZ);
230
% 
231
% simIn.signals.ATT.CtrlRollIn = single(sid.CTIN.rllCtrl);
232
% simIn.signals.ATT.CtrlPitchIn = single(sid.CTIN.pitCtrl);
233
% simIn.signals.ATT.CtrlYawIn = single(sid.CTIN.yawCtrl);
234
% simIn.signals.ATT.yawRateDesMeas = single(sid.ATIN.yawDes);
235

236
clear rollAngInit pitchAngInit yawAngInit attitudeTargetUpdate iInit
237

238
%% Attitude Controller General
239
simIn.param.ATC.RATE_FF_ENAB = getParamVal(sid, 'ATC_RATE_FF_ENAB');
240
simIn.param.ATC.INPUT_TC = getParamVal(sid, 'ATC_INPUT_TC'); % Attitude control input time constant
241
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
242
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
243
simIn.param.ATC.ANG_LIM_TC = getParamVal(sid, 'ATC_ANG_LIM_TC'); % Angle Limit (to maintain altitude) Time Constant
244
% Roll Angle Controller
245
simIn.param.ATC.ANG_RLL_P = getParamVal(sid, 'ATC_ANG_RLL_P');
246
simIn.param.ATC.ACCEL_R_MAX = getParamVal(sid, 'ATC_ACCEL_R_MAX');
247
simIn.param.ATC.RATE_R_MAX = getParamVal(sid, 'ATC_RATE_R_MAX');
248
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
249
simIn.param.ATC.ACCEL_RP_CONTROLLER_MAX_RADSS = 720*pi/180;
250
% Pitch Angle Controller
251
simIn.param.ATC.ANG_PIT_P = getParamVal(sid, 'ATC_ANG_PIT_P');
252
simIn.param.ATC.ACCEL_P_MAX = getParamVal(sid, 'ATC_ACCEL_P_MAX');
253
simIn.param.ATC.RATE_P_MAX = getParamVal(sid, 'ATC_RATE_P_MAX');
254
% Yaw Angle Controller
255
simIn.param.ATC.ANG_YAW_P = getParamVal(sid, 'ATC_ANG_YAW_P');
256
simIn.param.ATC.ACCEL_Y_MAX = getParamVal(sid, 'ATC_ACCEL_Y_MAX');
257
simIn.param.ATC.RATE_Y_MAX = getParamVal(sid, 'ATC_RATE_Y_MAX');
258
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
259
simIn.param.ATC.ACCEL_Y_CONTROLLER_MIN_RADSS = single(10*pi/180);
260
simIn.param.ATC.THRUST_ERROR_ANGLE = single(30*pi/180); % Thrust angle error above which yaw corrections are limited. Defined in AC_AttitudeControl.h
261

262
% Thrust and throttle calculation parameters
263
simIn.param.MODE.THR_MAN_FLG = thr_man;    % Flag for manual throttle, which depends on the current mode. 
264
if thr_man
265
    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().
266
else
267
    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)
268
end
269
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()
270
simIn.param.ATC.THR_MIX_MAN = getParamVal(sid, 'ATC_THR_MIX_MAN');   % Throttle vs. attitude priorisation during manual flight
271
simIn.param.ATC.THR_MIX_MIN = getParamVal(sid, 'ATC_THR_MIX_MIN');   % Throttle vs. attitude priorisation used when landing
272
simIn.param.ATC.THR_MIX_MAX = getParamVal(sid, 'ATC_THR_MIX_MAX');   % Throttle vs. attitude priorisation during active flights
273
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 
274
if thr_man
275
    simIn.init.ATC.THR_MIX = simIn.param.ATC.THR_MIX_MAN;
276
else
277
    simIn.init.ATC.THR_MIX = simIn.param.ATC.THR_MIX_DFLT;
278
end
279
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
280
simIn.param.MOT.THST_HOVER = getParamVal(sid, 'MOT_THST_HOVER');    % Motor thrust needed to hover. Default value of _throttle_hover.
281
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.
282
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
283
simIn.param.MOT.THST_HOVER_MIN = single(0.125);                                       % Minimum possible hover throttle, found as AP_MOTORS_THST_HOVER_MIN in AP_MotorsMulticopter.h
284
simIn.param.MOT.THST_HOVER_MAX = single(0.6875);                                      % Maximum possible hover throttle, found as AP_MOTORS_THST_HOVER_MAX in AP_MotorsMulticopter.h
285
simIn.param.MOT.BAT_VOLT_MAX = getParamVal(sid, 'MOT_BAT_VOLT_MAX');   % Maximum voltage above which no additional scaling on thrust is performed
286
simIn.param.MOT.BAT_VOLT_MIN = getParamVal(sid, 'MOT_BAT_VOLT_MIN');   % Minimum voltage below which no additional scaling on thrust is performed
287
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)
288
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)
289
simIn.param.MOT.BAT_CURR_TC = getParamVal(sid, 'MOT_BAT_CURR_TC');               % Time constant used to limit the maximum current
290
simIn.param.MOT.YAW_HEADROOM = getParamVal(sid, 'MOT_YAW_HEADROOM');             % Yaw control is goven at least this PWM in microseconds range
291

292
% Throttle inputs
293
iVec = (sid.CTUN.TimeS >= tStart & sid.CTUN.TimeS <= tEnd);
294
simIn.signals.CTUN.Time = single(sid.CTUN.TimeS(iVec)--tStart);
295
simIn.signals.CTUN.ThrIn = single(sid.CTUN.ThI(iVec)); % Throttle In, pilots input to Attitude Control (attitude_control->get_throttle_in())
296
simIn.signals.CTUN.ThrOut = single(sid.CTUN.ThO(iVec)); % Throttle Out, throttle given to motor mixer after filtering (motors->get_throttle())
297
simIn.signals.CTUN.AngBst = single(sid.CTUN.ABst(iVec)); % Extra amount of throttle, added to throttle_in in AC_AttitudeControl_Multi::get_throttle_boosted()
298
simIn.signals.CTUN.ThrHov = single(sid.CTUN.ThH(iVec)); % Estimated throttle required to hover throttle in range 0-1 (AP_MotorsMulticopter::get_throttle_hover())
299

300
% Throttle inital values
301
if simIn.param.rateCtrlVal || simIn.param.attCtrlVal || simIn.param.mdlVal || simIn.param.altCtrlVal
302
    simIn.init.CTUN.ThrOut = single(sid.CTUN.ThO(1));
303
    simIn.init.CTUN.ThrHov = single(sid.CTUN.ThH(1));
304
else
305
    simIn.init.CTUN.ThrOut = single(0);
306
    simIn.init.CTUN.ThrHov = single(simIn.param.MOT.THST_HOVER);
307
end
308

309
% Altitude signals
310
simIn.signals.CTUN.DSAlt = single(sid.CTUN.DSAlt(iVec)); % Desired rangefinder altitude
311
simIn.signals.CTUN.DAlt = single(sid.CTUN.DAlt(iVec)); % Desired altitude
312
simIn.signals.CTUN.Alt = single(sid.CTUN.Alt(iVec)); % Achieved altitude (inav)
313
simIn.signals.CTUN.SAlt = single(sid.CTUN.SAlt(iVec)); % Achieved rangefinder altitude
314
simIn.signals.CTUN.TAlt = single(sid.CTUN.TAlt(iVec)); % Terrain altitude
315
simIn.signals.CTUN.BAlt = single(sid.CTUN.BAlt(iVec)); % Barometric altitude
316
simIn.signals.CTUN.CRt = single(sid.CTUN.CRt(iVec)); % Climb rate inav
317
simIn.signals.CTUN.DCRt = single(sid.CTUN.DCRt(iVec)); % Desired climb rate 
318

319
% Altitude inital values
320
simIn.init.CTUN.Alt = single(sid.CTUN.Alt(1));
321
simIn.init.CTUN.CRt = single(sid.CTUN.CRt(1)*0.01);
322

323
% Intermediate throttle calculation quantities - only read if message
324
% exists
325
if isfield(sid, 'MOTQ')
326
    iVec = (sid.MOTQ.TimeS >= tStart & sid.MOTQ.TimeS <= tEnd);
327
    simIn.signals.MOTQ.Time = single(sid.MOTQ.TimeS(iVec)-tStart);
328
    simIn.signals.MOTQ.ThrAvgMax = single(sid.MOTQ.ThAvgMax(iVec));
329
    simIn.signals.MOTQ.ThrThstMaxMeas = single(sid.MOTQ.ThThstMax(iVec));
330
    simIn.signals.MOTQ.CompGain = single(sid.MOTQ.CompGain(iVec));
331
    simIn.signals.MOTQ.AirDensityRatio = single(sid.MOTQ.AirDensRat(iVec));
332
    simIn.signals.MOTQ.ThrMixOut = single(sid.MOTQ.ThrOut(iVec));
333
else
334
    iVec = (sid.CTUN.TimeS >= tStart & sid.CTUN.TimeS <= tEnd);
335
    simIn.signals.MOTQ.Time = single(sid.CTUN.TimeS(iVec)-tStart);
336
    simIn.signals.MOTQ.ThrAvgMax = single(zeros(length(simIn.signals.MOTQ.Time),1));
337
    simIn.signals.MOTQ.ThrThstMaxMeas = single(zeros(length(simIn.signals.MOTQ.Time),1));
338
    simIn.signals.MOTQ.CompGain = single(zeros(length(simIn.signals.MOTQ.Time),1));
339
    simIn.signals.MOTQ.AirDensityRatio = single(zeros(length(simIn.signals.MOTQ.Time),1));
340
    simIn.signals.MOTQ.ThrMixOut = single(zeros(length(simIn.signals.MOTQ.Time),1));
341
end
342

343

344
% Battery inputs
345
iVec = (sid.BAT.TimeS >= tStart & sid.BAT.TimeS <= tEnd);
346
simIn.signals.BAT.Time = single(sid.BAT.TimeS(iVec)-tStart);
347
simIn.signals.BAT.RestVoltEst = single(sid.BAT.VoltR(iVec)); % Estimated resting voltage of the battery
348
simIn.signals.BAT.Curr = single(sid.BAT.Curr(iVec)); % Measured battery current
349
simIn.signals.BAT.Res = single(sid.BAT.Res(iVec)); % Estimated battery resistance
350
simIn.signals.BAT.Volt = single(sid.BAT.Volt(iVec)); % Measured voltage resistance
351

352
% Battery inital values
353
if simIn.param.rateCtrlVal || simIn.param.attCtrlVal || simIn.param.mdlVal || simIn.param.altCtrlVal
354
    simIn.init.BAT.RestVoltEst = single(sid.BAT.VoltR(1));
355
else
356
    simIn.init.BAT.RestVoltEst = single(0);
357
end
358

359
% Baro inputs
360
iVec = (sid.BARO.TimeS >= tStart & sid.BARO.TimeS <= tEnd);
361
simIn.signals.BARO.Time = single(sid.BARO.TimeS(iVec)-tStart);
362
simIn.signals.BARO.Alt = single(sid.BARO.Alt(iVec));     % Calculated altitude by the barometer
363
simIn.signals.BARO.AirPress = single(sid.BARO.Press(iVec));  % Measured atmospheric pressure by the barometer
364
simIn.signals.BARO.GndTemp = single(sid.BARO.GndTemp(iVec)); % Temperature on ground, specified by parameter or measured while on ground
365

366
% Attitude controller outputs
367
iVec = (sid.MOTB.TimeS >= tStart & sid.MOTB.TimeS <= tEnd);
368
simIn.signals.MOTB.Time = single(sid.MOTB.TimeS(iVec)-tStart);
369
simIn.signals.MOTB.LiftMax = single(sid.MOTB.LiftMax(iVec)); % Maximum motor compensation gain, calculated in AP_MotorsMulticopter::update_lift_max_from_batt_voltage()
370
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()
371
simIn.signals.MOTB.ThrAvMx = single(sid.MOTB.ThrAvMx(iVec)); % Throttle average max
372

373
% SID Inputs
374
% Create array for sid signals, each element containing the signal struct
375
if (mode == 25)
376
    iVec = (sid.SIDD.TimeS >= tStart & sid.SIDD.TimeS <= tEnd);
377
    for i=1:13
378
        simIn.signals.SIDD.Time{i} = single(sid.SIDD.TimeS(iVec)-tStart);
379
        if (i == axis)
380
            simIn.signals.SIDD.Chirp{i} = single(sid.SIDD.Targ(iVec)); % SID test signal
381
        else
382
            simIn.signals.SIDD.Chirp{i} = single(zeros(length(simIn.signals.SIDD.Time{i}),1));
383
        end
384
    end
385
    simIn.signals.SIDD.AccX = single(sid.SIDD.Ax(iVec));
386
    simIn.signals.SIDD.AccY = single(sid.SIDD.Ay(iVec));
387
    simIn.signals.SIDD.AccZ = single(sid.SIDD.Az(iVec));
388
    simIn.signals.SIDD.GyrX = single(sid.SIDD.Gx(iVec));
389
    simIn.signals.SIDD.GyrY = single(sid.SIDD.Gy(iVec));
390
    simIn.signals.SIDD.GyrZ = single(sid.SIDD.Gz(iVec));
391
else
392
    iVec = (sid.RATE.TimeS >= tStart & sid.RATE.TimeS <= tEnd);
393
    for i=1:13
394
        simIn.signals.SIDD.Time{i} = single(sid.RATE.TimeS(iVec)-tStart);
395
        simIn.signals.SIDD.Chirp{i} = single(zeros(length(simIn.signals.SIDD.Time{i}),1));
396
    end
397
    simIn.signals.SIDD.AccX = single(zeros(length(simIn.signals.SIDD.Time{1}),1));
398
    simIn.signals.SIDD.AccY = single(zeros(length(simIn.signals.SIDD.Time{1}),1));
399
    simIn.signals.SIDD.AccZ = single(zeros(length(simIn.signals.SIDD.Time{1}),1));
400
    simIn.signals.SIDD.GyrX = single(zeros(length(simIn.signals.SIDD.Time{1}),1));
401
    simIn.signals.SIDD.GyrY = single(zeros(length(simIn.signals.SIDD.Time{1}),1));
402
    simIn.signals.SIDD.GyrZ = single(zeros(length(simIn.signals.SIDD.Time{1}),1));
403
end
404

405
%% Roll Rate Controller
406
iVec = (sid.PIDR.TimeS >= tStart & sid.PIDR.TimeS <= tEnd);
407
simIn.signals.PIDR.Time = single(sid.PIDR.TimeS(iVec)-tStart);
408
% Inputs
409
simIn.signals.PIDR.Tar = single(sid.PIDR.Tar(iVec)); % target values filtered
410
simIn.signals.PIDR.Act = single(sid.PIDR.Act(iVec)); % actual values
411
simIn.signals.PIDR.Err = single(sid.PIDR.Err(iVec)); % error values
412
if simIn.param.rateCtrlVal || simIn.param.attCtrlVal || simIn.param.mdlVal
413
    % Clock of Slew Limiter in ms
414
    % Use tracked logging time of PID message in validation modes
415
    simIn.signals.PIDR.ClockDMod = single(1000*simIn.signals.PIDR.Time); 
416
else
417
    simIn.signals.PIDR.ClockDMod = single(1000 * simIn.param.dt * (0:length(simIn.signals.PIDR.Time)-1)');
418
end
419
    
420
% Outputs
421
simIn.signals.PIDR.FF = single(sid.PIDR.FF(iVec));
422
simIn.signals.PIDR.P = single(sid.PIDR.P(iVec));
423
simIn.signals.PIDR.I = single(sid.PIDR.I(iVec));
424
simIn.signals.PIDR.D = single(sid.PIDR.D(iVec));
425
simIn.signals.PIDR.DMod = single(sid.PIDR.Dmod(iVec));
426
simIn.signals.PIDR.ILimit = single(sid.PIDR.Limit(iVec));
427
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
428

429
% parameters - Read from PARM.Value cell array with logical indexing
430
simIn.param.PIDR.TC = getParamVal(sid, 'ATC_INPUT_TC');
431
simIn.param.PIDR.FLTT_f = getParamVal(sid, 'ATC_RAT_RLL_FLTT');
432
simIn.param.PIDR.FLTE_f = getParamVal(sid, 'ATC_RAT_RLL_FLTE');
433
simIn.param.PIDR.FLTD_f = getParamVal(sid, 'ATC_RAT_RLL_FLTD');
434
simIn.param.PIDR.P = getParamVal(sid, 'ATC_RAT_RLL_P');
435
simIn.param.PIDR.I = getParamVal(sid, 'ATC_RAT_RLL_I');
436
simIn.param.PIDR.D = getParamVal(sid, 'ATC_RAT_RLL_D');
437
simIn.param.PIDR.IMAX = getParamVal(sid, 'ATC_RAT_RLL_IMAX');
438
simIn.param.PIDR.FF = getParamVal(sid, 'ATC_RAT_RLL_FF'); %0.05;
439
simIn.param.PIDR.SR_MAX = getParamVal(sid, 'ATC_RAT_RLL_SMAX'); %5.0;
440
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).
441
simIn.param.PIDR.SR_FLT_f = single(25.0);  % Slew Rate lowpass filter cutoff frequency. Defined in SlewLimiter.h, line 14.
442

443
% Inital inputs
444
if simIn.param.rateCtrlVal || simIn.param.attCtrlVal || simIn.param.mdlVal
445
    simIn.init.PIDR.P = single(sid.PIDR.P(1));
446
    simIn.init.PIDR.I = single(sid.PIDR.I(1));
447
    simIn.init.PIDR.D = single(sid.PIDR.D(1));
448
    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)
449
    simIn.init.PIDR.TarFilt = single(sid.PIDR.Tar(1));
450
    simIn.init.PIDR.ErrFilt = single(sid.PIDR.Err(1));
451
    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.
452
    if (sid.PIDR.D(1) ~= 0) % Prevent division by zero
453
        simIn.init.PIDR.DerFilt = single(sid.PIDR.D(1))/simIn.param.PIDR.D;
454
    else
455
        simIn.init.PIDR.DerFilt = single(0);
456
    end
457
else % Set to zero if we do not want to validate model with test run
458
    simIn.init.PIDR.P = single(0);
459
    simIn.init.PIDR.I = single(0);
460
    simIn.init.PIDR.D = single(0);
461
    simIn.init.PIDR.Tar = single(0);
462
    simIn.init.PIDR.TarFilt = single(0);
463
    simIn.init.PIDR.ErrFilt = single(0);
464
    simIn.init.PIDR.DerFilt = single(0);
465
    simIn.init.PIDR.SROut = single(0);
466
end
467

468
%% Pitch Rate Controller
469
iVec = (sid.PIDP.TimeS >= tStart & sid.PIDP.TimeS <= tEnd);
470
simIn.signals.PIDP.Time = single(sid.PIDP.TimeS(iVec)-tStart);
471
% Inputs
472
simIn.signals.PIDP.Tar = single(sid.PIDP.Tar(iVec)); % target values filtered
473
simIn.signals.PIDP.Act = single(sid.PIDP.Act(iVec)); % actual values
474
simIn.signals.PIDP.Err = single(sid.PIDP.Err(iVec)); % error values                                          
475
if simIn.param.rateCtrlVal || simIn.param.attCtrlVal || simIn.param.mdlVal
476
    % Clock of Slew Limiter
477
    % Use tracked logging time of PID message in validation modes
478
    simIn.signals.PIDP.ClockDMod = single(1000 * simIn.signals.PIDP.Time); 
479
else
480
    simIn.signals.PIDP.ClockDMod = single(1000 * simIn.param.dt * (0:length(simIn.signals.PIDP.Time)-1)');
481
end
482

483
% Outputs
484
simIn.signals.PIDP.FF = single(sid.PIDP.FF(iVec));
485
simIn.signals.PIDP.P = single(sid.PIDP.P(iVec));
486
simIn.signals.PIDP.I = single(sid.PIDP.I(iVec));
487
simIn.signals.PIDP.D = single(sid.PIDP.D(iVec));
488
simIn.signals.PIDP.DMod = single(sid.PIDP.Dmod(iVec));
489
simIn.signals.PIDP.ILimit = single(sid.PIDP.Limit(iVec));
490
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
491

492
% Parameters - Read from PARM.Value cell array with logical indexing
493
simIn.param.PIDP.TC = getParamVal(sid, 'ATC_INPUT_TC');
494
simIn.param.PIDP.FLTT_f = getParamVal(sid, 'ATC_RAT_PIT_FLTT');
495
simIn.param.PIDP.FLTE_f = getParamVal(sid, 'ATC_RAT_PIT_FLTE');
496
simIn.param.PIDP.FLTD_f = getParamVal(sid, 'ATC_RAT_PIT_FLTD');
497
simIn.param.PIDP.P = getParamVal(sid, 'ATC_RAT_PIT_P');
498
simIn.param.PIDP.I = getParamVal(sid, 'ATC_RAT_PIT_I');
499
simIn.param.PIDP.D = getParamVal(sid, 'ATC_RAT_PIT_D');
500
simIn.param.PIDP.IMAX = getParamVal(sid, 'ATC_RAT_PIT_IMAX');
501
simIn.param.PIDP.FF = getParamVal(sid, 'ATC_RAT_PIT_FF');
502
simIn.param.PIDP.SR_MAX = getParamVal(sid, 'ATC_RAT_PIT_SMAX');
503
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).
504
simIn.param.PIDP.SR_FLT_f = single(25.0);  % Slew Rate lowpass filter cutoff frequency. Defined in SlewLimiter.h, line 14.
505

506
% Inital inputs
507
if simIn.param.rateCtrlVal || simIn.param.attCtrlVal || simIn.param.mdlVal
508
    simIn.init.PIDP.P = single(sid.PIDP.P(1));
509
    simIn.init.PIDP.I = single(sid.PIDP.I(1));
510
    simIn.init.PIDP.D = single(sid.PIDP.D(1));
511
    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)
512
    simIn.init.PIDP.TarFilt = single(sid.PIDP.Tar(1));
513
    simIn.init.PIDP.ErrFilt = single(sid.PIDP.Err(1));
514
    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.
515
    if (simIn.param.PIDP.D ~= 0)
516
        simIn.init.PIDP.DerFilt = single(sid.PIDP.D(1))/simIn.param.PIDP.D;
517
    else
518
        simIn.init.PIDP.DerFilt = single(0);
519
    end
520
else
521
    simIn.init.PIDP.P = single(0);
522
    simIn.init.PIDP.I = single(0);
523
    simIn.init.PIDP.D = single(0);
524
    simIn.init.PIDP.Tar = single(0);
525
    simIn.init.PIDP.TarFilt = single(0);
526
    simIn.init.PIDP.ErrFilt = single(0);
527
    simIn.init.PIDP.DerFilt = single(0);
528
    simIn.init.PIDP.SrOut = single(0);
529
end
530

531
%% Yaw Rate Controller
532
iVec = (sid.PIDY.TimeS >= tStart & sid.PIDY.TimeS <= tEnd);
533
simIn.signals.PIDY.Time = single(sid.PIDY.TimeS(iVec)-tStart);
534
% Inputs
535
simIn.signals.PIDY.Tar = single(sid.PIDY.Tar(iVec)); % target values filtered
536
simIn.signals.PIDY.Act = single(sid.PIDY.Act(iVec)); % actual values
537
simIn.signals.PIDY.Err = single(sid.PIDY.Err(iVec)); % error values                                          
538
if simIn.param.rateCtrlVal || simIn.param.attCtrlVal || simIn.param.mdlVal
539
    % Clock of Slew Limiter
540
    % Use tracked logging time of PID message in validation modes
541
    simIn.signals.PIDY.ClockDMod = single(1000 * simIn.signals.PIDY.Time); 
542
else
543
    simIn.signals.PIDY.ClockDMod = single(1000 * simIn.param.dt * (0:length(simIn.signals.PIDY.Time)-1)');
544
end
545

546
% Outputs
547
simIn.signals.PIDY.FF = single(sid.PIDY.FF(iVec));
548
simIn.signals.PIDY.P = single(sid.PIDY.P(iVec));
549
simIn.signals.PIDY.I = single(sid.PIDY.I(iVec));
550
simIn.signals.PIDY.D = single(sid.PIDY.D(iVec));
551
simIn.signals.PIDY.DMod = single(sid.PIDY.Dmod(iVec));
552
simIn.signals.PIDY.ILimit = single(sid.PIDY.Limit(iVec));
553
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
554

555
% Parameters - Read from PARM.Value cell array with logical indexing
556
simIn.param.PIDY.TC = getParamVal(sid, 'ATC_INPUT_TC');
557
simIn.param.PIDY.FLTT_f = getParamVal(sid, 'ATC_RAT_YAW_FLTT');
558
simIn.param.PIDY.FLTE_f = getParamVal(sid, 'ATC_RAT_YAW_FLTE');
559
simIn.param.PIDY.FLTD_f = getParamVal(sid, 'ATC_RAT_YAW_FLTD');
560
simIn.param.PIDY.P = getParamVal(sid, 'ATC_RAT_YAW_P');
561
simIn.param.PIDY.I = getParamVal(sid, 'ATC_RAT_YAW_I');
562
simIn.param.PIDY.D = getParamVal(sid, 'ATC_RAT_YAW_D');
563
simIn.param.PIDY.IMAX = getParamVal(sid, 'ATC_RAT_YAW_IMAX');
564
simIn.param.PIDY.FF = getParamVal(sid, 'ATC_RAT_YAW_FF');
565
simIn.param.PIDY.SR_MAX = getParamVal(sid, 'ATC_RAT_YAW_SMAX');
566
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).
567
simIn.param.PIDY.SR_FLT_f = single(25.0);  % Slew Rate lowpass filter cutoff frequency. Defined in SlewLimiter.h, line 14.
568

569
% Inital inputs
570
if simIn.param.rateCtrlVal || simIn.param.attCtrlVal || simIn.param.mdlVal
571
    simIn.init.PIDY.P = single(sid.PIDY.P(1));
572
    simIn.init.PIDY.I = single(sid.PIDY.I(1));
573
    simIn.init.PIDY.D = single(sid.PIDY.D(1));
574
    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)
575
    simIn.init.PIDY.TarFilt = single(sid.PIDY.Tar(1));
576
    simIn.init.PIDY.ErrFilt = single(sid.PIDY.Err(1));
577
    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.
578
    if (simIn.param.PIDY.D ~= 0)
579
        simIn.init.PIDY.DerFilt = single(sid.PIDY.D(1))/simIn.param.PIDY.D;
580
    else
581
        simIn.init.PIDY.DerFilt = single(0);
582
    end
583
else
584
    simIn.init.PIDY.P = single(0);
585
    simIn.init.PIDY.I = single(0);
586
    simIn.init.PIDY.D = single(0);
587
    simIn.init.PIDY.Tar = single(0);
588
    simIn.init.PIDY.TarFilt = single(0);
589
    simIn.init.PIDY.ErrFilt = single(0);
590
    simIn.init.PIDY.DerFilt = single(0);
591
    simIn.init.PIDY.SrOut = single(0);
592
end
593

594
%% RATE message
595
iVec = (sid.RATE.TimeS >= tStart & sid.RATE.TimeS <= tEnd);
596
simIn.signals.RATE.Time = single(sid.RATE.TimeS(iVec)-tStart);
597

598
% Roll
599
simIn.signals.RATE.ROut = single(sid.RATE.ROut(iVec)); % Rate Controller total output (except FF)
600
% Rate Controller target (deg/s): Logged signal in val phase, step in opt phase.
601
if simIn.param.rateCtrlVal || simIn.param.attCtrlVal || simIn.param.mdlVal
602
    simIn.signals.RATE.RDes = single(sid.RATE.RDes(iVec));
603
elseif simIn.param.optCtrl && axis == 10
604
    simIn.signals.RATE.RDes = single((simIn.signals.RATE.Time > OPT.stepTime)'*OPT.stepMag); % Step at 5s
605
else
606
    simIn.signals.RATE.RDes = single(zeros(numel(simIn.signals.RATE.Time), 1)); % Set to zero if axis not optimized
607
end
608

609
% Pitch
610
simIn.signals.RATE.POut = single(sid.RATE.POut(iVec));
611
% Rate Controller target (deg/s): Logged signal in val phase, step in opt phase
612
if simIn.param.rateCtrlVal || simIn.param.attCtrlVal || simIn.param.mdlVal
613
    simIn.signals.RATE.PDes = single(sid.RATE.PDes(iVec));
614
elseif simIn.param.optCtrl && axis == 11
615
    simIn.signals.RATE.PDes = single((simIn.signals.RATE.Time > OPT.stepTime)'*OPT.stepMag); % Step at 5s
616
else
617
    simIn.signals.RATE.PDes = single(zeros(numel(simIn.signals.RATE.Time), 1)); % Set to zero if axis not optimized
618
end
619

620
% Yaw 
621
simIn.signals.RATE.YOut = single(sid.RATE.YOut(iVec));
622
% Rate Controller target (deg/s): Logged signal in val phase, step in opt phase
623
if simIn.param.rateCtrlVal || simIn.param.attCtrlVal || simIn.param.mdlVal
624
    simIn.signals.RATE.YDes = single(sid.RATE.YDes(iVec));
625
elseif simIn.param.optCtrl && axis == 12
626
    simIn.signals.RATE.YDes = single((simIn.signals.RATE.Time > OPT.stepTime)'*OPT.stepMag); % Step at 5s
627
else
628
    simIn.signals.RATE.YDes = single(zeros(numel(simIn.signals.RATE.Time), 1)); % Set to zero if axis not optimized
629
end
630

631
% Throttle
632
simIn.signals.RATE.AOut = single(sid.RATE.AOut(iVec)); % Throttle command to motors
633
simIn.signals.RATE.A = single(sid.RATE.A(iVec)); % Vertical acceleration in earth frame
634

635
%% Position Controller
636
% Default definitions
637
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
638
simIn.param.PSCD.VEL_MAX_DOWN_DFLT = single(-150); % Default descent rate in cm/s, defined in AC_PosControl.h, line 27
639
simIn.param.PSCD.VEL_MAX_UP_DFLT = single(250); % Default climb rate in cm/s, defined in AC_PosControl.h, line 28
640
simIn.param.PSCD.ACC_MAX_Z_DFLT = single(250); % Default vertical acceleration cm/s/s, defined in AC_PosControl.h, line 30
641
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.
642

643
simIn.param.PSCD.VEL_MAX_DN = getParamVal(sid, 'PILOT_SPEED_DN'); % Maximum vertical descending velocity in cm/s, defined in parameters.cpp, line 906
644
simIn.param.PSCD.VEL_MAX_UP = getParamVal(sid, 'PILOT_SPEED_UP'); % Maximum vertical ascending velocity in cm/s, defined in parameters.cpp, line 223
645
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
646
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
647
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 
648
%% Position Controller z
649

650
% Parameters z position controller
651
simIn.param.PSCD.P_POS = getParamVal(sid, 'PSC_POSZ_P'); % P gain of the vertical position controller
652

653
% Parameters z velocity controller
654
simIn.param.PSCD.P_VEL = getParamVal(sid, 'PSC_VELZ_P'); % P gain of the vertical velocity controller
655
simIn.param.PSCD.I_VEL = getParamVal(sid, 'PSC_VELZ_I'); % I gain of the vertical velocity controller
656
simIn.param.PSCD.D_VEL = getParamVal(sid, 'PSC_VELZ_D'); % D gain of the vertical velocity controller
657
simIn.param.PSCD.IMAX_VEL = getParamVal(sid, 'PSC_VELZ_IMAX'); % I gain maximu vertical velocity controller
658
simIn.param.PSCD.FF_VEL = getParamVal(sid, 'PSC_VELZ_FF'); % Feed Forward gain of the vertical velocity controller
659
simIn.param.PSCD.FLTE_VEL = getParamVal(sid, 'PSC_VELZ_FLTE'); % Cutoff frequency of the error filter (in Hz)
660
simIn.param.PSCD.FLTD_VEL = getParamVal(sid, 'PSC_VELZ_FLTD'); % Cutoff frequency of the D term filter (in Hz)
661
simIn.param.PSCD.CTRL_RATIO_INIT = single(2.0); % Initial value of _vel_z_control_ratio, defined in PosControl.h, line 456
662

663
% Parameters z acceleration controller
664
simIn.param.PIDA.P = getParamVal(sid, 'PSC_ACCZ_P'); % P gain of the vertical acceleration controller
665
simIn.param.PIDA.I = getParamVal(sid, 'PSC_ACCZ_I'); % I gain of the vertical acceleration controller
666
simIn.param.PIDA.D = getParamVal(sid, 'PSC_ACCZ_D'); % D gain of the vertical acceleration controller
667
simIn.param.PIDA.IMAX = getParamVal(sid, 'PSC_ACCZ_IMAX'); % I gain maximu vertical acceleration controller
668
simIn.param.PIDA.FF = getParamVal(sid, 'PSC_ACCZ_FF'); % Feed Forward gain of the vertical acceleration controller
669
simIn.param.PIDA.FLTE_f = getParamVal(sid, 'PSC_ACCZ_FLTE'); % Cutoff frequency of the error filter (in Hz)
670
simIn.param.PIDA.FLTD_f = getParamVal(sid, 'PSC_ACCZ_FLTD'); % Cutoff frequency of the D term filter (in Hz)
671
simIn.param.PIDA.FLTT_f = getParamVal(sid, 'PSC_ACCZ_FLTT'); % Cutoff frequency of the target filter (in Hz)
672
simIn.param.PIDA.SR_MAX = getParamVal(sid, 'PSC_ACCZ_SMAX'); % Upper limit of the slew rate produced by combined P and D gains
673
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).
674
simIn.param.PIDA.SR_FLT_f = single(25.0);  % Slew Rate lowpass filter cutoff frequency. Defined in SlewLimiter.h, line 14.
675

676
% Read signals for z position controller if PSCD message was logged
677
if isfield(sid, 'PSCD')
678
    % Signals of PSCD message
679
    iVec = (sid.PSCD.TimeS >= tStart & sid.PSCD.TimeS <= tEnd);
680
    simIn.signals.PSCD.Time = single(sid.PSCD.TimeS(iVec)-tStart);
681
    % Inputs - Multiply by 100 to get cm as used in controller
682
    % Inversion of signals necessary due to inverted logging
683
    simIn.signals.PSCD.zPosTar = single(-sid.PSCD.TPD(iVec)*100); % Target z-Position, calculated in set_pos_target_z_from_climb_rate_cm()
684
    simIn.signals.PSCD.zPosAct = single(-sid.PSCD.PD(iVec)*100); % Actual z-Position, obtained from INAV
685
    simIn.signals.PSCD.zVelDes = single(-sid.PSCD.DVD(iVec)*100); % Desired z-Velocity, calculated in set_pos_target_z_from_climb_rate_cm()
686
    simIn.signals.PSCD.zVelAct = single(-sid.PSCD.VD(iVec)*100); % Actual z-Velocity, obtained from INAV
687
    simIn.signals.PSCD.zVelTar = single(-sid.PSCD.TVD(iVec)*100); % Target z-Velocity, calculated in update_z_controller()
688
    simIn.signals.PSCD.zAccDes = single(-sid.PSCD.DAD(iVec)*100); % Desired z-Acceleration, calculated in set_pos_target_z_from_climb_rate_cm()
689
    simIn.signals.PSCD.zAccAct = single(-sid.PSCD.AD(iVec)*100); % Actual z-Acceleration, obtained from AHRS  
690
    simIn.signals.PSCD.zAccTar = single(-sid.PSCD.TAD(iVec)*100); % Target z-Acceleration, calculated in update_z_controller()  
691
    
692
    % Signals of PIDA message
693
    iVec = (sid.PIDA.TimeS >= tStart & sid.PIDA.TimeS <= tEnd);
694
    simIn.signals.PIDA.Time = single(sid.PIDA.TimeS(iVec)-tStart);
695
    % Inputs PID z acceleration
696
    simIn.signals.PIDA.Err = single(sid.PIDA.Err(iVec)); % Error between target and actual z-acceleration
697
    simIn.signals.PIDA.Tar = single(sid.PIDA.Tar(iVec));
698
    simIn.signals.PIDA.Act = single(sid.PIDA.Act(iVec));
699
    if simIn.param.rateCtrlVal || simIn.param.attCtrlVal || simIn.param.mdlVal || simIn.param.altCtrlVal
700
        % Clock of Slew Limiter
701
        % Use tracked logging time of PID message in validation modes
702
        simIn.signals.PIDA.ClockDMod = single(1000 * simIn.signals.PIDA.Time);
703
    else
704
        simIn.signals.PIDA.ClockDMod = single(1000 * simIn.param.dt * (0:length(simIn.signals.PIDA.Time)-1)');
705
    end
706
    % Outputs PID z acceleration
707
    simIn.signals.PIDA.FF = single(sid.PIDA.FF(iVec));
708
    simIn.signals.PIDA.P = single(sid.PIDA.P(iVec));
709
    simIn.signals.PIDA.I = single(sid.PIDA.I(iVec));
710
    simIn.signals.PIDA.D = single(sid.PIDA.D(iVec));
711
    simIn.signals.PIDA.DMod = single(sid.PIDA.Dmod(iVec));
712
    simIn.signals.PIDA.ILimit = single(sid.PIDA.Limit(iVec));
713
    simIn.signals.PIDA.SRate = single(sid.PIDA.SRate(iVec));    
714
    
715
    % Inital inputs
716
    if simIn.param.rateCtrlVal || simIn.param.attCtrlVal || simIn.param.mdlVal || simIn.param.altCtrlVal
717
        % Initial actual values - in meter
718
        simIn.init.PSCD.posAct = single(-sid.PSCD.PD(1));
719
        simIn.init.PSCD.velAct = single(-sid.PSCD.VD(1));
720
        simIn.init.PSCD.accAct = single(-sid.PSCD.AD(1));
721
        
722
        % Init the position controller to the current position, velocity
723
        % and acceleration (from AC_PosControl::init_z()) in cm
724
        simIn.init.PIDA.posTar = single(-sid.PSCD.TPD(1)*100);
725
        simIn.init.PIDA.velDes = single(-sid.PSCD.DVD(1)*100);
726
        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));
727
        
728
        simIn.init.PIDA.P = single(sid.PIDA.P(1));
729
        simIn.init.PIDA.I = single(simIn.signals.CTUN.ThrIn(1) - simIn.signals.CTUN.ThrHov(1)) * 1000 ...
730
            - simIn.param.PIDA.P * (-sid.PSCD.TAD(1)*100 - (-sid.PSCD.AD(1))*100) ...
731
            - simIn.param.PIDA.FF * sid.PIDA.Tar(1); % Integrator init. according to AC_PosControl::init_z_controller()
732
        simIn.init.PIDA.D = single(sid.PIDA.D(1));
733
        simIn.init.PIDA.TarFilt = single(sid.PIDA.Tar(1));
734
        simIn.init.PIDA.ErrFilt = single(sid.PIDA.Err(1));
735
        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.
736
        if (simIn.param.PIDA.D ~= 0)
737
            simIn.init.PIDA.DerFilt = single(sid.PIDA.D(1))/PSC_ACC_Z.param.D; % Divide through D gain to obtain inital D input
738
        else
739
            simIn.init.PIDA.DerFilt = single(0);
740
        end
741
    else
742
        % Initial actual values
743
        simIn.init.PSCD.posAct = single(0);
744
        simIn.init.PSCD.velAct = single(0);
745
        simIn.init.PSCD.accAct = single(0);
746
        
747
        simIn.init.PIDA.posTar = single(0);
748
        simIn.init.PIDA.velDes = single(0);
749
        simIn.init.PIDA.accDes = single(0);
750
        
751
        simIn.init.PIDA.P = single(0);
752
        simIn.init.PIDA.I = single(0);
753
        simIn.init.PIDA.D = single(0);
754
        simIn.init.PIDA.TarFilt = single(0);
755
        simIn.init.PIDA.ErrFilt = single(0);
756
        simIn.init.PIDA.DerFilt = single(0);
757
        simIn.init.PIDA.SROut = single(0);
758
    end
759
else
760
    % Set all signals to zero, if PSCD message was not logged and z
761
    % controller has been deactivated
762
    iVec = (sid.RATE.TimeS >= tStart & sid.RATE.TimeS <= tEnd);
763
    simIn.signals.PSCD.Time = single(sid.RATE.TimeS(iVec)-tStart);
764
    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()
765
    simIn.signals.PSCD.zPosAct = single(zeros(length(simIn.signals.PSCD.Time),1)); % Actual z-Position, obtained from INAV
766
    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()
767
    simIn.signals.PSCD.zVelAct = single(zeros(length(simIn.signals.PSCD.Time),1)); % Actual z-Velocity, obtained from INAV
768
    simIn.signals.PSCD.zVelTar = single(zeros(length(simIn.signals.PSCD.Time),1)); % Target z-Velocity, calculated in update_z_controller()
769
    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()
770
    simIn.signals.PSCD.zAccAct = single(zeros(length(simIn.signals.PSCD.Time),1)); % Actual z-Acceleration, obtained from AHRS  
771
    simIn.signals.PSCD.zAccTar = single(zeros(length(simIn.signals.PSCD.Time),1)); % Target z-Acceleration, calculated in update_z_controller()
772
    
773
    % Signals of PIDA message
774
    simIn.signals.PIDA.Time = single(simIn.signals.RATE.Time)-tStart;
775
    % Inputs PID z acceleration
776
    simIn.signals.PIDA.Err = single(zeros(length(simIn.signals.PIDA.Time),1)); % Error between target and actual z-acceleration
777
    simIn.signals.PIDA.ClockDMod = single(zeros(length(simIn.signals.PIDA.Time),1)); % Clock of Slew Limiter
778

779
    % Outputs PID z acceleration
780
    simIn.signals.PIDA.FF = single(zeros(length(simIn.signals.PIDA.Time),1));
781
    simIn.signals.PIDA.P = single(zeros(length(simIn.signals.PIDA.Time),1));
782
    simIn.signals.PIDA.I = single(zeros(length(simIn.signals.PIDA.Time),1));
783
    simIn.signals.PIDA.D = single(zeros(length(simIn.signals.PIDA.Time),1));
784
    simIn.signals.PIDA.DMod = single(zeros(length(simIn.signals.PIDA.Time),1));
785
    simIn.signals.PIDA.ILimit = single(zeros(length(simIn.signals.PIDA.Time),1));
786
    simIn.signals.PIDA.SRate = single(zeros(length(simIn.signals.PIDA.Time),1)); 
787
    simIn.signals.PIDA.Act = single(zeros(length(simIn.signals.PIDA.Time),1));
788
    simIn.signals.PIDA.Tar = single(zeros(length(simIn.signals.PIDA.Time),1));
789
    
790
    % Initial inputs
791
    % Initial actual values
792
    simIn.init.PSCD.posAct = single(0);
793
    simIn.init.PSCD.velAct = single(0);
794
    simIn.init.PSCD.accAct = single(0);
795
    
796
    simIn.init.PIDA.posTar = single(0);
797
    simIn.init.PIDA.velDes = single(0);
798
    simIn.init.PIDA.accDes = single(0);
799
    
800
    simIn.init.PIDA.P = single(0);
801
    simIn.init.PIDA.I = single(0);
802
    simIn.init.PIDA.D = single(0);
803
    simIn.init.PIDA.TarFilt = single(0);
804
    simIn.init.PIDA.ErrFilt = single(0);
805
    simIn.init.PIDA.DerFilt = single(0);
806
    simIn.init.PIDA.SROut = single(0);
807
end
808

809

810
% Real Flight Signals of PID z velocity - only load if log messages
811
% exists
812
if isfield(sid, 'PCVZ')
813
    iVec = (sid.PCVZ.TimeS >= tStart & sid.PCVZ.TimeS <= tEnd);
814
    simIn.signals.PCVZ.Time = single(sid.PCVZ.TimeS(iVec)-tStart);
815
    simIn.signals.PCVZ.Err = single(sid.PCVZ.Err(iVec));
816
    simIn.signals.PCVZ.P = single(sid.PCVZ.P(iVec));
817
    simIn.signals.PCVZ.I = single(sid.PCVZ.I(iVec));
818
    simIn.signals.PCVZ.D = single(sid.PCVZ.D(iVec));
819
    simIn.signals.PCVZ.FF = single(sid.PCVZ.FF(iVec));
820
else % Set to zero otherwise
821
    iVec = (sid.RATE.TimeS >= tStart & sid.RATE.TimeS <= tEnd);
822
    simIn.signals.PCVZ.Time = single(sid.RATE.TimeS(iVec)-tStart);
823
    simIn.signals.PCVZ.Err = single(zeros(length(simIn.signals.PCVZ.Time),1));
824
    simIn.signals.PCVZ.P = single(zeros(length(simIn.signals.PCVZ.Time),1));
825
    simIn.signals.PCVZ.I = single(zeros(length(simIn.signals.PCVZ.Time),1));
826
    simIn.signals.PCVZ.D = single(zeros(length(simIn.signals.PCVZ.Time),1));
827
    simIn.signals.PCVZ.FF = single(zeros(length(simIn.signals.PCVZ.Time),1));
828
end
829

830
%% Load identified plant models data
831
if simIn.param.optCtrl || simIn.param.mdlVal && exist('idModel', 'var')
832
    % Roll
833
    if isempty(findobj(idModel, 'axis', 'RLL'))
834
        simIn.models.RollAxis = idpoly(tf(1, [1 1], simIn.param.dt));
835
    else
836
        model = findobj(idModel, 'axis', 'RLL');
837
        % If more than one model is found, let user choose
838
        if length(model) > 1 
839
            mdlIdx = input('More than one model found for the roll axis. Input index of desired model: ');
840
        else
841
            mdlIdx = 1;
842
        end
843
        simIn.models.RollAxis = idss(c2d(model(mdlIdx).tfModel, simIn.param.dt));
844
        % Decrease delay to account for the additional feedback delay
845
        simIn.models.RollAxis.InputDelay = simIn.models.RollAxis.InputDelay-1;
846
    end
847
    % Pitch
848
    if isempty(findobj(idModel, 'axis', 'PIT'))
849
        simIn.models.PitchAxis = idpoly(tf(1, [1 1], simIn.param.dt));
850
    else
851
        model = findobj(idModel, 'axis', 'PIT');
852
        % If more than one model is found, let user choose
853
        if length(model) > 1 
854
            mdlIdx = input('More than one model found for the pitch axis. Input index of desired model: ');
855
        else
856
            mdlIdx = 1;
857
        end
858
        simIn.models.PitchAxis = idss(c2d(model(mdlIdx).tfModel, simIn.param.dt));
859
        simIn.models.PitchAxis.InputDelay = simIn.models.PitchAxis.InputDelay-1;
860
    end
861
    % Yaw
862
    if isempty(findobj(idModel, 'axis', 'YAW'))
863
        simIn.models.YawAxis = idpoly(tf(1, [1 1], simIn.param.dt));
864
    else
865
        model = findobj(idModel, 'axis', 'YAW');
866
        % If more than one model is found, let user choose
867
        if length(model) > 1 
868
            mdlIdx = input('More than one model found for the yaw axis. Input index of desired model: ');
869
        else
870
            mdlIdx = 1;
871
        end
872
        simIn.models.YawAxis = idss(c2d(model(mdlIdx).tfModel, simIn.param.dt));
873
        simIn.models.YawAxis.InputDelay = simIn.models.YawAxis.InputDelay-1;
874
    end
875
    % Vertical Motion - Throttle
876
    if isempty(findobj(idModel, 'axis', 'THR'))
877
        simIn.models.RollAxis = idpoly(tf(1, [1 1], simIn.param.dt));
878
    else
879
        model = findobj(idModel, 'axis', 'THR');
880
        % If more than one model is found, let user choose
881
        if length(model) > 1
882
            mdlIdx = input('More than one model found for the vertical axis. Input index of desired model: ');
883
        else
884
            mdlIdx = 1;
885
        end
886
        simIn.models.VerticalAxis = idss(c2d(model(mdlIdx).tfModel, simIn.param.dt));
887
        % Decrease delay to account for the additional feedback delay
888
        simIn.models.VerticalAxis.InputDelay = simIn.models.VerticalAxis.InputDelay-1;
889
    end
890
else % Create dummy models of optimization is not active
891
    simIn.models.RollAxis = idpoly(tf(1, [1 1], simIn.param.dt));
892
    simIn.models.PitchAxis = idpoly(tf(1, [1 1], simIn.param.dt));
893
    simIn.models.YawAxis = idpoly(tf(1, [1 1], simIn.param.dt));
894
    simIn.models.VerticalAxis = idpoly(tf(1, [1 1], simIn.param.dt));
895
end
896

897
% Initial values
898
if simIn.param.mdlVal && mode == 25
899
    simIn.models.RollInit = single(sid.SIDD.Gx(1)*pi/180);
900
    simIn.models.PitchInit = single(sid.SIDD.Gy(1)*pi/180);
901
    simIn.models.YawInit = single(sid.SIDD.Gz(1)*pi/180);
902
    simIn.models.VerticalOutInit = single(sid.SIDD.Az(1));
903
    simIn.models.VerticalInInit = single(sid.RATE.AOut(1));
904
else
905
    simIn.models.RollInit = single(0);
906
    simIn.models.PitchInit = single(0);
907
    simIn.models.YawInit = single(0);
908
    simIn.models.VerticalOutInit = single(-9.81);
909
    simIn.models.VerticalInInit = single(sid.RATE.AOut(1));
910
end
911

912
%% Delete all irrelevant data
913
clear i dist start thr_man rel_msg mode param_names msgs tEnd tStart simMode
914
clear phase_des runOutsideLoop mdlIdx pitchCh rollCh yawCh iVec Fs axis dt
915
clear simMode thrCh 
916

917
Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.

Product

Resources

Company

Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more, all in one place. Commercial Alternative to JupyterHub.

Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.
