Skip to content

Commit

Permalink
Copter: Simulink Model and init scripts for Copter-4.2
Browse files Browse the repository at this point in the history 
- arducopter.slx: Simulates ArduCopter Stabilize and Althold controller and optional plant model
- sid_pre.m: Loads *.bin files to Matlab structures
- sid_sim_init.m: Loads signals and parameters from Matlab structure into Simulink model
  • Loading branch information
@fbredeme @tridge
fbredeme authored and tridge committed Oct 12, 2022
1 parent a480c0a commit 6a69464
Show file tree
Hide file tree
Showing 12 changed files with 2,162 additions and 0 deletions.
17 changes: 17 additions & 0 deletions Tools/simulink/arducopter/README.md
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,17 @@
# 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.

## 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
Binary file added Tools/simulink/arducopter/arducopter.slx
View file
Binary file not shown.
201 changes: 201 additions & 0 deletions Tools/simulink/arducopter/functions/ModeConfig.m
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,201 @@
% This class provides important setting information for the flight modes
% that have been chosen for the SID process,

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'};
% 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
38 changes: 38 additions & 0 deletions Tools/simulink/arducopter/functions/demodYawAngle.m
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,38 @@
function [sigOut] = demodYawAngle(sigIn)
% Inverse the modulo of 360° operation used on the yaw angle signals

% 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

123 changes: 123 additions & 0 deletions Tools/simulink/arducopter/functions/getMotorMixerFactors.m
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,123 @@
% Function to get the copters number of motors and there configuration to
% calculate the motor mixer factors for roll, pitch and yaw
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
Loading

0 comments on commit 6a69464

Please sign in to comment.