├── .gitignore ├── .gitmodules ├── README.md ├── docs ├── Nonlinear Quadrotor Attitude Control - Technical Report.pdf ├── iris.jpg ├── model.png ├── px4.png ├── px4_model.PNG └── quadrotor dynamic and control thesis.pdf ├── setup.m └── src ├── ControlModel.slx ├── busses.mat ├── load_controller.m ├── load_plant.m ├── px4_6dof_quadrotor.slx ├── px4_6dof_quadrotor_bus.slx ├── px4_6dof_quadrotor_simscape.slx ├── quadcopter_parameters.m └── utils ├── lag.m ├── lead.m ├── notch.m └── quatVect.m /.gitignore: -------------------------------------------------------------------------------- 1 | *.slxc 2 | slprj/ 3 | src/+*/ -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "Quadcopter-Drone-Model-Simscape"] 2 | path = Quadcopter-Drone-Model-Simscape 3 | url = https://github.com/mathworks/Quadcopter-Drone-Model-Simscape.git 4 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # General Info 2 | 3 | This repository contains Simulink models of 6DOF systems based on [px4 autopilot](https://github.com/PX4/PX4-Autopilot) control system. 4 | 5 | ![](docs/px4_model.PNG) 6 | 7 | # Table of contents 8 | 9 | 1. [Installation](#installation) 10 | 2. [6DOF Quadcopter](#6dof-quadcopter) 11 | 3. [References](#references) 12 | 13 | # Installation 14 | 15 | All models built and tested on [Matlab/Simulink](https://www.mathworks.com/products/new_products/latest_features.html) 2023a. 16 | 17 | Clone the repository to your PC, add the folder to Matlab path and open the model. 18 | 19 | 20 | # 6DOF Quadrotor 21 | 22 | ![](docs/model.png) 23 | 24 | * `px4_6dof_quadrotor_simscape.slx` : 25 | 26 | The 6DOF model based on [this](https://github.com/mathworks/Quadcopter-Drone-Model-Simscape) Github repository. 27 | 28 | TODO 29 | - [x] Add attitude, position and velocity controllers. 30 | - [x] Convert signals to bus elements. 31 | - [ ] Add INS model. 32 | - [ ] Add EKF based on px4 architecture. 33 | - [ ] Add visualization to the simulation. 34 | - [x] Add motor models instead of the approximate ones. 35 | 36 | # References 37 | 38 | 1. [PX4-Autopilot on github](https://github.com/PX4/PX4-Autopilot) 39 | 2. [PX4 Attitude controller schematics](https://www.researchgate.net/figure/Position-and-attitude-controller-structure-of-a-Px4-based-UAV-33_fig3_341902425) 40 | 3. [MAVROS Controllers](https://github.com/Jaeyoung-Lim/mavros_controllers) 41 | 4. [Quadrotor plant and control on simulink](http://www.ritravvenlab.com/uploads/1/1/8/4/118484574/ferry.pdf) 42 | 5. [Quadcopter-Drone-Model-Simscape](https://github.com/mathworks/Quadcopter-Drone-Model-Simscape) 43 | 44 | 45 | 46 | -------------------------------------------------------------------------------- /docs/Nonlinear Quadrotor Attitude Control - Technical Report.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/iftahnaf/px4_simulink_simulator/486410ad588d43888ce81d23c0871ef20234c1a7/docs/Nonlinear Quadrotor Attitude Control - Technical Report.pdf -------------------------------------------------------------------------------- /docs/iris.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/iftahnaf/px4_simulink_simulator/486410ad588d43888ce81d23c0871ef20234c1a7/docs/iris.jpg -------------------------------------------------------------------------------- /docs/model.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/iftahnaf/px4_simulink_simulator/486410ad588d43888ce81d23c0871ef20234c1a7/docs/model.png -------------------------------------------------------------------------------- /docs/px4.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/iftahnaf/px4_simulink_simulator/486410ad588d43888ce81d23c0871ef20234c1a7/docs/px4.png -------------------------------------------------------------------------------- /docs/px4_model.PNG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/iftahnaf/px4_simulink_simulator/486410ad588d43888ce81d23c0871ef20234c1a7/docs/px4_model.PNG -------------------------------------------------------------------------------- /docs/quadrotor dynamic and control thesis.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/iftahnaf/px4_simulink_simulator/486410ad588d43888ce81d23c0871ef20234c1a7/docs/quadrotor dynamic and control thesis.pdf -------------------------------------------------------------------------------- /setup.m: -------------------------------------------------------------------------------- 1 | % Setup px4_6dof_quadrotor_x simulator 2 | 3 | %% Setup environment 4 | clear 5 | clc 6 | close all 7 | 8 | dir_path=pwd; 9 | 10 | os_type = ispc; 11 | 12 | if ~os_type 13 | backslash_vec=strfind(dir_path,'/'); 14 | else 15 | backslash_vec=strfind(dir_path,'\'); 16 | end 17 | 18 | dirnames={'px4_simulink_simulator'}; % change according to your needs 19 | for i=1:numel(dirnames) 20 | addpath(genpath([dir_path(1:backslash_vec(end)),dirnames{i}])); 21 | end 22 | 23 | clear dir_path backslash_vec dirnames i os_type 24 | %% Plant parameters 25 | load_plant; 26 | 27 | %% Control gains 28 | load_controller; 29 | 30 | %% Open main model 31 | load('busses.mat') 32 | open('px4_6dof_quadrotor_bus.slx') -------------------------------------------------------------------------------- /src/ControlModel.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/iftahnaf/px4_simulink_simulator/486410ad588d43888ce81d23c0871ef20234c1a7/src/ControlModel.slx -------------------------------------------------------------------------------- /src/busses.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/iftahnaf/px4_simulink_simulator/486410ad588d43888ce81d23c0871ef20234c1a7/src/busses.mat -------------------------------------------------------------------------------- /src/load_controller.m: -------------------------------------------------------------------------------- 1 | controller.rate.roll.k = 0.01; 2 | controller.rate.roll.p = 0.044; 3 | controller.rate.roll.i = 0.05; 4 | controller.rate.roll.d = 0.0012; 5 | controller.rate.roll.int_lim = 101; 6 | controller.rate.roll.max = 10.0; 7 | controller.rate.pitch.k = 0.01; 8 | controller.rate.pitch.p = 0.044; 9 | controller.rate.pitch.i = 0.05; 10 | controller.rate.pitch.d = 0.0012; 11 | controller.rate.pitch.int_lim = 100; 12 | controller.rate.pitch.max = 2.0; 13 | controller.rate.yaw.k = 0.01; 14 | controller.rate.yaw.p = 0.044; 15 | controller.rate.yaw.i = 0.05; 16 | controller.rate.yaw.d = 0.0012; 17 | controller.rate.yaw.int_lim = 100; 18 | controller.rate.yaw.max = 2.0; 19 | controller.rate.time_sample = 1/250; 20 | controller.rate.mc_rr_int_limit = 1000; 21 | controller.rate.mc_pr_int_limit = 1000; 22 | controller.rate.mc_yr_int_limit = 1000; 23 | controller.filters.gyro_cutoff = 0; 24 | controller.filters.dgyro_cutoff = 0; 25 | 26 | 27 | controller.attitude.roll.p = 4.0; 28 | controller.attitude.pitch.p = 4.0; 29 | controller.attitude.yaw.p = 4.0; 30 | 31 | controller.velocity.xy.p = 1.0; 32 | controller.velocity.xy.i = 0.1; 33 | controller.velocity.xy.d = 0.01; 34 | controller.velocity.xy.max = 10; 35 | controller.velocity.z.p = 1.0; 36 | controller.velocity.z.i = 0.1; 37 | controller.velocity.z.d = 0.01; 38 | controller.velocity.z.max_up = 100.0; 39 | controller.velocity.z.max_down = -100.0; 40 | 41 | controller.position.xy.p = 1.0; 42 | controller.position.z.p = 1.0; 43 | controller.mixer.matrix = [plant.mechanical.c_t plant.mechanical.c_t plant.mechanical.c_t plant.mechanical.c_t;... 44 | plant.mechanical.c_t*plant.mechanical.d*sind(45) -plant.mechanical.c_t*plant.mechanical.d*sind(45) -plant.mechanical.c_t*plant.mechanical.d*sind(45) plant.mechanical.c_t*plant.mechanical.d*sind(45);... 45 | -plant.mechanical.c_t*plant.mechanical.d*sind(45) -plant.mechanical.c_t*plant.mechanical.d*sind(45) plant.mechanical.c_t*plant.mechanical.d*sind(45) plant.mechanical.c_t*plant.mechanical.d*sind(45);... 46 | plant.mechanical.c_q -plant.mechanical.c_q plant.mechanical.c_q -plant.mechanical.c_q]; 47 | 48 | controller.attitude.time_sample = 1/250; -------------------------------------------------------------------------------- /src/load_plant.m: -------------------------------------------------------------------------------- 1 | plant.motor.r = 0.127; % [ohm] - resistor 2 | plant.motor.l = 1.6968e-4; % [H] - inductor 3 | plant.motor.k_t = 1/104.7197; % [N*m/A] - torque constant 4 | plant.motor.k_b = 1/104.7197; % [V/rad/sec] - back EMF constant 5 | plant.motor.j_m = 3.38e-7; % [kg*m^2] - shaft inertia 6 | plant.motor.max_v = 12.6; % [V] - maximum voltage 7 | plant.motor.max_i = 30.0; % [A] - maximum amp 8 | plant.motor.k_k = 1000; % propeller static flexability 9 | plant.motor.k_v = 1; % propeller dynamic flexability 10 | plant.motor.j_l = 2.8e-6; % propeller inertia 11 | plant.motor.dynamic_time_sample = 1e-5; % time sample for the dynamic part of simulation 12 | plant.motor.electric_time_sample = 1e-3; % time sample for electirc part of simulation 13 | 14 | plant.mechanical.c_t = 5.947e-6; % thrust constant 15 | plant.mechanical.c_q = 7.9217e-7; % drag constant 16 | plant.mechanical.d = 0.165; % quad arm length 17 | plant.mechanical.i_xx = 0.00115118; % i_xx on the diagonal inertial matrix 18 | plant.mechanical.i_yy = 0.00165651; % i_yy on the diagonal inertial matrix 19 | plant.mechanical.i_zz = 0.00190742; % i_zz on the diagonal inertial matrix 20 | plant.mechanical.j_l = plant.motor.j_l; 21 | plant.mechanical.m = 0.9; % [kg] quadrotor mass 22 | plant.mechanical.a_x = 0.007; % [kg/s] drag coefficient - x axis 23 | plant.mechanical.a_y = 0.007; % [kg/s] drag coefficient - y axis 24 | plant.mechanical.a_z = 0.007; % [kg/s] drag coefficient - z axis 25 | 26 | plant.ic.position.x = 0.0; 27 | plant.ic.position.y = 0.0; 28 | plant.ic.position.z = -10.0; 29 | plant.ic.velocity.x = 0.0; 30 | plant.ic.velocity.y = 0.0; 31 | plant.ic.velocity.z = 0.0; 32 | 33 | plant.general.g = 9.81; % gravity -------------------------------------------------------------------------------- /src/px4_6dof_quadrotor.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/iftahnaf/px4_simulink_simulator/486410ad588d43888ce81d23c0871ef20234c1a7/src/px4_6dof_quadrotor.slx -------------------------------------------------------------------------------- /src/px4_6dof_quadrotor_bus.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/iftahnaf/px4_simulink_simulator/486410ad588d43888ce81d23c0871ef20234c1a7/src/px4_6dof_quadrotor_bus.slx -------------------------------------------------------------------------------- /src/px4_6dof_quadrotor_simscape.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/iftahnaf/px4_simulink_simulator/486410ad588d43888ce81d23c0871ef20234c1a7/src/px4_6dof_quadrotor_simscape.slx -------------------------------------------------------------------------------- /src/quadcopter_parameters.m: -------------------------------------------------------------------------------- 1 | % Parameters for quadcopter_package_delivery 2 | % Copyright 2021-2023 The MathWorks, Inc. 3 | 4 | % Size of the ground 5 | planex = 12.5; % m 6 | planey = 8.5; % m 7 | planedepth = 0.2; % m, distance from plane to the reference frame 8 | 9 | % Battery Capacity 10 | battery_capacity = 7.6*3; 11 | 12 | %% Material Property 13 | % Assuming the arm of the drone is manufractured by 3D Printing, the ideal 14 | % material is PLA, safe, light and cheap, the only concern is its thermal 15 | % property 16 | rho_pla = 1.25; % g/cm^3 17 | 18 | % Measured drone mass 19 | drone_mass = 1.2726; 20 | %% package ground contact properties 21 | pkgGrndStiff = 1000; 22 | pkgGrndDamp = 300; 23 | pkgGrndTransW = 1e-3; 24 | 25 | 26 | %% Package parameters 27 | pkgSize = [1 1 1]*0.14; % m 28 | pkgDensity = 1/(pkgSize(1)*pkgSize(2)*pkgSize(3)); % kg/m^3 29 | 30 | %% Propeller parameters 31 | propeller.diameter = 0.254; % m 32 | propeller.Kthrust = 0.1072; 33 | propeller.Kdrag = 0.01; 34 | 35 | air_rho = 1.225; % kg/m^3 36 | air_temperature = 273+25; % degK 37 | wind_speed = 0; % Wind speed (m/s) 38 | 39 | %% Leg parameters 40 | drone_leg.Extr_Data = flipud([... 41 | 0 0; 42 | 0.5 0; 43 | 1 -1; 44 | 0.98 -1; 45 | 0.5 -0.02; 46 | -0.5 -0.02; 47 | -0.98 -1; 48 | -1 -1; 49 | -0.5 0].*[1 1]*0.15); 50 | 51 | drone_leg.width = 0.01; 52 | 53 | %% Motor parameters 54 | qc_motor.max_torque = 0.8; % N*m 55 | qc_motor.max_power = 160; % W 56 | qc_motor.time_const = 0.02; % sec 57 | qc_motor.efficiency = 25/30*100; % 0-100 58 | qc_motor.efficiency_spd = 5000; % rpm 59 | qc_motor.efficiency_trq = 0.05; % N*m 60 | qc_motor.rotor_damping = 1e-7; % N*m/(rad/s) 61 | 62 | qc_max_power = qc_motor.max_power; 63 | 64 | %% Controller parameters 65 | filtM_position = 0.005; 66 | kp_position = 8; 67 | ki_position = 0.04; 68 | kd_position = 3.2; 69 | filtD_position = 100; 70 | pos2attitude = 2.4; 71 | 72 | filtM_attitude = 0.01; 73 | kp_attitude = 128.505; 74 | ki_attitude = 5.9203; 75 | kd_attitude = 78.2000*2; 76 | filtD_attitude = 1000; 77 | limit_attitude = 800; 78 | 79 | filtM_yaw = 0.01; 80 | kp_yaw = 25.7010*4*2; 81 | ki_yaw = 5.9203*0.01; 82 | kd_yaw = 78.2000*0.01; 83 | filtD_yaw = 100; 84 | limit_yaw = 20; 85 | 86 | filtM_altitude = 0.05; 87 | kp_altitude = 0.27; 88 | ki_altitude = 0.07; 89 | kd_altitude = 0.35; 90 | filtD_altitude = 10000; 91 | limit_altitude = 10; 92 | 93 | kp_motor = 0.00375; 94 | ki_motor = 4.50000e-4; 95 | kd_motor = 0; 96 | filtD_motor = 10000; 97 | filtSpd_motor = 0.001; 98 | limit_motor = 0.25; 99 | 100 | %% Drag coefficients 101 | qd_drag.Cd_X = 0.35; 102 | qd_drag.Cd_Y = 0.35; 103 | qd_drag.Cd_Z = 0.6; 104 | qd_drag.Roll = 0.2; 105 | qd_drag.Pitch = 0.2; 106 | qd_drag.Yaw = 0.2; 107 | qd_area.YZ = 0.0875; 108 | qd_area.XZ = 0.0900; 109 | qd_area.XY = 0.2560; 110 | qd_area.Roll = qd_area.XY*2; 111 | qd_area.Pitch = qd_area.XY*2; 112 | qd_area.Yaw = qd_area.XY; 113 | 114 | -------------------------------------------------------------------------------- /src/utils/lag.m: -------------------------------------------------------------------------------- 1 | function Clag = lag(Freq,beta) 2 | %LAG returns a QCTRL lag compensator 3 | % 4 | % Usage: 5 | % 6 | % clag = QCTRL.LAG(Phase,W) returns a 1st oreder lag compensator 7 | % with a given phase-lag at frequeny W. 8 | % 9 | if nargin==2 % order=1 10 | s = tf('s'); 11 | Clag = (10*s+Freq)/(10*s+Freq/beta); 12 | else 13 | error('incorrect number of arguments') 14 | end 15 | end -------------------------------------------------------------------------------- /src/utils/lead.m: -------------------------------------------------------------------------------- 1 | function Clead = lead(Phase,Freq,Damping) 2 | %LEAD returns a QCTRL lead compensator 3 | % 4 | % Usage: 5 | % 6 | % clead = QCTRL.LEAD(Phase,W) returns a 1st oreder lead compensator 7 | % with a given phase-lead at frequeny W. 8 | % 9 | % clead = QCTRL.LEAD(Phase,W,damping) returns a 2nd order lead compensator 10 | % with a given phase-lead at frequeny W and specified damping 11 | 12 | if nargin == 2 13 | Damping = -1; 14 | end 15 | 16 | if nargin ~= 2 && nargin ~=3 17 | error('Wrong number of input arguments') 18 | end 19 | 20 | wm = Freq; 21 | Pm = Phase*pi/180; 22 | if Damping == -1 % order=1 23 | p = -wm*sqrt((1+sin(Pm))/(1-sin(Pm))); 24 | z = -wm/sqrt((1+sin(Pm))/(1-sin(Pm))); 25 | Clead = zpk(z,p,1); 26 | else 27 | zeta=Damping; 28 | if zeta>=1 || zeta<=0 29 | error('damping ratio must be between 0 and 1') 30 | end 31 | wz = wm*(-zeta*tan(Pm)+sqrt(zeta^2)*tan(Pm)^2+1); 32 | wp = wm*(zeta*tan(Pm)+sqrt(zeta^2)*tan(Pm)^2+1); 33 | z = [-zeta*wz+1j*wz*sqrt(1-zeta^2) -zeta*wz-1j*wz*sqrt(1-zeta^2)]; 34 | p = [-zeta*wp+1j*wp*sqrt(1-zeta^2) -zeta*wp-1j*wp*sqrt(1-zeta^2)]; 35 | Clead = zpk(z,p,1); 36 | end 37 | end 38 | -------------------------------------------------------------------------------- /src/utils/notch.m: -------------------------------------------------------------------------------- 1 | function Cnotch = notch(wn,wd,zn,zd) 2 | %NOTCH returns a QCTRL notch compensator 3 | % 4 | % Usage: 5 | % 6 | % cnotch = QCTRL.NOTCH(wn,wd,zn,zd) returns a notch compensator 7 | % at a given frequency. 8 | % 9 | % notch:zd > zn 10 | % anti-notch : zn>zd 11 | % skew notch: wn != wd 12 | 13 | if nargin==4 14 | zero_1 = -zn*wn + sqrt((wn^2)*(zn-1)); 15 | zero_2 = -zn*wn - sqrt((wn^2)*(zn-1)); 16 | pole_1 = -zd*wd + sqrt((wd^2)*(zd-1)); 17 | pole_2 = -zd*wd - sqrt((wd^2)*(zd-1)); 18 | zeros = [zero_1 zero_2]; 19 | poles = [pole_1 pole_2]; 20 | Cnotch = zpk(zeros,poles,1); 21 | else 22 | error('incorrect number of arguments') 23 | end -------------------------------------------------------------------------------- /src/utils/quatVect.m: -------------------------------------------------------------------------------- 1 | function rotMat = quatVect(acc,des_yaw) %#codegen 2 | %this function calculates the quaternion based on acceleration command and 3 | %desired yaw angle. based on 4 | %https://github.com/Jaeyoung-Lim/mavros_controllers/blob/master/geometric_controller/src/geometric_controller.cpp, 5 | % because of code generation limitations, the output is the rotation 6 | % matrix, then it needs to be converter to quaternions with rot2quat block. 7 | %Iftach Naftaly, 12.2020 8 | 9 | proj_xb_des = [cosd(des_yaw);sind(des_yaw);0]; 10 | 11 | zb_des = acc/norm(acc); 12 | yb_des = cross(zb_des,proj_xb_des)/norm(cross(zb_des,proj_xb_des)); 13 | xb_des = cross(yb_des,zb_des)/norm(cross(yb_des,zb_des)); 14 | 15 | 16 | rotMat = [xb_des;yb_des;zb_des]; 17 | 18 | 19 | 20 | end --------------------------------------------------------------------------------