├── Gravity_Forces.m ├── InitialCondition.m ├── LICENSE ├── Quadrotor_Aerodynamics.m ├── Quadrotor_Controller.m ├── Quadrotor_Navigation.m ├── Quadrotor_Simualtion.m ├── RigidBodyDynamics.m ├── auxGenerarTrayectoria.m ├── clipWUM7wxO1uRY.png ├── modelo2.mdl ├── modelo2Initialization.m ├── modelo2_fs_joy.mdl ├── modelo2_fs_step.mdl ├── modelo2a.slx ├── modelo2variable_step.mdl ├── qrsim2-block-diagram.png ├── readme.md ├── slQuadrotorController.m ├── slQuadrotorControllerInitialization.m ├── slQuadrotorNavigation.m ├── slQuadrotorPlanta.m ├── slQuadrotorPlantaInitialization.m └── slToFlightgear.mdl /Gravity_Forces.m: -------------------------------------------------------------------------------- 1 | % Outputs gravity force expressed in Body fixed frame 2 | % g acceleration due to gravity (kg.m/s^2) 3 | 4 | function [Gravity_Force] = Gravity_Forces(Atmosphere, Quadrotor, xi); 5 | 6 | g = Atmosphere.g; 7 | 8 | phi = xi(7,1); 9 | theta = xi(8,1); 10 | psi = xi(9,1); 11 | 12 | mass = Quadrotor.Mass_prop.Mass; 13 | 14 | Gravity_Force = mass*g*[-sin(theta) sin(phi)*cos(theta) cos(theta)*cos(phi)]; 15 | Gravity_Force = mass*g*[-sin(theta) sin(phi)*cos(theta) cos(theta)*cos(phi)]; -------------------------------------------------------------------------------- /InitialCondition.m: -------------------------------------------------------------------------------- 1 | function X0 = InitialCondition(IC, r_CG_O) 2 | % I = Inertial frame 3 | % B = Body fixed frame 4 | 5 | % IC.position = [Pos_x, Pos_y, Pos_z]; % XYZ position of origin of B w.r.t. I, expressed in I (m) 6 | % IC.orientation = [Roll, Pitch, Yaw]; % Roll, pitch and Yaw Euler angles, of B w.r.t. I (deg) 7 | % IC.velocity = [Vel_x, Vel_y, Vel_z]; % Velocity of origin of B w.r.t. I (m/s). ICVelOption selects in which frame the velocity is expressed. ICVelOption selects in which frame the velocity is expressed 8 | % IC.angular_velocity = [AngVel_x, AngVel_y, AngVel_z]; % Angular velocity of B w.r.t. I, expressed in I (deg/s) 9 | % IC.VelOption = 'Inertial'; % Choose in what way you want to define IC velocity - 'Inertial': velocity vector expressed in Inertial frame, 'Body': velocity vector expressed in Body frame, 'WindAngles': express it in terms of absolute velocity and wind angles (Vinf, alfa, beta) 10 | % IC.Vel = 30; % Absolute velocity (m/s) 11 | % IC.alfa = 0; % Angle of attack (deg) 12 | % IC.beta = 0; % Side slip angle (deg) 13 | 14 | 15 | phi = IC.orientation(1,1)*pi/180; 16 | theta = IC.orientation(1,2)*pi/180; 17 | psi = IC.orientation(1,3)*pi/180; 18 | 19 | % Building rotation matrix from Body to Inertial frame 20 | c_psi = cos(psi); s_psi = sin(psi); 21 | c_theta = cos(theta); s_theta = sin(theta); 22 | c_phi = cos(phi); s_phi = sin(phi); 23 | 24 | 25 | % Rotation matrix from B to N 26 | b_R_n = [c_psi*c_theta (-s_psi*c_phi+s_theta*s_phi*c_psi) s_psi*s_phi+s_theta*c_psi*c_phi; 27 | s_psi*c_theta (c_psi*c_phi +s_psi*s_theta*s_phi) -s_phi*c_psi+s_psi*s_theta*c_phi; 28 | -s_theta s_phi*c_theta c_theta*c_phi ]; 29 | 30 | switch IC.VelOption 31 | case 'Inertial' 32 | X0(1:3) = IC.position + r_CG_O; 33 | X0(4:6) = (b_R_n'*IC.velocity')' + cross(IC.angular_velocity*pi/180,r_CG_O); 34 | X0(7:9) = IC.orientation*pi/180; 35 | X0(10:12) = IC.angular_velocity*pi/180; 36 | case 'Body' 37 | X0(1:3) = IC.position + r_CG_O; 38 | X0(4:6) = IC.velocity + cross(IC.angular_velocity*pi/180,r_CG_O); 39 | X0(7:9) = IC.orientation*pi/180; 40 | X0(10:12) = IC.angular_velocity*pi/180; 41 | 42 | case 'WindAngles' 43 | alfa = IC.alfa*pi/180; 44 | beta = IC.beta*pi/180; 45 | 46 | Uinf_0 = IC.Vel*cos(alfa)*cos(beta); 47 | Vinf_0 = -IC.Vel*sin(beta); 48 | Winf_0 = IC.Vel*sin(alfa)*cos(beta); 49 | 50 | X0(1:3) = IC.position + r_CG_O; 51 | X0(4:6) = [Uinf_0 Vinf_0 Winf_0] + cross(IC.angular_velocity*pi/180,r_CG_O); 52 | X0(7:9) = IC.orientation*pi/180; 53 | X0(10:12) = IC.angular_velocity*pi/180; 54 | end 55 | 56 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | GNU GENERAL PUBLIC LICENSE 2 | Version 2, June 1991 3 | 4 | Copyright (C) 1989, 1991 Free Software Foundation, Inc., 5 | 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 6 | Everyone is permitted to copy and distribute verbatim copies 7 | of this license document, but changing it is not allowed. 8 | 9 | Preamble 10 | 11 | The licenses for most software are designed to take away your 12 | freedom to share and change it. By contrast, the GNU General Public 13 | License is intended to guarantee your freedom to share and change free 14 | software--to make sure the software is free for all its users. This 15 | General Public License applies to most of the Free Software 16 | Foundation's software and to any other program whose authors commit to 17 | using it. (Some other Free Software Foundation software is covered by 18 | the GNU Lesser General Public License instead.) You can apply it to 19 | your programs, too. 20 | 21 | When we speak of free software, we are referring to freedom, not 22 | price. Our General Public Licenses are designed to make sure that you 23 | have the freedom to distribute copies of free software (and charge for 24 | this service if you wish), that you receive source code or can get it 25 | if you want it, that you can change the software or use pieces of it 26 | in new free programs; and that you know you can do these things. 27 | 28 | To protect your rights, we need to make restrictions that forbid 29 | anyone to deny you these rights or to ask you to surrender the rights. 30 | These restrictions translate to certain responsibilities for you if you 31 | distribute copies of the software, or if you modify it. 32 | 33 | For example, if you distribute copies of such a program, whether 34 | gratis or for a fee, you must give the recipients all the rights that 35 | you have. You must make sure that they, too, receive or can get the 36 | source code. And you must show them these terms so they know their 37 | rights. 38 | 39 | We protect your rights with two steps: (1) copyright the software, and 40 | (2) offer you this license which gives you legal permission to copy, 41 | distribute and/or modify the software. 42 | 43 | Also, for each author's protection and ours, we want to make certain 44 | that everyone understands that there is no warranty for this free 45 | software. If the software is modified by someone else and passed on, we 46 | want its recipients to know that what they have is not the original, so 47 | that any problems introduced by others will not reflect on the original 48 | authors' reputations. 49 | 50 | Finally, any free program is threatened constantly by software 51 | patents. We wish to avoid the danger that redistributors of a free 52 | program will individually obtain patent licenses, in effect making the 53 | program proprietary. To prevent this, we have made it clear that any 54 | patent must be licensed for everyone's free use or not licensed at all. 55 | 56 | The precise terms and conditions for copying, distribution and 57 | modification follow. 58 | 59 | GNU GENERAL PUBLIC LICENSE 60 | TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION 61 | 62 | 0. This License applies to any program or other work which contains 63 | a notice placed by the copyright holder saying it may be distributed 64 | under the terms of this General Public License. The "Program", below, 65 | refers to any such program or work, and a "work based on the Program" 66 | means either the Program or any derivative work under copyright law: 67 | that is to say, a work containing the Program or a portion of it, 68 | either verbatim or with modifications and/or translated into another 69 | language. (Hereinafter, translation is included without limitation in 70 | the term "modification".) Each licensee is addressed as "you". 71 | 72 | Activities other than copying, distribution and modification are not 73 | covered by this License; they are outside its scope. The act of 74 | running the Program is not restricted, and the output from the Program 75 | is covered only if its contents constitute a work based on the 76 | Program (independent of having been made by running the Program). 77 | Whether that is true depends on what the Program does. 78 | 79 | 1. You may copy and distribute verbatim copies of the Program's 80 | source code as you receive it, in any medium, provided that you 81 | conspicuously and appropriately publish on each copy an appropriate 82 | copyright notice and disclaimer of warranty; keep intact all the 83 | notices that refer to this License and to the absence of any warranty; 84 | and give any other recipients of the Program a copy of this License 85 | along with the Program. 86 | 87 | You may charge a fee for the physical act of transferring a copy, and 88 | you may at your option offer warranty protection in exchange for a fee. 89 | 90 | 2. You may modify your copy or copies of the Program or any portion 91 | of it, thus forming a work based on the Program, and copy and 92 | distribute such modifications or work under the terms of Section 1 93 | above, provided that you also meet all of these conditions: 94 | 95 | a) You must cause the modified files to carry prominent notices 96 | stating that you changed the files and the date of any change. 97 | 98 | b) You must cause any work that you distribute or publish, that in 99 | whole or in part contains or is derived from the Program or any 100 | part thereof, to be licensed as a whole at no charge to all third 101 | parties under the terms of this License. 102 | 103 | c) If the modified program normally reads commands interactively 104 | when run, you must cause it, when started running for such 105 | interactive use in the most ordinary way, to print or display an 106 | announcement including an appropriate copyright notice and a 107 | notice that there is no warranty (or else, saying that you provide 108 | a warranty) and that users may redistribute the program under 109 | these conditions, and telling the user how to view a copy of this 110 | License. (Exception: if the Program itself is interactive but 111 | does not normally print such an announcement, your work based on 112 | the Program is not required to print an announcement.) 113 | 114 | These requirements apply to the modified work as a whole. If 115 | identifiable sections of that work are not derived from the Program, 116 | and can be reasonably considered independent and separate works in 117 | themselves, then this License, and its terms, do not apply to those 118 | sections when you distribute them as separate works. But when you 119 | distribute the same sections as part of a whole which is a work based 120 | on the Program, the distribution of the whole must be on the terms of 121 | this License, whose permissions for other licensees extend to the 122 | entire whole, and thus to each and every part regardless of who wrote it. 123 | 124 | Thus, it is not the intent of this section to claim rights or contest 125 | your rights to work written entirely by you; rather, the intent is to 126 | exercise the right to control the distribution of derivative or 127 | collective works based on the Program. 128 | 129 | In addition, mere aggregation of another work not based on the Program 130 | with the Program (or with a work based on the Program) on a volume of 131 | a storage or distribution medium does not bring the other work under 132 | the scope of this License. 133 | 134 | 3. You may copy and distribute the Program (or a work based on it, 135 | under Section 2) in object code or executable form under the terms of 136 | Sections 1 and 2 above provided that you also do one of the following: 137 | 138 | a) Accompany it with the complete corresponding machine-readable 139 | source code, which must be distributed under the terms of Sections 140 | 1 and 2 above on a medium customarily used for software interchange; or, 141 | 142 | b) Accompany it with a written offer, valid for at least three 143 | years, to give any third party, for a charge no more than your 144 | cost of physically performing source distribution, a complete 145 | machine-readable copy of the corresponding source code, to be 146 | distributed under the terms of Sections 1 and 2 above on a medium 147 | customarily used for software interchange; or, 148 | 149 | c) Accompany it with the information you received as to the offer 150 | to distribute corresponding source code. (This alternative is 151 | allowed only for noncommercial distribution and only if you 152 | received the program in object code or executable form with such 153 | an offer, in accord with Subsection b above.) 154 | 155 | The source code for a work means the preferred form of the work for 156 | making modifications to it. For an executable work, complete source 157 | code means all the source code for all modules it contains, plus any 158 | associated interface definition files, plus the scripts used to 159 | control compilation and installation of the executable. However, as a 160 | special exception, the source code distributed need not include 161 | anything that is normally distributed (in either source or binary 162 | form) with the major components (compiler, kernel, and so on) of the 163 | operating system on which the executable runs, unless that component 164 | itself accompanies the executable. 165 | 166 | If distribution of executable or object code is made by offering 167 | access to copy from a designated place, then offering equivalent 168 | access to copy the source code from the same place counts as 169 | distribution of the source code, even though third parties are not 170 | compelled to copy the source along with the object code. 171 | 172 | 4. You may not copy, modify, sublicense, or distribute the Program 173 | except as expressly provided under this License. Any attempt 174 | otherwise to copy, modify, sublicense or distribute the Program is 175 | void, and will automatically terminate your rights under this License. 176 | However, parties who have received copies, or rights, from you under 177 | this License will not have their licenses terminated so long as such 178 | parties remain in full compliance. 179 | 180 | 5. You are not required to accept this License, since you have not 181 | signed it. However, nothing else grants you permission to modify or 182 | distribute the Program or its derivative works. These actions are 183 | prohibited by law if you do not accept this License. Therefore, by 184 | modifying or distributing the Program (or any work based on the 185 | Program), you indicate your acceptance of this License to do so, and 186 | all its terms and conditions for copying, distributing or modifying 187 | the Program or works based on it. 188 | 189 | 6. Each time you redistribute the Program (or any work based on the 190 | Program), the recipient automatically receives a license from the 191 | original licensor to copy, distribute or modify the Program subject to 192 | these terms and conditions. You may not impose any further 193 | restrictions on the recipients' exercise of the rights granted herein. 194 | You are not responsible for enforcing compliance by third parties to 195 | this License. 196 | 197 | 7. If, as a consequence of a court judgment or allegation of patent 198 | infringement or for any other reason (not limited to patent issues), 199 | conditions are imposed on you (whether by court order, agreement or 200 | otherwise) that contradict the conditions of this License, they do not 201 | excuse you from the conditions of this License. If you cannot 202 | distribute so as to satisfy simultaneously your obligations under this 203 | License and any other pertinent obligations, then as a consequence you 204 | may not distribute the Program at all. For example, if a patent 205 | license would not permit royalty-free redistribution of the Program by 206 | all those who receive copies directly or indirectly through you, then 207 | the only way you could satisfy both it and this License would be to 208 | refrain entirely from distribution of the Program. 209 | 210 | If any portion of this section is held invalid or unenforceable under 211 | any particular circumstance, the balance of the section is intended to 212 | apply and the section as a whole is intended to apply in other 213 | circumstances. 214 | 215 | It is not the purpose of this section to induce you to infringe any 216 | patents or other property right claims or to contest validity of any 217 | such claims; this section has the sole purpose of protecting the 218 | integrity of the free software distribution system, which is 219 | implemented by public license practices. Many people have made 220 | generous contributions to the wide range of software distributed 221 | through that system in reliance on consistent application of that 222 | system; it is up to the author/donor to decide if he or she is willing 223 | to distribute software through any other system and a licensee cannot 224 | impose that choice. 225 | 226 | This section is intended to make thoroughly clear what is believed to 227 | be a consequence of the rest of this License. 228 | 229 | 8. If the distribution and/or use of the Program is restricted in 230 | certain countries either by patents or by copyrighted interfaces, the 231 | original copyright holder who places the Program under this License 232 | may add an explicit geographical distribution limitation excluding 233 | those countries, so that distribution is permitted only in or among 234 | countries not thus excluded. In such case, this License incorporates 235 | the limitation as if written in the body of this License. 236 | 237 | 9. The Free Software Foundation may publish revised and/or new versions 238 | of the General Public License from time to time. Such new versions will 239 | be similar in spirit to the present version, but may differ in detail to 240 | address new problems or concerns. 241 | 242 | Each version is given a distinguishing version number. If the Program 243 | specifies a version number of this License which applies to it and "any 244 | later version", you have the option of following the terms and conditions 245 | either of that version or of any later version published by the Free 246 | Software Foundation. If the Program does not specify a version number of 247 | this License, you may choose any version ever published by the Free Software 248 | Foundation. 249 | 250 | 10. If you wish to incorporate parts of the Program into other free 251 | programs whose distribution conditions are different, write to the author 252 | to ask for permission. For software which is copyrighted by the Free 253 | Software Foundation, write to the Free Software Foundation; we sometimes 254 | make exceptions for this. Our decision will be guided by the two goals 255 | of preserving the free status of all derivatives of our free software and 256 | of promoting the sharing and reuse of software generally. 257 | 258 | NO WARRANTY 259 | 260 | 11. BECAUSE THE PROGRAM IS LICENSED FREE OF CHARGE, THERE IS NO WARRANTY 261 | FOR THE PROGRAM, TO THE EXTENT PERMITTED BY APPLICABLE LAW. EXCEPT WHEN 262 | OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR OTHER PARTIES 263 | PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED 264 | OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF 265 | MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE ENTIRE RISK AS 266 | TO THE QUALITY AND PERFORMANCE OF THE PROGRAM IS WITH YOU. SHOULD THE 267 | PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF ALL NECESSARY SERVICING, 268 | REPAIR OR CORRECTION. 269 | 270 | 12. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING 271 | WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY AND/OR 272 | REDISTRIBUTE THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, 273 | INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING 274 | OUT OF THE USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED 275 | TO LOSS OF DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY 276 | YOU OR THIRD PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER 277 | PROGRAMS), EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE 278 | POSSIBILITY OF SUCH DAMAGES. 279 | 280 | END OF TERMS AND CONDITIONS 281 | 282 | How to Apply These Terms to Your New Programs 283 | 284 | If you develop a new program, and you want it to be of the greatest 285 | possible use to the public, the best way to achieve this is to make it 286 | free software which everyone can redistribute and change under these terms. 287 | 288 | To do so, attach the following notices to the program. It is safest 289 | to attach them to the start of each source file to most effectively 290 | convey the exclusion of warranty; and each file should have at least 291 | the "copyright" line and a pointer to where the full notice is found. 292 | 293 | {description} 294 | Copyright (C) {year} {fullname} 295 | 296 | This program is free software; you can redistribute it and/or modify 297 | it under the terms of the GNU General Public License as published by 298 | the Free Software Foundation; either version 2 of the License, or 299 | (at your option) any later version. 300 | 301 | This program is distributed in the hope that it will be useful, 302 | but WITHOUT ANY WARRANTY; without even the implied warranty of 303 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 304 | GNU General Public License for more details. 305 | 306 | You should have received a copy of the GNU General Public License along 307 | with this program; if not, write to the Free Software Foundation, Inc., 308 | 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. 309 | 310 | Also add information on how to contact you by electronic and paper mail. 311 | 312 | If the program is interactive, make it output a short notice like this 313 | when it starts in an interactive mode: 314 | 315 | Gnomovision version 69, Copyright (C) year name of author 316 | Gnomovision comes with ABSOLUTELY NO WARRANTY; for details type `show w'. 317 | This is free software, and you are welcome to redistribute it 318 | under certain conditions; type `show c' for details. 319 | 320 | The hypothetical commands `show w' and `show c' should show the appropriate 321 | parts of the General Public License. Of course, the commands you use may 322 | be called something other than `show w' and `show c'; they could even be 323 | mouse-clicks or menu items--whatever suits your program. 324 | 325 | You should also get your employer (if you work as a programmer) or your 326 | school, if any, to sign a "copyright disclaimer" for the program, if 327 | necessary. Here is a sample; alter the names: 328 | 329 | Yoyodyne, Inc., hereby disclaims all copyright interest in the program 330 | `Gnomovision' (which makes passes at compilers) written by James Hacker. 331 | 332 | {signature of Ty Coon}, 1 April 1989 333 | Ty Coon, President of Vice 334 | 335 | This General Public License does not permit incorporating your program into 336 | proprietary programs. If your program is a subroutine library, you may 337 | consider it more useful to permit linking proprietary applications with the 338 | library. If this is what you want to do, use the GNU Lesser General 339 | Public License instead of this License. 340 | 341 | -------------------------------------------------------------------------------- /Quadrotor_Aerodynamics.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/clausqr/qrsim2/617c327ecc7afbe210b440a9fcfad54d49c048c4/Quadrotor_Aerodynamics.m -------------------------------------------------------------------------------- /Quadrotor_Controller.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/clausqr/qrsim2/617c327ecc7afbe210b440a9fcfad54d49c048c4/Quadrotor_Controller.m -------------------------------------------------------------------------------- /Quadrotor_Navigation.m: -------------------------------------------------------------------------------- 1 | function [R_est, Omega_est] = Quadrotor_Navigation(CtrlParam, xi) 2 | 3 | 4 | x = xi(1,1); 5 | y = xi(2,1); 6 | z = xi(3,1); 7 | u = xi(4,1); 8 | v = xi(5,1); 9 | w = xi(6,1); 10 | phi = xi(7,1); 11 | theta = xi(8,1); 12 | psi = xi(9,1); 13 | p = xi(10,1); 14 | q = xi(11,1); 15 | r = xi(12,1); 16 | 17 | 18 | R = angle2dcm(psi, theta, phi)'; 19 | 20 | %% 21 | kappa_max = CtrlParam.kappa_max; 22 | chi_max = CtrlParam.chi_max; 23 | 24 | ruido='gaussiano'; 25 | 26 | if strcmp(ruido, 'gaussiano') 27 | z=randn(3,1)*kappa_max; 28 | chi=randn(3,1)*chi_max; 29 | elseif strcmp(ruido, 'uniforme') 30 | z=(rand(3,1)*2-1)*kappa_max; 31 | chi=(rand(3,1)*2-1)*chi_max; 32 | else 33 | z=[0 0 0]'; 34 | chi=[0 0 0]'; 35 | end 36 | %% 37 | z_hat=[0 -z(3) z(2); z(3) 0 -z(1); -z(2) z(1) 0]; 38 | Z=expm(z_hat); 39 | R_est=R*Z; 40 | 41 | R_est=R_est(:); 42 | Omega_est = [p q r]'+chi; 43 | 44 | 45 | -------------------------------------------------------------------------------- /Quadrotor_Simualtion.m: -------------------------------------------------------------------------------- 1 | %% QUADROTOR SIMULATION 2 | % Author: Roberto A. Bunge, Ph.D. Candidate 3 | % Department of Aeronautics and Astronautics 4 | % Stanford University 5 | % email address: rbunge@stanford.edu 6 | % August 2011; Last revision: 18-Sep-2011 7 | 8 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% N O T E S %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 9 | % All quantities should be in SI units. 10 | % 11 | % Frames of Reference: 12 | % SIMULATION 13 | % Body fixed frame (B): has X in forward direction, Y to the right, and Z 14 | % pointing down. Origin of B coincides with geometric center of 15 | % Quadrotor. 16 | % Inertial frame (I): fixed to earth. When all flight variables are zero, B 17 | % and I coincide. 18 | % ANIMATION 19 | % Body fixed frame: has X in forward direction, Y up, and Z pointing to 20 | % the right. Origin coincides with geometric center of 21 | % Quadrotor. 22 | % Inertial frame: fixed to the earth. When all flight variables are zero, B 23 | % and I coincide. 24 | % 25 | % Drag Coefficient: the simulation (especially the translational motion) is quite sensitive to the value chosen 26 | % for Cd. Often Cd is around 0.5 and 1.5 27 | %%%%%%%%%%%%%%%%%%%%%%%%% 28 | 29 | clear T X C 30 | close all 31 | %% Path Settings 32 | % path(path,'/Users/Robbie/Documents/AA Research/Flight Simulation Codes/Aircraft input files') 33 | % path(path,'/Users/Robbie/Documents/AA Research/Flight Simulation Codes') 34 | % path(path,'/Users/Robbie/Documents/AA Research/Flight Simulation Codes/Plotting') 35 | % path(path,'/Users/Robbie/Documents/AA Research/QuadAir/QuadAir 1.0') 36 | % path(path,'/Users/Robbie/Documents/AA Research/Flight Simulation Codes/Simulation 1') 37 | % path(path,'/Applications/MATLAB_R2010aSV.app/toolbox/aero/aero/@Aero/@Node/private') 38 | 39 | %% Open Virtual Reality Animation World 40 | if exist('QuadAnimation') 41 | else 42 | QuadAnimation = Aero.VirtualRealityAnimation; 43 | QuadAnimation.VRWorldFilename = 'Quadrotor_World.wrl'; 44 | QuadAnimation.initialize(); 45 | fig = QuadAnimation.VRFigure; 46 | set(fig, 'Viewpoint', 'Standard View'); 47 | end 48 | 49 | %% Atomspheric parameters 50 | Atmosphere.g = 9.81; % acceleration due to gravity (kg.m/s^2) 51 | Atmosphere.rho = 1.225; % air density (kg/cm^3) 52 | Atmosphere.Wind = [0 0 0]; % Constant wind (m/s) [not implemented] 53 | 54 | %% Quadrotor parameters 55 | Quadrotor.width = 0.330; % distance between center or propellers (m) 56 | Quadrotor.r_CG_O = [0 0 0]; % position of CG relative to origin of B, expressed in B (m). 57 | Quadrotor.prop_position = [ 1 1 0 58 | -1 1 0 59 | -1 -1 0 60 | 1 -1 0]*Quadrotor.width/2; % position of each propeller w.r.t to origin of B, expressed in B 61 | Quadrotor.prop_rotation_dir = [-1 1 -1 1]; % positive aligned with body Z axis (i.e. clockwise from the top) 62 | Quadrotor.t1 = -[0 0 1]; % unit vector aligned with nominal trhust direction of rotor 1 63 | Quadrotor.t2 = -[0 0 1]; % unit vector aligned with nominal trhust direction of rotor 2 64 | Quadrotor.t3 = -[0 0 1]; % unit vector aligned with nominal trhust direction of rotor 3 65 | Quadrotor.t4 = -[0 0 1]; % unit vector aligned with nominal trhust direction of rotor 4 66 | 67 | Quadrotor.Mass_prop.Mass = 0.550; % mass of quadrotor (kg) 68 | Quadrotor.Mass_prop.Ixx = 1e-2; % Moment of inertia in body x direction about CM (kg.m^2) 69 | Quadrotor.Mass_prop.Iyy = 1e-2; % Moment of inertia in body y direction about CM (kg.m^2) 70 | Quadrotor.Mass_prop.Izz = 1e-2; % Moment of inertia in body y direction about CM (kg.m^2) 71 | Quadrotor.Mass_prop.Ixy = 0; % Product of inertia in body x-y directions about CM (kg.m^2) 72 | Quadrotor.Mass_prop.Ixz = 0; % Product of inertia in body x-z directions about CM (kg.m^2) 73 | Quadrotor.Mass_prop.Iyz = 0; % Product of inertia in body y-z directions about CM (kg.m^2) 74 | 75 | Quadrotor.DiskDiameter = 0.203; % Diameter of propeller disk (m) 76 | Quadrotor.DiskArea = Quadrotor.DiskDiameter^2/4; 77 | 78 | Quadrotor.Hover_Command = 0.25; % Nominal average command given to propellers while in hover (duty cycle, 0 to 1) 79 | 80 | Quadrotor.vh = sqrt((Quadrotor.Mass_prop.Mass*Atmosphere.g/4)/(2*Atmosphere.rho*Quadrotor.DiskArea)); % propeller hower induced velocity (m/s). See Leishmann's book. 81 | Quadrotor.k_th = Quadrotor.Mass_prop.Mass*Atmosphere.g*Quadrotor.vh/(Quadrotor.Hover_Command*4); % Constant relating Motor command to power 82 | Quadrotor.k_tm = 1; % Constant relating thrust to torque 83 | Quadrotor.Cd = 0.5; % Drag coefficient for whole quadrotor. Drag = Cd*0.5*rho*V^2*ReferenceArea 84 | 85 | Quadrotor.Index_roll = [-1 -1 1 1]; % Propeller combination used for roll command in the controller 86 | Quadrotor.Index_pitch = [ 1 -1 -1 1]; % Propeller combination used for pitch command in the controller 87 | Quadrotor.Index_yaw = [ 1 -1 1 -1]; % Propeller combination used for yaw command in the controller 88 | 89 | 90 | 91 | %% Simulation parameters 92 | t_final = 15; 93 | t_step = 0.05; % Simulation time step (sec) 94 | Aero_Model = 'Aero Model 2'; % Selects aerodynamic model 95 | 96 | %% Control Parameters 97 | Control_Period = 0.05; % time between control command refresh (sec) 98 | Control_Mode = 'Hover PID'; 99 | CtrlParam.ConstantControl = 1; 100 | CtrlParam.z_des = -6; 101 | CtrlParam.psi_des = 0; 102 | 103 | CtrlParam.wn_z = 1; 104 | CtrlParam.zeta_z = 0.5; 105 | 106 | CtrlParam.wn_phi = 5; 107 | CtrlParam.zeta_phi = 1; 108 | 109 | CtrlParam.wn_theta = 5; 110 | CtrlParam.zeta_theta = 1; 111 | 112 | CtrlParam.wn_psi = 2; 113 | CtrlParam.zeta_psi = 1; 114 | 115 | 116 | %% Initial conditions 117 | % All expressed in SIMULATION frames of reference 118 | Pos_x = 0; % (m) 119 | Pos_y = 0; % (m) 120 | Pos_z = -4; % (m) 121 | 122 | Roll = 70; % (deg) 123 | Pitch = 0; % (deg) 124 | Yaw = 0; % (deg) 125 | 126 | Vel_x = 0; % (m/s) 127 | Vel_y = 0; % (m/s) 128 | Vel_z = 0; % (m/s) 129 | 130 | AngVel_x = 0; % (deg/s) 131 | AngVel_y = 0; % (deg/s) 132 | AngVel_z = 0; % (deg/s) 133 | 134 | Velocity = 0; % (m/s) 135 | alfa = 0; % (deg) 136 | beta = 0; % (deg) 137 | 138 | IC.position = [Pos_x, Pos_y, Pos_z]; % XYZ position of origin of B w.r.t. I, expressed in I (m) 139 | IC.velocity = [Vel_x, Vel_y, Vel_z]; % Velocity of origin of B w.r.t. I (m/s). ICVelOption selects in which frame the velocity is expressed 140 | IC.orientation = [Roll, Pitch, Yaw]; % Roll, pitch and Yaw Euler angles, of B w.r.t. I (deg) 141 | IC.angular_velocity = [AngVel_x, AngVel_y, AngVel_z]; % Angular velocity of B w.r.t. I, expressed in I (deg/s) 142 | IC.LatLonAltOnEarth = [37.665543 -122.480847 7224]; % Geodetic latitude, longitude and altitude (for FliqhtGear simualtion) 143 | IC.Vel = Velocity; % Modulus of velocity of origin of B w.r.t. I 144 | IC.alfa = alfa; % angle of attack (deg) 145 | IC.beta = beta; % side slip angle (deg) 146 | 147 | IC.VelOption = 'Inertial'; % Choose in what way you want to define IC velocity - 'Inertial': velocity vector expressed in Inertial frame, 'Body': velocity vector expressed in Body frame, 'WindAngles': express it in terms of absolute velocity and wind angles (Vinf, alfa, beta) 148 | 149 | X0 = InitialCondition(IC,Quadrotor.r_CG_O); 150 | 151 | %% START SIMULATION 152 | % Initialization 153 | N=t_final/t_step+1; dt = t_step; T_last_control = -Control_Period; 154 | X = zeros(length(X0),N); X(:,1) = X0'; T=zeros(N,1); C=zeros(N,4); 155 | 156 | % Main integration loop 157 | for i = 2:N 158 | 159 | % Get current vehicle state 160 | xi = X(:,i-1); 161 | if max(isnan(xi)) % Check that simulation didn't go bad 162 | X(:,i-1) = zeros(length(X0),1); 163 | break 164 | end 165 | 166 | %girarara? 167 | % CtrlParam.psi_des = -T_last_control*pi/180*100; %36 grados/s 168 | %rebotara? 169 | % CtrlParam.z_des = -6-5*sin(2*pi*T_last_control/2.5); 170 | 171 | % Simulaci?n de Ruido 172 | 173 | % Estimaci?n 174 | 175 | 176 | % Control 177 | if T(i-1) - T_last_control >= Control_Period 178 | Control = Quadrotor_Controller(Atmosphere, Quadrotor, Control_Mode, CtrlParam, xi); 179 | T_last_control = T(i-1); 180 | end 181 | 182 | % Gravity force 183 | [Gravity_Force] = Gravity_Forces(Atmosphere, Quadrotor, xi); 184 | 185 | % Aerodynamic forces and moments 186 | [Aero_Forces, Aero_Moments_CM] = Quadrotor_Aerodynamics(Atmosphere, Quadrotor, xi, Control, Aero_Model); 187 | 188 | % Net forces and moments 189 | Net_Forces = Gravity_Force + Aero_Forces; 190 | Net_Moments_CM = zeros(1,3) + Aero_Moments_CM; 191 | 192 | % Propagate rigid body dynamics forward in time, with RK4 integration of the equations of motion 193 | Mass_prop = Quadrotor.Mass_prop; 194 | F(:,1) = dt*feval('RigidBodyDynamics', xi, Quadrotor.Mass_prop, Net_Forces, Net_Moments_CM); 195 | F(:,2) = dt*feval('RigidBodyDynamics', xi+0.5*F(:,1), Quadrotor.Mass_prop, Net_Forces, Net_Moments_CM); 196 | F(:,3) = dt*feval('RigidBodyDynamics', xi+0.5*F(:,2), Quadrotor.Mass_prop, Net_Forces, Net_Moments_CM); 197 | F(:,4) = dt*feval('RigidBodyDynamics', xi+F(:,3), Quadrotor.Mass_prop, Net_Forces, Net_Moments_CM); 198 | X(:,i) = xi + (1/6)*(F(:,1) + 2*F(:,2) + 2*F(:,3) + F(:,4)); 199 | T(i) = T(i-1)+dt; 200 | C(i,:) = Control; 201 | end 202 | 203 | %% Plot outputs 204 | %Trayectory_Plotter_2(X, T, C, 0, 'Quadrotor') 205 | 206 | %% Animation 207 | Quad = QuadAnimation.nodes{8}.VRNode; 208 | False_Quad = QuadAnimation.nodes{5}.VRNode; 209 | shadow = QuadAnimation.nodes{2}.VRNode; 210 | 211 | playVRML(T,X,Quadrotor.r_CG_O,QuadAnimation,Quad,False_Quad,shadow) 212 | 213 | 214 | 215 | 216 | 217 | 218 | 219 | 220 | 221 | -------------------------------------------------------------------------------- /RigidBodyDynamics.m: -------------------------------------------------------------------------------- 1 | %% RigidBodyDynamics - Differential equations of 3D motion of a rigid body 2 | 3 | % Author: Roberto A. Bunge, Ph.D. Candidate 4 | % Department of Aeronautics and Astronautics 5 | % Stanford University 6 | % email address: rbunge@stanford.edu 7 | % June 2011; Last revision: 18-Sep-2011 8 | 9 | % Given the mass properties and the net force and moment about the 10 | % center of mass, returns the derivative of the state 11 | % vector of the rigid body. 12 | % 13 | % Notation: 14 | % I = inertial frame 15 | % B = body fixed frame. Origin of B doesn't necessarily coincide with CM 16 | % CM = center of mass 17 | % p = the suffix "p", indicates the derivative of the variable (e.g. 18 | % "wxp" means derivative of wx, which is angular acceleration in x) 19 | % 20 | % Inputs: 21 | % X - state vector of the rigid body 22 | % X(1,1) = x; X-position of CM w.r.t. I, expressed in I 23 | % X(2,1) = y; Y-position of CM w.r.t. I, expressed in I 24 | % X(3,1) = z; Y-position of CM w.r.t. I, expressed in I 25 | % X(4,1) = u; X-velocity of CM w.r.t. I, expressed in B 26 | % X(5,1) = v; Y-velocity of CM w.r.t. I, expressed in B 27 | % X(6,1) = w; Z-velocity of CM w.r.t. I, expressed in B 28 | % X(7,1) = phi; Standard Roll Euler angle of B w.r.t I 29 | % X(8,1) = theta; Standard Pitch Euler angle of B w.r.t I 30 | % X(9,1) = psi; Standard Yaw Euler angle of B w.r.t I 31 | % X(10,1) = wx; X-angular velocity of B frame w.r.t. I, expressed in B 32 | % X(11,1) = wy; Y-angular velocity of B frame w.r.t. I, expressed in B 33 | % X(12,1) = wz; Z-angular velocity of B frame w.r.t. I, expressed in B 34 | % 35 | % Mass_prop - structure that contains mass and inertia scalars 36 | % 37 | % Net_Forces - total force acting on rigid body, expressed in B 38 | % 39 | % Net_Moments_CM - total moment acting on rigid body about center of mass, expressed in B. 40 | % 41 | % Outputs: 42 | % Xp - first derivative of state vector X 43 | % 44 | % Other m-files required: none 45 | % Subfunctions: none 46 | % MAT-files required: none 47 | % 48 | % See also: none 49 | 50 | function Xp = RigidBodyDynamics(X, Mass_prop, Net_Forces, Net_Moments_CM) 51 | % Reading STATE VARIABLES 52 | u = X(4,1); 53 | v = X(5,1); 54 | w = X(6,1); 55 | phi = X(7,1); 56 | theta = X(8,1); 57 | psi = X(9,1); 58 | wx = X(10,1); 59 | wy = X(11,1); 60 | wz = X(12,1); 61 | 62 | % Reading Mass properties. Inertias are about CM 63 | mass = Mass_prop.Mass; % kg 64 | 65 | Ixx = Mass_prop.Ixx; % kg.m^2 66 | Iyy = Mass_prop.Iyy; % kg.m^2 67 | Izz = Mass_prop.Izz; % kg.m^2 68 | Ixy = Mass_prop.Ixy; % kg.m^2 69 | Ixz = Mass_prop.Ixz; % kg.m^2 70 | Iyz = Mass_prop.Iyz; % kg.m^2 71 | 72 | % Applied force and torque 73 | Fx = Net_Forces(1); 74 | Fy = Net_Forces(2); 75 | Fz = Net_Forces(3); 76 | Tx = Net_Moments_CM(1); 77 | Ty = Net_Moments_CM(2); 78 | Tz = Net_Moments_CM(3); 79 | 80 | % Differential equations 81 | m = mass; 82 | up = Fx/m + v*wz - w*wy; 83 | vp = Fy/m + w*wx - u*wz; 84 | wp = Fz/m + u*wy - v*wx; 85 | 86 | wxp = ((Ixy*Izz-Ixz*Iyz)*(Ty+wx*(Ixz*wx+Iyz*wy+Izz*wz)-wz*(Ixx*wx+Ixy*wy+Ixz*wz))+(Ixy*Iyz-Ixz*Iyy)*(wx*(Ixy*wx+Iyy*wy+Iyz*wz)-Tz-wy*(Ixx*wx+Ixy*wy+Ixz*wz))+(Iyy*Izz-Iyz^2)*(wy*(Ixz*wx+Iyz*wy+Izz*wz)-Tx-wz*(Ixy*wx+Iyy*wy+Iyz*wz)))/( ... 87 | Ixx*Iyz^2-Izz*(Ixx*Iyy-Ixy^2)-Ixz*(2*Ixy*Iyz-Ixz*Iyy)); 88 | 89 | wyp = -((Ixx*Izz-Ixz^2)*(Ty+wx*(Ixz*wx+Iyz*wy+Izz*wz)-wz*(Ixx*wx+Ixy*wy+Ixz*wz))+(Ixx*Iyz-Ixy*Ixz)*(wx*(Ixy*wx+Iyy*wy+Iyz*wz)-Tz-wy*(Ixx*wx+Ixy*wy+Ixz*wz))+(Ixy*Izz-Ixz*Iyz)*(wy*(Ixz*wx+Iyz*wy+Izz*wz)-Tx-wz*(Ixy*wx+Iyy*wy+Iyz*wz)))/( ... 90 | Ixx*Iyz^2-Izz*(Ixx*Iyy-Ixy^2)-Ixz*(2*Ixy*Iyz-Ixz*Iyy)); 91 | 92 | wzp = ((Ixx*Iyz-Ixy*Ixz)*(Ty+wx*(Ixz*wx+Iyz*wy+Izz*wz)-wz*(Ixx*wx+Ixy*wy+Ixz*wz))+(Ixx*Iyy-Ixy^2)*(wx*(Ixy*wx+Iyy*wy+Iyz*wz)-Tz-wy*(Ixx*wx+Ixy*wy+Ixz*wz))+(Ixy*Iyz-Ixz*Iyy)*(wy*(Ixz*wx+Iyz*wy+Izz*wz)-Tx-wz*(Ixy*wx+Iyy*wy+Iyz*wz)))/( ... 93 | Ixx*Iyz^2-Izz*(Ixx*Iyy-Ixy^2)-Ixz*(2*Ixy*Iyz-Ixz*Iyy)); 94 | 95 | % Converting from wx, wy, wz, to psi_p, theta_p, phi_p 96 | c_psi = cos(psi); s_psi = sin(psi); 97 | c_theta = cos(theta); s_theta = sin(theta); 98 | c_phi = cos(phi); s_phi = sin(phi); 99 | A = (1/c_theta)*[c_theta s_theta*s_phi s_theta*c_phi; 100 | 0 c_theta*c_phi -c_theta*s_phi; 101 | 0 s_phi c_phi]; 102 | vect = A*[wx wy wz]'; 103 | phi_p = vect(1,1); 104 | theta_p = vect(2,1); 105 | psi_p = vect(3,1); 106 | 107 | % Rotation matrix from B to I 108 | B_R_I = [c_psi*c_theta (-s_psi*c_phi+s_theta*s_phi*c_psi) s_psi*s_phi+s_theta*c_psi*c_phi; 109 | s_psi*c_theta (c_psi*c_phi +s_psi*s_theta*s_phi) -s_phi*c_psi+s_psi*s_theta*c_phi; 110 | -s_theta s_phi*c_theta c_theta*c_phi ]; 111 | 112 | % [ cy*cz, cy*sz, -sy] 113 | % [ sy*sx*cz-sz*cx, sy*sx*sz+cz*cx, cy*sx] 114 | % [ sy*cx*cz+sz*sx, sy*cx*sz-cz*sx, cy*cx] 115 | 116 | Xp(1:3,1) = B_R_I*[u v w]'; 117 | Xp(4,1) = up; 118 | Xp(5,1) = vp; 119 | Xp(6,1) = wp; 120 | Xp(7,1) = phi_p; 121 | Xp(8,1) = theta_p; 122 | Xp(9,1) = psi_p; 123 | Xp(10,1) = wxp; 124 | Xp(11,1) = wyp; 125 | Xp(12,1) = wzp; 126 | 127 | 128 | 129 | -------------------------------------------------------------------------------- /auxGenerarTrayectoria.m: -------------------------------------------------------------------------------- 1 | %AUX Generar trayectoria del flip 2 | 3 | a=1500 *pi/180; 4 | 5 | ts=0:0.001:15; 6 | 7 | T1=3; 8 | T2=T1+sqrt(4*2*pi/a); 9 | T3=7; 10 | T4=10; 11 | x=0*ts; 12 | for k=1:length(ts); 13 | t=ts(k); 14 | 15 | if t" 40 | LastModifiedBy "claus" 41 | ModifiedDateFormat "%" 42 | LastModifiedDate "Wed Aug 29 21:14:14 2012" 43 | RTWModifiedTimeStamp 268175189 44 | ModelVersionFormat "1.%" 45 | ConfigurationManager "None" 46 | SampleTimeColors off 47 | SampleTimeAnnotations off 48 | LibraryLinkDisplay "disabled" 49 | WideLines off 50 | ShowLineDimensions off 51 | ShowPortDataTypes off 52 | ShowDesignRanges off 53 | ShowLoopsOnError on 54 | IgnoreBidirectionalLines off 55 | ShowStorageClass off 56 | ShowTestPointIcons on 57 | ShowSignalResolutionIcons on 58 | ShowViewerIcons on 59 | SortedOrder off 60 | ExecutionContextIcon off 61 | ShowLinearizationAnnotations on 62 | BlockNameDataTip off 63 | BlockParametersDataTip off 64 | BlockDescriptionStringDataTip off 65 | ToolBar on 66 | StatusBar on 67 | BrowserShowLibraryLinks off 68 | BrowserLookUnderMasks off 69 | SimulationMode "normal" 70 | LinearizationMsg "none" 71 | Profile off 72 | ParamWorkspaceSource "MATLABWorkspace" 73 | AccelSystemTargetFile "accel.tlc" 74 | AccelTemplateMakefile "accel_default_tmf" 75 | AccelMakeCommand "make_rtw" 76 | TryForcingSFcnDF off 77 | Object { 78 | $PropName "DataLoggingOverride" 79 | $ObjectID 2 80 | $ClassName "Simulink.SimulationData.ModelLoggingInfo" 81 | model_ "modelo2_fs_joy" 82 | overrideMode_ [0.0] 83 | Array { 84 | Type "Cell" 85 | Dimension 1 86 | Cell "modelo2_fs_joy" 87 | PropName "logAsSpecifiedByModels_" 88 | } 89 | Array { 90 | Type "Cell" 91 | Dimension 1 92 | Cell [] 93 | PropName "logAsSpecifiedByModelsSSIDs_" 94 | } 95 | } 96 | RecordCoverage off 97 | CovPath "/" 98 | CovSaveName "covdata" 99 | CovMetricSettings "dw" 100 | CovNameIncrementing off 101 | CovHtmlReporting on 102 | CovForceBlockReductionOff on 103 | covSaveCumulativeToWorkspaceVar on 104 | CovSaveSingleToWorkspaceVar on 105 | CovCumulativeVarName "covCumulativeData" 106 | CovCumulativeReport off 107 | CovReportOnPause on 108 | CovModelRefEnable "Off" 109 | CovExternalEMLEnable off 110 | ExtModeBatchMode off 111 | ExtModeEnableFloating on 112 | ExtModeTrigType "manual" 113 | ExtModeTrigMode "normal" 114 | ExtModeTrigPort "1" 115 | ExtModeTrigElement "any" 116 | ExtModeTrigDuration 1000 117 | ExtModeTrigDurationFloating "auto" 118 | ExtModeTrigHoldOff 0 119 | ExtModeTrigDelay 0 120 | ExtModeTrigDirection "rising" 121 | ExtModeTrigLevel 0 122 | ExtModeArchiveMode "off" 123 | ExtModeAutoIncOneShot off 124 | ExtModeIncDirWhenArm off 125 | ExtModeAddSuffixToVar off 126 | ExtModeWriteAllDataToWs off 127 | ExtModeArmWhenConnect on 128 | ExtModeSkipDownloadWhenConnect off 129 | ExtModeLogAll on 130 | ExtModeAutoUpdateStatusClock on 131 | BufferReuse on 132 | ShowModelReferenceBlockVersion off 133 | ShowModelReferenceBlockIO off 134 | Array { 135 | Type "Handle" 136 | Dimension 1 137 | Simulink.ConfigSet { 138 | $ObjectID 3 139 | Version "1.12.0" 140 | Array { 141 | Type "Handle" 142 | Dimension 9 143 | Simulink.SolverCC { 144 | $ObjectID 4 145 | Version "1.12.0" 146 | StartTime "0.0" 147 | StopTime "10" 148 | AbsTol "auto" 149 | FixedStep "0.1" 150 | InitialStep "auto" 151 | MaxNumMinSteps "-1" 152 | MaxOrder 5 153 | ZcThreshold "auto" 154 | ConsecutiveZCsStepRelTol "10*128*eps" 155 | MaxConsecutiveZCs "1000" 156 | ExtrapolationOrder 4 157 | NumberNewtonIterations 1 158 | MaxStep "auto" 159 | MinStep "auto" 160 | MaxConsecutiveMinStep "1" 161 | RelTol "1e-3" 162 | SolverMode "Auto" 163 | EnableConcurrentExecution off 164 | ConcurrentTasks off 165 | Solver "ode3" 166 | SolverName "ode3" 167 | SolverJacobianMethodControl "auto" 168 | ShapePreserveControl "EnableAll" 169 | ZeroCrossControl "UseLocalSettings" 170 | ZeroCrossAlgorithm "Nonadaptive" 171 | AlgebraicLoopSolver "TrustRegion" 172 | SolverResetMethod "Fast" 173 | PositivePriorityOrder off 174 | AutoInsertRateTranBlk off 175 | SampleTimeConstraint "Unconstrained" 176 | InsertRTBMode "Whenever possible" 177 | } 178 | Simulink.DataIOCC { 179 | $ObjectID 5 180 | Version "1.12.0" 181 | Decimation "1" 182 | ExternalInput "[t, u]" 183 | FinalStateName "xFinal" 184 | InitialState "xInitial" 185 | LimitDataPoints on 186 | MaxDataPoints "1000" 187 | LoadExternalInput off 188 | LoadInitialState off 189 | SaveFinalState off 190 | SaveCompleteFinalSimState off 191 | SaveFormat "Array" 192 | SignalLoggingSaveFormat "ModelDataLogs" 193 | SaveOutput on 194 | SaveState off 195 | SignalLogging on 196 | DSMLogging on 197 | InspectSignalLogs off 198 | SaveTime on 199 | ReturnWorkspaceOutputs off 200 | StateSaveName "xout" 201 | TimeSaveName "tout" 202 | OutputSaveName "yout" 203 | SignalLoggingName "logsout" 204 | DSMLoggingName "dsmout" 205 | OutputOption "RefineOutputTimes" 206 | OutputTimes "[]" 207 | ReturnWorkspaceOutputsName "out" 208 | Refine "1" 209 | } 210 | Simulink.OptimizationCC { 211 | $ObjectID 6 212 | Version "1.12.0" 213 | Array { 214 | Type "Cell" 215 | Dimension 8 216 | Cell "BooleansAsBitfields" 217 | Cell "PassReuseOutputArgsAs" 218 | Cell "PassReuseOutputArgsThreshold" 219 | Cell "ZeroExternalMemoryAtStartup" 220 | Cell "ZeroInternalMemoryAtStartup" 221 | Cell "OptimizeModelRefInitCode" 222 | Cell "NoFixptDivByZeroProtection" 223 | Cell "UseSpecifiedMinMax" 224 | PropName "DisabledProps" 225 | } 226 | BlockReduction on 227 | BooleanDataType on 228 | ConditionallyExecuteInputs on 229 | InlineParams off 230 | UseIntDivNetSlope off 231 | UseFloatMulNetSlope off 232 | UseSpecifiedMinMax off 233 | InlineInvariantSignals off 234 | OptimizeBlockIOStorage on 235 | BufferReuse on 236 | EnhancedBackFolding off 237 | StrengthReduction off 238 | ExpressionFolding on 239 | BooleansAsBitfields off 240 | BitfieldContainerType "uint_T" 241 | EnableMemcpy on 242 | MemcpyThreshold 64 243 | PassReuseOutputArgsAs "Structure reference" 244 | ExpressionDepthLimit 2147483647 245 | FoldNonRolledExpr on 246 | LocalBlockOutputs on 247 | RollThreshold 5 248 | SystemCodeInlineAuto off 249 | StateBitsets off 250 | DataBitsets off 251 | UseTempVars off 252 | ZeroExternalMemoryAtStartup on 253 | ZeroInternalMemoryAtStartup on 254 | InitFltsAndDblsToZero off 255 | NoFixptDivByZeroProtection off 256 | EfficientFloat2IntCast off 257 | EfficientMapNaN2IntZero on 258 | OptimizeModelRefInitCode off 259 | LifeSpan "inf" 260 | MaxStackSize "Inherit from target" 261 | BufferReusableBoundary on 262 | SimCompilerOptimization "On" 263 | AccelVerboseBuild on 264 | ParallelExecutionInRapidAccelerator on 265 | } 266 | Simulink.DebuggingCC { 267 | $ObjectID 7 268 | Version "1.12.0" 269 | RTPrefix "error" 270 | ConsistencyChecking "none" 271 | ArrayBoundsChecking "none" 272 | SignalInfNanChecking "none" 273 | SignalRangeChecking "none" 274 | ReadBeforeWriteMsg "UseLocalSettings" 275 | WriteAfterWriteMsg "UseLocalSettings" 276 | WriteAfterReadMsg "UseLocalSettings" 277 | AlgebraicLoopMsg "warning" 278 | ArtificialAlgebraicLoopMsg "warning" 279 | SaveWithDisabledLinksMsg "warning" 280 | SaveWithParameterizedLinksMsg "warning" 281 | CheckSSInitialOutputMsg on 282 | UnderspecifiedInitializationDetection "Classic" 283 | MergeDetectMultiDrivingBlocksExec "none" 284 | CheckExecutionContextPreStartOutputMsg off 285 | CheckExecutionContextRuntimeOutputMsg off 286 | SignalResolutionControl "UseLocalSettings" 287 | BlockPriorityViolationMsg "warning" 288 | MinStepSizeMsg "warning" 289 | TimeAdjustmentMsg "none" 290 | MaxConsecutiveZCsMsg "error" 291 | MaskedZcDiagnostic "warning" 292 | IgnoredZcDiagnostic "warning" 293 | SolverPrmCheckMsg "warning" 294 | InheritedTsInSrcMsg "warning" 295 | DiscreteInheritContinuousMsg "warning" 296 | MultiTaskDSMMsg "error" 297 | MultiTaskCondExecSysMsg "error" 298 | MultiTaskRateTransMsg "error" 299 | SingleTaskRateTransMsg "none" 300 | TasksWithSamePriorityMsg "warning" 301 | SigSpecEnsureSampleTimeMsg "warning" 302 | CheckMatrixSingularityMsg "none" 303 | IntegerOverflowMsg "warning" 304 | Int32ToFloatConvMsg "warning" 305 | ParameterDowncastMsg "error" 306 | ParameterOverflowMsg "error" 307 | ParameterUnderflowMsg "none" 308 | ParameterPrecisionLossMsg "warning" 309 | ParameterTunabilityLossMsg "warning" 310 | FixptConstUnderflowMsg "none" 311 | FixptConstOverflowMsg "none" 312 | FixptConstPrecisionLossMsg "none" 313 | UnderSpecifiedDataTypeMsg "none" 314 | UnnecessaryDatatypeConvMsg "none" 315 | VectorMatrixConversionMsg "none" 316 | InvalidFcnCallConnMsg "error" 317 | FcnCallInpInsideContextMsg "Enable All" 318 | SignalLabelMismatchMsg "none" 319 | UnconnectedInputMsg "warning" 320 | UnconnectedOutputMsg "warning" 321 | UnconnectedLineMsg "warning" 322 | SFcnCompatibilityMsg "none" 323 | FrameProcessingCompatibilityMsg "warning" 324 | UniqueDataStoreMsg "none" 325 | BusObjectLabelMismatch "warning" 326 | RootOutportRequireBusObject "warning" 327 | AssertControl "UseLocalSettings" 328 | EnableOverflowDetection off 329 | ModelReferenceIOMsg "none" 330 | ModelReferenceMultiInstanceNormalModeStructChecksumCheck "error" 331 | ModelReferenceVersionMismatchMessage "none" 332 | ModelReferenceIOMismatchMessage "none" 333 | ModelReferenceCSMismatchMessage "none" 334 | UnknownTsInhSupMsg "warning" 335 | ModelReferenceDataLoggingMessage "warning" 336 | ModelReferenceSymbolNameMessage "warning" 337 | ModelReferenceExtraNoncontSigs "error" 338 | StateNameClashWarn "warning" 339 | SimStateInterfaceChecksumMismatchMsg "warning" 340 | SimStateOlderReleaseMsg "error" 341 | InitInArrayFormatMsg "warning" 342 | StrictBusMsg "ErrorLevel1" 343 | BusNameAdapt "WarnAndRepair" 344 | NonBusSignalsTreatedAsBus "none" 345 | LoggingUnavailableSignals "error" 346 | BlockIODiagnostic "none" 347 | SFUnusedDataAndEventsDiag "warning" 348 | SFUnexpectedBacktrackingDiag "warning" 349 | SFInvalidInputDataAccessInChartInitDiag "warning" 350 | SFNoUnconditionalDefaultTransitionDiag "warning" 351 | SFTransitionOutsideNaturalParentDiag "warning" 352 | SFUnconditionalTransitionShadowingDiag "warning" 353 | } 354 | Simulink.HardwareCC { 355 | $ObjectID 8 356 | Version "1.12.0" 357 | ProdBitPerChar 8 358 | ProdBitPerShort 16 359 | ProdBitPerInt 32 360 | ProdBitPerLong 32 361 | ProdBitPerFloat 32 362 | ProdBitPerDouble 64 363 | ProdBitPerPointer 32 364 | ProdLargestAtomicInteger "Char" 365 | ProdLargestAtomicFloat "None" 366 | ProdIntDivRoundTo "Undefined" 367 | ProdEndianess "Unspecified" 368 | ProdWordSize 32 369 | ProdShiftRightIntArith on 370 | ProdHWDeviceType "32-bit Generic" 371 | TargetBitPerChar 8 372 | TargetBitPerShort 16 373 | TargetBitPerInt 32 374 | TargetBitPerLong 32 375 | TargetBitPerFloat 32 376 | TargetBitPerDouble 64 377 | TargetBitPerPointer 32 378 | TargetLargestAtomicInteger "Char" 379 | TargetLargestAtomicFloat "None" 380 | TargetShiftRightIntArith on 381 | TargetIntDivRoundTo "Undefined" 382 | TargetEndianess "Unspecified" 383 | TargetWordSize 32 384 | TargetTypeEmulationWarnSuppressLevel 0 385 | TargetPreprocMaxBitsSint 32 386 | TargetPreprocMaxBitsUint 32 387 | TargetHWDeviceType "Specified" 388 | TargetUnknown off 389 | ProdEqTarget on 390 | } 391 | Simulink.ModelReferenceCC { 392 | $ObjectID 9 393 | Version "1.12.0" 394 | UpdateModelReferenceTargets "IfOutOfDateOrStructuralChange" 395 | CheckModelReferenceTargetMessage "error" 396 | EnableParallelModelReferenceBuilds on 397 | ParallelModelReferenceErrorOnInvalidPool on 398 | ParallelModelReferenceMATLABWorkerInit "None" 399 | ModelReferenceNumInstancesAllowed "Multi" 400 | PropagateVarSize "Infer from blocks in model" 401 | ModelReferencePassRootInputsByReference on 402 | ModelReferenceMinAlgLoopOccurrences off 403 | PropagateSignalLabelsOutOfModel off 404 | SupportModelReferenceSimTargetCustomCode off 405 | } 406 | Simulink.SFSimCC { 407 | $ObjectID 10 408 | Version "1.12.0" 409 | SFSimEnableDebug on 410 | SFSimOverflowDetection on 411 | SFSimEcho on 412 | SimBlas on 413 | SimCtrlC on 414 | SimExtrinsic on 415 | SimIntegrity on 416 | SimUseLocalCustomCode off 417 | SimParseCustomCode on 418 | SimBuildMode "sf_incremental_build" 419 | } 420 | Simulink.RTWCC { 421 | $BackupClass "Simulink.RTWCC" 422 | $ObjectID 11 423 | Version "1.12.0" 424 | Array { 425 | Type "Cell" 426 | Dimension 9 427 | Cell "IncludeHyperlinkInReport" 428 | Cell "GenerateTraceInfo" 429 | Cell "GenerateTraceReport" 430 | Cell "GenerateTraceReportSl" 431 | Cell "GenerateTraceReportSf" 432 | Cell "GenerateTraceReportEml" 433 | Cell "GenerateWebview" 434 | Cell "GenerateCodeMetricsReport" 435 | Cell "GenerateCodeReplacementReport" 436 | PropName "DisabledProps" 437 | } 438 | SystemTargetFile "grt.tlc" 439 | GenCodeOnly off 440 | MakeCommand "make_rtw" 441 | GenerateMakefile on 442 | TemplateMakefile "grt_default_tmf" 443 | GenerateReport off 444 | SaveLog off 445 | RTWVerbose on 446 | RetainRTWFile off 447 | ProfileTLC off 448 | TLCDebug off 449 | TLCCoverage off 450 | TLCAssert off 451 | ProcessScriptMode "Default" 452 | ConfigurationMode "Optimized" 453 | ConfigAtBuild off 454 | RTWUseLocalCustomCode off 455 | RTWUseSimCustomCode off 456 | IncludeHyperlinkInReport off 457 | LaunchReport off 458 | TargetLang "C" 459 | IncludeBusHierarchyInRTWFileBlockHierarchyMap off 460 | IncludeERTFirstTime off 461 | GenerateTraceInfo off 462 | GenerateTraceReport off 463 | GenerateTraceReportSl off 464 | GenerateTraceReportSf off 465 | GenerateTraceReportEml off 466 | GenerateCodeInfo off 467 | GenerateWebview off 468 | GenerateCodeMetricsReport off 469 | GenerateCodeReplacementReport off 470 | RTWCompilerOptimization "Off" 471 | CheckMdlBeforeBuild "Off" 472 | CustomRebuildMode "OnUpdate" 473 | Array { 474 | Type "Handle" 475 | Dimension 2 476 | Simulink.CodeAppCC { 477 | $ObjectID 12 478 | Version "1.12.0" 479 | Array { 480 | Type "Cell" 481 | Dimension 21 482 | Cell "IgnoreCustomStorageClasses" 483 | Cell "IgnoreTestpoints" 484 | Cell "InsertBlockDesc" 485 | Cell "InsertPolySpaceComments" 486 | Cell "SFDataObjDesc" 487 | Cell "MATLABFcnDesc" 488 | Cell "SimulinkDataObjDesc" 489 | Cell "DefineNamingRule" 490 | Cell "SignalNamingRule" 491 | Cell "ParamNamingRule" 492 | Cell "InlinedPrmAccess" 493 | Cell "CustomSymbolStr" 494 | Cell "CustomSymbolStrGlobalVar" 495 | Cell "CustomSymbolStrType" 496 | Cell "CustomSymbolStrField" 497 | Cell "CustomSymbolStrFcn" 498 | Cell "CustomSymbolStrFcnArg" 499 | Cell "CustomSymbolStrBlkIO" 500 | Cell "CustomSymbolStrTmpVar" 501 | Cell "CustomSymbolStrMacro" 502 | Cell "ReqsInCode" 503 | PropName "DisabledProps" 504 | } 505 | ForceParamTrailComments off 506 | GenerateComments on 507 | IgnoreCustomStorageClasses on 508 | IgnoreTestpoints off 509 | IncHierarchyInIds off 510 | MaxIdLength 31 511 | PreserveName off 512 | PreserveNameWithParent off 513 | ShowEliminatedStatement off 514 | IncAutoGenComments off 515 | SimulinkDataObjDesc off 516 | SFDataObjDesc off 517 | MATLABFcnDesc off 518 | IncDataTypeInIds off 519 | MangleLength 1 520 | CustomSymbolStrGlobalVar "$R$N$M" 521 | CustomSymbolStrType "$N$R$M" 522 | CustomSymbolStrField "$N$M" 523 | CustomSymbolStrFcn "$R$N$M$F" 524 | CustomSymbolStrFcnArg "rt$I$N$M" 525 | CustomSymbolStrBlkIO "rtb_$N$M" 526 | CustomSymbolStrTmpVar "$N$M" 527 | CustomSymbolStrMacro "$R$N$M" 528 | DefineNamingRule "None" 529 | ParamNamingRule "None" 530 | SignalNamingRule "None" 531 | InsertBlockDesc off 532 | InsertPolySpaceComments off 533 | SimulinkBlockComments on 534 | MATLABSourceComments off 535 | EnableCustomComments off 536 | InlinedPrmAccess "Literals" 537 | ReqsInCode off 538 | UseSimReservedNames off 539 | } 540 | Simulink.GRTTargetCC { 541 | $BackupClass "Simulink.TargetCC" 542 | $ObjectID 13 543 | Version "1.12.0" 544 | Array { 545 | Type "Cell" 546 | Dimension 16 547 | Cell "GeneratePreprocessorConditionals" 548 | Cell "IncludeMdlTerminateFcn" 549 | Cell "CombineOutputUpdateFcns" 550 | Cell "SuppressErrorStatus" 551 | Cell "ERTCustomFileBanners" 552 | Cell "GenerateSampleERTMain" 553 | Cell "GenerateTestInterfaces" 554 | Cell "ModelStepFunctionPrototypeControlCompliant" 555 | Cell "CPPClassGenCompliant" 556 | Cell "MultiInstanceERTCode" 557 | Cell "PurelyIntegerCode" 558 | Cell "SupportComplex" 559 | Cell "SupportAbsoluteTime" 560 | Cell "SupportContinuousTime" 561 | Cell "SupportNonInlinedSFcns" 562 | Cell "PortableWordSizes" 563 | PropName "DisabledProps" 564 | } 565 | TargetFcnLib "ansi_tfl_table_tmw.mat" 566 | TargetLibSuffix "" 567 | TargetPreCompLibLocation "" 568 | CodeReplacementLibrary "ANSI_C" 569 | UtilityFuncGeneration "Auto" 570 | ERTMultiwordTypeDef "System defined" 571 | CodeExecutionProfiling off 572 | ERTMultiwordLength 256 573 | MultiwordLength 2048 574 | GenerateFullHeader on 575 | GenerateSampleERTMain off 576 | GenerateTestInterfaces off 577 | IsPILTarget off 578 | ModelReferenceCompliant on 579 | ParMdlRefBuildCompliant on 580 | CompOptLevelCompliant on 581 | ConcurrentExecutionCompliant on 582 | IncludeMdlTerminateFcn on 583 | GeneratePreprocessorConditionals "Disable all" 584 | CombineOutputUpdateFcns on 585 | CombineSignalStateStructs off 586 | SuppressErrorStatus off 587 | ERTFirstTimeCompliant off 588 | IncludeFileDelimiter "Auto" 589 | ERTCustomFileBanners off 590 | SupportAbsoluteTime on 591 | LogVarNameModifier "rt_" 592 | MatFileLogging on 593 | MultiInstanceERTCode off 594 | SupportNonFinite on 595 | SupportComplex on 596 | PurelyIntegerCode off 597 | SupportContinuousTime on 598 | SupportNonInlinedSFcns on 599 | SupportVariableSizeSignals off 600 | EnableShiftOperators on 601 | ParenthesesLevel "Nominal" 602 | PortableWordSizes off 603 | ModelStepFunctionPrototypeControlCompliant off 604 | CPPClassGenCompliant off 605 | AutosarCompliant off 606 | GRTInterface off 607 | UseMalloc off 608 | ExtMode off 609 | ExtModeStaticAlloc off 610 | ExtModeTesting off 611 | ExtModeStaticAllocSize 1000000 612 | ExtModeTransport 0 613 | ExtModeMexFile "ext_comm" 614 | ExtModeIntrfLevel "Level1" 615 | RTWCAPISignals off 616 | RTWCAPIParams off 617 | RTWCAPIStates off 618 | RTWCAPIRootIO off 619 | GenerateASAP2 off 620 | } 621 | PropName "Components" 622 | } 623 | } 624 | hdlcoderui.hdlcc { 625 | $ObjectID 14 626 | Version "1.12.0" 627 | Description "HDL Coder custom configuration component" 628 | Name "HDL Coder" 629 | } 630 | PropName "Components" 631 | } 632 | Name "Configuration" 633 | CurrentDlgPage "Solver" 634 | ConfigPrmDlgPosition [ 536, 69, 1435, 811 ] 635 | } 636 | PropName "ConfigurationSets" 637 | } 638 | Simulink.ConfigSet { 639 | $PropName "ActiveConfigurationSet" 640 | $ObjectID 3 641 | } 642 | ExplicitPartitioning off 643 | BlockDefaults { 644 | ForegroundColor "black" 645 | BackgroundColor "white" 646 | DropShadow off 647 | NamePlacement "normal" 648 | FontName "Helvetica" 649 | FontSize 10 650 | FontWeight "normal" 651 | FontAngle "normal" 652 | ShowName on 653 | BlockRotation 0 654 | BlockMirror off 655 | } 656 | AnnotationDefaults { 657 | HorizontalAlignment "center" 658 | VerticalAlignment "middle" 659 | ForegroundColor "black" 660 | BackgroundColor "white" 661 | DropShadow off 662 | FontName "Helvetica" 663 | FontSize 10 664 | FontWeight "normal" 665 | FontAngle "normal" 666 | UseDisplayTextAsClickCallback off 667 | } 668 | LineDefaults { 669 | FontName "Helvetica" 670 | FontSize 9 671 | FontWeight "normal" 672 | FontAngle "normal" 673 | } 674 | BlockParameterDefaults { 675 | Block { 676 | BlockType Constant 677 | Value "1" 678 | VectorParams1D on 679 | SamplingMode "Sample based" 680 | OutMin "[]" 681 | OutMax "[]" 682 | OutDataTypeStr "Inherit: Inherit from 'Constant value'" 683 | LockScale off 684 | SampleTime "inf" 685 | FramePeriod "inf" 686 | PreserveConstantTs off 687 | } 688 | Block { 689 | BlockType DataTypeConversion 690 | OutMin "[]" 691 | OutMax "[]" 692 | OutDataTypeStr "Inherit: Inherit via back propagation" 693 | LockScale off 694 | ConvertRealWorld "Real World Value (RWV)" 695 | RndMeth "Zero" 696 | SaturateOnIntegerOverflow on 697 | SampleTime "-1" 698 | } 699 | Block { 700 | BlockType Demux 701 | Outputs "4" 702 | DisplayOption "none" 703 | BusSelectionMode off 704 | } 705 | Block { 706 | BlockType Gain 707 | Gain "1" 708 | Multiplication "Element-wise(K.*u)" 709 | ParamMin "[]" 710 | ParamMax "[]" 711 | ParamDataTypeStr "Inherit: Same as input" 712 | OutMin "[]" 713 | OutMax "[]" 714 | OutDataTypeStr "Inherit: Same as input" 715 | LockScale off 716 | RndMeth "Floor" 717 | SaturateOnIntegerOverflow on 718 | SampleTime "-1" 719 | } 720 | Block { 721 | BlockType M-S-Function 722 | FunctionName "matlabfile" 723 | DisplayMFileStacktrace on 724 | } 725 | Block { 726 | BlockType Scope 727 | ModelBased off 728 | TickLabels "OneTimeTick" 729 | ZoomMode "on" 730 | Grid "on" 731 | TimeRange "auto" 732 | YMin "-5" 733 | YMax "5" 734 | SaveToWorkspace off 735 | SaveName "ScopeData" 736 | DataFormat "Array" 737 | LimitDataPoints on 738 | MaxDataPoints "5000" 739 | Decimation "1" 740 | SampleInput off 741 | SampleTime "-1" 742 | } 743 | Block { 744 | BlockType Sum 745 | IconShape "rectangular" 746 | Inputs "++" 747 | CollapseMode "All dimensions" 748 | CollapseDim "1" 749 | InputSameDT on 750 | AccumDataTypeStr "Inherit: Inherit via internal rule" 751 | OutMin "[]" 752 | OutMax "[]" 753 | OutDataTypeStr "Inherit: Same as first input" 754 | LockScale off 755 | RndMeth "Floor" 756 | SaturateOnIntegerOverflow on 757 | SampleTime "-1" 758 | } 759 | Block { 760 | BlockType ToWorkspace 761 | VariableName "simulink_output" 762 | MaxDataPoints "1000" 763 | Decimation "1" 764 | SampleTime "0" 765 | SaveFormat "Array" 766 | FixptAsFi off 767 | NumInputs "1" 768 | } 769 | } 770 | System { 771 | Name "modelo2_fs_joy" 772 | Location [933, 399, 1900, 963] 773 | Open on 774 | ModelBrowserVisibility off 775 | ModelBrowserWidth 200 776 | ScreenColor "white" 777 | PaperOrientation "landscape" 778 | PaperPositionMode "auto" 779 | PaperType "usletter" 780 | PaperUnits "inches" 781 | TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] 782 | TiledPageScale 1 783 | ShowPageBoundaries off 784 | ZoomFactor "100" 785 | ReportName "simulink-default.rpt" 786 | SIDHighWatermark "62" 787 | Block { 788 | BlockType Constant 789 | Name "Constant" 790 | SID "59" 791 | Position [545, 450, 575, 480] 792 | ZOrder -4 793 | Value "240" 794 | } 795 | Block { 796 | BlockType DataTypeConversion 797 | Name "Data Type Conversion1" 798 | SID "23" 799 | Position [730, 367, 800, 403] 800 | ZOrder -7 801 | RndMeth "Floor" 802 | SaturateOnIntegerOverflow off 803 | } 804 | Block { 805 | BlockType DataTypeConversion 806 | Name "Data Type Conversion2" 807 | SID "55" 808 | Position [960, 242, 1030, 278] 809 | ZOrder -7 810 | OutDataTypeStr "double" 811 | RndMeth "Floor" 812 | SaturateOnIntegerOverflow off 813 | } 814 | Block { 815 | BlockType DataTypeConversion 816 | Name "Data Type Conversion3" 817 | SID "56" 818 | Position [945, 292, 1015, 328] 819 | ZOrder -7 820 | OutDataTypeStr "double" 821 | RndMeth "Floor" 822 | SaturateOnIntegerOverflow off 823 | } 824 | Block { 825 | BlockType Reference 826 | Name "Degrees to\nRadians" 827 | SID "57" 828 | Ports [1, 1] 829 | Position [900, 245, 930, 275] 830 | LibraryVersion "1.44" 831 | SourceBlock "simulink_extras/Transformations/Degrees to\nRadians" 832 | SourceType "DegreesToRadians" 833 | } 834 | Block { 835 | BlockType Reference 836 | Name "Degrees to\nRadians1" 837 | SID "58" 838 | Ports [1, 1] 839 | Position [890, 295, 920, 325] 840 | LibraryVersion "1.44" 841 | SourceBlock "simulink_extras/Transformations/Degrees to\nRadians" 842 | SourceType "DegreesToRadians" 843 | } 844 | Block { 845 | BlockType Demux 846 | Name "Demux" 847 | SID "18" 848 | Ports [1, 2] 849 | Position [860, 273, 865, 322] 850 | ZOrder -6 851 | ShowName off 852 | Outputs "2" 853 | DisplayOption "bar" 854 | } 855 | Block { 856 | BlockType Demux 857 | Name "Demux1" 858 | SID "20" 859 | Ports [1, 3] 860 | Position [860, 350, 865, 420] 861 | ZOrder -6 862 | ShowName off 863 | Outputs "3" 864 | DisplayOption "bar" 865 | } 866 | Block { 867 | BlockType Demux 868 | Name "Demux2" 869 | SID "33" 870 | Ports [1, 3] 871 | Position [675, 295, 680, 345] 872 | ZOrder -6 873 | NamePlacement "alternate" 874 | ShowName off 875 | Outputs "3" 876 | DisplayOption "bar" 877 | } 878 | Block { 879 | BlockType Reference 880 | Name "Flat Earth to LLA" 881 | SID "30" 882 | Ports [2, 2] 883 | Position [730, 284, 800, 351] 884 | ZOrder -10 885 | LibraryVersion "1.275" 886 | SourceBlock "aerolibtransform2/Flat Earth to LLA" 887 | SourceType "Flat Earth to LLA" 888 | units "Metric (MKS)" 889 | ptype "Earth (WGS84)" 890 | F "1/298.257223563" 891 | R "6378137" 892 | LL0 "[37.665543 -122.480847]" 893 | psi "0" 894 | } 895 | Block { 896 | BlockType Gain 897 | Name "Gain" 898 | SID "10" 899 | Position [285, 235, 315, 265] 900 | ZOrder -8 901 | BlockMirror on 902 | NamePlacement "alternate" 903 | ParamDataTypeStr "Inherit: Inherit via internal rule" 904 | OutDataTypeStr "Inherit: Inherit via internal rule" 905 | SaturateOnIntegerOverflow off 906 | } 907 | Block { 908 | BlockType Reference 909 | Name "Pack\nnet_fdm Packet\nfor FlightGear" 910 | SID "16" 911 | Ports [6, 1] 912 | Position [985, 264, 1100, 431] 913 | ZOrder -3 914 | AttributesFormatString "Version Selected: %" 915 | LibraryVersion "1.200" 916 | SourceBlock "aerolibfltsims/Pack\nnet_fdm Packet\nfor FlightGear" 917 | SourceType "FlightGearPackNetFdm" 918 | FlightGearVersion "v2.0" 919 | ShowPositionAttitudeInputs on 920 | ShowVelocityAccelerationInputs off 921 | ShowControlSurfacePositionInputs off 922 | ShowEngineFuelInputs off 923 | ShowLandingGearInputs off 924 | ShowEnvironmentInputs off 925 | SampleTime "-1" 926 | } 927 | Block { 928 | BlockType Reference 929 | Name "Pilot Joystick" 930 | SID "61" 931 | Ports [0, 4] 932 | Position [15, 98, 85, 167] 933 | ZOrder -1 934 | LibraryVersion "1.72" 935 | SourceBlock "aerolibanimutils/Pilot Joystick" 936 | SourceType "PilotJoystick" 937 | JoystickID "None" 938 | OutputConfiguration "FourAxis" 939 | SampleTime "0.1" 940 | } 941 | Block { 942 | BlockType M-S-Function 943 | Name "Quadrotor" 944 | SID "1" 945 | Ports [1, 5] 946 | Position [365, 78, 605, 202] 947 | ZOrder -3 948 | FunctionName "slQuadrotorPlanta" 949 | MaskInitialization "slQuadrotorPlantaInitialization" 950 | MaskDisplay "port_label('output', 1, 'state x(1:12)')\nport_label('output', 2, 'pos (I)')\nport_label('out" 951 | "put', 3, 'vel (B)')\nport_label('output', 4, 'Euler (I) [Roll Pitch Yaw]')\nport_label('output', 5, '\\omega (B)" 952 | "')\nport_label('input', 1, 'Control [f1 f2 f3 f4]')\n\n" 953 | MaskIconFrame on 954 | MaskIconOpaque on 955 | MaskIconRotate "none" 956 | MaskPortRotate "default" 957 | MaskIconUnits "autoscale" 958 | } 959 | Block { 960 | BlockType M-S-Function 961 | Name "Quadrotor Controller" 962 | SID "5" 963 | Ports [5, 1] 964 | Position [145, 104, 335, 176] 965 | ZOrder -3 966 | FunctionName "slQuadrotorController" 967 | MaskDisplay "port_label('output', 1, 'Control=[f1 f2 f3 f4]')\nport_label('input', 1, 'Joystick roll')\npo" 968 | "rt_label('input', 2, 'Joystick pitch')\nport_label('input', 3, 'Joystick yaw')\nport_label('input', 4, 'Joystick" 969 | " throttle')\nport_label('input', 5, 'state x(1:12)')\n" 970 | MaskIconFrame on 971 | MaskIconOpaque on 972 | MaskIconRotate "none" 973 | MaskPortRotate "default" 974 | MaskIconUnits "autoscale" 975 | } 976 | Block { 977 | BlockType Scope 978 | Name "Scope" 979 | SID "4" 980 | Ports [1] 981 | Position [660, 74, 690, 106] 982 | ZOrder -3 983 | Floating off 984 | Location [158, 753, 482, 1008] 985 | Open off 986 | NumInputPorts "1" 987 | ZoomMode "yonly" 988 | List { 989 | ListType AxesTitles 990 | axes1 "%" 991 | } 992 | List { 993 | ListType ScopeGraphics 994 | FigureColor "[0.5 0.5 0.5]" 995 | AxesColor "[0 0 0]" 996 | AxesTickColor "[1 1 1]" 997 | LineColors "[1 1 0;1 0 1;0 1 1;1 0 0;0 1 0;0 0 1]" 998 | LineStyles "-|-|-|-|-|-" 999 | LineWidths "[0.5 0.5 0.5 0.5 0.5 0.5]" 1000 | MarkerStyles "none|none|none|none|none|none" 1001 | } 1002 | ShowLegends off 1003 | TimeRange "1" 1004 | YMin "-45" 1005 | YMax "2.5" 1006 | DataFormat "StructureWithTime" 1007 | SampleTime "0" 1008 | } 1009 | Block { 1010 | BlockType Scope 1011 | Name "Scope1" 1012 | SID "11" 1013 | Ports [1] 1014 | Position [710, 99, 740, 131] 1015 | ZOrder -3 1016 | Floating off 1017 | Location [482, 516, 806, 771] 1018 | Open off 1019 | NumInputPorts "1" 1020 | ZoomMode "yonly" 1021 | List { 1022 | ListType AxesTitles 1023 | axes1 "%" 1024 | } 1025 | List { 1026 | ListType ScopeGraphics 1027 | FigureColor "[0.5 0.5 0.5]" 1028 | AxesColor "[0 0 0]" 1029 | AxesTickColor "[1 1 1]" 1030 | LineColors "[1 1 0;1 0 1;0 1 1;1 0 0;0 1 0;0 0 1]" 1031 | LineStyles "-|-|-|-|-|-" 1032 | LineWidths "[0.5 0.5 0.5 0.5 0.5 0.5]" 1033 | MarkerStyles "none|none|none|none|none|none" 1034 | } 1035 | ShowLegends off 1036 | TimeRange "1" 1037 | YMin "-45" 1038 | YMax "2.5" 1039 | SaveName "ScopeData1" 1040 | DataFormat "StructureWithTime" 1041 | SampleTime "0" 1042 | } 1043 | Block { 1044 | BlockType Scope 1045 | Name "Scope2" 1046 | SID "12" 1047 | Ports [1] 1048 | Position [760, 124, 790, 156] 1049 | ZOrder -3 1050 | Floating off 1051 | Location [504, 172, 828, 427] 1052 | Open off 1053 | NumInputPorts "1" 1054 | ZoomMode "yonly" 1055 | List { 1056 | ListType AxesTitles 1057 | axes1 "%" 1058 | } 1059 | List { 1060 | ListType ScopeGraphics 1061 | FigureColor "[0.5 0.5 0.5]" 1062 | AxesColor "[0 0 0]" 1063 | AxesTickColor "[1 1 1]" 1064 | LineColors "[1 1 0;1 0 1;0 1 1;1 0 0;0 1 0;0 0 1]" 1065 | LineStyles "-|-|-|-|-|-" 1066 | LineWidths "[0.5 0.5 0.5 0.5 0.5 0.5]" 1067 | MarkerStyles "none|none|none|none|none|none" 1068 | } 1069 | ShowLegends off 1070 | TimeRange "1" 1071 | YMin "-45" 1072 | YMax "2.5" 1073 | SaveName "ScopeData2" 1074 | DataFormat "StructureWithTime" 1075 | SampleTime "0" 1076 | } 1077 | Block { 1078 | BlockType Scope 1079 | Name "Scope3" 1080 | SID "13" 1081 | Ports [1] 1082 | Position [810, 149, 840, 181] 1083 | ZOrder -3 1084 | Floating off 1085 | Location [137, 130, 461, 385] 1086 | Open off 1087 | NumInputPorts "1" 1088 | ZoomMode "yonly" 1089 | List { 1090 | ListType AxesTitles 1091 | axes1 "%" 1092 | } 1093 | List { 1094 | ListType ScopeGraphics 1095 | FigureColor "[0.5 0.5 0.5]" 1096 | AxesColor "[0 0 0]" 1097 | AxesTickColor "[1 1 1]" 1098 | LineColors "[1 1 0;1 0 1;0 1 1;1 0 0;0 1 0;0 0 1]" 1099 | LineStyles "-|-|-|-|-|-" 1100 | LineWidths "[0.5 0.5 0.5 0.5 0.5 0.5]" 1101 | MarkerStyles "none|none|none|none|none|none" 1102 | } 1103 | ShowLegends off 1104 | TimeRange "1" 1105 | YMin "-0.0025" 1106 | YMax "0.03" 1107 | SaveName "ScopeData3" 1108 | DataFormat "StructureWithTime" 1109 | SampleTime "0" 1110 | } 1111 | Block { 1112 | BlockType Scope 1113 | Name "Scope4" 1114 | SID "14" 1115 | Ports [1] 1116 | Position [855, 169, 885, 201] 1117 | ZOrder -3 1118 | Floating off 1119 | Location [130, 457, 454, 712] 1120 | Open off 1121 | NumInputPorts "1" 1122 | ZoomMode "yonly" 1123 | List { 1124 | ListType AxesTitles 1125 | axes1 "%" 1126 | } 1127 | List { 1128 | ListType ScopeGraphics 1129 | FigureColor "[0.5 0.5 0.5]" 1130 | AxesColor "[0 0 0]" 1131 | AxesTickColor "[1 1 1]" 1132 | LineColors "[1 1 0;1 0 1;0 1 1;1 0 0;0 1 0;0 0 1]" 1133 | LineStyles "-|-|-|-|-|-" 1134 | LineWidths "[0.5 0.5 0.5 0.5 0.5 0.5]" 1135 | MarkerStyles "none|none|none|none|none|none" 1136 | } 1137 | ShowLegends off 1138 | TimeRange "1" 1139 | YMin "-0.01" 1140 | YMax "0.11" 1141 | SaveName "ScopeData4" 1142 | DataFormat "StructureWithTime" 1143 | SampleTime "0" 1144 | } 1145 | Block { 1146 | BlockType Scope 1147 | Name "Scope5" 1148 | SID "15" 1149 | Ports [1] 1150 | Position [455, 334, 485, 366] 1151 | ZOrder -3 1152 | Floating off 1153 | Location [158, 753, 482, 1008] 1154 | Open on 1155 | NumInputPorts "1" 1156 | ZoomMode "yonly" 1157 | List { 1158 | ListType AxesTitles 1159 | axes1 "%" 1160 | } 1161 | List { 1162 | ListType ScopeGraphics 1163 | FigureColor "[0.5 0.5 0.5]" 1164 | AxesColor "[0 0 0]" 1165 | AxesTickColor "[1 1 1]" 1166 | LineColors "[1 1 0;1 0 1;0 1 1;1 0 0;0 1 0;0 0 1]" 1167 | LineStyles "-|-|-|-|-|-" 1168 | LineWidths "[0.5 0.5 0.5 0.5 0.5 0.5]" 1169 | MarkerStyles "none|none|none|none|none|none" 1170 | } 1171 | ShowLegends off 1172 | TimeRange "1" 1173 | YMin "0.265" 1174 | YMax "0.36" 1175 | SaveName "ScopeData5" 1176 | DataFormat "StructureWithTime" 1177 | SampleTime "0" 1178 | } 1179 | Block { 1180 | BlockType Reference 1181 | Name "Send\nnet_fdm Packet\nto FlightGear" 1182 | SID "17" 1183 | Ports [1] 1184 | Position [1065, 324, 1145, 376] 1185 | ZOrder -4 1186 | LibraryVersion "1.200" 1187 | SourceBlock "aerolibfltsims/Send\nnet_fdm Packet\nto FlightGear" 1188 | SourceType "FlightGearSendNetFdm" 1189 | DestinationIpAddress "127.0.0.1" 1190 | DestinationPort "5502" 1191 | LocalIpPort "-1" 1192 | varLen off 1193 | SampleTime "-1" 1194 | } 1195 | Block { 1196 | BlockType Reference 1197 | Name "Simulation Pace" 1198 | SID "62" 1199 | Ports [] 1200 | Position [120, 375, 181, 420] 1201 | ZOrder -3 1202 | AttributesFormatString "% sec/sec" 1203 | LibraryVersion "1.72" 1204 | SourceBlock "aerolibanimutils/Simulation Pace" 1205 | SourceType "Simulation Pace" 1206 | SimulationPace "1" 1207 | SleepMode "MATLAB Thread" 1208 | OutputPaceError off 1209 | SampleTime "0.1" 1210 | } 1211 | Block { 1212 | BlockType Sum 1213 | Name "Sum" 1214 | SID "60" 1215 | Ports [2, 1] 1216 | Position [820, 325, 840, 345] 1217 | ZOrder -18 1218 | ShowName off 1219 | IconShape "round" 1220 | Inputs "|++" 1221 | InputSameDT off 1222 | OutDataTypeStr "Inherit: Inherit via internal rule" 1223 | SaturateOnIntegerOverflow off 1224 | } 1225 | Block { 1226 | BlockType ToWorkspace 1227 | Name "To Workspace" 1228 | SID "8" 1229 | Ports [1] 1230 | Position [655, 15, 715, 45] 1231 | ZOrder -7 1232 | VariableName "X_simulink" 1233 | MaxDataPoints "inf" 1234 | SampleTime "-1" 1235 | SaveFormat "Timeseries" 1236 | } 1237 | Line { 1238 | SrcBlock "Quadrotor Controller" 1239 | SrcPort 1 1240 | DstBlock "Quadrotor" 1241 | DstPort 1 1242 | } 1243 | Line { 1244 | SrcBlock "Quadrotor" 1245 | SrcPort 1 1246 | Points [15, 0] 1247 | Branch { 1248 | Points [0, 0] 1249 | Branch { 1250 | DstBlock "Scope" 1251 | DstPort 1 1252 | } 1253 | Branch { 1254 | Points [0, -60] 1255 | DstBlock "To Workspace" 1256 | DstPort 1 1257 | } 1258 | } 1259 | Branch { 1260 | Points [0, 160] 1261 | DstBlock "Gain" 1262 | DstPort 1 1263 | } 1264 | } 1265 | Line { 1266 | SrcBlock "Quadrotor" 1267 | SrcPort 2 1268 | Points [25, 0] 1269 | Branch { 1270 | Points [0, 155] 1271 | Branch { 1272 | Points [0, 50] 1273 | DstBlock "Demux2" 1274 | DstPort 1 1275 | } 1276 | Branch { 1277 | Points [55, 0; 0, 30] 1278 | DstBlock "Flat Earth to LLA" 1279 | DstPort 1 1280 | } 1281 | } 1282 | Branch { 1283 | DstBlock "Scope1" 1284 | DstPort 1 1285 | } 1286 | } 1287 | Line { 1288 | SrcBlock "Quadrotor" 1289 | SrcPort 3 1290 | DstBlock "Scope2" 1291 | DstPort 1 1292 | } 1293 | Line { 1294 | SrcBlock "Quadrotor" 1295 | SrcPort 4 1296 | Points [40, 0] 1297 | Branch { 1298 | Points [0, 220] 1299 | DstBlock "Data Type Conversion1" 1300 | DstPort 1 1301 | } 1302 | Branch { 1303 | DstBlock "Scope3" 1304 | DstPort 1 1305 | } 1306 | } 1307 | Line { 1308 | SrcBlock "Quadrotor" 1309 | SrcPort 5 1310 | Points [190, 0; 0, 10; 45, 0; 0, -15] 1311 | DstBlock "Scope4" 1312 | DstPort 1 1313 | } 1314 | Line { 1315 | SrcBlock "Pack\nnet_fdm Packet\nfor FlightGear" 1316 | SrcPort 1 1317 | DstBlock "Send\nnet_fdm Packet\nto FlightGear" 1318 | DstPort 1 1319 | } 1320 | Line { 1321 | SrcBlock "Demux1" 1322 | SrcPort 1 1323 | DstBlock "Pack\nnet_fdm Packet\nfor FlightGear" 1324 | DstPort 4 1325 | } 1326 | Line { 1327 | SrcBlock "Demux1" 1328 | SrcPort 2 1329 | DstBlock "Pack\nnet_fdm Packet\nfor FlightGear" 1330 | DstPort 5 1331 | } 1332 | Line { 1333 | SrcBlock "Demux1" 1334 | SrcPort 3 1335 | DstBlock "Pack\nnet_fdm Packet\nfor FlightGear" 1336 | DstPort 6 1337 | } 1338 | Line { 1339 | SrcBlock "Data Type Conversion1" 1340 | SrcPort 1 1341 | DstBlock "Demux1" 1342 | DstPort 1 1343 | } 1344 | Line { 1345 | SrcBlock "Flat Earth to LLA" 1346 | SrcPort 1 1347 | DstBlock "Demux" 1348 | DstPort 1 1349 | } 1350 | Line { 1351 | SrcBlock "Demux2" 1352 | SrcPort 3 1353 | DstBlock "Flat Earth to LLA" 1354 | DstPort 2 1355 | } 1356 | Line { 1357 | SrcBlock "Degrees to\nRadians" 1358 | SrcPort 1 1359 | DstBlock "Data Type Conversion2" 1360 | DstPort 1 1361 | } 1362 | Line { 1363 | SrcBlock "Degrees to\nRadians1" 1364 | SrcPort 1 1365 | DstBlock "Data Type Conversion3" 1366 | DstPort 1 1367 | } 1368 | Line { 1369 | SrcBlock "Demux" 1370 | SrcPort 1 1371 | Points [5, 0; 0, -25] 1372 | DstBlock "Degrees to\nRadians" 1373 | DstPort 1 1374 | } 1375 | Line { 1376 | SrcBlock "Demux" 1377 | SrcPort 2 1378 | DstBlock "Degrees to\nRadians1" 1379 | DstPort 1 1380 | } 1381 | Line { 1382 | SrcBlock "Data Type Conversion3" 1383 | SrcPort 1 1384 | Points [-50, 0] 1385 | DstBlock "Pack\nnet_fdm Packet\nfor FlightGear" 1386 | DstPort 1 1387 | } 1388 | Line { 1389 | SrcBlock "Data Type Conversion2" 1390 | SrcPort 1 1391 | Points [-65, 0] 1392 | DstBlock "Pack\nnet_fdm Packet\nfor FlightGear" 1393 | DstPort 2 1394 | } 1395 | Line { 1396 | SrcBlock "Flat Earth to LLA" 1397 | SrcPort 2 1398 | DstBlock "Sum" 1399 | DstPort 1 1400 | } 1401 | Line { 1402 | SrcBlock "Constant" 1403 | SrcPort 1 1404 | Points [250, 0] 1405 | DstBlock "Sum" 1406 | DstPort 2 1407 | } 1408 | Line { 1409 | SrcBlock "Sum" 1410 | SrcPort 1 1411 | Points [30, 0; 0, 20; 160, 0; 0, -10; 5, 0; 0, -10] 1412 | DstBlock "Pack\nnet_fdm Packet\nfor FlightGear" 1413 | DstPort 3 1414 | } 1415 | Line { 1416 | SrcBlock "Pilot Joystick" 1417 | SrcPort 1 1418 | DstBlock "Quadrotor Controller" 1419 | DstPort 1 1420 | } 1421 | Line { 1422 | SrcBlock "Pilot Joystick" 1423 | SrcPort 2 1424 | DstBlock "Quadrotor Controller" 1425 | DstPort 2 1426 | } 1427 | Line { 1428 | SrcBlock "Pilot Joystick" 1429 | SrcPort 3 1430 | DstBlock "Quadrotor Controller" 1431 | DstPort 3 1432 | } 1433 | Line { 1434 | SrcBlock "Pilot Joystick" 1435 | SrcPort 4 1436 | Points [10, 0] 1437 | Branch { 1438 | DstBlock "Quadrotor Controller" 1439 | DstPort 4 1440 | } 1441 | Branch { 1442 | Points [0, 195] 1443 | DstBlock "Scope5" 1444 | DstPort 1 1445 | } 1446 | } 1447 | Line { 1448 | SrcBlock "Gain" 1449 | SrcPort 1 1450 | Points [-160, 0; 0, -80] 1451 | DstBlock "Quadrotor Controller" 1452 | DstPort 5 1453 | } 1454 | } 1455 | } 1456 | -------------------------------------------------------------------------------- /modelo2a.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/clausqr/qrsim2/617c327ecc7afbe210b440a9fcfad54d49c048c4/modelo2a.slx -------------------------------------------------------------------------------- /modelo2variable_step.mdl: -------------------------------------------------------------------------------- 1 | Model { 2 | Name "modelo2variable_step" 3 | Version 7.9 4 | MdlSubVersion 0 5 | GraphicalInterface { 6 | NumRootInports 0 7 | NumRootOutports 0 8 | ParameterArgumentNames "" 9 | ComputedModelVersion "1.23" 10 | NumModelReferences 0 11 | NumTestPointedSignals 0 12 | } 13 | SavedCharacterEncoding "ISO-8859-1" 14 | slprops.hdlmdlprops { 15 | $PropName "HDLParams" 16 | $ObjectID 1 17 | Array { 18 | Type "Cell" 19 | Dimension 2 20 | Cell "HDLSubsystem" 21 | Cell "modelo2" 22 | PropName "mdlProps" 23 | } 24 | } 25 | SaveDefaultBlockParams on 26 | ScopeRefreshTime 0.035000 27 | OverrideScopeRefreshTime on 28 | DisableAllScopes off 29 | DataTypeOverride "UseLocalSettings" 30 | DataTypeOverrideAppliesTo "AllNumericTypes" 31 | MinMaxOverflowLogging "UseLocalSettings" 32 | MinMaxOverflowArchiveMode "Overwrite" 33 | FPTRunName "Run 1" 34 | MaxMDLFileLineLength 120 35 | InitFcn "modelo2Initialization" 36 | Created "Tue Aug 28 10:09:52 2012" 37 | Creator "claus" 38 | UpdateHistory "UpdateHistoryNever" 39 | ModifiedByFormat "%" 40 | LastModifiedBy "claus" 41 | ModifiedDateFormat "%" 42 | LastModifiedDate "Wed Aug 29 15:59:05 2012" 43 | RTWModifiedTimeStamp 268071541 44 | ModelVersionFormat "1.%" 45 | ConfigurationManager "None" 46 | SampleTimeColors off 47 | SampleTimeAnnotations off 48 | LibraryLinkDisplay "disabled" 49 | WideLines off 50 | ShowLineDimensions off 51 | ShowPortDataTypes off 52 | ShowDesignRanges off 53 | ShowLoopsOnError on 54 | IgnoreBidirectionalLines off 55 | ShowStorageClass off 56 | ShowTestPointIcons on 57 | ShowSignalResolutionIcons on 58 | ShowViewerIcons on 59 | SortedOrder off 60 | ExecutionContextIcon off 61 | ShowLinearizationAnnotations on 62 | BlockNameDataTip off 63 | BlockParametersDataTip off 64 | BlockDescriptionStringDataTip off 65 | ToolBar on 66 | StatusBar on 67 | BrowserShowLibraryLinks off 68 | BrowserLookUnderMasks off 69 | SimulationMode "accelerator" 70 | LinearizationMsg "none" 71 | Profile off 72 | ParamWorkspaceSource "MATLABWorkspace" 73 | AccelSystemTargetFile "accel.tlc" 74 | AccelTemplateMakefile "accel_default_tmf" 75 | AccelMakeCommand "make_rtw" 76 | TryForcingSFcnDF off 77 | Object { 78 | $PropName "DataLoggingOverride" 79 | $ObjectID 2 80 | $ClassName "Simulink.SimulationData.ModelLoggingInfo" 81 | model_ "modelo2variable_step" 82 | overrideMode_ [0.0] 83 | Array { 84 | Type "Cell" 85 | Dimension 1 86 | Cell "modelo2variable_step" 87 | PropName "logAsSpecifiedByModels_" 88 | } 89 | Array { 90 | Type "Cell" 91 | Dimension 1 92 | Cell [] 93 | PropName "logAsSpecifiedByModelsSSIDs_" 94 | } 95 | } 96 | RecordCoverage off 97 | CovPath "/" 98 | CovSaveName "covdata" 99 | CovMetricSettings "dw" 100 | CovNameIncrementing off 101 | CovHtmlReporting on 102 | CovForceBlockReductionOff on 103 | covSaveCumulativeToWorkspaceVar on 104 | CovSaveSingleToWorkspaceVar on 105 | CovCumulativeVarName "covCumulativeData" 106 | CovCumulativeReport off 107 | CovReportOnPause on 108 | CovModelRefEnable "Off" 109 | CovExternalEMLEnable off 110 | ExtModeBatchMode off 111 | ExtModeEnableFloating on 112 | ExtModeTrigType "manual" 113 | ExtModeTrigMode "normal" 114 | ExtModeTrigPort "1" 115 | ExtModeTrigElement "any" 116 | ExtModeTrigDuration 1000 117 | ExtModeTrigDurationFloating "auto" 118 | ExtModeTrigHoldOff 0 119 | ExtModeTrigDelay 0 120 | ExtModeTrigDirection "rising" 121 | ExtModeTrigLevel 0 122 | ExtModeArchiveMode "off" 123 | ExtModeAutoIncOneShot off 124 | ExtModeIncDirWhenArm off 125 | ExtModeAddSuffixToVar off 126 | ExtModeWriteAllDataToWs off 127 | ExtModeArmWhenConnect on 128 | ExtModeSkipDownloadWhenConnect off 129 | ExtModeLogAll on 130 | ExtModeAutoUpdateStatusClock on 131 | BufferReuse on 132 | ShowModelReferenceBlockVersion off 133 | ShowModelReferenceBlockIO off 134 | Array { 135 | Type "Handle" 136 | Dimension 1 137 | Simulink.ConfigSet { 138 | $ObjectID 3 139 | Version "1.12.0" 140 | Array { 141 | Type "Handle" 142 | Dimension 9 143 | Simulink.SolverCC { 144 | $ObjectID 4 145 | Version "1.12.0" 146 | StartTime "0.0" 147 | StopTime "10" 148 | AbsTol "auto" 149 | FixedStep "0.1" 150 | InitialStep "auto" 151 | MaxNumMinSteps "-1" 152 | MaxOrder 5 153 | ZcThreshold "auto" 154 | ConsecutiveZCsStepRelTol "10*128*eps" 155 | MaxConsecutiveZCs "1000" 156 | ExtrapolationOrder 4 157 | NumberNewtonIterations 1 158 | MaxStep "auto" 159 | MinStep "auto" 160 | MaxConsecutiveMinStep "1" 161 | RelTol "1e-3" 162 | SolverMode "Auto" 163 | EnableConcurrentExecution off 164 | ConcurrentTasks off 165 | Solver "ode3" 166 | SolverName "ode3" 167 | SolverJacobianMethodControl "auto" 168 | ShapePreserveControl "EnableAll" 169 | ZeroCrossControl "UseLocalSettings" 170 | ZeroCrossAlgorithm "Nonadaptive" 171 | AlgebraicLoopSolver "TrustRegion" 172 | SolverResetMethod "Fast" 173 | PositivePriorityOrder off 174 | AutoInsertRateTranBlk off 175 | SampleTimeConstraint "Unconstrained" 176 | InsertRTBMode "Whenever possible" 177 | } 178 | Simulink.DataIOCC { 179 | $ObjectID 5 180 | Version "1.12.0" 181 | Decimation "1" 182 | ExternalInput "[t, u]" 183 | FinalStateName "xFinal" 184 | InitialState "xInitial" 185 | LimitDataPoints on 186 | MaxDataPoints "1000" 187 | LoadExternalInput off 188 | LoadInitialState off 189 | SaveFinalState off 190 | SaveCompleteFinalSimState off 191 | SaveFormat "Array" 192 | SignalLoggingSaveFormat "ModelDataLogs" 193 | SaveOutput on 194 | SaveState off 195 | SignalLogging on 196 | DSMLogging on 197 | InspectSignalLogs off 198 | SaveTime on 199 | ReturnWorkspaceOutputs off 200 | StateSaveName "xout" 201 | TimeSaveName "tout" 202 | OutputSaveName "yout" 203 | SignalLoggingName "logsout" 204 | DSMLoggingName "dsmout" 205 | OutputOption "RefineOutputTimes" 206 | OutputTimes "[]" 207 | ReturnWorkspaceOutputsName "out" 208 | Refine "1" 209 | } 210 | Simulink.OptimizationCC { 211 | $ObjectID 6 212 | Version "1.12.0" 213 | Array { 214 | Type "Cell" 215 | Dimension 8 216 | Cell "BooleansAsBitfields" 217 | Cell "PassReuseOutputArgsAs" 218 | Cell "PassReuseOutputArgsThreshold" 219 | Cell "ZeroExternalMemoryAtStartup" 220 | Cell "ZeroInternalMemoryAtStartup" 221 | Cell "OptimizeModelRefInitCode" 222 | Cell "NoFixptDivByZeroProtection" 223 | Cell "UseSpecifiedMinMax" 224 | PropName "DisabledProps" 225 | } 226 | BlockReduction on 227 | BooleanDataType on 228 | ConditionallyExecuteInputs on 229 | InlineParams off 230 | UseIntDivNetSlope off 231 | UseFloatMulNetSlope off 232 | UseSpecifiedMinMax off 233 | InlineInvariantSignals off 234 | OptimizeBlockIOStorage on 235 | BufferReuse on 236 | EnhancedBackFolding off 237 | StrengthReduction off 238 | ExpressionFolding on 239 | BooleansAsBitfields off 240 | BitfieldContainerType "uint_T" 241 | EnableMemcpy on 242 | MemcpyThreshold 64 243 | PassReuseOutputArgsAs "Structure reference" 244 | ExpressionDepthLimit 2147483647 245 | FoldNonRolledExpr on 246 | LocalBlockOutputs on 247 | RollThreshold 5 248 | SystemCodeInlineAuto off 249 | StateBitsets off 250 | DataBitsets off 251 | UseTempVars off 252 | ZeroExternalMemoryAtStartup on 253 | ZeroInternalMemoryAtStartup on 254 | InitFltsAndDblsToZero off 255 | NoFixptDivByZeroProtection off 256 | EfficientFloat2IntCast off 257 | EfficientMapNaN2IntZero on 258 | OptimizeModelRefInitCode off 259 | LifeSpan "inf" 260 | MaxStackSize "Inherit from target" 261 | BufferReusableBoundary on 262 | SimCompilerOptimization "On" 263 | AccelVerboseBuild on 264 | ParallelExecutionInRapidAccelerator on 265 | } 266 | Simulink.DebuggingCC { 267 | $ObjectID 7 268 | Version "1.12.0" 269 | RTPrefix "error" 270 | ConsistencyChecking "none" 271 | ArrayBoundsChecking "none" 272 | SignalInfNanChecking "none" 273 | SignalRangeChecking "none" 274 | ReadBeforeWriteMsg "UseLocalSettings" 275 | WriteAfterWriteMsg "UseLocalSettings" 276 | WriteAfterReadMsg "UseLocalSettings" 277 | AlgebraicLoopMsg "warning" 278 | ArtificialAlgebraicLoopMsg "warning" 279 | SaveWithDisabledLinksMsg "warning" 280 | SaveWithParameterizedLinksMsg "warning" 281 | CheckSSInitialOutputMsg on 282 | UnderspecifiedInitializationDetection "Classic" 283 | MergeDetectMultiDrivingBlocksExec "none" 284 | CheckExecutionContextPreStartOutputMsg off 285 | CheckExecutionContextRuntimeOutputMsg off 286 | SignalResolutionControl "UseLocalSettings" 287 | BlockPriorityViolationMsg "warning" 288 | MinStepSizeMsg "warning" 289 | TimeAdjustmentMsg "none" 290 | MaxConsecutiveZCsMsg "error" 291 | MaskedZcDiagnostic "warning" 292 | IgnoredZcDiagnostic "warning" 293 | SolverPrmCheckMsg "warning" 294 | InheritedTsInSrcMsg "warning" 295 | DiscreteInheritContinuousMsg "warning" 296 | MultiTaskDSMMsg "error" 297 | MultiTaskCondExecSysMsg "error" 298 | MultiTaskRateTransMsg "error" 299 | SingleTaskRateTransMsg "none" 300 | TasksWithSamePriorityMsg "warning" 301 | SigSpecEnsureSampleTimeMsg "warning" 302 | CheckMatrixSingularityMsg "none" 303 | IntegerOverflowMsg "warning" 304 | Int32ToFloatConvMsg "warning" 305 | ParameterDowncastMsg "error" 306 | ParameterOverflowMsg "error" 307 | ParameterUnderflowMsg "none" 308 | ParameterPrecisionLossMsg "warning" 309 | ParameterTunabilityLossMsg "warning" 310 | FixptConstUnderflowMsg "none" 311 | FixptConstOverflowMsg "none" 312 | FixptConstPrecisionLossMsg "none" 313 | UnderSpecifiedDataTypeMsg "none" 314 | UnnecessaryDatatypeConvMsg "none" 315 | VectorMatrixConversionMsg "none" 316 | InvalidFcnCallConnMsg "error" 317 | FcnCallInpInsideContextMsg "Enable All" 318 | SignalLabelMismatchMsg "none" 319 | UnconnectedInputMsg "warning" 320 | UnconnectedOutputMsg "warning" 321 | UnconnectedLineMsg "warning" 322 | SFcnCompatibilityMsg "none" 323 | FrameProcessingCompatibilityMsg "warning" 324 | UniqueDataStoreMsg "none" 325 | BusObjectLabelMismatch "warning" 326 | RootOutportRequireBusObject "warning" 327 | AssertControl "UseLocalSettings" 328 | EnableOverflowDetection off 329 | ModelReferenceIOMsg "none" 330 | ModelReferenceMultiInstanceNormalModeStructChecksumCheck "error" 331 | ModelReferenceVersionMismatchMessage "none" 332 | ModelReferenceIOMismatchMessage "none" 333 | ModelReferenceCSMismatchMessage "none" 334 | UnknownTsInhSupMsg "warning" 335 | ModelReferenceDataLoggingMessage "warning" 336 | ModelReferenceSymbolNameMessage "warning" 337 | ModelReferenceExtraNoncontSigs "error" 338 | StateNameClashWarn "warning" 339 | SimStateInterfaceChecksumMismatchMsg "warning" 340 | SimStateOlderReleaseMsg "error" 341 | InitInArrayFormatMsg "warning" 342 | StrictBusMsg "ErrorLevel1" 343 | BusNameAdapt "WarnAndRepair" 344 | NonBusSignalsTreatedAsBus "none" 345 | LoggingUnavailableSignals "error" 346 | BlockIODiagnostic "none" 347 | SFUnusedDataAndEventsDiag "warning" 348 | SFUnexpectedBacktrackingDiag "warning" 349 | SFInvalidInputDataAccessInChartInitDiag "warning" 350 | SFNoUnconditionalDefaultTransitionDiag "warning" 351 | SFTransitionOutsideNaturalParentDiag "warning" 352 | SFUnconditionalTransitionShadowingDiag "warning" 353 | } 354 | Simulink.HardwareCC { 355 | $ObjectID 8 356 | Version "1.12.0" 357 | ProdBitPerChar 8 358 | ProdBitPerShort 16 359 | ProdBitPerInt 32 360 | ProdBitPerLong 32 361 | ProdBitPerFloat 32 362 | ProdBitPerDouble 64 363 | ProdBitPerPointer 32 364 | ProdLargestAtomicInteger "Char" 365 | ProdLargestAtomicFloat "None" 366 | ProdIntDivRoundTo "Undefined" 367 | ProdEndianess "Unspecified" 368 | ProdWordSize 32 369 | ProdShiftRightIntArith on 370 | ProdHWDeviceType "32-bit Generic" 371 | TargetBitPerChar 8 372 | TargetBitPerShort 16 373 | TargetBitPerInt 32 374 | TargetBitPerLong 32 375 | TargetBitPerFloat 32 376 | TargetBitPerDouble 64 377 | TargetBitPerPointer 32 378 | TargetLargestAtomicInteger "Char" 379 | TargetLargestAtomicFloat "None" 380 | TargetShiftRightIntArith on 381 | TargetIntDivRoundTo "Undefined" 382 | TargetEndianess "Unspecified" 383 | TargetWordSize 32 384 | TargetTypeEmulationWarnSuppressLevel 0 385 | TargetPreprocMaxBitsSint 32 386 | TargetPreprocMaxBitsUint 32 387 | TargetHWDeviceType "Specified" 388 | TargetUnknown off 389 | ProdEqTarget on 390 | } 391 | Simulink.ModelReferenceCC { 392 | $ObjectID 9 393 | Version "1.12.0" 394 | UpdateModelReferenceTargets "IfOutOfDateOrStructuralChange" 395 | CheckModelReferenceTargetMessage "error" 396 | EnableParallelModelReferenceBuilds on 397 | ParallelModelReferenceErrorOnInvalidPool on 398 | ParallelModelReferenceMATLABWorkerInit "None" 399 | ModelReferenceNumInstancesAllowed "Multi" 400 | PropagateVarSize "Infer from blocks in model" 401 | ModelReferencePassRootInputsByReference on 402 | ModelReferenceMinAlgLoopOccurrences off 403 | PropagateSignalLabelsOutOfModel off 404 | SupportModelReferenceSimTargetCustomCode off 405 | } 406 | Simulink.SFSimCC { 407 | $ObjectID 10 408 | Version "1.12.0" 409 | SFSimEnableDebug on 410 | SFSimOverflowDetection on 411 | SFSimEcho on 412 | SimBlas on 413 | SimCtrlC on 414 | SimExtrinsic on 415 | SimIntegrity on 416 | SimUseLocalCustomCode off 417 | SimParseCustomCode on 418 | SimBuildMode "sf_incremental_build" 419 | } 420 | Simulink.RTWCC { 421 | $BackupClass "Simulink.RTWCC" 422 | $ObjectID 11 423 | Version "1.12.0" 424 | Array { 425 | Type "Cell" 426 | Dimension 9 427 | Cell "IncludeHyperlinkInReport" 428 | Cell "GenerateTraceInfo" 429 | Cell "GenerateTraceReport" 430 | Cell "GenerateTraceReportSl" 431 | Cell "GenerateTraceReportSf" 432 | Cell "GenerateTraceReportEml" 433 | Cell "GenerateWebview" 434 | Cell "GenerateCodeMetricsReport" 435 | Cell "GenerateCodeReplacementReport" 436 | PropName "DisabledProps" 437 | } 438 | SystemTargetFile "grt.tlc" 439 | GenCodeOnly off 440 | MakeCommand "make_rtw" 441 | GenerateMakefile on 442 | TemplateMakefile "grt_default_tmf" 443 | GenerateReport off 444 | SaveLog off 445 | RTWVerbose on 446 | RetainRTWFile off 447 | ProfileTLC off 448 | TLCDebug off 449 | TLCCoverage off 450 | TLCAssert off 451 | ProcessScriptMode "Default" 452 | ConfigurationMode "Optimized" 453 | ConfigAtBuild off 454 | RTWUseLocalCustomCode off 455 | RTWUseSimCustomCode off 456 | IncludeHyperlinkInReport off 457 | LaunchReport off 458 | TargetLang "C" 459 | IncludeBusHierarchyInRTWFileBlockHierarchyMap off 460 | IncludeERTFirstTime off 461 | GenerateTraceInfo off 462 | GenerateTraceReport off 463 | GenerateTraceReportSl off 464 | GenerateTraceReportSf off 465 | GenerateTraceReportEml off 466 | GenerateCodeInfo off 467 | GenerateWebview off 468 | GenerateCodeMetricsReport off 469 | GenerateCodeReplacementReport off 470 | RTWCompilerOptimization "Off" 471 | CheckMdlBeforeBuild "Off" 472 | CustomRebuildMode "OnUpdate" 473 | Array { 474 | Type "Handle" 475 | Dimension 2 476 | Simulink.CodeAppCC { 477 | $ObjectID 12 478 | Version "1.12.0" 479 | Array { 480 | Type "Cell" 481 | Dimension 21 482 | Cell "IgnoreCustomStorageClasses" 483 | Cell "IgnoreTestpoints" 484 | Cell "InsertBlockDesc" 485 | Cell "InsertPolySpaceComments" 486 | Cell "SFDataObjDesc" 487 | Cell "MATLABFcnDesc" 488 | Cell "SimulinkDataObjDesc" 489 | Cell "DefineNamingRule" 490 | Cell "SignalNamingRule" 491 | Cell "ParamNamingRule" 492 | Cell "InlinedPrmAccess" 493 | Cell "CustomSymbolStr" 494 | Cell "CustomSymbolStrGlobalVar" 495 | Cell "CustomSymbolStrType" 496 | Cell "CustomSymbolStrField" 497 | Cell "CustomSymbolStrFcn" 498 | Cell "CustomSymbolStrFcnArg" 499 | Cell "CustomSymbolStrBlkIO" 500 | Cell "CustomSymbolStrTmpVar" 501 | Cell "CustomSymbolStrMacro" 502 | Cell "ReqsInCode" 503 | PropName "DisabledProps" 504 | } 505 | ForceParamTrailComments off 506 | GenerateComments on 507 | IgnoreCustomStorageClasses on 508 | IgnoreTestpoints off 509 | IncHierarchyInIds off 510 | MaxIdLength 31 511 | PreserveName off 512 | PreserveNameWithParent off 513 | ShowEliminatedStatement off 514 | IncAutoGenComments off 515 | SimulinkDataObjDesc off 516 | SFDataObjDesc off 517 | MATLABFcnDesc off 518 | IncDataTypeInIds off 519 | MangleLength 1 520 | CustomSymbolStrGlobalVar "$R$N$M" 521 | CustomSymbolStrType "$N$R$M" 522 | CustomSymbolStrField "$N$M" 523 | CustomSymbolStrFcn "$R$N$M$F" 524 | CustomSymbolStrFcnArg "rt$I$N$M" 525 | CustomSymbolStrBlkIO "rtb_$N$M" 526 | CustomSymbolStrTmpVar "$N$M" 527 | CustomSymbolStrMacro "$R$N$M" 528 | DefineNamingRule "None" 529 | ParamNamingRule "None" 530 | SignalNamingRule "None" 531 | InsertBlockDesc off 532 | InsertPolySpaceComments off 533 | SimulinkBlockComments on 534 | MATLABSourceComments off 535 | EnableCustomComments off 536 | InlinedPrmAccess "Literals" 537 | ReqsInCode off 538 | UseSimReservedNames off 539 | } 540 | Simulink.GRTTargetCC { 541 | $BackupClass "Simulink.TargetCC" 542 | $ObjectID 13 543 | Version "1.12.0" 544 | Array { 545 | Type "Cell" 546 | Dimension 16 547 | Cell "GeneratePreprocessorConditionals" 548 | Cell "IncludeMdlTerminateFcn" 549 | Cell "CombineOutputUpdateFcns" 550 | Cell "SuppressErrorStatus" 551 | Cell "ERTCustomFileBanners" 552 | Cell "GenerateSampleERTMain" 553 | Cell "GenerateTestInterfaces" 554 | Cell "ModelStepFunctionPrototypeControlCompliant" 555 | Cell "CPPClassGenCompliant" 556 | Cell "MultiInstanceERTCode" 557 | Cell "PurelyIntegerCode" 558 | Cell "SupportComplex" 559 | Cell "SupportAbsoluteTime" 560 | Cell "SupportContinuousTime" 561 | Cell "SupportNonInlinedSFcns" 562 | Cell "PortableWordSizes" 563 | PropName "DisabledProps" 564 | } 565 | TargetFcnLib "ansi_tfl_table_tmw.mat" 566 | TargetLibSuffix "" 567 | TargetPreCompLibLocation "" 568 | CodeReplacementLibrary "ANSI_C" 569 | UtilityFuncGeneration "Auto" 570 | ERTMultiwordTypeDef "System defined" 571 | CodeExecutionProfiling off 572 | ERTMultiwordLength 256 573 | MultiwordLength 2048 574 | GenerateFullHeader on 575 | GenerateSampleERTMain off 576 | GenerateTestInterfaces off 577 | IsPILTarget off 578 | ModelReferenceCompliant on 579 | ParMdlRefBuildCompliant on 580 | CompOptLevelCompliant on 581 | ConcurrentExecutionCompliant on 582 | IncludeMdlTerminateFcn on 583 | GeneratePreprocessorConditionals "Disable all" 584 | CombineOutputUpdateFcns on 585 | CombineSignalStateStructs off 586 | SuppressErrorStatus off 587 | ERTFirstTimeCompliant off 588 | IncludeFileDelimiter "Auto" 589 | ERTCustomFileBanners off 590 | SupportAbsoluteTime on 591 | LogVarNameModifier "rt_" 592 | MatFileLogging on 593 | MultiInstanceERTCode off 594 | SupportNonFinite on 595 | SupportComplex on 596 | PurelyIntegerCode off 597 | SupportContinuousTime on 598 | SupportNonInlinedSFcns on 599 | SupportVariableSizeSignals off 600 | EnableShiftOperators on 601 | ParenthesesLevel "Nominal" 602 | PortableWordSizes off 603 | ModelStepFunctionPrototypeControlCompliant off 604 | CPPClassGenCompliant off 605 | AutosarCompliant off 606 | GRTInterface off 607 | UseMalloc off 608 | ExtMode off 609 | ExtModeStaticAlloc off 610 | ExtModeTesting off 611 | ExtModeStaticAllocSize 1000000 612 | ExtModeTransport 0 613 | ExtModeMexFile "ext_comm" 614 | ExtModeIntrfLevel "Level1" 615 | RTWCAPISignals off 616 | RTWCAPIParams off 617 | RTWCAPIStates off 618 | RTWCAPIRootIO off 619 | GenerateASAP2 off 620 | } 621 | PropName "Components" 622 | } 623 | } 624 | hdlcoderui.hdlcc { 625 | $ObjectID 14 626 | Version "1.12.0" 627 | Description "HDL Coder custom configuration component" 628 | Name "HDL Coder" 629 | } 630 | PropName "Components" 631 | } 632 | Name "Configuration" 633 | CurrentDlgPage "Solver" 634 | ConfigPrmDlgPosition [ 536, 69, 1435, 811 ] 635 | } 636 | PropName "ConfigurationSets" 637 | } 638 | Simulink.ConfigSet { 639 | $PropName "ActiveConfigurationSet" 640 | $ObjectID 3 641 | } 642 | ExplicitPartitioning off 643 | BlockDefaults { 644 | ForegroundColor "black" 645 | BackgroundColor "white" 646 | DropShadow off 647 | NamePlacement "normal" 648 | FontName "Helvetica" 649 | FontSize 10 650 | FontWeight "normal" 651 | FontAngle "normal" 652 | ShowName on 653 | BlockRotation 0 654 | BlockMirror off 655 | } 656 | AnnotationDefaults { 657 | HorizontalAlignment "center" 658 | VerticalAlignment "middle" 659 | ForegroundColor "black" 660 | BackgroundColor "white" 661 | DropShadow off 662 | FontName "Helvetica" 663 | FontSize 10 664 | FontWeight "normal" 665 | FontAngle "normal" 666 | UseDisplayTextAsClickCallback off 667 | } 668 | LineDefaults { 669 | FontName "Helvetica" 670 | FontSize 9 671 | FontWeight "normal" 672 | FontAngle "normal" 673 | } 674 | BlockParameterDefaults { 675 | Block { 676 | BlockType Constant 677 | Value "1" 678 | VectorParams1D on 679 | SamplingMode "Sample based" 680 | OutMin "[]" 681 | OutMax "[]" 682 | OutDataTypeStr "Inherit: Inherit from 'Constant value'" 683 | LockScale off 684 | SampleTime "inf" 685 | FramePeriod "inf" 686 | PreserveConstantTs off 687 | } 688 | Block { 689 | BlockType DataTypeConversion 690 | OutMin "[]" 691 | OutMax "[]" 692 | OutDataTypeStr "Inherit: Inherit via back propagation" 693 | LockScale off 694 | ConvertRealWorld "Real World Value (RWV)" 695 | RndMeth "Zero" 696 | SaturateOnIntegerOverflow on 697 | SampleTime "-1" 698 | } 699 | Block { 700 | BlockType Demux 701 | Outputs "4" 702 | DisplayOption "none" 703 | BusSelectionMode off 704 | } 705 | Block { 706 | BlockType Gain 707 | Gain "1" 708 | Multiplication "Element-wise(K.*u)" 709 | ParamMin "[]" 710 | ParamMax "[]" 711 | ParamDataTypeStr "Inherit: Same as input" 712 | OutMin "[]" 713 | OutMax "[]" 714 | OutDataTypeStr "Inherit: Same as input" 715 | LockScale off 716 | RndMeth "Floor" 717 | SaturateOnIntegerOverflow on 718 | SampleTime "-1" 719 | } 720 | Block { 721 | BlockType M-S-Function 722 | FunctionName "matlabfile" 723 | DisplayMFileStacktrace on 724 | } 725 | Block { 726 | BlockType Scope 727 | ModelBased off 728 | TickLabels "OneTimeTick" 729 | ZoomMode "on" 730 | Grid "on" 731 | TimeRange "auto" 732 | YMin "-5" 733 | YMax "5" 734 | SaveToWorkspace off 735 | SaveName "ScopeData" 736 | DataFormat "Array" 737 | LimitDataPoints on 738 | MaxDataPoints "5000" 739 | Decimation "1" 740 | SampleInput off 741 | SampleTime "-1" 742 | } 743 | Block { 744 | BlockType ToWorkspace 745 | VariableName "simulink_output" 746 | MaxDataPoints "1000" 747 | Decimation "1" 748 | SampleTime "0" 749 | SaveFormat "Array" 750 | FixptAsFi off 751 | NumInputs "1" 752 | } 753 | } 754 | System { 755 | Name "modelo2variable_step" 756 | Location [889, 101, 1856, 665] 757 | Open on 758 | ModelBrowserVisibility off 759 | ModelBrowserWidth 200 760 | ScreenColor "white" 761 | PaperOrientation "landscape" 762 | PaperPositionMode "auto" 763 | PaperType "usletter" 764 | PaperUnits "inches" 765 | TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] 766 | TiledPageScale 1 767 | ShowPageBoundaries off 768 | ZoomFactor "125" 769 | ReportName "simulink-default.rpt" 770 | SIDHighWatermark "47" 771 | Block { 772 | BlockType Reference 773 | Name "6DoF Animation" 774 | SID "45" 775 | Ports [2] 776 | Position [780, 79, 860, 161] 777 | ZOrder -2 778 | LibraryVersion "1.58" 779 | SourceBlock "aerolibanim/6DoF Animation" 780 | SourceType "6DoF_Animation" 781 | u1 "[0 40 -20 20 -50 -30]" 782 | u2 "0.1" 783 | u3 "1" 784 | u4 "[0 0 0]" 785 | u5 "Fixed position" 786 | u6 "[20 50 -31]" 787 | u7 "10" 788 | u8 on 789 | } 790 | Block { 791 | BlockType Constant 792 | Name "Constant" 793 | SID "42" 794 | Position [765, 245, 795, 275] 795 | ZOrder -4 796 | Value "10" 797 | } 798 | Block { 799 | BlockType Constant 800 | Name "Constant1" 801 | SID "43" 802 | Position [745, 295, 775, 325] 803 | ZOrder -4 804 | Value "20" 805 | } 806 | Block { 807 | BlockType DataTypeConversion 808 | Name "Data Type Conversion1" 809 | SID "23" 810 | Position [590, 367, 660, 403] 811 | ZOrder -7 812 | RndMeth "Floor" 813 | SaturateOnIntegerOverflow off 814 | } 815 | Block { 816 | BlockType Demux 817 | Name "Demux" 818 | SID "18" 819 | Ports [1, 2] 820 | Position [720, 273, 725, 322] 821 | ZOrder -6 822 | ShowName off 823 | Outputs "2" 824 | DisplayOption "bar" 825 | } 826 | Block { 827 | BlockType Demux 828 | Name "Demux1" 829 | SID "20" 830 | Ports [1, 3] 831 | Position [720, 350, 725, 420] 832 | ZOrder -6 833 | ShowName off 834 | Outputs "3" 835 | DisplayOption "bar" 836 | } 837 | Block { 838 | BlockType Demux 839 | Name "Demux2" 840 | SID "33" 841 | Ports [1, 3] 842 | Position [535, 295, 540, 345] 843 | ZOrder -6 844 | NamePlacement "alternate" 845 | ShowName off 846 | Outputs "3" 847 | DisplayOption "bar" 848 | } 849 | Block { 850 | BlockType Reference 851 | Name "Flat Earth to LLA" 852 | SID "30" 853 | Ports [2, 2] 854 | Position [590, 284, 660, 351] 855 | ZOrder -10 856 | LibraryVersion "1.275" 857 | SourceBlock "aerolibtransform2/Flat Earth to LLA" 858 | SourceType "Flat Earth to LLA" 859 | units "Metric (MKS)" 860 | ptype "Earth (WGS84)" 861 | F "1/298.257223563" 862 | R "6378137" 863 | LL0 "[37.665543 -122.480847]" 864 | psi "90" 865 | } 866 | Block { 867 | BlockType Gain 868 | Name "Gain" 869 | SID "10" 870 | Position [145, 235, 175, 265] 871 | ZOrder -8 872 | BlockMirror on 873 | NamePlacement "alternate" 874 | ParamDataTypeStr "Inherit: Inherit via internal rule" 875 | OutDataTypeStr "Inherit: Inherit via internal rule" 876 | SaturateOnIntegerOverflow off 877 | } 878 | Block { 879 | BlockType Reference 880 | Name "Pack\nnet_fdm Packet\nfor FlightGear" 881 | SID "16" 882 | Ports [6, 1] 883 | Position [820, 264, 935, 431] 884 | ZOrder -3 885 | AttributesFormatString "Version Selected: %" 886 | LibraryVersion "1.200" 887 | SourceBlock "aerolibfltsims/Pack\nnet_fdm Packet\nfor FlightGear" 888 | SourceType "FlightGearPackNetFdm" 889 | FlightGearVersion "v2.0" 890 | ShowPositionAttitudeInputs on 891 | ShowVelocityAccelerationInputs off 892 | ShowControlSurfacePositionInputs off 893 | ShowEngineFuelInputs off 894 | ShowLandingGearInputs off 895 | ShowEnvironmentInputs off 896 | SampleTime "-1" 897 | } 898 | Block { 899 | BlockType M-S-Function 900 | Name "Quadrotor" 901 | SID "1" 902 | Ports [1, 5] 903 | Position [225, 78, 465, 202] 904 | ZOrder -3 905 | FunctionName "slQuadrotorPlanta" 906 | MaskInitialization "slQuadrotorPlantaInitialization" 907 | MaskDisplay "port_label('output', 1, 'state x(1:12)')\nport_label('output', 2, 'pos (I)')\nport_label('out" 908 | "put', 3, 'vel (B)')\nport_label('output', 4, 'Euler (I) [Roll Pitch Yaw]')\nport_label('output', 5, '\\omega (B)" 909 | "')\nport_label('input', 1, 'Control [f1 f2 f3 f4]')\n\n" 910 | MaskIconFrame on 911 | MaskIconOpaque on 912 | MaskIconRotate "none" 913 | MaskPortRotate "default" 914 | MaskIconUnits "autoscale" 915 | } 916 | Block { 917 | BlockType M-S-Function 918 | Name "Quadrotor Controller" 919 | SID "5" 920 | Ports [1, 1] 921 | Position [65, 118, 195, 162] 922 | ZOrder -3 923 | FunctionName "slQuadrotorController" 924 | } 925 | Block { 926 | BlockType Scope 927 | Name "Scope" 928 | SID "4" 929 | Ports [1] 930 | Position [520, 74, 550, 106] 931 | ZOrder -3 932 | Floating off 933 | Location [158, 753, 482, 1008] 934 | Open off 935 | NumInputPorts "1" 936 | ZoomMode "yonly" 937 | List { 938 | ListType AxesTitles 939 | axes1 "%" 940 | } 941 | List { 942 | ListType ScopeGraphics 943 | FigureColor "[0.5 0.5 0.5]" 944 | AxesColor "[0 0 0]" 945 | AxesTickColor "[1 1 1]" 946 | LineColors "[1 1 0;1 0 1;0 1 1;1 0 0;0 1 0;0 0 1]" 947 | LineStyles "-|-|-|-|-|-" 948 | LineWidths "[0.5 0.5 0.5 0.5 0.5 0.5]" 949 | MarkerStyles "none|none|none|none|none|none" 950 | } 951 | ShowLegends off 952 | TimeRange "1" 953 | YMin "-45" 954 | YMax "2.5" 955 | DataFormat "StructureWithTime" 956 | SampleTime "0" 957 | } 958 | Block { 959 | BlockType Scope 960 | Name "Scope1" 961 | SID "11" 962 | Ports [1] 963 | Position [570, 99, 600, 131] 964 | ZOrder -3 965 | Floating off 966 | Location [482, 516, 806, 771] 967 | Open off 968 | NumInputPorts "1" 969 | ZoomMode "yonly" 970 | List { 971 | ListType AxesTitles 972 | axes1 "%" 973 | } 974 | List { 975 | ListType ScopeGraphics 976 | FigureColor "[0.5 0.5 0.5]" 977 | AxesColor "[0 0 0]" 978 | AxesTickColor "[1 1 1]" 979 | LineColors "[1 1 0;1 0 1;0 1 1;1 0 0;0 1 0;0 0 1]" 980 | LineStyles "-|-|-|-|-|-" 981 | LineWidths "[0.5 0.5 0.5 0.5 0.5 0.5]" 982 | MarkerStyles "none|none|none|none|none|none" 983 | } 984 | ShowLegends off 985 | TimeRange "1" 986 | YMin "-45" 987 | YMax "2.5" 988 | SaveName "ScopeData1" 989 | DataFormat "StructureWithTime" 990 | SampleTime "0" 991 | } 992 | Block { 993 | BlockType Scope 994 | Name "Scope2" 995 | SID "12" 996 | Ports [1] 997 | Position [620, 124, 650, 156] 998 | ZOrder -3 999 | Floating off 1000 | Location [504, 172, 828, 427] 1001 | Open off 1002 | NumInputPorts "1" 1003 | ZoomMode "yonly" 1004 | List { 1005 | ListType AxesTitles 1006 | axes1 "%" 1007 | } 1008 | List { 1009 | ListType ScopeGraphics 1010 | FigureColor "[0.5 0.5 0.5]" 1011 | AxesColor "[0 0 0]" 1012 | AxesTickColor "[1 1 1]" 1013 | LineColors "[1 1 0;1 0 1;0 1 1;1 0 0;0 1 0;0 0 1]" 1014 | LineStyles "-|-|-|-|-|-" 1015 | LineWidths "[0.5 0.5 0.5 0.5 0.5 0.5]" 1016 | MarkerStyles "none|none|none|none|none|none" 1017 | } 1018 | ShowLegends off 1019 | TimeRange "1" 1020 | YMin "-45" 1021 | YMax "2.5" 1022 | SaveName "ScopeData2" 1023 | DataFormat "StructureWithTime" 1024 | SampleTime "0" 1025 | } 1026 | Block { 1027 | BlockType Scope 1028 | Name "Scope3" 1029 | SID "13" 1030 | Ports [1] 1031 | Position [670, 149, 700, 181] 1032 | ZOrder -3 1033 | Floating off 1034 | Location [137, 130, 461, 385] 1035 | Open off 1036 | NumInputPorts "1" 1037 | ZoomMode "yonly" 1038 | List { 1039 | ListType AxesTitles 1040 | axes1 "%" 1041 | } 1042 | List { 1043 | ListType ScopeGraphics 1044 | FigureColor "[0.5 0.5 0.5]" 1045 | AxesColor "[0 0 0]" 1046 | AxesTickColor "[1 1 1]" 1047 | LineColors "[1 1 0;1 0 1;0 1 1;1 0 0;0 1 0;0 0 1]" 1048 | LineStyles "-|-|-|-|-|-" 1049 | LineWidths "[0.5 0.5 0.5 0.5 0.5 0.5]" 1050 | MarkerStyles "none|none|none|none|none|none" 1051 | } 1052 | ShowLegends off 1053 | TimeRange "1" 1054 | YMin "-0.0025" 1055 | YMax "0.03" 1056 | SaveName "ScopeData3" 1057 | DataFormat "StructureWithTime" 1058 | SampleTime "0" 1059 | } 1060 | Block { 1061 | BlockType Scope 1062 | Name "Scope4" 1063 | SID "14" 1064 | Ports [1] 1065 | Position [715, 169, 745, 201] 1066 | ZOrder -3 1067 | Floating off 1068 | Location [130, 457, 454, 712] 1069 | Open off 1070 | NumInputPorts "1" 1071 | ZoomMode "yonly" 1072 | List { 1073 | ListType AxesTitles 1074 | axes1 "%" 1075 | } 1076 | List { 1077 | ListType ScopeGraphics 1078 | FigureColor "[0.5 0.5 0.5]" 1079 | AxesColor "[0 0 0]" 1080 | AxesTickColor "[1 1 1]" 1081 | LineColors "[1 1 0;1 0 1;0 1 1;1 0 0;0 1 0;0 0 1]" 1082 | LineStyles "-|-|-|-|-|-" 1083 | LineWidths "[0.5 0.5 0.5 0.5 0.5 0.5]" 1084 | MarkerStyles "none|none|none|none|none|none" 1085 | } 1086 | ShowLegends off 1087 | TimeRange "1" 1088 | YMin "-0.01" 1089 | YMax "0.11" 1090 | SaveName "ScopeData4" 1091 | DataFormat "StructureWithTime" 1092 | SampleTime "0" 1093 | } 1094 | Block { 1095 | BlockType Scope 1096 | Name "Scope5" 1097 | SID "15" 1098 | Ports [1] 1099 | Position [315, 334, 345, 366] 1100 | ZOrder -3 1101 | Floating off 1102 | Location [158, 753, 482, 1008] 1103 | Open off 1104 | NumInputPorts "1" 1105 | ZoomMode "yonly" 1106 | List { 1107 | ListType AxesTitles 1108 | axes1 "%" 1109 | } 1110 | List { 1111 | ListType ScopeGraphics 1112 | FigureColor "[0.5 0.5 0.5]" 1113 | AxesColor "[0 0 0]" 1114 | AxesTickColor "[1 1 1]" 1115 | LineColors "[1 1 0;1 0 1;0 1 1;1 0 0;0 1 0;0 0 1]" 1116 | LineStyles "-|-|-|-|-|-" 1117 | LineWidths "[0.5 0.5 0.5 0.5 0.5 0.5]" 1118 | MarkerStyles "none|none|none|none|none|none" 1119 | } 1120 | ShowLegends off 1121 | TimeRange "1" 1122 | YMin "0.265" 1123 | YMax "0.36" 1124 | SaveName "ScopeData5" 1125 | DataFormat "StructureWithTime" 1126 | SampleTime "0" 1127 | } 1128 | Block { 1129 | BlockType Reference 1130 | Name "Send\nnet_fdm Packet\nto FlightGear" 1131 | SID "17" 1132 | Ports [1] 1133 | Position [970, 324, 1050, 376] 1134 | ZOrder -4 1135 | LibraryVersion "1.200" 1136 | SourceBlock "aerolibfltsims/Send\nnet_fdm Packet\nto FlightGear" 1137 | SourceType "FlightGearSendNetFdm" 1138 | DestinationIpAddress "127.0.0.1" 1139 | DestinationPort "5502" 1140 | LocalIpPort "-1" 1141 | varLen off 1142 | SampleTime "-1" 1143 | } 1144 | Block { 1145 | BlockType ToWorkspace 1146 | Name "To Workspace" 1147 | SID "8" 1148 | Ports [1] 1149 | Position [515, 15, 575, 45] 1150 | ZOrder -7 1151 | VariableName "X_simulink" 1152 | MaxDataPoints "inf" 1153 | SampleTime "-1" 1154 | SaveFormat "Timeseries" 1155 | } 1156 | Line { 1157 | SrcBlock "Quadrotor Controller" 1158 | SrcPort 1 1159 | Points [5, 0] 1160 | Branch { 1161 | DstBlock "Quadrotor" 1162 | DstPort 1 1163 | } 1164 | Branch { 1165 | Points [0, 210] 1166 | DstBlock "Scope5" 1167 | DstPort 1 1168 | } 1169 | } 1170 | Line { 1171 | SrcBlock "Quadrotor" 1172 | SrcPort 1 1173 | Points [15, 0] 1174 | Branch { 1175 | Points [0, 0] 1176 | Branch { 1177 | DstBlock "Scope" 1178 | DstPort 1 1179 | } 1180 | Branch { 1181 | Points [0, -60] 1182 | DstBlock "To Workspace" 1183 | DstPort 1 1184 | } 1185 | } 1186 | Branch { 1187 | Points [0, 160] 1188 | DstBlock "Gain" 1189 | DstPort 1 1190 | } 1191 | } 1192 | Line { 1193 | SrcBlock "Gain" 1194 | SrcPort 1 1195 | Points [-105, 0; 0, -110] 1196 | DstBlock "Quadrotor Controller" 1197 | DstPort 1 1198 | } 1199 | Line { 1200 | SrcBlock "Quadrotor" 1201 | SrcPort 2 1202 | Points [25, 0] 1203 | Branch { 1204 | Points [0, 155] 1205 | Branch { 1206 | Points [0, 50] 1207 | DstBlock "Demux2" 1208 | DstPort 1 1209 | } 1210 | Branch { 1211 | Points [55, 0; 0, 30] 1212 | DstBlock "Flat Earth to LLA" 1213 | DstPort 1 1214 | } 1215 | } 1216 | Branch { 1217 | Points [60, 0] 1218 | Branch { 1219 | DstBlock "Scope1" 1220 | DstPort 1 1221 | } 1222 | Branch { 1223 | Points [0, -15] 1224 | DstBlock "6DoF Animation" 1225 | DstPort 1 1226 | } 1227 | } 1228 | } 1229 | Line { 1230 | SrcBlock "Quadrotor" 1231 | SrcPort 3 1232 | DstBlock "Scope2" 1233 | DstPort 1 1234 | } 1235 | Line { 1236 | SrcBlock "Quadrotor" 1237 | SrcPort 4 1238 | Points [40, 0] 1239 | Branch { 1240 | Points [0, 220] 1241 | DstBlock "Data Type Conversion1" 1242 | DstPort 1 1243 | } 1244 | Branch { 1245 | Points [140, 0] 1246 | Branch { 1247 | DstBlock "Scope3" 1248 | DstPort 1 1249 | } 1250 | Branch { 1251 | Points [0, -25] 1252 | DstBlock "6DoF Animation" 1253 | DstPort 2 1254 | } 1255 | } 1256 | } 1257 | Line { 1258 | SrcBlock "Quadrotor" 1259 | SrcPort 5 1260 | Points [190, 0; 0, 10; 45, 0; 0, -15] 1261 | DstBlock "Scope4" 1262 | DstPort 1 1263 | } 1264 | Line { 1265 | SrcBlock "Pack\nnet_fdm Packet\nfor FlightGear" 1266 | SrcPort 1 1267 | DstBlock "Send\nnet_fdm Packet\nto FlightGear" 1268 | DstPort 1 1269 | } 1270 | Line { 1271 | SrcBlock "Demux1" 1272 | SrcPort 1 1273 | DstBlock "Pack\nnet_fdm Packet\nfor FlightGear" 1274 | DstPort 4 1275 | } 1276 | Line { 1277 | SrcBlock "Demux1" 1278 | SrcPort 2 1279 | DstBlock "Pack\nnet_fdm Packet\nfor FlightGear" 1280 | DstPort 5 1281 | } 1282 | Line { 1283 | SrcBlock "Demux1" 1284 | SrcPort 3 1285 | DstBlock "Pack\nnet_fdm Packet\nfor FlightGear" 1286 | DstPort 6 1287 | } 1288 | Line { 1289 | SrcBlock "Data Type Conversion1" 1290 | SrcPort 1 1291 | DstBlock "Demux1" 1292 | DstPort 1 1293 | } 1294 | Line { 1295 | SrcBlock "Flat Earth to LLA" 1296 | SrcPort 1 1297 | DstBlock "Demux" 1298 | DstPort 1 1299 | } 1300 | Line { 1301 | SrcBlock "Flat Earth to LLA" 1302 | SrcPort 2 1303 | DstBlock "Pack\nnet_fdm Packet\nfor FlightGear" 1304 | DstPort 3 1305 | } 1306 | Line { 1307 | SrcBlock "Demux2" 1308 | SrcPort 3 1309 | DstBlock "Flat Earth to LLA" 1310 | DstPort 2 1311 | } 1312 | Line { 1313 | SrcBlock "Constant" 1314 | SrcPort 1 1315 | Points [10, 0; 0, 25] 1316 | DstBlock "Pack\nnet_fdm Packet\nfor FlightGear" 1317 | DstPort 1 1318 | } 1319 | Line { 1320 | SrcBlock "Constant1" 1321 | SrcPort 1 1322 | DstBlock "Pack\nnet_fdm Packet\nfor FlightGear" 1323 | DstPort 2 1324 | } 1325 | } 1326 | } 1327 | -------------------------------------------------------------------------------- /qrsim2-block-diagram.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/clausqr/qrsim2/617c327ecc7afbe210b440a9fcfad54d49c048c4/qrsim2-block-diagram.png -------------------------------------------------------------------------------- /readme.md: -------------------------------------------------------------------------------- 1 | # Quadrotor Simulator using Matlab/Simulink and Flightgear 2 | 3 | The aim of this simulator is to 4 | 1. provide a framework for control and dynamics simulation, 5 | 2. focus on the rigid body attitude control problem, 6 | 3. provide a nice way to visualize what's going on. 7 | 8 | I used it on my thesis to test a global, robust, non-linear controller based on Lyapunov functions. This controller can to track any arbitrary desired orientation of the Quadrotor within a specified error bound, and the results provide a way to translate bounded measurement and navigation errors to final pointing accuracy errors, and also to obtain a bound on the tracking accuracy given the timestep used for the digital implementation of the continuous controller. If you want to find out more please have a look at my [thesis](http://psic.fi.uba.ar/Publicaciones/Tesis/Rosito/Rosito.pdf)! 9 | 10 | [![ScreenShot](https://github.com/clausqr/qrsim2/raw/master/clipWUM7wxO1uRY.png)](https://www.youtube.com/watch?v=WUM7wxO1uRY) 11 | 12 | ##Blocks 13 | The modular approach allows for customization of the different blocks: 14 | ![ScreenShot](https://github.com/clausqr/qrsim2/raw/master/qrsim2-block-diagram.png) 15 | 16 | a. Command Generation Block / Joystick Input 17 | b. Controller Block 18 | c. Rigid Body Dynamics (UAV model) Block 19 | d. Navigation/Measurement Block 20 | e. Visualization Block 21 | 22 | Each block is implemented inside a standalone .m file that gets called from within simulink as a level-2 s-file. The name of the .m files is self explanatory (Quadrotor_Navigation.m, Quadrotor_Controller.m, etc.). 23 | 24 | Global initialization is done with a callback hooked up on ModelInit, implemented through modelo2Initialization.m 25 | 26 | ##Visualization with Flightgear 27 | 28 | The last block allows for Flightgear visualization, see [Flightgear-QUADLSE-Model](https://github.com/clausqr/Flightgear-QUADLSE-Model) for a nice UAV model to use with Flightgear. Otherwise it's straightforward. 29 | 30 | ##Structure 31 | 32 | Almost all functionality is implemented in separate .m files for easy tweaking. 33 | 34 | ## Launching 35 | 36 | 1. Load with simulink: 37 | ```bash 38 | load_model('modelo2a.slx') 39 | ``` 40 | ... 41 | 2. Start Flightgear with arguments to listen for network connections: 42 | ```bash 43 | ./fgfs --fdm=network,localhost,5501,5502,5503 --fog-fastest --disable-clouds --in-air --enable-freeze --aircraft=QUADLSE 44 | ``` 45 | ... 46 | 3. Fly 47 | 4. Tweak 48 | 5. goto 3 49 | -------------------------------------------------------------------------------- /slQuadrotorController.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/clausqr/qrsim2/617c327ecc7afbe210b440a9fcfad54d49c048c4/slQuadrotorController.m -------------------------------------------------------------------------------- /slQuadrotorControllerInitialization.m: -------------------------------------------------------------------------------- 1 | global Quadrotor; %#ok<*NUSED> 2 | global Atmosphere; 3 | global X0; 4 | global Control_Mode; 5 | global CtrlParam; -------------------------------------------------------------------------------- /slQuadrotorNavigation.m: -------------------------------------------------------------------------------- 1 | function slQuadrotorNavigation(block) 2 | %% sl_RigidBodyDynamics 3 | % MSFUNTMPL A Template for a MATLAB S-Function 4 | % The MATLAB S-function is written as a MATLAB function with the 5 | % same name as the S-function. Replace 'msfuntmpl' with the name 6 | % of your S-function. 7 | % 8 | % It should be noted that the MATLAB S-function is very similar 9 | % to Level-2 C-Mex S-functions. You should be able to get more 10 | % information for each of the block methods by referring to the 11 | % documentation for C-Mex S-functions. 12 | % 13 | % Copyright 2003-2010 The MathWorks, Inc. 14 | % $Revision: 1.1.6.22 $ 15 | 16 | % 17 | % The setup method is used to setup the basic attributes of the 18 | % S-function such as ports, parameters, etc. Do not add any other 19 | % calls to the main body of the function. 20 | % 21 | 22 | 23 | setup(block); 24 | 25 | %endfunction 26 | 27 | % Function: setup =================================================== 28 | % Abstract: 29 | % Set up the S-function block's basic characteristics such as: 30 | % - Input ports 31 | % - Output ports 32 | % - Dialog parameters 33 | % - Options 34 | % 35 | % Required : Yes 36 | % C-Mex counterpart: mdlInitializeSizes 37 | % 38 | 39 | function setup(block) 40 | 41 | % Register the number of ports. 42 | block.NumInputPorts = 1; 43 | block.NumOutputPorts = 2; 44 | 45 | % Set up the port properties to be inherited or dynamic. 46 | block.SetPreCompInpPortInfoToDynamic; 47 | block.SetPreCompOutPortInfoToDynamic; 48 | 49 | % Override the input port properties. 50 | block.InputPort(1).DatatypeID = 0; % double 51 | block.InputPort(1).Complexity = 'Real'; 52 | 53 | 54 | 55 | block.OutputPort(1).Dimensions = 9; 56 | block.OutputPort(2).Dimensions = 3; 57 | 58 | % Override the output port properties. 59 | 60 | for k=1:2 61 | block.OutputPort(k).SamplingMode = 'sample'; 62 | block.OutputPort(k).DatatypeID = 0; % double 63 | block.OutputPort(k).Complexity = 'Real'; 64 | end 65 | 66 | 67 | 68 | % Register the parameters. 69 | block.NumDialogPrms = 0; 70 | %block.DialogPrmsTunable = {'Tunable'}; 71 | %block.DialogPrmsTunable = {'Tunable','Nontunable','SimOnlyTunable'}; 72 | 73 | 74 | % Set up the continuous states. 75 | block.NumContStates = 12; 76 | 77 | % Register the sample times. 78 | % [0 offset] : Continuous sample time 79 | % [positive_num offset] : Discrete sample time 80 | % 81 | % [-1, 0] : Inherited sample time 82 | % [-2, 0] : Variable sample time 83 | block.SampleTimes = [0 0]; 84 | 85 | % ----------------------------------------------------------------- 86 | % Options 87 | % ----------------------------------------------------------------- 88 | % Specify if Accelerator should use TLC or call back to the 89 | % MATLAB file 90 | block.SetAccelRunOnTLC(false); 91 | 92 | % Specify the block simStateCompliance. The allowed values are: 93 | % 'UnknownSimState', < The default setting; warn and assume DefaultSimState 94 | % 'DefaultSimState', < Same SimState as a built-in block 95 | % 'HasNoSimState', < No SimState 96 | % 'CustomSimState', < Has GetSimState and SetSimState methods 97 | % 'DisallowSimState' < Errors out when saving or restoring the SimState 98 | block.SimStateCompliance = 'DefaultSimState'; 99 | 100 | % ----------------------------------------------------------------- 101 | % The MATLAB S-function uses an internal registry for all 102 | % block methods. You should register all relevant methods 103 | % (optional and required) as illustrated below. You may choose 104 | % any suitable name for the methods and implement these methods 105 | % as local functions within the same file. 106 | % ----------------------------------------------------------------- 107 | 108 | % ----------------------------------------------------------------- 109 | % Register the methods called during update diagram/compilation. 110 | % ----------------------------------------------------------------- 111 | 112 | % 113 | % CheckParameters: 114 | % Functionality : Called in order to allow validation of the 115 | % block dialog parameters. You are 116 | % responsible for calling this method 117 | % explicitly at the start of the setup method. 118 | % C-Mex counterpart: mdlCheckParameters 119 | % 120 | block.RegBlockMethod('CheckParameters', @CheckPrms); 121 | 122 | % 123 | % SetInputPortSamplingMode: 124 | % Functionality : Check and set input and output port 125 | % attributes and specify whether the port is operating 126 | % in sample-based or frame-based mode 127 | % C-Mex counterpart: mdlSetInputPortFrameData. 128 | % (The DSP System Toolbox is required to set a port as frame-based) 129 | % 130 | block.RegBlockMethod('SetInputPortSamplingMode', @SetInpPortFrameData); 131 | 132 | % 133 | % SetInputPortDimensions: 134 | % Functionality : Check and set the input and optionally the output 135 | % port dimensions. 136 | % C-Mex counterpart: mdlSetInputPortDimensionInfo 137 | % 138 | block.RegBlockMethod('SetInputPortDimensions', @SetInpPortDims); 139 | 140 | % 141 | % SetOutputPortDimensions: 142 | % Functionality : Check and set the output and optionally the input 143 | % port dimensions. 144 | % C-Mex counterpart: mdlSetOutputPortDimensionInfo 145 | % 146 | block.RegBlockMethod('SetOutputPortDimensions', @SetOutPortDims); 147 | 148 | % 149 | % SetInputPortDatatype: 150 | % Functionality : Check and set the input and optionally the output 151 | % port datatypes. 152 | % C-Mex counterpart: mdlSetInputPortDataType 153 | % 154 | block.RegBlockMethod('SetInputPortDataType', @SetInpPortDataType); 155 | 156 | % 157 | % SetOutputPortDatatype: 158 | % Functionality : Check and set the output and optionally the input 159 | % port datatypes. 160 | % C-Mex counterpart: mdlSetOutputPortDataType 161 | % 162 | block.RegBlockMethod('SetOutputPortDataType', @SetOutPortDataType); 163 | 164 | % 165 | % SetInputPortComplexSignal: 166 | % Functionality : Check and set the input and optionally the output 167 | % port complexity attributes. 168 | % C-Mex counterpart: mdlSetInputPortComplexSignal 169 | % 170 | block.RegBlockMethod('SetInputPortComplexSignal', @SetInpPortComplexSig); 171 | 172 | % 173 | % SetOutputPortComplexSignal: 174 | % Functionality : Check and set the output and optionally the input 175 | % port complexity attributes. 176 | % C-Mex counterpart: mdlSetOutputPortComplexSignal 177 | % 178 | block.RegBlockMethod('SetOutputPortComplexSignal', @SetOutPortComplexSig); 179 | 180 | % 181 | % PostPropagationSetup: 182 | % Functionality : Set up the work areas and the state variables. You can 183 | % also register run-time methods here. 184 | % C-Mex counterpart: mdlSetWorkWidths 185 | % 186 | block.RegBlockMethod('PostPropagationSetup', @DoPostPropSetup); 187 | 188 | % ----------------------------------------------------------------- 189 | % Register methods called at run-time 190 | % ----------------------------------------------------------------- 191 | 192 | % 193 | % ProcessParameters: 194 | % Functionality : Call to allow an update of run-time parameters. 195 | % C-Mex counterpart: mdlProcessParameters 196 | % 197 | block.RegBlockMethod('ProcessParameters', @ProcessPrms); 198 | 199 | % 200 | % InitializeConditions: 201 | % Functionality : Call to initialize the state and the work 202 | % area values. 203 | % C-Mex counterpart: mdlInitializeConditions 204 | % 205 | block.RegBlockMethod('InitializeConditions', @InitializeConditions); 206 | 207 | % 208 | % Start: 209 | % Functionality : Call to initialize the state and the work 210 | % area values. 211 | % C-Mex counterpart: mdlStart 212 | % 213 | block.RegBlockMethod('Start', @Start); 214 | 215 | % 216 | % Outputs: 217 | % Functionality : Call to generate the block outputs during a 218 | % simulation step. 219 | % C-Mex counterpart: mdlOutputs 220 | % 221 | block.RegBlockMethod('Outputs', @Outputs); 222 | 223 | % 224 | % Update: 225 | % Functionality : Call to update the discrete states 226 | % during a simulation step. 227 | % C-Mex counterpart: mdlUpdate 228 | % 229 | block.RegBlockMethod('Update', @Update); 230 | 231 | % 232 | % Derivatives: 233 | % Functionality : Call to update the derivatives of the 234 | % continuous states during a simulation step. 235 | % C-Mex counterpart: mdlDerivatives 236 | % 237 | block.RegBlockMethod('Derivatives', @Derivatives); 238 | 239 | % 240 | % Projection: 241 | % Functionality : Call to update the projections during a 242 | % simulation step. 243 | % C-Mex counterpart: mdlProjections 244 | % 245 | block.RegBlockMethod('Projection', @Projection); 246 | 247 | % 248 | % SimStatusChange: 249 | % Functionality : Call when simulation enters pause mode 250 | % or leaves pause mode. 251 | % C-Mex counterpart: mdlSimStatusChange 252 | % 253 | block.RegBlockMethod('SimStatusChange', @SimStatusChange); 254 | 255 | % 256 | % Terminate: 257 | % Functionality : Call at the end of a simulation for cleanup. 258 | % C-Mex counterpart: mdlTerminate 259 | % 260 | block.RegBlockMethod('Terminate', @Terminate); 261 | 262 | % 263 | % GetSimState: 264 | % Functionality : Return the SimState of the block. 265 | % C-Mex counterpart: mdlGetSimState 266 | % 267 | block.RegBlockMethod('GetSimState', @GetSimState); 268 | 269 | % 270 | % SetSimState: 271 | % Functionality : Set the SimState of the block using a given value. 272 | % C-Mex counterpart: mdlSetSimState 273 | % 274 | block.RegBlockMethod('SetSimState', @SetSimState); 275 | 276 | % ----------------------------------------------------------------- 277 | % Register the methods called during code generation. 278 | % ----------------------------------------------------------------- 279 | 280 | % 281 | % WriteRTW: 282 | % Functionality : Write specific information to model.rtw file. 283 | % C-Mex counterpart: mdlRTW 284 | % 285 | block.RegBlockMethod('WriteRTW', @WriteRTW); 286 | %endfunction 287 | 288 | % ------------------------------------------------------------------- 289 | % The local functions below are provided to illustrate how you may implement 290 | % the various block methods listed above. 291 | % ------------------------------------------------------------------- 292 | 293 | function CheckPrms(block) 294 | 295 | % a = block.DialogPrm(1).Data; 296 | % if ~strcmp(class(a), 'double') 297 | % me = MSLException(block.BlockHandle, message('Simulink:blocks:invalidParameter')); 298 | % throw(me); 299 | % end 300 | % 301 | %endfunction 302 | 303 | function ProcessPrms(block) 304 | 305 | block.AutoUpdateRuntimePrms; 306 | 307 | %endfunction 308 | 309 | function SetInpPortFrameData(block, idx, fd) 310 | 311 | block.InputPort(idx).SamplingMode = fd; 312 | % block.OutputPort(idx).SamplingMode = fd; 313 | 314 | %endfunction 315 | 316 | function SetInpPortDims(block, idx, di) 317 | 318 | block.InputPort(idx).Dimensions = di; 319 | 320 | 321 | %endfunction 322 | 323 | function SetOutPortDims(block, idx, di) 324 | 325 | 326 | block.OutputPort(idx).Dimensions = di; 327 | % if (idx==1) 328 | % block.OutputPort(1).Dimensions = 12; 329 | % end 330 | % 331 | %block.InputPort(1).Dimensions = di; 332 | 333 | %endfunction 334 | 335 | function SetInpPortDataType(block, idx, dt) 336 | 337 | block.InputPort(idx).DataTypeID = dt; 338 | %block.OutputPort(1).DataTypeID = dt; 339 | 340 | %endfunction 341 | 342 | function SetOutPortDataType(block, idx, dt) 343 | 344 | block.OutputPort(idx).DataTypeID = dt; 345 | %block.InputPort(1).DataTypeID = dt; 346 | 347 | %endfunction 348 | 349 | function SetInpPortComplexSig(block, idx, c) 350 | 351 | block.InputPort(idx).Complexity = c; 352 | % block.OutputPort(idx).Complexity = c; 353 | 354 | %endfunction 355 | 356 | function SetOutPortComplexSig(block, idx, c) 357 | 358 | block.OutputPort(idx).Complexity = c; 359 | %block.InputPort(1).Complexity = c; 360 | 361 | %endfunction 362 | 363 | function DoPostPropSetup(block) 364 | block.NumDworks = 0; 365 | %No tenemos estados discretos!! 366 | % block.Dwork(1).Name = 'x1'; 367 | % block.Dwork(1).Dimensions = 12; 368 | % block.Dwork(1).DatatypeID = 0; % double 369 | % block.Dwork(1).Complexity = 'Real'; % real 370 | % block.Dwork(1).UsedAsDiscState = false; 371 | % 372 | 373 | % Register all tunable parameters as runtime parameters. 374 | block.AutoRegRuntimePrms; 375 | 376 | %endfunction 377 | 378 | function InitializeConditions(block) 379 | 380 | %esto se llama cada vez que se resetea el estado 381 | 382 | global X0; 383 | block.ContStates.Data=X0; 384 | 385 | %endfunction 386 | 387 | function Start(block) 388 | 389 | %esto se llama una sola vez al comienzo 390 | global X0; 391 | block.ContStates.Data=X0; 392 | 393 | %endfunction 394 | 395 | function WriteRTW(block) 396 | 397 | % block.WriteRTWParam('matrix', 'M', [1 2; 3 4]); 398 | % block.WriteRTWParam('string', 'Mode', 'Auto'); 399 | % 400 | %endfunction 401 | 402 | function Outputs(block) 403 | % X - state vector of the rigid body 404 | % X(1,1) = x; X-position of CM w.r.t. I, expressed in I 405 | % X(2,1) = y; Y-position of CM w.r.t. I, expressed in I 406 | % X(3,1) = z; Y-position of CM w.r.t. I, expressed in I 407 | % X(4,1) = u; X-velocity of CM w.r.t. I, expressed in B 408 | % X(5,1) = v; Y-velocity of CM w.r.t. I, expressed in B 409 | % X(6,1) = w; Z-velocity of CM w.r.t. I, expressed in B 410 | % X(7,1) = phi; Standard Roll Euler angle of B w.r.t I 411 | % X(8,1) = theta; Standard Pitch Euler angle of B w.r.t I 412 | % X(9,1) = psi; Standard Yaw Euler angle of B w.r.t I 413 | % X(10,1) = wx; X-angular velocity of B frame w.r.t. I, expressed in B 414 | % X(11,1) = wy; Y-angular velocity of B frame w.r.t. I, expressed in B 415 | % X(12,1) = wz; Z-angular velocity of B frame w.r.t. I, expressed in B 416 | %X=block.ContStates.Data; 417 | 418 | global Quadrotor; 419 | global Atmosphere; 420 | global Control_Mode; 421 | global CtrlParam; 422 | global t_step; 423 | persistent t_old; 424 | 425 | if isempty(t_old) 426 | t_old=0; 427 | end 428 | 429 | X = block.InputPort(1).Data; 430 | 431 | [R_est, Omega_est] = Quadrotor_Navigation(CtrlParam, X); 432 | 433 | block.OutputPort(1).Data = R_est; %State 434 | block.OutputPort(2).Data = Omega_est; 435 | 436 | 437 | %endfunction 438 | 439 | function Update(block) 440 | 441 | 442 | 443 | %endfunction 444 | 445 | function Derivatives(block) 446 | 447 | %% RigidBodyDynamics - Differential equations of 3D motion of a rigid body 448 | 449 | % Author: Roberto A. Bunge, Ph.D. Candidate 450 | % Department of Aeronautics and Astronautics 451 | % Stanford University 452 | % email address: rbunge@stanford.edu 453 | % June 2011; Last revision: 18-Sep-2011 454 | 455 | % Given the mass properties and the net force and moment about the 456 | % center of mass, returns the derivative of the state 457 | % vector of the rigid body. 458 | % 459 | % Notation: 460 | % I = inertial frame 461 | % B = body fixed frame. Origin of B doesn't necessarily coincide with CM 462 | % CM = center of mass 463 | % p = the suffix "p", indicates the derivative of the variable (e.g. 464 | % "wxp" means derivative of wx, which is angular acceleration in x) 465 | % 466 | % Inputs: 467 | % X - state vector of the rigid body 468 | % X(1,1) = x; X-position of CM w.r.t. I, expressed in I 469 | % X(2,1) = y; Y-position of CM w.r.t. I, expressed in I 470 | % X(3,1) = z; Y-position of CM w.r.t. I, expressed in I 471 | % X(4,1) = u; X-velocity of CM w.r.t. I, expressed in B 472 | % X(5,1) = v; Y-velocity of CM w.r.t. I, expressed in B 473 | % X(6,1) = w; Z-velocity of CM w.r.t. I, expressed in B 474 | % X(7,1) = phi; Standard Roll Euler angle of B w.r.t I 475 | % X(8,1) = theta; Standard Pitch Euler angle of B w.r.t I 476 | % X(9,1) = psi; Standard Yaw Euler angle of B w.r.t I 477 | % X(10,1) = wx; X-angular velocity of B frame w.r.t. I, expressed in B 478 | % X(11,1) = wy; Y-angular velocity of B frame w.r.t. I, expressed in B 479 | % X(12,1) = wz; Z-angular velocity of B frame w.r.t. I, expressed in B 480 | % 481 | % Mass_prop - structure that contains mass and inertia scalars 482 | % 483 | % Net_Forces - total force acting on rigid body, expressed in B 484 | % 485 | % Net_Moments_CM - total moment acting on rigid body about center of mass, expressed in B. 486 | % 487 | % Outputs: 488 | % Xp - first derivative of state vector X 489 | % 490 | % Other m-files required: none 491 | % Subfunctions: none 492 | % MAT-files required: none 493 | % 494 | % See also: none 495 | 496 | %function Xp = RigidBodyDynamics(X, Mass_prop, Net_Forces, Net_Moments_CM) 497 | % Reading STATE VARIABLES 498 | 499 | %%PREPARE function parameters 500 | 501 | 502 | 503 | %endfunction 504 | 505 | function Projection(block) 506 | 507 | states = block.ContStates.Data; 508 | block.ContStates.Data = states+eps; 509 | 510 | %endfunction 511 | 512 | function SimStatusChange(block, s) 513 | 514 | if s == 0 515 | disp('Pause in simulation.'); 516 | elseif s == 1 517 | disp('Resume simulation.'); 518 | end 519 | 520 | %endfunction 521 | 522 | function Terminate(block) 523 | 524 | disp(['Terminating the block with handle ' num2str(block.BlockHandle) '.']); 525 | 526 | %endfunction 527 | 528 | function outSimState = GetSimState(block) 529 | 530 | %outSimState = block.Dwork(1).Data; 531 | 532 | %endfunction 533 | 534 | function SetSimState(block, inSimState) 535 | 536 | %block.Dwork(1).Data = inSimState; 537 | 538 | %endfunction 539 | -------------------------------------------------------------------------------- /slQuadrotorPlanta.m: -------------------------------------------------------------------------------- 1 | function slQUadrotorPlanta(block) 2 | %% sl_RigidBodyDynamics 3 | % MSFUNTMPL A Template for a MATLAB S-Function 4 | % The MATLAB S-function is written as a MATLAB function with the 5 | % same name as the S-function. Replace 'msfuntmpl' with the name 6 | % of your S-function. 7 | % 8 | % It should be noted that the MATLAB S-function is very similar 9 | % to Level-2 C-Mex S-functions. You should be able to get more 10 | % information for each of the block methods by referring to the 11 | % documentation for C-Mex S-functions. 12 | % 13 | % Copyright 2003-2010 The MathWorks, Inc. 14 | % $Revision: 1.1.6.22 $ 15 | 16 | % 17 | % The setup method is used to setup the basic attributes of the 18 | % S-function such as ports, parameters, etc. Do not add any other 19 | % calls to the main body of the function. 20 | % 21 | setup(block); 22 | 23 | %endfunction 24 | 25 | % Function: setup =================================================== 26 | % Abstract: 27 | % Set up the S-function block's basic characteristics such as: 28 | % - Input ports 29 | % - Output ports 30 | % - Dialog parameters 31 | % - Options 32 | % 33 | % Required : Yes 34 | % C-Mex counterpart: mdlInitializeSizes 35 | % 36 | 37 | function setup(block) 38 | 39 | % Register the number of ports. 40 | block.NumInputPorts = 1; 41 | block.NumOutputPorts = 5; 42 | 43 | % Set up the port properties to be inherited or dynamic. 44 | block.SetPreCompInpPortInfoToDynamic; 45 | block.SetPreCompOutPortInfoToDynamic; 46 | 47 | % Override the input port properties. 48 | block.InputPort(1).DatatypeID = 0; % double 49 | block.InputPort(1).Complexity = 'Real'; 50 | 51 | 52 | block.OutputPort(1).Dimensions = 12; 53 | block.OutputPort(2).Dimensions = 3; 54 | block.OutputPort(3).Dimensions = 3; 55 | block.OutputPort(4).Dimensions = 3; 56 | block.OutputPort(5).Dimensions = 3; 57 | % Override the output port properties. 58 | 59 | for k=1:5 60 | block.OutputPort(k).SamplingMode = 'sample'; 61 | block.OutputPort(k).DatatypeID = 0; % double 62 | block.OutputPort(k).Complexity = 'Real'; 63 | end 64 | 65 | 66 | 67 | % Register the parameters. 68 | block.NumDialogPrms = 0; 69 | %block.DialogPrmsTunable = {'Tunable'}; 70 | %block.DialogPrmsTunable = {'Tunable','Nontunable','SimOnlyTunable'}; 71 | 72 | 73 | % Set up the continuous states. 74 | block.NumContStates = 12; 75 | 76 | % Register the sample times. 77 | % [0 offset] : Continuous sample time 78 | % [positive_num offset] : Discrete sample time 79 | % 80 | % [-1, 0] : Inherited sample time 81 | % [-2, 0] : Variable sample time 82 | block.SampleTimes = [0 0]; 83 | 84 | % ----------------------------------------------------------------- 85 | % Options 86 | % ----------------------------------------------------------------- 87 | % Specify if Accelerator should use TLC or call back to the 88 | % MATLAB file 89 | block.SetAccelRunOnTLC(false); 90 | 91 | % Specify the block simStateCompliance. The allowed values are: 92 | % 'UnknownSimState', < The default setting; warn and assume DefaultSimState 93 | % 'DefaultSimState', < Same SimState as a built-in block 94 | % 'HasNoSimState', < No SimState 95 | % 'CustomSimState', < Has GetSimState and SetSimState methods 96 | % 'DisallowSimState' < Errors out when saving or restoring the SimState 97 | block.SimStateCompliance = 'DefaultSimState'; 98 | 99 | % ----------------------------------------------------------------- 100 | % The MATLAB S-function uses an internal registry for all 101 | % block methods. You should register all relevant methods 102 | % (optional and required) as illustrated below. You may choose 103 | % any suitable name for the methods and implement these methods 104 | % as local functions within the same file. 105 | % ----------------------------------------------------------------- 106 | 107 | % ----------------------------------------------------------------- 108 | % Register the methods called during update diagram/compilation. 109 | % ----------------------------------------------------------------- 110 | 111 | % 112 | % CheckParameters: 113 | % Functionality : Called in order to allow validation of the 114 | % block dialog parameters. You are 115 | % responsible for calling this method 116 | % explicitly at the start of the setup method. 117 | % C-Mex counterpart: mdlCheckParameters 118 | % 119 | block.RegBlockMethod('CheckParameters', @CheckPrms); 120 | 121 | % 122 | % SetInputPortSamplingMode: 123 | % Functionality : Check and set input and output port 124 | % attributes and specify whether the port is operating 125 | % in sample-based or frame-based mode 126 | % C-Mex counterpart: mdlSetInputPortFrameData. 127 | % (The DSP System Toolbox is required to set a port as frame-based) 128 | % 129 | block.RegBlockMethod('SetInputPortSamplingMode', @SetInpPortFrameData); 130 | 131 | % 132 | % SetInputPortDimensions: 133 | % Functionality : Check and set the input and optionally the output 134 | % port dimensions. 135 | % C-Mex counterpart: mdlSetInputPortDimensionInfo 136 | % 137 | block.RegBlockMethod('SetInputPortDimensions', @SetInpPortDims); 138 | 139 | % 140 | % SetOutputPortDimensions: 141 | % Functionality : Check and set the output and optionally the input 142 | % port dimensions. 143 | % C-Mex counterpart: mdlSetOutputPortDimensionInfo 144 | % 145 | block.RegBlockMethod('SetOutputPortDimensions', @SetOutPortDims); 146 | 147 | % 148 | % SetInputPortDatatype: 149 | % Functionality : Check and set the input and optionally the output 150 | % port datatypes. 151 | % C-Mex counterpart: mdlSetInputPortDataType 152 | % 153 | block.RegBlockMethod('SetInputPortDataType', @SetInpPortDataType); 154 | 155 | % 156 | % SetOutputPortDatatype: 157 | % Functionality : Check and set the output and optionally the input 158 | % port datatypes. 159 | % C-Mex counterpart: mdlSetOutputPortDataType 160 | % 161 | block.RegBlockMethod('SetOutputPortDataType', @SetOutPortDataType); 162 | 163 | % 164 | % SetInputPortComplexSignal: 165 | % Functionality : Check and set the input and optionally the output 166 | % port complexity attributes. 167 | % C-Mex counterpart: mdlSetInputPortComplexSignal 168 | % 169 | block.RegBlockMethod('SetInputPortComplexSignal', @SetInpPortComplexSig); 170 | 171 | % 172 | % SetOutputPortComplexSignal: 173 | % Functionality : Check and set the output and optionally the input 174 | % port complexity attributes. 175 | % C-Mex counterpart: mdlSetOutputPortComplexSignal 176 | % 177 | block.RegBlockMethod('SetOutputPortComplexSignal', @SetOutPortComplexSig); 178 | 179 | % 180 | % PostPropagationSetup: 181 | % Functionality : Set up the work areas and the state variables. You can 182 | % also register run-time methods here. 183 | % C-Mex counterpart: mdlSetWorkWidths 184 | % 185 | block.RegBlockMethod('PostPropagationSetup', @DoPostPropSetup); 186 | 187 | % ----------------------------------------------------------------- 188 | % Register methods called at run-time 189 | % ----------------------------------------------------------------- 190 | 191 | % 192 | % ProcessParameters: 193 | % Functionality : Call to allow an update of run-time parameters. 194 | % C-Mex counterpart: mdlProcessParameters 195 | % 196 | block.RegBlockMethod('ProcessParameters', @ProcessPrms); 197 | 198 | % 199 | % InitializeConditions: 200 | % Functionality : Call to initialize the state and the work 201 | % area values. 202 | % C-Mex counterpart: mdlInitializeConditions 203 | % 204 | block.RegBlockMethod('InitializeConditions', @InitializeConditions); 205 | 206 | % 207 | % Start: 208 | % Functionality : Call to initialize the state and the work 209 | % area values. 210 | % C-Mex counterpart: mdlStart 211 | % 212 | block.RegBlockMethod('Start', @Start); 213 | 214 | % 215 | % Outputs: 216 | % Functionality : Call to generate the block outputs during a 217 | % simulation step. 218 | % C-Mex counterpart: mdlOutputs 219 | % 220 | block.RegBlockMethod('Outputs', @Outputs); 221 | 222 | % 223 | % Update: 224 | % Functionality : Call to update the discrete states 225 | % during a simulation step. 226 | % C-Mex counterpart: mdlUpdate 227 | % 228 | block.RegBlockMethod('Update', @Update); 229 | 230 | % 231 | % Derivatives: 232 | % Functionality : Call to update the derivatives of the 233 | % continuous states during a simulation step. 234 | % C-Mex counterpart: mdlDerivatives 235 | % 236 | block.RegBlockMethod('Derivatives', @Derivatives); 237 | 238 | % 239 | % Projection: 240 | % Functionality : Call to update the projections during a 241 | % simulation step. 242 | % C-Mex counterpart: mdlProjections 243 | % 244 | block.RegBlockMethod('Projection', @Projection); 245 | 246 | % 247 | % SimStatusChange: 248 | % Functionality : Call when simulation enters pause mode 249 | % or leaves pause mode. 250 | % C-Mex counterpart: mdlSimStatusChange 251 | % 252 | block.RegBlockMethod('SimStatusChange', @SimStatusChange); 253 | 254 | % 255 | % Terminate: 256 | % Functionality : Call at the end of a simulation for cleanup. 257 | % C-Mex counterpart: mdlTerminate 258 | % 259 | block.RegBlockMethod('Terminate', @Terminate); 260 | 261 | % 262 | % GetSimState: 263 | % Functionality : Return the SimState of the block. 264 | % C-Mex counterpart: mdlGetSimState 265 | % 266 | block.RegBlockMethod('GetSimState', @GetSimState); 267 | 268 | % 269 | % SetSimState: 270 | % Functionality : Set the SimState of the block using a given value. 271 | % C-Mex counterpart: mdlSetSimState 272 | % 273 | block.RegBlockMethod('SetSimState', @SetSimState); 274 | 275 | % ----------------------------------------------------------------- 276 | % Register the methods called during code generation. 277 | % ----------------------------------------------------------------- 278 | 279 | % 280 | % WriteRTW: 281 | % Functionality : Write specific information to model.rtw file. 282 | % C-Mex counterpart: mdlRTW 283 | % 284 | block.RegBlockMethod('WriteRTW', @WriteRTW); 285 | %endfunction 286 | 287 | % ------------------------------------------------------------------- 288 | % The local functions below are provided to illustrate how you may implement 289 | % the various block methods listed above. 290 | % ------------------------------------------------------------------- 291 | 292 | function CheckPrms(block) 293 | 294 | % a = block.DialogPrm(1).Data; 295 | % if ~strcmp(class(a), 'double') 296 | % me = MSLException(block.BlockHandle, message('Simulink:blocks:invalidParameter')); 297 | % throw(me); 298 | % end 299 | % 300 | %endfunction 301 | 302 | function ProcessPrms(block) 303 | 304 | block.AutoUpdateRuntimePrms; 305 | 306 | %endfunction 307 | 308 | function SetInpPortFrameData(block, idx, fd) 309 | 310 | block.InputPort(idx).SamplingMode = fd; 311 | % block.OutputPort(idx).SamplingMode = fd; 312 | 313 | %endfunction 314 | 315 | function SetInpPortDims(block, idx, di) 316 | 317 | block.InputPort(idx).Dimensions = di; 318 | 319 | 320 | %endfunction 321 | 322 | function SetOutPortDims(block, idx, di) 323 | 324 | 325 | block.OutputPort(idx).Dimensions = di; 326 | % if (idx==1) 327 | % block.OutputPort(1).Dimensions = 12; 328 | % end 329 | % 330 | %block.InputPort(1).Dimensions = di; 331 | 332 | %endfunction 333 | 334 | function SetInpPortDataType(block, idx, dt) 335 | 336 | block.InputPort(idx).DataTypeID = dt; 337 | %block.OutputPort(1).DataTypeID = dt; 338 | 339 | %endfunction 340 | 341 | function SetOutPortDataType(block, idx, dt) 342 | 343 | block.OutputPort(idx).DataTypeID = dt; 344 | %block.InputPort(1).DataTypeID = dt; 345 | 346 | %endfunction 347 | 348 | function SetInpPortComplexSig(block, idx, c) 349 | 350 | block.InputPort(idx).Complexity = c; 351 | % block.OutputPort(idx).Complexity = c; 352 | 353 | %endfunction 354 | 355 | function SetOutPortComplexSig(block, idx, c) 356 | 357 | block.OutputPort(idx).Complexity = c; 358 | %block.InputPort(1).Complexity = c; 359 | 360 | %endfunction 361 | 362 | function DoPostPropSetup(block) 363 | block.NumDworks = 0; 364 | %No tenemos estados discretos!! 365 | % block.Dwork(1).Name = 'x1'; 366 | % block.Dwork(1).Dimensions = 12; 367 | % block.Dwork(1).DatatypeID = 0; % double 368 | % block.Dwork(1).Complexity = 'Real'; % real 369 | % block.Dwork(1).UsedAsDiscState = false; 370 | % 371 | 372 | % Register all tunable parameters as runtime parameters. 373 | block.AutoRegRuntimePrms; 374 | 375 | %endfunction 376 | 377 | function InitializeConditions(block) 378 | 379 | %esto se llama cada vez que se resetea el estado 380 | 381 | global X0; 382 | block.ContStates.Data=X0; 383 | 384 | %endfunction 385 | 386 | function Start(block) 387 | 388 | %esto se llama una sola vez al comienzo 389 | global X0; 390 | block.ContStates.Data=X0; 391 | 392 | %endfunction 393 | 394 | function WriteRTW(block) 395 | 396 | % block.WriteRTWParam('matrix', 'M', [1 2; 3 4]); 397 | % block.WriteRTWParam('string', 'Mode', 'Auto'); 398 | % 399 | %endfunction 400 | 401 | function Outputs(block) 402 | % X - state vector of the rigid body 403 | % X(1,1) = x; X-position of CM w.r.t. I, expressed in I 404 | % X(2,1) = y; Y-position of CM w.r.t. I, expressed in I 405 | % X(3,1) = z; Y-position of CM w.r.t. I, expressed in I 406 | % X(4,1) = u; X-velocity of CM w.r.t. I, expressed in B 407 | % X(5,1) = v; Y-velocity of CM w.r.t. I, expressed in B 408 | % X(6,1) = w; Z-velocity of CM w.r.t. I, expressed in B 409 | % X(7,1) = phi; Standard Roll Euler angle of B w.r.t I 410 | % X(8,1) = theta; Standard Pitch Euler angle of B w.r.t I 411 | % X(9,1) = psi; Standard Yaw Euler angle of B w.r.t I 412 | % X(10,1) = wx; X-angular velocity of B frame w.r.t. I, expressed in B 413 | % X(11,1) = wy; Y-angular velocity of B frame w.r.t. I, expressed in B 414 | % X(12,1) = wz; Z-angular velocity of B frame w.r.t. I, expressed in B 415 | X=block.ContStates.Data; 416 | block.OutputPort(1).Data = X; %State 417 | block.OutputPort(2).Data = X(1:3); %Position (I) 418 | block.OutputPort(3).Data = X(4:6); %Velocity (B) 419 | block.OutputPort(4).Data = X(7:9); %Actitude (Euler Angles) (I) 420 | block.OutputPort(5).Data = X(10:12); %Angular Velocity (B) 421 | 422 | 423 | %endfunction 424 | 425 | function Update(block) 426 | 427 | 428 | 429 | %endfunction 430 | 431 | function Derivatives(block) 432 | 433 | %% RigidBodyDynamics - Differential equations of 3D motion of a rigid body 434 | 435 | % Author: Roberto A. Bunge, Ph.D. Candidate 436 | % Department of Aeronautics and Astronautics 437 | % Stanford University 438 | % email address: rbunge@stanford.edu 439 | % June 2011; Last revision: 18-Sep-2011 440 | 441 | % Given the mass properties and the net force and moment about the 442 | % center of mass, returns the derivative of the state 443 | % vector of the rigid body. 444 | % 445 | % Notation: 446 | % I = inertial frame 447 | % B = body fixed frame. Origin of B doesn't necessarily coincide with CM 448 | % CM = center of mass 449 | % p = the suffix "p", indicates the derivative of the variable (e.g. 450 | % "wxp" means derivative of wx, which is angular acceleration in x) 451 | % 452 | % Inputs: 453 | % X - state vector of the rigid body 454 | % X(1,1) = x; X-position of CM w.r.t. I, expressed in I 455 | % X(2,1) = y; Y-position of CM w.r.t. I, expressed in I 456 | % X(3,1) = z; Y-position of CM w.r.t. I, expressed in I 457 | % X(4,1) = u; X-velocity of CM w.r.t. I, expressed in B 458 | % X(5,1) = v; Y-velocity of CM w.r.t. I, expressed in B 459 | % X(6,1) = w; Z-velocity of CM w.r.t. I, expressed in B 460 | % X(7,1) = phi; Standard Roll Euler angle of B w.r.t I 461 | % X(8,1) = theta; Standard Pitch Euler angle of B w.r.t I 462 | % X(9,1) = psi; Standard Yaw Euler angle of B w.r.t I 463 | % X(10,1) = wx; X-angular velocity of B frame w.r.t. I, expressed in B 464 | % X(11,1) = wy; Y-angular velocity of B frame w.r.t. I, expressed in B 465 | % X(12,1) = wz; Z-angular velocity of B frame w.r.t. I, expressed in B 466 | % 467 | % Mass_prop - structure that contains mass and inertia scalars 468 | % 469 | % Net_Forces - total force acting on rigid body, expressed in B 470 | % 471 | % Net_Moments_CM - total moment acting on rigid body about center of mass, expressed in B. 472 | % 473 | % Outputs: 474 | % Xp - first derivative of state vector X 475 | % 476 | % Other m-files required: none 477 | % Subfunctions: none 478 | % MAT-files required: none 479 | % 480 | % See also: none 481 | 482 | %function Xp = RigidBodyDynamics(X, Mass_prop, Net_Forces, Net_Moments_CM) 483 | % Reading STATE VARIABLES 484 | 485 | %%PREPARE function parameters 486 | X=block.ContStates.Data; 487 | Control = block.InputPort(1).Data; 488 | %Thrust = block.InputPort(2).Data; 489 | 490 | 491 | %% CALL function RigidBodyDynamics 492 | global Quadrotor; 493 | global Atmosphere; 494 | global Aero_Model; 495 | global CtrlParam; 496 | deltaR=CtrlParam.deltaR; 497 | [Aero_Forces, Aero_Moments_CM] = Quadrotor_Aerodynamics(Atmosphere, Quadrotor, X, Control, Aero_Model,deltaR); 498 | [Gravity_Force] = Gravity_Forces(Atmosphere, Quadrotor, X); 499 | 500 | Net_Forces = Gravity_Force + Aero_Forces; 501 | Net_Moments_CM = zeros(1,3) + Aero_Moments_CM; 502 | 503 | 504 | Xp = RigidBodyDynamics(X, Quadrotor.Mass_prop, Net_Forces, Net_Moments_CM); 505 | 506 | block.Derivatives.Data = Xp; 507 | 508 | %endfunction 509 | 510 | function Projection(block) 511 | 512 | states = block.ContStates.Data; 513 | block.ContStates.Data = states+eps; 514 | 515 | %endfunction 516 | 517 | function SimStatusChange(block, s) 518 | 519 | if s == 0 520 | disp('Pause in simulation.'); 521 | elseif s == 1 522 | disp('Resume simulation.'); 523 | end 524 | 525 | %endfunction 526 | 527 | function Terminate(block) 528 | 529 | disp(['Terminating the block with handle ' num2str(block.BlockHandle) '.']); 530 | 531 | %endfunction 532 | 533 | function outSimState = GetSimState(block) 534 | 535 | %outSimState = block.Dwork(1).Data; 536 | 537 | %endfunction 538 | 539 | function SetSimState(block, inSimState) 540 | 541 | %block.Dwork(1).Data = inSimState; 542 | 543 | %endfunction 544 | -------------------------------------------------------------------------------- /slQuadrotorPlantaInitialization.m: -------------------------------------------------------------------------------- 1 | global Quadrotor; 2 | global Atmosphere; 3 | global Aero_Model; 4 | global X0; -------------------------------------------------------------------------------- /slToFlightgear.mdl: -------------------------------------------------------------------------------- 1 | Model { 2 | Name "slToFlightgear" 3 | Version 7.9 4 | MdlSubVersion 0 5 | GraphicalInterface { 6 | NumRootInports 2 7 | Inport { 8 | BusObject "" 9 | Name "position" 10 | } 11 | Inport { 12 | BusObject "" 13 | Name "attitude" 14 | } 15 | NumRootOutports 0 16 | ParameterArgumentNames "" 17 | ComputedModelVersion "1.28" 18 | NumModelReferences 0 19 | NumTestPointedSignals 0 20 | } 21 | SavedCharacterEncoding "ISO-8859-1" 22 | slprops.hdlmdlprops { 23 | $PropName "HDLParams" 24 | $ObjectID 1 25 | Array { 26 | Type "Cell" 27 | Dimension 2 28 | Cell "HDLSubsystem" 29 | Cell "modelo2" 30 | PropName "mdlProps" 31 | } 32 | } 33 | SaveDefaultBlockParams on 34 | ScopeRefreshTime 0.035000 35 | OverrideScopeRefreshTime on 36 | DisableAllScopes off 37 | DataTypeOverride "UseLocalSettings" 38 | DataTypeOverrideAppliesTo "AllNumericTypes" 39 | MinMaxOverflowLogging "UseLocalSettings" 40 | MinMaxOverflowArchiveMode "Overwrite" 41 | FPTRunName "Run 1" 42 | MaxMDLFileLineLength 120 43 | InitFcn "modelo2Initialization" 44 | Created "Tue Aug 28 10:09:52 2012" 45 | Creator "claus" 46 | UpdateHistory "UpdateHistoryNever" 47 | ModifiedByFormat "%" 48 | LastModifiedBy "claus" 49 | ModifiedDateFormat "%" 50 | LastModifiedDate "Wed Oct 17 19:21:13 2012" 51 | RTWModifiedTimeStamp 272402106 52 | ModelVersionFormat "1.%" 53 | ConfigurationManager "None" 54 | SampleTimeColors off 55 | SampleTimeAnnotations off 56 | LibraryLinkDisplay "disabled" 57 | WideLines off 58 | ShowLineDimensions off 59 | ShowPortDataTypes off 60 | ShowDesignRanges off 61 | ShowLoopsOnError on 62 | IgnoreBidirectionalLines off 63 | ShowStorageClass off 64 | ShowTestPointIcons on 65 | ShowSignalResolutionIcons on 66 | ShowViewerIcons on 67 | SortedOrder off 68 | ExecutionContextIcon off 69 | ShowLinearizationAnnotations on 70 | BlockNameDataTip off 71 | BlockParametersDataTip off 72 | BlockDescriptionStringDataTip off 73 | ToolBar on 74 | StatusBar on 75 | BrowserShowLibraryLinks off 76 | BrowserLookUnderMasks off 77 | SimulationMode "normal" 78 | LinearizationMsg "none" 79 | Profile off 80 | ParamWorkspaceSource "MATLABWorkspace" 81 | AccelSystemTargetFile "accel.tlc" 82 | AccelTemplateMakefile "accel_default_tmf" 83 | AccelMakeCommand "make_rtw" 84 | TryForcingSFcnDF off 85 | Object { 86 | $PropName "DataLoggingOverride" 87 | $ObjectID 2 88 | $ClassName "Simulink.SimulationData.ModelLoggingInfo" 89 | model_ "slToFlightgear" 90 | signals_ [] 91 | overrideMode_ [0.0] 92 | Array { 93 | Type "Cell" 94 | Dimension 1 95 | Cell "slToFlightgear" 96 | PropName "logAsSpecifiedByModels_" 97 | } 98 | Array { 99 | Type "Cell" 100 | Dimension 1 101 | Cell [] 102 | PropName "logAsSpecifiedByModelsSSIDs_" 103 | } 104 | } 105 | RecordCoverage off 106 | CovPath "/" 107 | CovSaveName "covdata" 108 | CovMetricSettings "dw" 109 | CovNameIncrementing off 110 | CovHtmlReporting on 111 | CovForceBlockReductionOff on 112 | covSaveCumulativeToWorkspaceVar on 113 | CovSaveSingleToWorkspaceVar on 114 | CovCumulativeVarName "covCumulativeData" 115 | CovCumulativeReport off 116 | CovReportOnPause on 117 | CovModelRefEnable "Off" 118 | CovExternalEMLEnable off 119 | ExtModeBatchMode off 120 | ExtModeEnableFloating on 121 | ExtModeTrigType "manual" 122 | ExtModeTrigMode "normal" 123 | ExtModeTrigPort "1" 124 | ExtModeTrigElement "any" 125 | ExtModeTrigDuration 1000 126 | ExtModeTrigDurationFloating "auto" 127 | ExtModeTrigHoldOff 0 128 | ExtModeTrigDelay 0 129 | ExtModeTrigDirection "rising" 130 | ExtModeTrigLevel 0 131 | ExtModeArchiveMode "off" 132 | ExtModeAutoIncOneShot off 133 | ExtModeIncDirWhenArm off 134 | ExtModeAddSuffixToVar off 135 | ExtModeWriteAllDataToWs off 136 | ExtModeArmWhenConnect on 137 | ExtModeSkipDownloadWhenConnect off 138 | ExtModeLogAll on 139 | ExtModeAutoUpdateStatusClock on 140 | BufferReuse on 141 | ShowModelReferenceBlockVersion off 142 | ShowModelReferenceBlockIO off 143 | Array { 144 | Type "Handle" 145 | Dimension 1 146 | Simulink.ConfigSet { 147 | $ObjectID 3 148 | Version "1.12.0" 149 | Array { 150 | Type "Handle" 151 | Dimension 9 152 | Simulink.SolverCC { 153 | $ObjectID 4 154 | Version "1.12.0" 155 | StartTime "0.0" 156 | StopTime "-1" 157 | AbsTol "auto" 158 | FixedStep "-1" 159 | InitialStep "auto" 160 | MaxNumMinSteps "-1" 161 | MaxOrder 5 162 | ZcThreshold "auto" 163 | ConsecutiveZCsStepRelTol "10*128*eps" 164 | MaxConsecutiveZCs "1000" 165 | ExtrapolationOrder 4 166 | NumberNewtonIterations 1 167 | MaxStep "auto" 168 | MinStep "auto" 169 | MaxConsecutiveMinStep "1" 170 | RelTol "1e-3" 171 | SolverMode "Auto" 172 | EnableConcurrentExecution off 173 | ConcurrentTasks off 174 | Solver "ode4" 175 | SolverName "ode4" 176 | SolverJacobianMethodControl "auto" 177 | ShapePreserveControl "EnableAll" 178 | ZeroCrossControl "UseLocalSettings" 179 | ZeroCrossAlgorithm "Nonadaptive" 180 | AlgebraicLoopSolver "TrustRegion" 181 | SolverResetMethod "Fast" 182 | PositivePriorityOrder off 183 | AutoInsertRateTranBlk off 184 | SampleTimeConstraint "Unconstrained" 185 | InsertRTBMode "Whenever possible" 186 | } 187 | Simulink.DataIOCC { 188 | $ObjectID 5 189 | Version "1.12.0" 190 | Decimation "1" 191 | ExternalInput "[t, u]" 192 | FinalStateName "xFinal" 193 | InitialState "xInitial" 194 | LimitDataPoints on 195 | MaxDataPoints "1000" 196 | LoadExternalInput off 197 | LoadInitialState off 198 | SaveFinalState off 199 | SaveCompleteFinalSimState off 200 | SaveFormat "Array" 201 | SignalLoggingSaveFormat "ModelDataLogs" 202 | SaveOutput on 203 | SaveState off 204 | SignalLogging on 205 | DSMLogging on 206 | InspectSignalLogs off 207 | SaveTime on 208 | ReturnWorkspaceOutputs off 209 | StateSaveName "xout" 210 | TimeSaveName "tout" 211 | OutputSaveName "yout" 212 | SignalLoggingName "logsout" 213 | DSMLoggingName "dsmout" 214 | OutputOption "RefineOutputTimes" 215 | OutputTimes "[]" 216 | ReturnWorkspaceOutputsName "out" 217 | Refine "1" 218 | } 219 | Simulink.OptimizationCC { 220 | $ObjectID 6 221 | Version "1.12.0" 222 | Array { 223 | Type "Cell" 224 | Dimension 8 225 | Cell "BooleansAsBitfields" 226 | Cell "PassReuseOutputArgsAs" 227 | Cell "PassReuseOutputArgsThreshold" 228 | Cell "ZeroExternalMemoryAtStartup" 229 | Cell "ZeroInternalMemoryAtStartup" 230 | Cell "OptimizeModelRefInitCode" 231 | Cell "NoFixptDivByZeroProtection" 232 | Cell "UseSpecifiedMinMax" 233 | PropName "DisabledProps" 234 | } 235 | BlockReduction on 236 | BooleanDataType on 237 | ConditionallyExecuteInputs on 238 | InlineParams off 239 | UseIntDivNetSlope off 240 | UseFloatMulNetSlope off 241 | UseSpecifiedMinMax off 242 | InlineInvariantSignals off 243 | OptimizeBlockIOStorage on 244 | BufferReuse on 245 | EnhancedBackFolding off 246 | StrengthReduction off 247 | ExpressionFolding on 248 | BooleansAsBitfields off 249 | BitfieldContainerType "uint_T" 250 | EnableMemcpy on 251 | MemcpyThreshold 64 252 | PassReuseOutputArgsAs "Structure reference" 253 | ExpressionDepthLimit 2147483647 254 | FoldNonRolledExpr on 255 | LocalBlockOutputs on 256 | RollThreshold 5 257 | SystemCodeInlineAuto off 258 | StateBitsets off 259 | DataBitsets off 260 | UseTempVars off 261 | ZeroExternalMemoryAtStartup on 262 | ZeroInternalMemoryAtStartup on 263 | InitFltsAndDblsToZero off 264 | NoFixptDivByZeroProtection off 265 | EfficientFloat2IntCast off 266 | EfficientMapNaN2IntZero on 267 | OptimizeModelRefInitCode off 268 | LifeSpan "inf" 269 | MaxStackSize "Inherit from target" 270 | BufferReusableBoundary on 271 | SimCompilerOptimization "On" 272 | AccelVerboseBuild on 273 | ParallelExecutionInRapidAccelerator on 274 | } 275 | Simulink.DebuggingCC { 276 | $ObjectID 7 277 | Version "1.12.0" 278 | RTPrefix "error" 279 | ConsistencyChecking "none" 280 | ArrayBoundsChecking "none" 281 | SignalInfNanChecking "none" 282 | SignalRangeChecking "none" 283 | ReadBeforeWriteMsg "UseLocalSettings" 284 | WriteAfterWriteMsg "UseLocalSettings" 285 | WriteAfterReadMsg "UseLocalSettings" 286 | AlgebraicLoopMsg "warning" 287 | ArtificialAlgebraicLoopMsg "warning" 288 | SaveWithDisabledLinksMsg "warning" 289 | SaveWithParameterizedLinksMsg "warning" 290 | CheckSSInitialOutputMsg on 291 | UnderspecifiedInitializationDetection "Classic" 292 | MergeDetectMultiDrivingBlocksExec "none" 293 | CheckExecutionContextPreStartOutputMsg off 294 | CheckExecutionContextRuntimeOutputMsg off 295 | SignalResolutionControl "UseLocalSettings" 296 | BlockPriorityViolationMsg "warning" 297 | MinStepSizeMsg "warning" 298 | TimeAdjustmentMsg "none" 299 | MaxConsecutiveZCsMsg "error" 300 | MaskedZcDiagnostic "warning" 301 | IgnoredZcDiagnostic "warning" 302 | SolverPrmCheckMsg "warning" 303 | InheritedTsInSrcMsg "warning" 304 | DiscreteInheritContinuousMsg "warning" 305 | MultiTaskDSMMsg "error" 306 | MultiTaskCondExecSysMsg "error" 307 | MultiTaskRateTransMsg "error" 308 | SingleTaskRateTransMsg "none" 309 | TasksWithSamePriorityMsg "warning" 310 | SigSpecEnsureSampleTimeMsg "warning" 311 | CheckMatrixSingularityMsg "none" 312 | IntegerOverflowMsg "warning" 313 | Int32ToFloatConvMsg "warning" 314 | ParameterDowncastMsg "error" 315 | ParameterOverflowMsg "error" 316 | ParameterUnderflowMsg "none" 317 | ParameterPrecisionLossMsg "warning" 318 | ParameterTunabilityLossMsg "warning" 319 | FixptConstUnderflowMsg "none" 320 | FixptConstOverflowMsg "none" 321 | FixptConstPrecisionLossMsg "none" 322 | UnderSpecifiedDataTypeMsg "none" 323 | UnnecessaryDatatypeConvMsg "none" 324 | VectorMatrixConversionMsg "none" 325 | InvalidFcnCallConnMsg "error" 326 | FcnCallInpInsideContextMsg "Enable All" 327 | SignalLabelMismatchMsg "none" 328 | UnconnectedInputMsg "warning" 329 | UnconnectedOutputMsg "warning" 330 | UnconnectedLineMsg "warning" 331 | SFcnCompatibilityMsg "none" 332 | FrameProcessingCompatibilityMsg "warning" 333 | UniqueDataStoreMsg "none" 334 | BusObjectLabelMismatch "warning" 335 | RootOutportRequireBusObject "warning" 336 | AssertControl "UseLocalSettings" 337 | EnableOverflowDetection off 338 | ModelReferenceIOMsg "none" 339 | ModelReferenceMultiInstanceNormalModeStructChecksumCheck "error" 340 | ModelReferenceVersionMismatchMessage "none" 341 | ModelReferenceIOMismatchMessage "none" 342 | ModelReferenceCSMismatchMessage "none" 343 | UnknownTsInhSupMsg "warning" 344 | ModelReferenceDataLoggingMessage "warning" 345 | ModelReferenceSymbolNameMessage "warning" 346 | ModelReferenceExtraNoncontSigs "error" 347 | StateNameClashWarn "warning" 348 | SimStateInterfaceChecksumMismatchMsg "warning" 349 | SimStateOlderReleaseMsg "error" 350 | InitInArrayFormatMsg "warning" 351 | StrictBusMsg "ErrorLevel1" 352 | BusNameAdapt "WarnAndRepair" 353 | NonBusSignalsTreatedAsBus "none" 354 | LoggingUnavailableSignals "error" 355 | BlockIODiagnostic "none" 356 | SFUnusedDataAndEventsDiag "warning" 357 | SFUnexpectedBacktrackingDiag "warning" 358 | SFInvalidInputDataAccessInChartInitDiag "warning" 359 | SFNoUnconditionalDefaultTransitionDiag "warning" 360 | SFTransitionOutsideNaturalParentDiag "warning" 361 | SFUnconditionalTransitionShadowingDiag "warning" 362 | } 363 | Simulink.HardwareCC { 364 | $ObjectID 8 365 | Version "1.12.0" 366 | ProdBitPerChar 8 367 | ProdBitPerShort 16 368 | ProdBitPerInt 32 369 | ProdBitPerLong 32 370 | ProdBitPerFloat 32 371 | ProdBitPerDouble 64 372 | ProdBitPerPointer 32 373 | ProdLargestAtomicInteger "Char" 374 | ProdLargestAtomicFloat "None" 375 | ProdIntDivRoundTo "Undefined" 376 | ProdEndianess "Unspecified" 377 | ProdWordSize 32 378 | ProdShiftRightIntArith on 379 | ProdHWDeviceType "32-bit Generic" 380 | TargetBitPerChar 8 381 | TargetBitPerShort 16 382 | TargetBitPerInt 32 383 | TargetBitPerLong 32 384 | TargetBitPerFloat 32 385 | TargetBitPerDouble 64 386 | TargetBitPerPointer 32 387 | TargetLargestAtomicInteger "Char" 388 | TargetLargestAtomicFloat "None" 389 | TargetShiftRightIntArith on 390 | TargetIntDivRoundTo "Undefined" 391 | TargetEndianess "Unspecified" 392 | TargetWordSize 32 393 | TargetTypeEmulationWarnSuppressLevel 0 394 | TargetPreprocMaxBitsSint 32 395 | TargetPreprocMaxBitsUint 32 396 | TargetHWDeviceType "Specified" 397 | TargetUnknown off 398 | ProdEqTarget on 399 | } 400 | Simulink.ModelReferenceCC { 401 | $ObjectID 9 402 | Version "1.12.0" 403 | UpdateModelReferenceTargets "IfOutOfDateOrStructuralChange" 404 | CheckModelReferenceTargetMessage "error" 405 | EnableParallelModelReferenceBuilds on 406 | ParallelModelReferenceErrorOnInvalidPool on 407 | ParallelModelReferenceMATLABWorkerInit "None" 408 | ModelReferenceNumInstancesAllowed "Multi" 409 | PropagateVarSize "Infer from blocks in model" 410 | ModelReferencePassRootInputsByReference on 411 | ModelReferenceMinAlgLoopOccurrences off 412 | PropagateSignalLabelsOutOfModel off 413 | SupportModelReferenceSimTargetCustomCode off 414 | } 415 | Simulink.SFSimCC { 416 | $ObjectID 10 417 | Version "1.12.0" 418 | SFSimEnableDebug on 419 | SFSimOverflowDetection on 420 | SFSimEcho on 421 | SimBlas on 422 | SimCtrlC on 423 | SimExtrinsic on 424 | SimIntegrity on 425 | SimUseLocalCustomCode off 426 | SimParseCustomCode on 427 | SimBuildMode "sf_incremental_build" 428 | } 429 | Simulink.RTWCC { 430 | $BackupClass "Simulink.RTWCC" 431 | $ObjectID 11 432 | Version "1.12.0" 433 | Array { 434 | Type "Cell" 435 | Dimension 9 436 | Cell "IncludeHyperlinkInReport" 437 | Cell "GenerateTraceInfo" 438 | Cell "GenerateTraceReport" 439 | Cell "GenerateTraceReportSl" 440 | Cell "GenerateTraceReportSf" 441 | Cell "GenerateTraceReportEml" 442 | Cell "GenerateWebview" 443 | Cell "GenerateCodeMetricsReport" 444 | Cell "GenerateCodeReplacementReport" 445 | PropName "DisabledProps" 446 | } 447 | SystemTargetFile "grt.tlc" 448 | GenCodeOnly off 449 | MakeCommand "make_rtw" 450 | GenerateMakefile on 451 | TemplateMakefile "grt_default_tmf" 452 | GenerateReport off 453 | SaveLog off 454 | RTWVerbose on 455 | RetainRTWFile off 456 | ProfileTLC off 457 | TLCDebug off 458 | TLCCoverage off 459 | TLCAssert off 460 | ProcessScriptMode "Default" 461 | ConfigurationMode "Optimized" 462 | ConfigAtBuild off 463 | RTWUseLocalCustomCode off 464 | RTWUseSimCustomCode off 465 | IncludeHyperlinkInReport off 466 | LaunchReport off 467 | TargetLang "C" 468 | IncludeBusHierarchyInRTWFileBlockHierarchyMap off 469 | IncludeERTFirstTime off 470 | GenerateTraceInfo off 471 | GenerateTraceReport off 472 | GenerateTraceReportSl off 473 | GenerateTraceReportSf off 474 | GenerateTraceReportEml off 475 | GenerateCodeInfo off 476 | GenerateWebview off 477 | GenerateCodeMetricsReport off 478 | GenerateCodeReplacementReport off 479 | RTWCompilerOptimization "Off" 480 | CheckMdlBeforeBuild "Off" 481 | CustomRebuildMode "OnUpdate" 482 | Array { 483 | Type "Handle" 484 | Dimension 2 485 | Simulink.CodeAppCC { 486 | $ObjectID 12 487 | Version "1.12.0" 488 | Array { 489 | Type "Cell" 490 | Dimension 21 491 | Cell "IgnoreCustomStorageClasses" 492 | Cell "IgnoreTestpoints" 493 | Cell "InsertBlockDesc" 494 | Cell "InsertPolySpaceComments" 495 | Cell "SFDataObjDesc" 496 | Cell "MATLABFcnDesc" 497 | Cell "SimulinkDataObjDesc" 498 | Cell "DefineNamingRule" 499 | Cell "SignalNamingRule" 500 | Cell "ParamNamingRule" 501 | Cell "InlinedPrmAccess" 502 | Cell "CustomSymbolStr" 503 | Cell "CustomSymbolStrGlobalVar" 504 | Cell "CustomSymbolStrType" 505 | Cell "CustomSymbolStrField" 506 | Cell "CustomSymbolStrFcn" 507 | Cell "CustomSymbolStrFcnArg" 508 | Cell "CustomSymbolStrBlkIO" 509 | Cell "CustomSymbolStrTmpVar" 510 | Cell "CustomSymbolStrMacro" 511 | Cell "ReqsInCode" 512 | PropName "DisabledProps" 513 | } 514 | ForceParamTrailComments off 515 | GenerateComments on 516 | IgnoreCustomStorageClasses on 517 | IgnoreTestpoints off 518 | IncHierarchyInIds off 519 | MaxIdLength 31 520 | PreserveName off 521 | PreserveNameWithParent off 522 | ShowEliminatedStatement off 523 | IncAutoGenComments off 524 | SimulinkDataObjDesc off 525 | SFDataObjDesc off 526 | MATLABFcnDesc off 527 | IncDataTypeInIds off 528 | MangleLength 1 529 | CustomSymbolStrGlobalVar "$R$N$M" 530 | CustomSymbolStrType "$N$R$M" 531 | CustomSymbolStrField "$N$M" 532 | CustomSymbolStrFcn "$R$N$M$F" 533 | CustomSymbolStrFcnArg "rt$I$N$M" 534 | CustomSymbolStrBlkIO "rtb_$N$M" 535 | CustomSymbolStrTmpVar "$N$M" 536 | CustomSymbolStrMacro "$R$N$M" 537 | DefineNamingRule "None" 538 | ParamNamingRule "None" 539 | SignalNamingRule "None" 540 | InsertBlockDesc off 541 | InsertPolySpaceComments off 542 | SimulinkBlockComments on 543 | MATLABSourceComments off 544 | EnableCustomComments off 545 | InlinedPrmAccess "Literals" 546 | ReqsInCode off 547 | UseSimReservedNames off 548 | } 549 | Simulink.GRTTargetCC { 550 | $BackupClass "Simulink.TargetCC" 551 | $ObjectID 13 552 | Version "1.12.0" 553 | Array { 554 | Type "Cell" 555 | Dimension 16 556 | Cell "GeneratePreprocessorConditionals" 557 | Cell "IncludeMdlTerminateFcn" 558 | Cell "CombineOutputUpdateFcns" 559 | Cell "SuppressErrorStatus" 560 | Cell "ERTCustomFileBanners" 561 | Cell "GenerateSampleERTMain" 562 | Cell "GenerateTestInterfaces" 563 | Cell "ModelStepFunctionPrototypeControlCompliant" 564 | Cell "CPPClassGenCompliant" 565 | Cell "MultiInstanceERTCode" 566 | Cell "PurelyIntegerCode" 567 | Cell "SupportComplex" 568 | Cell "SupportAbsoluteTime" 569 | Cell "SupportContinuousTime" 570 | Cell "SupportNonInlinedSFcns" 571 | Cell "PortableWordSizes" 572 | PropName "DisabledProps" 573 | } 574 | TargetFcnLib "ansi_tfl_table_tmw.mat" 575 | TargetLibSuffix "" 576 | TargetPreCompLibLocation "" 577 | CodeReplacementLibrary "ANSI_C" 578 | UtilityFuncGeneration "Auto" 579 | ERTMultiwordTypeDef "System defined" 580 | CodeExecutionProfiling off 581 | ERTMultiwordLength 256 582 | MultiwordLength 2048 583 | GenerateFullHeader on 584 | GenerateSampleERTMain off 585 | GenerateTestInterfaces off 586 | IsPILTarget off 587 | ModelReferenceCompliant on 588 | ParMdlRefBuildCompliant on 589 | CompOptLevelCompliant on 590 | ConcurrentExecutionCompliant on 591 | IncludeMdlTerminateFcn on 592 | GeneratePreprocessorConditionals "Disable all" 593 | CombineOutputUpdateFcns on 594 | CombineSignalStateStructs off 595 | SuppressErrorStatus off 596 | ERTFirstTimeCompliant off 597 | IncludeFileDelimiter "Auto" 598 | ERTCustomFileBanners off 599 | SupportAbsoluteTime on 600 | LogVarNameModifier "rt_" 601 | MatFileLogging on 602 | MultiInstanceERTCode off 603 | SupportNonFinite on 604 | SupportComplex on 605 | PurelyIntegerCode off 606 | SupportContinuousTime on 607 | SupportNonInlinedSFcns on 608 | SupportVariableSizeSignals off 609 | EnableShiftOperators on 610 | ParenthesesLevel "Nominal" 611 | PortableWordSizes off 612 | ModelStepFunctionPrototypeControlCompliant off 613 | CPPClassGenCompliant off 614 | AutosarCompliant off 615 | GRTInterface off 616 | UseMalloc off 617 | ExtMode off 618 | ExtModeStaticAlloc off 619 | ExtModeTesting off 620 | ExtModeStaticAllocSize 1000000 621 | ExtModeTransport 0 622 | ExtModeMexFile "ext_comm" 623 | ExtModeIntrfLevel "Level1" 624 | RTWCAPISignals off 625 | RTWCAPIParams off 626 | RTWCAPIStates off 627 | RTWCAPIRootIO off 628 | GenerateASAP2 off 629 | } 630 | PropName "Components" 631 | } 632 | } 633 | hdlcoderui.hdlcc { 634 | $ObjectID 14 635 | Version "1.12.0" 636 | Description "HDL Coder custom configuration component" 637 | Name "HDL Coder" 638 | } 639 | PropName "Components" 640 | } 641 | Name "Configuration" 642 | CurrentDlgPage "Solver" 643 | ConfigPrmDlgPosition [ 536, 69, 1435, 811 ] 644 | } 645 | PropName "ConfigurationSets" 646 | } 647 | Simulink.ConfigSet { 648 | $PropName "ActiveConfigurationSet" 649 | $ObjectID 3 650 | } 651 | ExplicitPartitioning off 652 | BlockDefaults { 653 | ForegroundColor "black" 654 | BackgroundColor "white" 655 | DropShadow off 656 | NamePlacement "normal" 657 | FontName "Helvetica" 658 | FontSize 10 659 | FontWeight "normal" 660 | FontAngle "normal" 661 | ShowName on 662 | BlockRotation 0 663 | BlockMirror off 664 | } 665 | AnnotationDefaults { 666 | HorizontalAlignment "center" 667 | VerticalAlignment "middle" 668 | ForegroundColor "black" 669 | BackgroundColor "white" 670 | DropShadow off 671 | FontName "Helvetica" 672 | FontSize 10 673 | FontWeight "normal" 674 | FontAngle "normal" 675 | UseDisplayTextAsClickCallback off 676 | } 677 | LineDefaults { 678 | FontName "Helvetica" 679 | FontSize 9 680 | FontWeight "normal" 681 | FontAngle "normal" 682 | } 683 | BlockParameterDefaults { 684 | Block { 685 | BlockType Constant 686 | Value "1" 687 | VectorParams1D on 688 | SamplingMode "Sample based" 689 | OutMin "[]" 690 | OutMax "[]" 691 | OutDataTypeStr "Inherit: Inherit from 'Constant value'" 692 | LockScale off 693 | SampleTime "inf" 694 | FramePeriod "inf" 695 | PreserveConstantTs off 696 | } 697 | Block { 698 | BlockType DataTypeConversion 699 | OutMin "[]" 700 | OutMax "[]" 701 | OutDataTypeStr "Inherit: Inherit via back propagation" 702 | LockScale off 703 | ConvertRealWorld "Real World Value (RWV)" 704 | RndMeth "Zero" 705 | SaturateOnIntegerOverflow on 706 | SampleTime "-1" 707 | } 708 | Block { 709 | BlockType Demux 710 | Outputs "4" 711 | DisplayOption "none" 712 | BusSelectionMode off 713 | } 714 | Block { 715 | BlockType Inport 716 | Port "1" 717 | OutputFunctionCall off 718 | OutMin "[]" 719 | OutMax "[]" 720 | OutDataTypeStr "Inherit: auto" 721 | LockScale off 722 | BusOutputAsStruct off 723 | PortDimensions "-1" 724 | VarSizeSig "Inherit" 725 | SampleTime "-1" 726 | SignalType "auto" 727 | SamplingMode "auto" 728 | LatchByDelayingOutsideSignal off 729 | LatchInputForFeedbackSignals off 730 | Interpolate on 731 | } 732 | Block { 733 | BlockType Sum 734 | IconShape "rectangular" 735 | Inputs "++" 736 | CollapseMode "All dimensions" 737 | CollapseDim "1" 738 | InputSameDT on 739 | AccumDataTypeStr "Inherit: Inherit via internal rule" 740 | OutMin "[]" 741 | OutMax "[]" 742 | OutDataTypeStr "Inherit: Same as first input" 743 | LockScale off 744 | RndMeth "Floor" 745 | SaturateOnIntegerOverflow on 746 | SampleTime "-1" 747 | } 748 | } 749 | System { 750 | Name "slToFlightgear" 751 | Location [698, 392, 1803, 973] 752 | Open on 753 | ModelBrowserVisibility off 754 | ModelBrowserWidth 200 755 | ScreenColor "white" 756 | PaperOrientation "landscape" 757 | PaperPositionMode "auto" 758 | PaperType "usletter" 759 | PaperUnits "inches" 760 | TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] 761 | TiledPageScale 1 762 | ShowPageBoundaries off 763 | ZoomFactor "100" 764 | ReportName "simulink-default.rpt" 765 | SIDHighWatermark "66" 766 | Block { 767 | BlockType Inport 768 | Name "position" 769 | SID "63" 770 | Position [30, 33, 60, 47] 771 | ZOrder -1 772 | IconDisplay "Port number" 773 | } 774 | Block { 775 | BlockType Inport 776 | Name "attitude" 777 | SID "64" 778 | Position [30, 83, 60, 97] 779 | ZOrder -1 780 | Port "2" 781 | IconDisplay "Port number" 782 | } 783 | Block { 784 | BlockType DataTypeConversion 785 | Name "Data Type Conversion1" 786 | SID "23" 787 | Position [545, 292, 615, 328] 788 | ZOrder -7 789 | RndMeth "Floor" 790 | SaturateOnIntegerOverflow off 791 | } 792 | Block { 793 | BlockType DataTypeConversion 794 | Name "Data Type Conversion2" 795 | SID "55" 796 | Position [565, 22, 635, 58] 797 | ZOrder -7 798 | OutDataTypeStr "double" 799 | RndMeth "Floor" 800 | SaturateOnIntegerOverflow off 801 | } 802 | Block { 803 | BlockType DataTypeConversion 804 | Name "Data Type Conversion3" 805 | SID "56" 806 | Position [565, 87, 635, 123] 807 | ZOrder -7 808 | OutDataTypeStr "double" 809 | RndMeth "Floor" 810 | SaturateOnIntegerOverflow off 811 | } 812 | Block { 813 | BlockType Reference 814 | Name "Degrees to\nRadians" 815 | SID "57" 816 | Ports [1, 1] 817 | Position [460, 25, 490, 55] 818 | LibraryVersion "1.44" 819 | SourceBlock "simulink_extras/Transformations/Degrees to\nRadians" 820 | SourceType "DegreesToRadians" 821 | } 822 | Block { 823 | BlockType Reference 824 | Name "Degrees to\nRadians1" 825 | SID "58" 826 | Ports [1, 1] 827 | Position [460, 90, 490, 120] 828 | LibraryVersion "1.44" 829 | SourceBlock "simulink_extras/Transformations/Degrees to\nRadians" 830 | SourceType "DegreesToRadians" 831 | } 832 | Block { 833 | BlockType Demux 834 | Name "Demux" 835 | SID "18" 836 | Ports [1, 2] 837 | Position [410, 28, 415, 77] 838 | ZOrder -6 839 | ShowName off 840 | Outputs "2" 841 | DisplayOption "bar" 842 | } 843 | Block { 844 | BlockType Demux 845 | Name "Demux1" 846 | SID "20" 847 | Ports [1, 3] 848 | Position [675, 275, 680, 345] 849 | ZOrder -6 850 | ShowName off 851 | Outputs "3" 852 | DisplayOption "bar" 853 | } 854 | Block { 855 | BlockType Demux 856 | Name "Demux2" 857 | SID "33" 858 | Ports [1, 3] 859 | Position [205, 65, 210, 115] 860 | ZOrder -6 861 | NamePlacement "alternate" 862 | ShowName off 863 | Outputs "3" 864 | DisplayOption "bar" 865 | } 866 | Block { 867 | BlockType Reference 868 | Name "Flat Earth to LLA" 869 | SID "30" 870 | Ports [2, 2] 871 | Position [250, 39, 320, 106] 872 | ZOrder -10 873 | LibraryVersion "1.275" 874 | SourceBlock "aerolibtransform2/Flat Earth to LLA" 875 | SourceType "Flat Earth to LLA" 876 | units "Metric (MKS)" 877 | ptype "Earth (WGS84)" 878 | F "1/298.257223563" 879 | R "6378137" 880 | LL0 "[37.665543 -122.480847]" 881 | psi "0" 882 | } 883 | Block { 884 | BlockType Reference 885 | Name "Pack\nnet_fdm Packet\nfor FlightGear" 886 | SID "16" 887 | Ports [6, 1] 888 | Position [800, 189, 915, 356] 889 | ZOrder -3 890 | AttributesFormatString "Version Selected: %" 891 | LibraryVersion "1.200" 892 | SourceBlock "aerolibfltsims/Pack\nnet_fdm Packet\nfor FlightGear" 893 | SourceType "FlightGearPackNetFdm" 894 | FlightGearVersion "v2.0" 895 | ShowPositionAttitudeInputs on 896 | ShowVelocityAccelerationInputs off 897 | ShowControlSurfacePositionInputs off 898 | ShowEngineFuelInputs off 899 | ShowLandingGearInputs off 900 | ShowEnvironmentInputs off 901 | SampleTime "-1" 902 | } 903 | Block { 904 | BlockType Reference 905 | Name "Send\nnet_fdm Packet\nto FlightGear" 906 | SID "17" 907 | Ports [1] 908 | Position [970, 249, 1050, 301] 909 | ZOrder -4 910 | LibraryVersion "1.200" 911 | SourceBlock "aerolibfltsims/Send\nnet_fdm Packet\nto FlightGear" 912 | SourceType "FlightGearSendNetFdm" 913 | DestinationIpAddress "127.0.0.1" 914 | DestinationPort "5502" 915 | LocalIpPort "-1" 916 | varLen off 917 | SampleTime "-1" 918 | } 919 | Block { 920 | BlockType Reference 921 | Name "Simulation Pace" 922 | SID "62" 923 | Ports [] 924 | Position [45, 370, 106, 415] 925 | ZOrder -3 926 | AttributesFormatString "% sec/sec" 927 | LibraryVersion "1.72" 928 | SourceBlock "aerolibanimutils/Simulation Pace" 929 | SourceType "Simulation Pace" 930 | SimulationPace "1" 931 | SleepMode "MATLAB Thread" 932 | OutputPaceError off 933 | SampleTime "0.1" 934 | } 935 | Block { 936 | BlockType Sum 937 | Name "Sum" 938 | SID "60" 939 | Ports [2, 1] 940 | Position [465, 235, 485, 255] 941 | ZOrder -18 942 | ShowName off 943 | IconShape "round" 944 | Inputs "|++" 945 | InputSameDT off 946 | OutDataTypeStr "Inherit: Inherit via internal rule" 947 | SaturateOnIntegerOverflow off 948 | } 949 | Block { 950 | BlockType Constant 951 | Name "altura de referencia" 952 | SID "66" 953 | Position [355, 260, 385, 290] 954 | ZOrder -5 955 | Value "240" 956 | } 957 | Line { 958 | SrcBlock "position" 959 | SrcPort 1 960 | Points [95, 0] 961 | Branch { 962 | Points [0, 50] 963 | DstBlock "Demux2" 964 | DstPort 1 965 | } 966 | Branch { 967 | Points [55, 0; 0, 15] 968 | DstBlock "Flat Earth to LLA" 969 | DstPort 1 970 | } 971 | } 972 | Line { 973 | SrcBlock "attitude" 974 | SrcPort 1 975 | Points [70, 0; 0, 220] 976 | DstBlock "Data Type Conversion1" 977 | DstPort 1 978 | } 979 | Line { 980 | SrcBlock "Pack\nnet_fdm Packet\nfor FlightGear" 981 | SrcPort 1 982 | DstBlock "Send\nnet_fdm Packet\nto FlightGear" 983 | DstPort 1 984 | } 985 | Line { 986 | SrcBlock "Demux1" 987 | SrcPort 1 988 | DstBlock "Pack\nnet_fdm Packet\nfor FlightGear" 989 | DstPort 4 990 | } 991 | Line { 992 | SrcBlock "Demux1" 993 | SrcPort 2 994 | DstBlock "Pack\nnet_fdm Packet\nfor FlightGear" 995 | DstPort 5 996 | } 997 | Line { 998 | SrcBlock "Demux1" 999 | SrcPort 3 1000 | DstBlock "Pack\nnet_fdm Packet\nfor FlightGear" 1001 | DstPort 6 1002 | } 1003 | Line { 1004 | SrcBlock "Data Type Conversion1" 1005 | SrcPort 1 1006 | DstBlock "Demux1" 1007 | DstPort 1 1008 | } 1009 | Line { 1010 | SrcBlock "Flat Earth to LLA" 1011 | SrcPort 1 1012 | DstBlock "Demux" 1013 | DstPort 1 1014 | } 1015 | Line { 1016 | SrcBlock "Demux2" 1017 | SrcPort 3 1018 | Points [20, 0] 1019 | DstBlock "Flat Earth to LLA" 1020 | DstPort 2 1021 | } 1022 | Line { 1023 | SrcBlock "Degrees to\nRadians" 1024 | SrcPort 1 1025 | DstBlock "Data Type Conversion2" 1026 | DstPort 1 1027 | } 1028 | Line { 1029 | SrcBlock "Degrees to\nRadians1" 1030 | SrcPort 1 1031 | DstBlock "Data Type Conversion3" 1032 | DstPort 1 1033 | } 1034 | Line { 1035 | SrcBlock "Demux" 1036 | SrcPort 1 1037 | DstBlock "Degrees to\nRadians" 1038 | DstPort 1 1039 | } 1040 | Line { 1041 | SrcBlock "Demux" 1042 | SrcPort 2 1043 | Points [25, 0] 1044 | DstBlock "Degrees to\nRadians1" 1045 | DstPort 1 1046 | } 1047 | Line { 1048 | SrcBlock "Data Type Conversion3" 1049 | SrcPort 1 1050 | Points [0, 105] 1051 | DstBlock "Pack\nnet_fdm Packet\nfor FlightGear" 1052 | DstPort 1 1053 | } 1054 | Line { 1055 | SrcBlock "Data Type Conversion2" 1056 | SrcPort 1 1057 | Points [135, 0; 0, 195] 1058 | DstBlock "Pack\nnet_fdm Packet\nfor FlightGear" 1059 | DstPort 2 1060 | } 1061 | Line { 1062 | SrcBlock "Flat Earth to LLA" 1063 | SrcPort 2 1064 | Points [40, 0; 0, 155] 1065 | DstBlock "Sum" 1066 | DstPort 1 1067 | } 1068 | Line { 1069 | SrcBlock "altura de referencia" 1070 | SrcPort 1 1071 | Points [85, 0] 1072 | DstBlock "Sum" 1073 | DstPort 2 1074 | } 1075 | Line { 1076 | SrcBlock "Sum" 1077 | SrcPort 1 1078 | Points [200, 0; 0, 15] 1079 | DstBlock "Pack\nnet_fdm Packet\nfor FlightGear" 1080 | DstPort 3 1081 | } 1082 | } 1083 | } 1084 | --------------------------------------------------------------------------------