├── ISA_library.py ├── LICENSE ├── PSim.py ├── README.md └── fgDFM.py /ISA_library.py: -------------------------------------------------------------------------------- 1 | #v2.1 - Andre Celere 2 | import math 3 | import matplotlib.pyplot as plt 4 | import numpy as np 5 | import sys 6 | import pandas as pd 7 | 8 | #conversion constants 9 | m2ft = 3.28084 #1 meter to feet 10 | ft2m = 1/m2ft 11 | kt2ms = 0.51444444 #knots to meters per second 12 | ms2kt = 1/kt2ms 13 | RPM2rads = 1/60*2*math.pi 14 | C2K = 273.15 #add to go from C to K 15 | kgm32slug = 0.00237717/1.225 # 16 | 17 | #constants definitions for ISA Atmosphere 18 | TROPOSPHERE = 36089.24 #feet 19 | T0 = 288.15 #Kelvin 20 | p0 = 101325 #Pa 21 | L_m = -6.5/1000 #K/m 22 | L = L_m/m2ft #K/ft 23 | a0 = 340.3 #m/s 24 | 25 | STRATOSPHERE = 65617 #feet 26 | Ts = 216.5 #Kelvin - temp at stratosphere 27 | ps = 22632.06 #Pa 28 | 29 | 30 | rho0 = 1.225 #kg/m3 31 | R = 287.053 #m2/s2/K 32 | ag_zero = -5.25588 * R * L #in m/s2 -> gravity acceleration 33 | gamma = 1.4 #adiabatic coefficient for air 34 | 35 | #This function returns a pressure given an altitude 36 | def getPressure(h): 37 | #h in feet 38 | #p in Pascals 39 | if h <= TROPOSPHERE: 40 | p = p0*(1+L/T0*h)**(-ag_zero/(R*L)) 41 | else: 42 | p = ps*math.exp(-1*(h-TROPOSPHERE)/(R*Ts/ag_zero)) 43 | return p 44 | 45 | # temperature ratio 46 | def theta(h): 47 | #h in feet 48 | if h <= TROPOSPHERE: 49 | theta_calc = 1+(L/T0)*h 50 | else: 51 | theta_calc = Ts/T0 52 | return theta_calc 53 | 54 | #this function returns the temperature ratio given OAT 55 | def thetaOAT(OAT): 56 | #OAT in C 57 | return (OAT+C2K)/T0 58 | 59 | # pressure ratio 60 | def delta(h): 61 | #h in feet 62 | delta_calc = getPressure(h)/p0 63 | return delta_calc 64 | 65 | #this function returns the altitude given a pressure ratio (delta) 66 | def inv_delta(delta): 67 | #return the altitude for that delta 68 | idelta = (delta**(1/5.255863)-1)/(-0.00000687535) 69 | return idelta 70 | 71 | #density ratio 72 | def sigma(h): 73 | #h in feet 74 | sigma_calc = delta(h)/theta(h) 75 | return sigma_calc 76 | 77 | #this function returns the altitude given a density ratio (sigma) 78 | def inv_sigma(sigma): 79 | isigma = ((sigma**0.235)-1)/(-0.00000687535) 80 | return isigma 81 | 82 | #this function returns an altitude given a pressure 83 | def getAltitude(p): 84 | #p in Pascals 85 | #h in feet 86 | if p >= ps: 87 | h = T0/L*((p/p0)**(-(R*L/ag_zero))-1) 88 | else: 89 | h = TROPOSPHERE+R*Ts/ag_zero*math.log(p/ps) 90 | return h 91 | 92 | #this function returns the speed of sound given an absolute temperature 93 | def aSpdSound(T): 94 | #T comes in Kelvin 95 | if T >= 0: 96 | return math.sqrt(gamma*R*T) 97 | else: 98 | return 0 99 | 100 | #this function returns the density altitude given an altitude and a temperature 101 | def dAltitude(h, t): 102 | temp_constant = 0.00000687535 103 | DA = (((((1-temp_constant*h)**5.2561)/((t+C2K)/T0))**0.235)-1)/(-temp_constant) 104 | return DA 105 | 106 | #this function returns the OAT for ISA atmosphere, given and altitude 107 | def getOAT_ISA(h): 108 | #given height, what is the ISA OAT? 109 | OAT = ((T0*(1-0.00000687535*h)))-C2K 110 | #outouts in C 111 | return OAT 112 | 113 | #statistic function to calculate coefficient of determination (R squared) 114 | #inputs are a fitted function and x,y original vectors 115 | def get_r(fitted_fn, x, y): 116 | yhat = fitted_fn(x) 117 | ybar = np.sum(y)/len(y) 118 | ssreg = np.sum((yhat-ybar)**2) 119 | sstot = np.sum((y-ybar)**2) 120 | r_line = ssreg/sstot 121 | return r_line 122 | 123 | #these are the vectorized function forms for speed 124 | vtheta = np.vectorize(theta) 125 | vdelta = np.vectorize(delta) 126 | vsigma = np.vectorize(sigma) 127 | 128 | 129 | #this function returns Mach number for KTAS and Hp 130 | def getMach(KTAS, Hp): 131 | #KTAS speed in knots 132 | #Hp altitude in feet 133 | return((KTAS)/(a0*ms2kt*theta(Hp))) 134 | 135 | #this function returns KEAS from KCAS and Hp 136 | def Vc2Ve(KCAS, Hp): 137 | current_delta = delta(Hp) 138 | p1 = 1 + 0.2*(KCAS/(a0*ms2kt))**2 139 | p2 = p1**(3.5) 140 | p3 = (p2-1)/current_delta 141 | p4 = (p3+1)**(1/3.5) 142 | p5 = np.sqrt((p4-1)*(current_delta*(a0*ms2kt)**2)/(0.2)) 143 | return p5 144 | 145 | #this function returns KTAS from KEAS and Hp 146 | def Ve2Vt(KEAS, Hp): 147 | current_sigma = sigma(Hp) 148 | return KEAS/np.sqrt(current_sigma) 149 | 150 | #wrapper for directly going from KCAS to KTAS 151 | def Vc2Vt(KCAS, Hp): 152 | KEAS = Vc2Ve(KCAS, Hp) 153 | return Ve2Vt(KEAS, Hp) 154 | 155 | #this function returns KCAS from KEAS and Hp 156 | def Ve2Vc(KEAS, Hp): 157 | current_delta = delta(Hp) 158 | p1 = 1 + (0.2/current_delta)*(KEAS/(a0*ms2kt))**2 159 | p2 = p1**(3.5) 160 | p3 = (p2-1)*current_delta 161 | p4 = (p3+1)**(1/3.5) 162 | p5 = np.sqrt((p4-1)*(current_delta*(a0*ms2kt)**2)/(0.2)) 163 | return p5 164 | 165 | #this function returns KEAS from KTAS and Hp 166 | def Vt2Ve(KTAS, Hp): 167 | current_sigma = sigma(Hp) 168 | return KTAS*np.sqrt(current_sigma) 169 | 170 | #wrapper for directly going from KTAS to KCAS 171 | def Vt2Vc(KTAS, Hp): 172 | KEAS = Vt2Ve(KTAS, Hp) 173 | return Ve2Vc(KEAS, Hp) 174 | 175 | #this function returns KTAS number for Mach and Hp 176 | def M2Vt(M, Hp): 177 | #Mach speed in knots 178 | #Hp altitude in feet 179 | #returns true airspeed in kts 180 | return M*a0*ms2kt*theta(Hp) 181 | 182 | #this function returns KTAS number for Mach and Hp 183 | def Vc2M(KCAS, Hp): 184 | #Calibrated speed in knots 185 | #Hp altitude in feet 186 | #returns Mach 187 | return getMach(Vc2Vt(KCAS, Hp), Hp) 188 | 189 | #this function returns the total pressure for a given speed and altitude, considering compressibility 190 | def PTot(KTAS, Hp): 191 | #speed in kts 192 | #alt in feet 193 | #returns pressure in pascals 194 | p = getPressure(Hp) 195 | u = KTAS*kt2ms 196 | calc_sigma = sigma(Hp) 197 | rho = rho0 * calc_sigma 198 | M = getMach(KTAS, Hp) 199 | series = 1 + (M**2)/4 + (M**4)/40 + (M**6)/240 200 | return p + 0.5 * rho * u**2 * series 201 | 202 | #this function returns the total temperature for a given speed and altitude, considering ISA 203 | def TAT(KTAS, Hp, k): 204 | #speed in kts 205 | #alt in feet 206 | #returns temperature in K 207 | M = getMach(KTAS, Hp) 208 | T = T0 * theta(Hp) 209 | return T * (1 + 0.2*k*M**2) 210 | 211 | #this function returns KTAS from KEAS and Hp, considering deltaISA 212 | def Ve2Vt_dISA(KEAS, Hp, deltaISA): 213 | current_delta = delta(Hp) 214 | current_OAT = getOAT_ISA(Hp) + deltaISA + C2K 215 | current_theta = current_OAT / T0 216 | current_sigma = current_delta / current_theta 217 | return KEAS/np.sqrt(current_sigma) 218 | 219 | #wrapper for directly going from KCAS to KTAS 220 | def Vc2Vt_dISA(KCAS, Hp, deltaISA): 221 | KEAS = Vc2Ve(KCAS, Hp) 222 | return Ve2Vt_dISA(KEAS, Hp, deltaISA) 223 | 224 | #calculate indicated airspeed from dynamic pressure 225 | def getVc(pd): 226 | #pd in Pascals 227 | # Vc in KIAS 228 | Vc = np.sqrt((2 * gamma)/(gamma - 1) * (p0) / (rho0) * (((abs(pd) / p0) + 1)**((gamma - 1) / gamma) - 1)) * ms2kt 229 | return Vc -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | GNU GENERAL PUBLIC LICENSE 2 | Version 3, 29 June 2007 3 | 4 | Copyright (C) 2007 Free Software Foundation, Inc. 5 | Everyone is permitted to copy and distribute verbatim copies 6 | of this license document, but changing it is not allowed. 7 | 8 | Preamble 9 | 10 | The GNU General Public License is a free, copyleft license for 11 | software and other kinds of works. 12 | 13 | The licenses for most software and other practical works are designed 14 | to take away your freedom to share and change the works. By contrast, 15 | the GNU General Public License is intended to guarantee your freedom to 16 | share and change all versions of a program--to make sure it remains free 17 | software for all its users. We, the Free Software Foundation, use the 18 | GNU General Public License for most of our software; it applies also to 19 | any other work released this way by its authors. You can apply it to 20 | your programs, too. 21 | 22 | When we speak of free software, we are referring to freedom, not 23 | price. Our General Public Licenses are designed to make sure that you 24 | have the freedom to distribute copies of free software (and charge for 25 | them if you wish), that you receive source code or can get it if you 26 | want it, that you can change the software or use pieces of it in new 27 | free programs, and that you know you can do these things. 28 | 29 | To protect your rights, we need to prevent others from denying you 30 | these rights or asking you to surrender the rights. Therefore, you have 31 | certain responsibilities if you distribute copies of the software, or if 32 | you modify it: responsibilities to respect the freedom of others. 33 | 34 | For example, if you distribute copies of such a program, whether 35 | gratis or for a fee, you must pass on to the recipients the same 36 | freedoms that you received. You must make sure that they, too, receive 37 | or can get the source code. And you must show them these terms so they 38 | know their rights. 39 | 40 | Developers that use the GNU GPL protect your rights with two steps: 41 | (1) assert copyright on the software, and (2) offer you this License 42 | giving you legal permission to copy, distribute and/or modify it. 43 | 44 | For the developers' and authors' protection, the GPL clearly explains 45 | that there is no warranty for this free software. For both users' and 46 | authors' sake, the GPL requires that modified versions be marked as 47 | changed, so that their problems will not be attributed erroneously to 48 | authors of previous versions. 49 | 50 | Some devices are designed to deny users access to install or run 51 | modified versions of the software inside them, although the manufacturer 52 | can do so. This is fundamentally incompatible with the aim of 53 | protecting users' freedom to change the software. The systematic 54 | pattern of such abuse occurs in the area of products for individuals to 55 | use, which is precisely where it is most unacceptable. Therefore, we 56 | have designed this version of the GPL to prohibit the practice for those 57 | products. If such problems arise substantially in other domains, we 58 | stand ready to extend this provision to those domains in future versions 59 | of the GPL, as needed to protect the freedom of users. 60 | 61 | Finally, every program is threatened constantly by software patents. 62 | States should not allow patents to restrict development and use of 63 | software on general-purpose computers, but in those that do, we wish to 64 | avoid the special danger that patents applied to a free program could 65 | make it effectively proprietary. To prevent this, the GPL assures that 66 | patents cannot be used to render the program non-free. 67 | 68 | The precise terms and conditions for copying, distribution and 69 | modification follow. 70 | 71 | TERMS AND CONDITIONS 72 | 73 | 0. Definitions. 74 | 75 | "This License" refers to version 3 of the GNU General Public License. 76 | 77 | "Copyright" also means copyright-like laws that apply to other kinds of 78 | works, such as semiconductor masks. 79 | 80 | "The Program" refers to any copyrightable work licensed under this 81 | License. Each licensee is addressed as "you". "Licensees" and 82 | "recipients" may be individuals or organizations. 83 | 84 | To "modify" a work means to copy from or adapt all or part of the work 85 | in a fashion requiring copyright permission, other than the making of an 86 | exact copy. The resulting work is called a "modified version" of the 87 | earlier work or a work "based on" the earlier work. 88 | 89 | A "covered work" means either the unmodified Program or a work based 90 | on the Program. 91 | 92 | To "propagate" a work means to do anything with it that, without 93 | permission, would make you directly or secondarily liable for 94 | infringement under applicable copyright law, except executing it on a 95 | computer or modifying a private copy. Propagation includes copying, 96 | distribution (with or without modification), making available to the 97 | public, and in some countries other activities as well. 98 | 99 | To "convey" a work means any kind of propagation that enables other 100 | parties to make or receive copies. Mere interaction with a user through 101 | a computer network, with no transfer of a copy, is not conveying. 102 | 103 | An interactive user interface displays "Appropriate Legal Notices" 104 | to the extent that it includes a convenient and prominently visible 105 | feature that (1) displays an appropriate copyright notice, and (2) 106 | tells the user that there is no warranty for the work (except to the 107 | extent that warranties are provided), that licensees may convey the 108 | work under this License, and how to view a copy of this License. If 109 | the interface presents a list of user commands or options, such as a 110 | menu, a prominent item in the list meets this criterion. 111 | 112 | 1. Source Code. 113 | 114 | The "source code" for a work means the preferred form of the work 115 | for making modifications to it. "Object code" means any non-source 116 | form of a work. 117 | 118 | A "Standard Interface" means an interface that either is an official 119 | standard defined by a recognized standards body, or, in the case of 120 | interfaces specified for a particular programming language, one that 121 | is widely used among developers working in that language. 122 | 123 | The "System Libraries" of an executable work include anything, other 124 | than the work as a whole, that (a) is included in the normal form of 125 | packaging a Major Component, but which is not part of that Major 126 | Component, and (b) serves only to enable use of the work with that 127 | Major Component, or to implement a Standard Interface for which an 128 | implementation is available to the public in source code form. A 129 | "Major Component", in this context, means a major essential component 130 | (kernel, window system, and so on) of the specific operating system 131 | (if any) on which the executable work runs, or a compiler used to 132 | produce the work, or an object code interpreter used to run it. 133 | 134 | The "Corresponding Source" for a work in object code form means all 135 | the source code needed to generate, install, and (for an executable 136 | work) run the object code and to modify the work, including scripts to 137 | control those activities. However, it does not include the work's 138 | System Libraries, or general-purpose tools or generally available free 139 | programs which are used unmodified in performing those activities but 140 | which are not part of the work. For example, Corresponding Source 141 | includes interface definition files associated with source files for 142 | the work, and the source code for shared libraries and dynamically 143 | linked subprograms that the work is specifically designed to require, 144 | such as by intimate data communication or control flow between those 145 | subprograms and other parts of the work. 146 | 147 | The Corresponding Source need not include anything that users 148 | can regenerate automatically from other parts of the Corresponding 149 | Source. 150 | 151 | The Corresponding Source for a work in source code form is that 152 | same work. 153 | 154 | 2. Basic Permissions. 155 | 156 | All rights granted under this License are granted for the term of 157 | copyright on the Program, and are irrevocable provided the stated 158 | conditions are met. This License explicitly affirms your unlimited 159 | permission to run the unmodified Program. The output from running a 160 | covered work is covered by this License only if the output, given its 161 | content, constitutes a covered work. This License acknowledges your 162 | rights of fair use or other equivalent, as provided by copyright law. 163 | 164 | You may make, run and propagate covered works that you do not 165 | convey, without conditions so long as your license otherwise remains 166 | in force. You may convey covered works to others for the sole purpose 167 | of having them make modifications exclusively for you, or provide you 168 | with facilities for running those works, provided that you comply with 169 | the terms of this License in conveying all material for which you do 170 | not control copyright. Those thus making or running the covered works 171 | for you must do so exclusively on your behalf, under your direction 172 | and control, on terms that prohibit them from making any copies of 173 | your copyrighted material outside their relationship with you. 174 | 175 | Conveying under any other circumstances is permitted solely under 176 | the conditions stated below. Sublicensing is not allowed; section 10 177 | makes it unnecessary. 178 | 179 | 3. Protecting Users' Legal Rights From Anti-Circumvention Law. 180 | 181 | No covered work shall be deemed part of an effective technological 182 | measure under any applicable law fulfilling obligations under article 183 | 11 of the WIPO copyright treaty adopted on 20 December 1996, or 184 | similar laws prohibiting or restricting circumvention of such 185 | measures. 186 | 187 | When you convey a covered work, you waive any legal power to forbid 188 | circumvention of technological measures to the extent such circumvention 189 | is effected by exercising rights under this License with respect to 190 | the covered work, and you disclaim any intention to limit operation or 191 | modification of the work as a means of enforcing, against the work's 192 | users, your or third parties' legal rights to forbid circumvention of 193 | technological measures. 194 | 195 | 4. Conveying Verbatim Copies. 196 | 197 | You may convey verbatim copies of the Program's source code as you 198 | receive it, in any medium, provided that you conspicuously and 199 | appropriately publish on each copy an appropriate copyright notice; 200 | keep intact all notices stating that this License and any 201 | non-permissive terms added in accord with section 7 apply to the code; 202 | keep intact all notices of the absence of any warranty; and give all 203 | recipients a copy of this License along with the Program. 204 | 205 | You may charge any price or no price for each copy that you convey, 206 | and you may offer support or warranty protection for a fee. 207 | 208 | 5. Conveying Modified Source Versions. 209 | 210 | You may convey a work based on the Program, or the modifications to 211 | produce it from the Program, in the form of source code under the 212 | terms of section 4, provided that you also meet all of these conditions: 213 | 214 | a) The work must carry prominent notices stating that you modified 215 | it, and giving a relevant date. 216 | 217 | b) The work must carry prominent notices stating that it is 218 | released under this License and any conditions added under section 219 | 7. This requirement modifies the requirement in section 4 to 220 | "keep intact all notices". 221 | 222 | c) You must license the entire work, as a whole, under this 223 | License to anyone who comes into possession of a copy. This 224 | License will therefore apply, along with any applicable section 7 225 | additional terms, to the whole of the work, and all its parts, 226 | regardless of how they are packaged. This License gives no 227 | permission to license the work in any other way, but it does not 228 | invalidate such permission if you have separately received it. 229 | 230 | d) If the work has interactive user interfaces, each must display 231 | Appropriate Legal Notices; however, if the Program has interactive 232 | interfaces that do not display Appropriate Legal Notices, your 233 | work need not make them do so. 234 | 235 | A compilation of a covered work with other separate and independent 236 | works, which are not by their nature extensions of the covered work, 237 | and which are not combined with it such as to form a larger program, 238 | in or on a volume of a storage or distribution medium, is called an 239 | "aggregate" if the compilation and its resulting copyright are not 240 | used to limit the access or legal rights of the compilation's users 241 | beyond what the individual works permit. Inclusion of a covered work 242 | in an aggregate does not cause this License to apply to the other 243 | parts of the aggregate. 244 | 245 | 6. Conveying Non-Source Forms. 246 | 247 | You may convey a covered work in object code form under the terms 248 | of sections 4 and 5, provided that you also convey the 249 | machine-readable Corresponding Source under the terms of this License, 250 | in one of these ways: 251 | 252 | a) Convey the object code in, or embodied in, a physical product 253 | (including a physical distribution medium), accompanied by the 254 | Corresponding Source fixed on a durable physical medium 255 | customarily used for software interchange. 256 | 257 | b) Convey the object code in, or embodied in, a physical product 258 | (including a physical distribution medium), accompanied by a 259 | written offer, valid for at least three years and valid for as 260 | long as you offer spare parts or customer support for that product 261 | model, to give anyone who possesses the object code either (1) a 262 | copy of the Corresponding Source for all the software in the 263 | product that is covered by this License, on a durable physical 264 | medium customarily used for software interchange, for a price no 265 | more than your reasonable cost of physically performing this 266 | conveying of source, or (2) access to copy the 267 | Corresponding Source from a network server at no charge. 268 | 269 | c) Convey individual copies of the object code with a copy of the 270 | written offer to provide the Corresponding Source. This 271 | alternative is allowed only occasionally and noncommercially, and 272 | only if you received the object code with such an offer, in accord 273 | with subsection 6b. 274 | 275 | d) Convey the object code by offering access from a designated 276 | place (gratis or for a charge), and offer equivalent access to the 277 | Corresponding Source in the same way through the same place at no 278 | further charge. You need not require recipients to copy the 279 | Corresponding Source along with the object code. If the place to 280 | copy the object code is a network server, the Corresponding Source 281 | may be on a different server (operated by you or a third party) 282 | that supports equivalent copying facilities, provided you maintain 283 | clear directions next to the object code saying where to find the 284 | Corresponding Source. Regardless of what server hosts the 285 | Corresponding Source, you remain obligated to ensure that it is 286 | available for as long as needed to satisfy these requirements. 287 | 288 | e) Convey the object code using peer-to-peer transmission, provided 289 | you inform other peers where the object code and Corresponding 290 | Source of the work are being offered to the general public at no 291 | charge under subsection 6d. 292 | 293 | A separable portion of the object code, whose source code is excluded 294 | from the Corresponding Source as a System Library, need not be 295 | included in conveying the object code work. 296 | 297 | A "User Product" is either (1) a "consumer product", which means any 298 | tangible personal property which is normally used for personal, family, 299 | or household purposes, or (2) anything designed or sold for incorporation 300 | into a dwelling. In determining whether a product is a consumer product, 301 | doubtful cases shall be resolved in favor of coverage. For a particular 302 | product received by a particular user, "normally used" refers to a 303 | typical or common use of that class of product, regardless of the status 304 | of the particular user or of the way in which the particular user 305 | actually uses, or expects or is expected to use, the product. A product 306 | is a consumer product regardless of whether the product has substantial 307 | commercial, industrial or non-consumer uses, unless such uses represent 308 | the only significant mode of use of the product. 309 | 310 | "Installation Information" for a User Product means any methods, 311 | procedures, authorization keys, or other information required to install 312 | and execute modified versions of a covered work in that User Product from 313 | a modified version of its Corresponding Source. The information must 314 | suffice to ensure that the continued functioning of the modified object 315 | code is in no case prevented or interfered with solely because 316 | modification has been made. 317 | 318 | If you convey an object code work under this section in, or with, or 319 | specifically for use in, a User Product, and the conveying occurs as 320 | part of a transaction in which the right of possession and use of the 321 | User Product is transferred to the recipient in perpetuity or for a 322 | fixed term (regardless of how the transaction is characterized), the 323 | Corresponding Source conveyed under this section must be accompanied 324 | by the Installation Information. But this requirement does not apply 325 | if neither you nor any third party retains the ability to install 326 | modified object code on the User Product (for example, the work has 327 | been installed in ROM). 328 | 329 | The requirement to provide Installation Information does not include a 330 | requirement to continue to provide support service, warranty, or updates 331 | for a work that has been modified or installed by the recipient, or for 332 | the User Product in which it has been modified or installed. Access to a 333 | network may be denied when the modification itself materially and 334 | adversely affects the operation of the network or violates the rules and 335 | protocols for communication across the network. 336 | 337 | Corresponding Source conveyed, and Installation Information provided, 338 | in accord with this section must be in a format that is publicly 339 | documented (and with an implementation available to the public in 340 | source code form), and must require no special password or key for 341 | unpacking, reading or copying. 342 | 343 | 7. Additional Terms. 344 | 345 | "Additional permissions" are terms that supplement the terms of this 346 | License by making exceptions from one or more of its conditions. 347 | Additional permissions that are applicable to the entire Program shall 348 | be treated as though they were included in this License, to the extent 349 | that they are valid under applicable law. If additional permissions 350 | apply only to part of the Program, that part may be used separately 351 | under those permissions, but the entire Program remains governed by 352 | this License without regard to the additional permissions. 353 | 354 | When you convey a copy of a covered work, you may at your option 355 | remove any additional permissions from that copy, or from any part of 356 | it. (Additional permissions may be written to require their own 357 | removal in certain cases when you modify the work.) You may place 358 | additional permissions on material, added by you to a covered work, 359 | for which you have or can give appropriate copyright permission. 360 | 361 | Notwithstanding any other provision of this License, for material you 362 | add to a covered work, you may (if authorized by the copyright holders of 363 | that material) supplement the terms of this License with terms: 364 | 365 | a) Disclaiming warranty or limiting liability differently from the 366 | terms of sections 15 and 16 of this License; or 367 | 368 | b) Requiring preservation of specified reasonable legal notices or 369 | author attributions in that material or in the Appropriate Legal 370 | Notices displayed by works containing it; or 371 | 372 | c) Prohibiting misrepresentation of the origin of that material, or 373 | requiring that modified versions of such material be marked in 374 | reasonable ways as different from the original version; or 375 | 376 | d) Limiting the use for publicity purposes of names of licensors or 377 | authors of the material; or 378 | 379 | e) Declining to grant rights under trademark law for use of some 380 | trade names, trademarks, or service marks; or 381 | 382 | f) Requiring indemnification of licensors and authors of that 383 | material by anyone who conveys the material (or modified versions of 384 | it) with contractual assumptions of liability to the recipient, for 385 | any liability that these contractual assumptions directly impose on 386 | those licensors and authors. 387 | 388 | All other non-permissive additional terms are considered "further 389 | restrictions" within the meaning of section 10. If the Program as you 390 | received it, or any part of it, contains a notice stating that it is 391 | governed by this License along with a term that is a further 392 | restriction, you may remove that term. If a license document contains 393 | a further restriction but permits relicensing or conveying under this 394 | License, you may add to a covered work material governed by the terms 395 | of that license document, provided that the further restriction does 396 | not survive such relicensing or conveying. 397 | 398 | If you add terms to a covered work in accord with this section, you 399 | must place, in the relevant source files, a statement of the 400 | additional terms that apply to those files, or a notice indicating 401 | where to find the applicable terms. 402 | 403 | Additional terms, permissive or non-permissive, may be stated in the 404 | form of a separately written license, or stated as exceptions; 405 | the above requirements apply either way. 406 | 407 | 8. Termination. 408 | 409 | You may not propagate or modify a covered work except as expressly 410 | provided under this License. Any attempt otherwise to propagate or 411 | modify it is void, and will automatically terminate your rights under 412 | this License (including any patent licenses granted under the third 413 | paragraph of section 11). 414 | 415 | However, if you cease all violation of this License, then your 416 | license from a particular copyright holder is reinstated (a) 417 | provisionally, unless and until the copyright holder explicitly and 418 | finally terminates your license, and (b) permanently, if the copyright 419 | holder fails to notify you of the violation by some reasonable means 420 | prior to 60 days after the cessation. 421 | 422 | Moreover, your license from a particular copyright holder is 423 | reinstated permanently if the copyright holder notifies you of the 424 | violation by some reasonable means, this is the first time you have 425 | received notice of violation of this License (for any work) from that 426 | copyright holder, and you cure the violation prior to 30 days after 427 | your receipt of the notice. 428 | 429 | Termination of your rights under this section does not terminate the 430 | licenses of parties who have received copies or rights from you under 431 | this License. If your rights have been terminated and not permanently 432 | reinstated, you do not qualify to receive new licenses for the same 433 | material under section 10. 434 | 435 | 9. Acceptance Not Required for Having Copies. 436 | 437 | You are not required to accept this License in order to receive or 438 | run a copy of the Program. Ancillary propagation of a covered work 439 | occurring solely as a consequence of using peer-to-peer transmission 440 | to receive a copy likewise does not require acceptance. However, 441 | nothing other than this License grants you permission to propagate or 442 | modify any covered work. These actions infringe copyright if you do 443 | not accept this License. Therefore, by modifying or propagating a 444 | covered work, you indicate your acceptance of this License to do so. 445 | 446 | 10. Automatic Licensing of Downstream Recipients. 447 | 448 | Each time you convey a covered work, the recipient automatically 449 | receives a license from the original licensors, to run, modify and 450 | propagate that work, subject to this License. You are not responsible 451 | for enforcing compliance by third parties with this License. 452 | 453 | An "entity transaction" is a transaction transferring control of an 454 | organization, or substantially all assets of one, or subdividing an 455 | organization, or merging organizations. If propagation of a covered 456 | work results from an entity transaction, each party to that 457 | transaction who receives a copy of the work also receives whatever 458 | licenses to the work the party's predecessor in interest had or could 459 | give under the previous paragraph, plus a right to possession of the 460 | Corresponding Source of the work from the predecessor in interest, if 461 | the predecessor has it or can get it with reasonable efforts. 462 | 463 | You may not impose any further restrictions on the exercise of the 464 | rights granted or affirmed under this License. For example, you may 465 | not impose a license fee, royalty, or other charge for exercise of 466 | rights granted under this License, and you may not initiate litigation 467 | (including a cross-claim or counterclaim in a lawsuit) alleging that 468 | any patent claim is infringed by making, using, selling, offering for 469 | sale, or importing the Program or any portion of it. 470 | 471 | 11. Patents. 472 | 473 | A "contributor" is a copyright holder who authorizes use under this 474 | License of the Program or a work on which the Program is based. The 475 | work thus licensed is called the contributor's "contributor version". 476 | 477 | A contributor's "essential patent claims" are all patent claims 478 | owned or controlled by the contributor, whether already acquired or 479 | hereafter acquired, that would be infringed by some manner, permitted 480 | by this License, of making, using, or selling its contributor version, 481 | but do not include claims that would be infringed only as a 482 | consequence of further modification of the contributor version. For 483 | purposes of this definition, "control" includes the right to grant 484 | patent sublicenses in a manner consistent with the requirements of 485 | this License. 486 | 487 | Each contributor grants you a non-exclusive, worldwide, royalty-free 488 | patent license under the contributor's essential patent claims, to 489 | make, use, sell, offer for sale, import and otherwise run, modify and 490 | propagate the contents of its contributor version. 491 | 492 | In the following three paragraphs, a "patent license" is any express 493 | agreement or commitment, however denominated, not to enforce a patent 494 | (such as an express permission to practice a patent or covenant not to 495 | sue for patent infringement). To "grant" such a patent license to a 496 | party means to make such an agreement or commitment not to enforce a 497 | patent against the party. 498 | 499 | If you convey a covered work, knowingly relying on a patent license, 500 | and the Corresponding Source of the work is not available for anyone 501 | to copy, free of charge and under the terms of this License, through a 502 | publicly available network server or other readily accessible means, 503 | then you must either (1) cause the Corresponding Source to be so 504 | available, or (2) arrange to deprive yourself of the benefit of the 505 | patent license for this particular work, or (3) arrange, in a manner 506 | consistent with the requirements of this License, to extend the patent 507 | license to downstream recipients. "Knowingly relying" means you have 508 | actual knowledge that, but for the patent license, your conveying the 509 | covered work in a country, or your recipient's use of the covered work 510 | in a country, would infringe one or more identifiable patents in that 511 | country that you have reason to believe are valid. 512 | 513 | If, pursuant to or in connection with a single transaction or 514 | arrangement, you convey, or propagate by procuring conveyance of, a 515 | covered work, and grant a patent license to some of the parties 516 | receiving the covered work authorizing them to use, propagate, modify 517 | or convey a specific copy of the covered work, then the patent license 518 | you grant is automatically extended to all recipients of the covered 519 | work and works based on it. 520 | 521 | A patent license is "discriminatory" if it does not include within 522 | the scope of its coverage, prohibits the exercise of, or is 523 | conditioned on the non-exercise of one or more of the rights that are 524 | specifically granted under this License. You may not convey a covered 525 | work if you are a party to an arrangement with a third party that is 526 | in the business of distributing software, under which you make payment 527 | to the third party based on the extent of your activity of conveying 528 | the work, and under which the third party grants, to any of the 529 | parties who would receive the covered work from you, a discriminatory 530 | patent license (a) in connection with copies of the covered work 531 | conveyed by you (or copies made from those copies), or (b) primarily 532 | for and in connection with specific products or compilations that 533 | contain the covered work, unless you entered into that arrangement, 534 | or that patent license was granted, prior to 28 March 2007. 535 | 536 | Nothing in this License shall be construed as excluding or limiting 537 | any implied license or other defenses to infringement that may 538 | otherwise be available to you under applicable patent law. 539 | 540 | 12. No Surrender of Others' Freedom. 541 | 542 | If conditions are imposed on you (whether by court order, agreement or 543 | otherwise) that contradict the conditions of this License, they do not 544 | excuse you from the conditions of this License. If you cannot convey a 545 | covered work so as to satisfy simultaneously your obligations under this 546 | License and any other pertinent obligations, then as a consequence you may 547 | not convey it at all. For example, if you agree to terms that obligate you 548 | to collect a royalty for further conveying from those to whom you convey 549 | the Program, the only way you could satisfy both those terms and this 550 | License would be to refrain entirely from conveying the Program. 551 | 552 | 13. Use with the GNU Affero General Public License. 553 | 554 | Notwithstanding any other provision of this License, you have 555 | permission to link or combine any covered work with a work licensed 556 | under version 3 of the GNU Affero General Public License into a single 557 | combined work, and to convey the resulting work. The terms of this 558 | License will continue to apply to the part which is the covered work, 559 | but the special requirements of the GNU Affero General Public License, 560 | section 13, concerning interaction through a network will apply to the 561 | combination as such. 562 | 563 | 14. Revised Versions of this License. 564 | 565 | The Free Software Foundation may publish revised and/or new versions of 566 | the GNU General Public License from time to time. Such new versions will 567 | be similar in spirit to the present version, but may differ in detail to 568 | address new problems or concerns. 569 | 570 | Each version is given a distinguishing version number. If the 571 | Program specifies that a certain numbered version of the GNU General 572 | Public License "or any later version" applies to it, you have the 573 | option of following the terms and conditions either of that numbered 574 | version or of any later version published by the Free Software 575 | Foundation. If the Program does not specify a version number of the 576 | GNU General Public License, you may choose any version ever published 577 | by the Free Software Foundation. 578 | 579 | If the Program specifies that a proxy can decide which future 580 | versions of the GNU General Public License can be used, that proxy's 581 | public statement of acceptance of a version permanently authorizes you 582 | to choose that version for the Program. 583 | 584 | Later license versions may give you additional or different 585 | permissions. However, no additional obligations are imposed on any 586 | author or copyright holder as a result of your choosing to follow a 587 | later version. 588 | 589 | 15. Disclaimer of Warranty. 590 | 591 | THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY 592 | APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT 593 | HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY 594 | OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, 595 | THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 596 | PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM 597 | IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF 598 | ALL NECESSARY SERVICING, REPAIR OR CORRECTION. 599 | 600 | 16. Limitation of Liability. 601 | 602 | IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING 603 | WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS 604 | THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY 605 | GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE 606 | USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF 607 | DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD 608 | PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS), 609 | EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF 610 | SUCH DAMAGES. 611 | 612 | 17. Interpretation of Sections 15 and 16. 613 | 614 | If the disclaimer of warranty and limitation of liability provided 615 | above cannot be given local legal effect according to their terms, 616 | reviewing courts shall apply local law that most closely approximates 617 | an absolute waiver of all civil liability in connection with the 618 | Program, unless a warranty or assumption of liability accompanies a 619 | copy of the Program in return for a fee. 620 | 621 | END OF TERMS AND CONDITIONS 622 | 623 | How to Apply These Terms to Your New Programs 624 | 625 | If you develop a new program, and you want it to be of the greatest 626 | possible use to the public, the best way to achieve this is to make it 627 | free software which everyone can redistribute and change under these terms. 628 | 629 | To do so, attach the following notices to the program. It is safest 630 | to attach them to the start of each source file to most effectively 631 | state the exclusion of warranty; and each file should have at least 632 | the "copyright" line and a pointer to where the full notice is found. 633 | 634 | 635 | Copyright (C) 636 | 637 | This program is free software: you can redistribute it and/or modify 638 | it under the terms of the GNU General Public License as published by 639 | the Free Software Foundation, either version 3 of the License, or 640 | (at your option) any later version. 641 | 642 | This program is distributed in the hope that it will be useful, 643 | but WITHOUT ANY WARRANTY; without even the implied warranty of 644 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 645 | GNU General Public License for more details. 646 | 647 | You should have received a copy of the GNU General Public License 648 | along with this program. If not, see . 649 | 650 | Also add information on how to contact you by electronic and paper mail. 651 | 652 | If the program does terminal interaction, make it output a short 653 | notice like this when it starts in an interactive mode: 654 | 655 | Copyright (C) 656 | This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'. 657 | This is free software, and you are welcome to redistribute it 658 | under certain conditions; type `show c' for details. 659 | 660 | The hypothetical commands `show w' and `show c' should show the appropriate 661 | parts of the General Public License. Of course, your program's commands 662 | might be different; for a GUI interface, you would use an "about box". 663 | 664 | You should also get your employer (if you work as a programmer) or school, 665 | if any, to sign a "copyright disclaimer" for the program, if necessary. 666 | For more information on this, and how to apply and follow the GNU GPL, see 667 | . 668 | 669 | The GNU General Public License does not permit incorporating your program 670 | into proprietary programs. If your program is a subroutine library, you 671 | may consider it more useful to permit linking proprietary applications with 672 | the library. If this is what you want to do, use the GNU Lesser General 673 | Public License instead of this License. But first, please read 674 | . 675 | -------------------------------------------------------------------------------- /PSim.py: -------------------------------------------------------------------------------- 1 | # FGROOT = /usr/share/games/flightgear 2 | 3 | # DRI_PRIME=1 fgfs --airport=SBGP --aircraft=Embraer170 --aircraft-dir=./FlightGear/Aircraft/E-jet-family/ --native-fdm=socket,in,60,,5500,udp --fdm=null --enable-hud --in-air --fog-disable --shading-smooth --texture-filtering=4 --timeofday=morning --altitude=2500 --prop:/sim/hud/path[1]=Huds/NTPS.xml 4 | # DRI_PRIME=1 fgfs --airport=LOWI --aircraft=Embraer170 --aircraft-dir=./FlightGear/Aircraft/E-jet-family/ --native-fdm=socket,in,60,,5500,udp --fdm=null --enable-hud --in-air --fog-disable --shading-smooth --texture-filtering=4 --timeofday=morning --altitude=2500 --prop:/sim/hud/path[1]=Huds/fte.xml 2>/dev/null 5 | 6 | 7 | # FG with JSBSim: 8 | # DRI_PRIME=1 fgfs --airport=SBGP --aircraft=Embraer170 --aircraft-dir=./FlightGear/Aircraft/E-jet-family/ --enable-hud --fog-disable --shading-smooth --texture-filtering=4 --timeofday=morning 9 | # DRI_PRIME=1 fgfs --airport=KSFO --runway=28R --aircraft=757-200-RB211 --aircraft-dir=~/.fgfs/Aircraft/org.flightgear.fgaddon.stable_2020/Aircraft/757-200 --enable-hud --fog-disable --shading-smooth --texture-filtering=4 --timeofday=morning 10 | 11 | # "v" muda o visual 12 | # https://wiki.flightgear.org/Command_line_options 13 | 14 | ''' 15 | Partial Python implementation of the non-linear flight dynamics model proposed by: 16 | Group for Aeronautical Research and Technology Europe (GARTEUR) - Research Civil Aircraft Model (RCAM) 17 | http://garteur.org/wp-content/reports/FM/FM_AG-08_TP-088-3.pdf 18 | 19 | The excellent tutorials by Christopher Lum (for Matlab/Simulink) were used as a guide: 20 | 1 - Equations/Modeling 21 | https://www.youtube.com/watch?v=bFFAL9lI2IQ 22 | 2 - Matlab implementation 23 | https://www.youtube.com/watch?v=m5sEln5bWuM 24 | 25 | The program runs the integration loop as fast as possible, adjusting the integration steps to the available computing cycles 26 | It uses Numba to speed up the main functions involved in the integration loop 27 | 28 | Output is sent to FlightGear (FG), over UDP, at a reduced frame rate (60) 29 | The FG interface uses the class implemented by Andrew Tridgel (fgFDM): 30 | https://github.com/ArduPilot/pymavlink/blob/master/fgFDM.py 31 | 32 | currently, the UDP address is set to the local machine. 33 | 34 | Run this program and from a separate terminal, start FG with one of the following commands (depending on the aircraft addons installed): 35 | fgfs --airport=KSFO --runway=28R --aircraft=ufo --native-fdm=socket,in,60,,5500,udp --fdm=null 36 | fgfs --airport=KSFO --runway=28R --aircraft=Embraer170 --aircraft-dir=./FlightGear/Aircraft/E-jet-family/ --native-fdm=socket,in,60,,5500,udp --fdm=null 37 | fgfs --airport=KSFO --runway=28R --aircraft=757-200-RB211 --aircraft-dir=~/.fgfs/Aircraft/org.flightgear.fgaddon.stable_2020/Aircraft/757-200 --native-fdm=socket,in,60,,5500,udp --fdm=null 38 | fgfs --airport=KSFO --runway=28R --aircraft=757-200-RB211 --aircraft-dir=~/.fgfs/Aircraft/org.flightgear.fgaddon.stable_2020/Aircraft/757-200 --native-fdm=socket,in,60,,5500,udp --fdm=null --enable-hud --turbulence=0.5 --in-air --enable-rembrandt 39 | DRI_PRIME=1 fgfs --airport=LOWI --aircraft=Embraer170 --aircraft-dir=./FlightGear/Aircraft/E-jet-family/ --native-fdm=socket,in,60,,5500,udp --fdm=null --enable-hud --in-air --fog-disable --shading-smooth --texture-filtering=4 --timeofday=morning --altitude=2500 --prop:/sim/hud/path[1]=Huds/fte.xml 2>/dev/null 40 | 41 | REQUIRES a joystick to work. 42 | 43 | 44 | TODO: 45 | 1) add engine dynamics (spool up/down) 46 | 2) add atmospheric disturbances/turbulence 47 | 3) add other actuator dynamics 48 | 4) save/read trim point 49 | 5) fuel detot / inertia update 50 | 51 | 52 | ''' 53 | # imports 54 | import numpy as np 55 | import pandas as pd 56 | from scipy import integrate 57 | # for trimming routine 58 | from scipy.optimize import minimize 59 | 60 | import time 61 | 62 | from numba import jit 63 | #import numba 64 | 65 | import math 66 | import csv 67 | import sys 68 | 69 | sys.path.insert(1, '../') 70 | 71 | # FlightGear comm class 72 | from fgDFM import * 73 | import socket 74 | 75 | # International Standard Atmosphere library 76 | from ISA_library import * 77 | 78 | #joystick interface 79 | import pygame 80 | 81 | 82 | # # helper functions 83 | def make_plots(x_data=np.array([0,1,2]), y_data=np.array([0,1,2]), \ 84 | header=['PSim_Time', 'u', 'v', 'w', 'p', 'q', 'r', 'phi', 'theta', 'psi', 'lat', 'lon', 'h', 'V_N', 'V_E', 'V_D', 'dA', 'dE', 'dR', 'dT1', 'dT2'], skip=0): 85 | 86 | ''' 87 | Function to plot results. 88 | Inputs: 89 | x_data - time vector 90 | y_data - n-dimentional array with parameters to be plotted 91 | header has standard sequence of parameters/labels as generated by simulator 92 | skip: number of header items to skip 93 | ''' 94 | plotlist = [] 95 | 96 | plt.ioff() 97 | plt.clf() 98 | counter = 1 99 | myfig = plt.figure(figsize = (16,(y_data.shape[1]*4))) 100 | myfig.patch.set_edgecolor('w') 101 | plt.subplots_adjust(hspace = 0.0) 102 | for y_data_idx in range(y_data.shape[1]): 103 | strip_chart_y_data = y_data[:,y_data_idx] 104 | ax = myfig.add_subplot(y_data.shape[1], 1, counter) 105 | ax.plot(x_data, strip_chart_y_data) 106 | plt.ylabel(header[y_data_idx+skip]) 107 | plt.grid(True) 108 | counter += 1 109 | return myfig 110 | 111 | 112 | def save2disk(filename, x_data=np.array([0,1,2]), y_data=np.array([0,1,2]), \ 113 | header=['u', 'v', 'w', 'p', 'q', 'r', 'phi', 'theta', 'psi', 'lat', 'lon', 'h', 'V_N', 'V_E', 'V_D', 'dA', 'dE', 'dR', 'dT1', 'dT2'], skip=0): 114 | ''' 115 | saves data to disk 116 | ''' 117 | with open(filename, 'w') as f: 118 | y_dim = y_data.shape[1] 119 | data_header = header[skip:y_dim] 120 | data_header.insert(0, 'PSim_Time') 121 | writer = csv.writer(f) 122 | writer.writerow(data_header) 123 | for idx, row in enumerate(collector): 124 | row_list = row.tolist() 125 | row_list.insert(0, x_data[idx].astype('float')) 126 | writer.writerow(row_list) 127 | 128 | @jit 129 | def VA(uvw:np.ndarray) -> float: 130 | ''' 131 | Calculate true airspeed 132 | input: 133 | uvw: vector of 3 speeds u, v, w 134 | returns: 135 | true airspeed 136 | ''' 137 | return np.sqrt(np.dot(uvw.T, uvw)) 138 | 139 | 140 | def get_rho(altitude:float)->float: 141 | ''' 142 | calculate the air density given an altitude in feet 143 | ''' 144 | return rho0 * sigma(altitude * m2ft) 145 | 146 | @jit 147 | def fpa(V_NED)->float: 148 | ''' 149 | returns flight path angle 150 | input is a vector with North, East and Down velocities 151 | ''' 152 | return np.arctan2(-V_NED[2], np.sqrt(V_NED[0]**2 + V_NED[1]**2)) 153 | 154 | 155 | def course(V_NED)->float: 156 | ''' 157 | returns the course, given NED velocities 158 | ''' 159 | return np.pi/2 - np.arctan2(V_NED[0], V_NED[1]) 160 | 161 | @jit 162 | def add_wind(NED:np.ndarray, std_dev:np.ndarray)->np.ndarray: 163 | ''' 164 | returns wind at altitude Hp. 165 | inputs: 166 | NED: vector with wind speed 167 | std_dev: vector with standard deviations for wind (one value for each N, E, D) 168 | output: 169 | wind speed vector 170 | ''' 171 | return NED + np.multiply(np.random.rand(3), std_dev) 172 | 173 | 174 | def get_doublet(t_vector, t=0, duration=1, amplitude=0.1): 175 | ''' 176 | calculates a doublet input 177 | inputs: 178 | t_vector: time vector 179 | t: value at which the doublet should start 180 | duration: duration of the high/low input states 181 | amplituyde: multiplication factor to set amplitude 182 | returns: 183 | doublet vector 184 | ''' 185 | rise_idx = np.argmax(t_vector>=t) 186 | drop_idx = np.argmax(t_vector >=(t+duration/2)) 187 | zero_idx = np.argmax(t_vector >=(t+duration)) 188 | res = np.zeros(t_vector.shape) 189 | res[rise_idx:drop_idx] = 1 * amplitude 190 | res[drop_idx:zero_idx] = -1 * amplitude 191 | return res 192 | 193 | 194 | def get_step(t_vector, t=0, amplitude=0.1): 195 | ''' 196 | calculates a step input 197 | inputs: 198 | t_vector: time vector 199 | t: value at which the doublet should start 200 | amplituyde: multiplication factor to set amplitude 201 | returns: 202 | step vector 203 | ''' 204 | rise_idx = np.argmax(t_vector>=t) 205 | res = np.zeros(t_vector.shape) 206 | res[rise_idx:] = 1 * amplitude 207 | return res 208 | 209 | 210 | def create_cmd(t_vector=np.zeros(5), input_channel='ail', cmd_type='doublet', at_time=0.0, duration=1.0, amplitude=0.0): 211 | ''' 212 | helper function to create a doublet or step in a channel 213 | inputs: 214 | t_vector: time vector 215 | input_channel: selector for axis (see if/else below) 216 | cmd_type: selector for doublet or step 217 | at_time: value at which the inpute should start 218 | duration: duration of the high/low input states 219 | amplituyde: multiplication factor to set amplitude 220 | returns: 221 | input_ch_number: integer with the index of command to be added to integration loop 222 | cmd: vector with command 223 | ''' 224 | 225 | if input_channel == 'ail': 226 | input_ch_num = 0 227 | elif input_channel == 'elev': 228 | input_ch_num = 1 229 | elif input_channel == 'rud': 230 | input_ch_num = 2 231 | elif input_channel == 'thru': 232 | input_ch_num = 3 233 | elif input_channel == 'none' or input_channel == 'None': 234 | cmd = np.zeros(t_vector.shape) 235 | input_ch_num = 0 236 | else: 237 | input_ch_num = -1 238 | 239 | 240 | if cmd_type=='doublet' and input_ch_num>=0: 241 | cmd = get_doublet(t_vector, t=at_time, duration=duration, amplitude=amplitude) 242 | elif cmd_type=='step' and input_ch_num>=0: 243 | cmd = get_step(t_vector, t=at_time, amplitude=amplitude) 244 | else: 245 | print('error - command type not recognized') 246 | cmd = np.zeros(t_vector.shape) 247 | input_ch_num = 0 248 | 249 | return input_ch_num, cmd 250 | 251 | def set_FDM(this_fgFDM, X, U_norm, latlon, alt, body_accels): 252 | ''' 253 | function to set the current time step data to be sent to FlightGear 254 | inputs are: 255 | X - states 256 | U - controls 257 | latlon - in radians 258 | alt - in meters 259 | NED - velocities in m/s 260 | ''' 261 | this_fgFDM.set('phi', X[6]) 262 | this_fgFDM.set('theta', X[7]) 263 | this_fgFDM.set('psi', X[8]) 264 | 265 | this_fgFDM.set('phidot', X[3]) 266 | this_fgFDM.set('thetadot', X[4]) 267 | this_fgFDM.set('psidot', X[5]) 268 | 269 | # this sets units to kts because the HUD does not apply any conversions to the speed 270 | # if we send speed in fps as the API requires, the HUD displays wrong value 271 | this_fgFDM.set('vcas', Vt2Vc(VA(X[:3]), alt*m2ft) * ms2kt) 272 | this_fgFDM.set('cur_time', int(time.perf_counter() ), units='seconds') 273 | this_fgFDM.set('latitude', latlon[0], units='radians') 274 | this_fgFDM.set('longitude', latlon[1], units='radians') 275 | this_fgFDM.set('altitude', alt, units='meters') 276 | 277 | this_fgFDM.set('left_aileron', -U_norm[0]) 278 | this_fgFDM.set('right_aileron', +U_norm[0]) 279 | this_fgFDM.set('elevator', U_norm[1]) 280 | this_fgFDM.set('rudder', -U_norm[2]) 281 | 282 | this_fgFDM.set('A_X_pilot', body_accels[0], units='mpss') 283 | this_fgFDM.set('A_Y_pilot', body_accels[1], units='mpss') 284 | this_fgFDM.set('A_Z_pilot', body_accels[2], units='mpss') 285 | 286 | 287 | 288 | def get_joy_inputs(joystick, U_trim, fr): 289 | ''' 290 | function that will read joystick positions and adjust controls: 291 | 1. joy will change controls on top of trim point 292 | 2. trim settings (buttons) will change trim point 293 | 3. engine does not have trim function, but depending on 294 | button pressed, throttle should be commanded left/right/both 295 | ''' 296 | U = np.zeros(U_trim.shape) 297 | 298 | # # # TRIM 299 | 300 | # multipliers to adjust how much trim is added per integration step. 301 | pitch_trim_step = 0.006 / fr 302 | aileron_trim_step = 0.003 / fr 303 | #rudder_trim_step = 0.005 # not implemented yet 304 | throttle_trim_step = 0.001 / fr 305 | 306 | # read joystick button states for trimming 307 | zero_ail_rud_thr = joystick.get_button(0) 308 | pitch_dn = joystick.get_button(4) 309 | pitch_up = joystick.get_button(2) 310 | roll_rt = joystick.get_button(7) 311 | roll_lt = joystick.get_button(6) 312 | T1_fd = joystick.get_button(8) 313 | T1_af = joystick.get_button(10) 314 | T2_fd = joystick.get_button(9) 315 | T2_af = joystick.get_button(11) 316 | exit_signal = joystick.get_button(1) 317 | 318 | # if trigger is pressed, then zero out aileron, rudder states and make thrust equal on both sides 319 | if zero_ail_rud_thr == 1: 320 | U_trim[0] = 0.0 321 | U_trim[2] = 0.0 322 | U_trim[3] = U_trim[4] 323 | 324 | 325 | U_trim[0] = U_trim[0] - aileron_trim_step * roll_rt + aileron_trim_step * roll_lt 326 | U_trim[1] = U_trim[1] - pitch_trim_step * pitch_up + pitch_trim_step * pitch_dn 327 | #U_trim[2] = U_trim[2] + rudder_trim_step * - rudder_trim_step * roll_lt 328 | U_trim[3] = U_trim[3] - throttle_trim_step * T1_af + throttle_trim_step * T1_fd 329 | U_trim[4] = U_trim[4] - throttle_trim_step * T2_af + throttle_trim_step * T2_fd 330 | 331 | # # # JOYSTICK COMMAND 332 | 333 | # joystick constants/multipliers to adjust correct movement and amplitude 334 | ail_factor = -0.7 335 | elev_factor = -0.5 336 | rud_factor = -0.52 337 | thr_factor = -0.2 338 | 339 | U[0] = U_trim[0] + joystick.get_axis(0) * ail_factor 340 | U[1] = U_trim[1] + joystick.get_axis(1) * elev_factor 341 | U[2] = U_trim[2] + joystick.get_axis(2) * rud_factor 342 | U[3] = U_trim[3] + joystick.get_axis(3) * thr_factor 343 | U[4] = U_trim[4] + joystick.get_axis(3) * thr_factor 344 | 345 | 346 | return U, U_trim, exit_signal 347 | 348 | 349 | 350 | # geodsy 351 | # https://www.youtube.com/watch?v=4BJ-GpYbZlU 352 | @jit 353 | def WGS84_MN(lat:float): 354 | ''' 355 | Meridian Radius of Curvature 356 | Prime Vertical Radius of Curvature 357 | for WGS-84 358 | 359 | Input is latitude in degress (decimal) 360 | ''' 361 | a = 6378137.0 #meters 362 | e_sqrd = 6.69437999014E-3 363 | M = (a * (1 - e_sqrd)) / ((1 - e_sqrd * np.sin(lat)**2)**(1.5)) 364 | N = a / ((1 - e_sqrd * np.sin(lat)**2)**(0.5)) 365 | return M, N 366 | 367 | @jit 368 | def latlonh_dot(V_NED, lat, h): 369 | ''' 370 | V_north: m/s 371 | M: m 372 | h: m 373 | ''' 374 | M, N = WGS84_MN(lat) 375 | return np.array([(V_NED[0]) / (M + h), 376 | (V_NED[1]) / ((N + h) * np.cos(lat)), 377 | -V_NED[2]]) 378 | 379 | 380 | # controls 381 | def control_norm(U:np.array, U_lim:np.array) -> np.array: 382 | ''' 383 | normalizes controls to be sent to FG 384 | inputs: 385 | U controls: positions (in radians) 386 | U_lim: control limits (in radians) 387 | returns: 388 | vector with control positions normalized between 1 and -1 389 | ''' 390 | U_norm = [] 391 | for i in range(U.shape[0]): 392 | if U[i] <= 0: 393 | U_norm.append(-U[i] / U_lim[i][0]) 394 | else: 395 | U_norm.append(U[i] / U_lim[i][1]) 396 | 397 | return np.array(U_norm) 398 | 399 | @jit 400 | def control_sat(U:np.ndarray) -> np.ndarray: 401 | ''' 402 | saturates the control inputs to maximum allowable in RCAM model 403 | ''' 404 | 405 | 406 | #----------------- control limits / saturation --------------------- 407 | u0min = -25 * deg2rad 408 | u0max = 25 * deg2rad 409 | 410 | u1min = -25 * deg2rad 411 | u1max = 10 * deg2rad 412 | 413 | u2min = -30 * deg2rad 414 | u2max = 30 * deg2rad 415 | 416 | u3min = 0.5 * deg2rad # need to implement engine shutoff - with drag instead of thrust 417 | u3max = 10 * deg2rad 418 | 419 | u4min = 0.5 * deg2rad 420 | u4max = 10 * deg2rad 421 | 422 | 423 | #value_if_true if condition else value_if_false 424 | i = 0 425 | u0 = U[i] if (U[i]>=u0min and U[i]<=u0max) else u0min if U[i]=u1min and U[i]<=u1max) else u1min if U[i]=u2min and U[i]<=u2max) else u2min if U[i]=u3min and U[i]<=u3max) else u3min if U[i]=u4min and U[i]<=u4max) else u4min if U[i] np.ndarray: 437 | ''' 438 | RCAM model implementation 439 | sources: RCAM docs and Christopher Lum 440 | Group for Aeronautical Research and Technology Europe (GARTEUR) - Research Civil Aircraft Model (RCAM) 441 | http://garteur.org/wp-content/reports/FM/FM_AG-08_TP-088-3.pdf 442 | 443 | Christopher Lum - Equations/Modeling 444 | https://www.youtube.com/watch?v=bFFAL9lI2IQ 445 | Christopher Lum - Matlab implementation 446 | https://www.youtube.com/watch?v=m5sEln5bWuM 447 | 448 | inputs: 449 | X: states 450 | 0: u (m/s) 451 | 1: v (m/s) 452 | 2: w (m/s) 453 | 3: p (rad/s) 454 | 4: q (rad/s) 455 | 5: r (rad/s) 456 | 6: phi (rad) 457 | 7: theta (rad) 458 | 8: psi (rad) 459 | U: controls 460 | 0: aileron (rad) 461 | 1: stabilator (rad) 462 | 2: rudder (rad) 463 | 3: throttle 1 (rad) 464 | 4: throttle 2 (rad) 465 | rho: density for current altitude 466 | outputs: 467 | X_dot: derivatives of states (same order) 468 | ''' 469 | 470 | #------------------------ constants ------------------------------- 471 | 472 | # Nominal vehicle constants 473 | m = 120000; # kg - total mass 474 | 475 | cbar = 6.6 # m - mean aerodynamic chord 476 | lt = 24.8 # m - tail AC distance to CG 477 | S = 260 # m2 - wing area 478 | St = 64 # m2 - tail area 479 | 480 | # centre of gravity position 481 | Xcg = 0.23 * cbar # m - x pos of CG in Fm 482 | Ycg = 0.0 # m - y pos of CG in Fm 483 | Zcg = 0.10 * cbar # m - z pos of CG in Fm ERRATA - table 2.4 has 0.0 and table 2.5 has 0.10 cbar 484 | 485 | # aerodynamic center position 486 | Xac = 0.12 * cbar # m - x pos of aerodynamic center in Fm 487 | Yac = 0.0 # m - y pos of aerodynamic center in Fm 488 | Zac = 0.0 # m - z pos of aerodynamic center in Fm 489 | 490 | # engine point of thrust aplication 491 | Xapt1 = 0 # m - x position of engine 1 in Fm 492 | Yapt1 = -7.94 # m - y position of engine 1 in Fm 493 | Zapt1 = -1.9 # m - z position of engine 1 in Fm 494 | 495 | Xapt2 = 0 # m - x position of engine 2 in Fm 496 | Yapt2 = 7.94 # m - y position of engine 2 in Fm 497 | Zapt2 = -1.9 # m - z position of engine 2 in Fm 498 | 499 | # other constants 500 | #rho = 1.225 # kg/m3 - air density 501 | g = 9.81 # m/s2 - gravity 502 | depsda = 0.25 # rad/rad - change in downwash wrt alpha 503 | deg2rad = np.pi / 180 # from degrees to radians 504 | alpha_L0 = -11.5 * deg2rad # rad - zero lift AOA 505 | n = 5.5 # adm - slope of linear ragion of lift slope 506 | a3 = -768.5 # adm - coeff of alpha^3 507 | a2 = 609.2 # adm - - coeff of alpha^2 508 | a1 = -155.2 # adm - - coeff of alpha^1 509 | a0 = 15.212 # adm - - coeff of alpha^0 ERRATA RCAM has 15.2 510 | alpha_switch = 14.5 * deg2rad # rad - kink point of lift slope 511 | 512 | 513 | #----------------- intermediate variables --------------------------- 514 | # airspeed 515 | Va = np.sqrt(X[0]**2 + X[1]**2 + X[2]**2) # m/s 516 | 517 | # alpha and beta 518 | #np.arctan2 --> y, x 519 | alpha = np.arctan2(X[2], X[0]) 520 | beta = np.arcsin(X[1]/Va) 521 | 522 | # dynamic pressure 523 | Q = 0.5 * rho * Va**2 524 | 525 | # define vectors wbe_b and V_b 526 | wbe_b = np.array([X[3], X[4], X[5]]) 527 | V_b = np.array([X[0], X[1], X[2]]) 528 | 529 | #----------------- aerodynamic force coefficients --------------------- 530 | # CL - wing + body 531 | CL_wb = n * (alpha - alpha_L0) if alpha <= alpha_switch else a3 * alpha**3 + a2 * alpha**2 + a1 * alpha + a0 532 | 533 | # CL thrust 534 | epsilon = depsda * (alpha - alpha_L0) 535 | alpha_t = alpha - epsilon + U[1] + 1.3 * X[4] * lt / Va 536 | CL_t = 3.1 * (St / S) * alpha_t 537 | 538 | # Total CL 539 | CL = CL_wb + CL_t 540 | 541 | # Total CD 542 | CD = 0.13 + 0.07 * (n * alpha + 0.654)**2 543 | 544 | # Total CY 545 | CY = -1.6 * beta + 0.24 * U[2] 546 | 547 | 548 | #------------------- dimensional aerodynamic forces -------------------- 549 | # forces in F_s 550 | FA_s = np.array([-CD * Q* S, CY * Q * S, -CL * Q * S]) 551 | 552 | # rotate forces to body axis (F_b) 553 | C_bs = np.array([[np.cos(alpha), 0.0, -np.sin(alpha)], 554 | [0.0, 1.0, 0.0], 555 | [np.sin(alpha), 0.0, np.cos(alpha)]], dtype=np.dtype('f8')) 556 | 557 | FA_b = np.dot(C_bs, FA_s) 558 | 559 | 560 | #------------------ aerodynamic moment coefficients about AC ----------- 561 | # moments in F_b 562 | eta11 = -1.4 * beta 563 | eta21 = -0.59 - (3.1 * (St * lt) / (S * cbar)) * (alpha - epsilon) 564 | eta31 = (1 - alpha * (180 / (15 * np.pi))) * beta 565 | 566 | eta = np.array([eta11, eta21, eta31]) 567 | 568 | dCMdx = (cbar / Va) * np.array([[-11.0, 0.0, 5.0], 569 | [ 0.0, (-4.03 * (St * lt**2) / (S * cbar**2)), 0.0], 570 | [1.7, 0.0, -11.5]], dtype=np.dtype('f8')) 571 | dCMdu = np.array([[-0.6, 0.0, 0.22], 572 | [0.0, (-3.1 * (St * lt) / (S * cbar)), 0.0], 573 | [0.0, 0.0, -0.63]], dtype=np.dtype('f8')) 574 | 575 | 576 | # CM about AC in Fb 577 | CMac_b = eta + np.dot(dCMdx, wbe_b) + np.dot(dCMdu, np.array([U[0], U[1], U[2]])) 578 | 579 | #------------------- aerodynamic moment about AC ------------------------- 580 | # normalize to aerodynamic moment 581 | MAac_b = CMac_b * Q * S * cbar 582 | 583 | #-------------------- aerodynamic moment about CG ------------------------ 584 | rcg_b = np.array([Xcg, Ycg, Zcg]) 585 | rac_b = np.array([Xac, Yac, Zac]) 586 | 587 | MAcg_b = MAac_b + np.cross(FA_b, rcg_b - rac_b) 588 | 589 | #---------------------- engine force and moment -------------------------- 590 | # thrust 591 | F1 = U[3] * m * g 592 | F2 = U[4] * m * g 593 | 594 | #thrust vectors (assuming aligned with x axis) 595 | FE1_b = np.array([F1, 0, 0]) 596 | FE2_b = np.array([F2, 0, 0]) 597 | 598 | FE_b = FE1_b + FE2_b 599 | 600 | # engine moments 601 | mew1 = np.array([Xcg - Xapt1, Yapt1 - Ycg, Zcg - Zapt1]) 602 | mew2 = np.array([Xcg - Xapt2, Yapt2 - Ycg, Zcg - Zapt2]) 603 | 604 | MEcg1_b = np.cross(mew1, FE1_b) 605 | MEcg2_b = np.cross(mew2, FE2_b) 606 | 607 | MEcg_b = MEcg1_b + MEcg2_b 608 | 609 | #---------------------- gravity effects ---------------------------------- 610 | g_b = np.array([-g * np.sin(X[7]), g * np.cos(X[7]) * np.sin(X[6]), g * np.cos(X[7]) * np.cos(X[6])]) 611 | 612 | Fg_b = m * g_b 613 | 614 | #---------------------- state derivatives -------------------------------- 615 | # inertia tensor 616 | Ib = m * np.array([[40.07, 0.0, -2.0923], 617 | [0.0, 64.0, 0.0], 618 | [-2.0923, 0.0, 99.92]], dtype=np.dtype('f8')) # ERRATA on Ixz p. 12 vs p. 91 619 | invIb = np.linalg.inv(Ib) 620 | 621 | # form F_b and calculate u, v, w dot 622 | F_b = Fg_b + FE_b + FA_b 623 | 624 | x0x1x2_dot = (1 / m) * F_b - np.cross(wbe_b, V_b) 625 | 626 | # form Mcg_b and calc p, q r dot 627 | Mcg_b = MAcg_b + MEcg_b 628 | 629 | x3x4x5_dot = np.dot(invIb, (Mcg_b - np.cross(wbe_b, np.dot(Ib , wbe_b)))) 630 | 631 | #phi, theta, psi dot 632 | H_phi = np.array([[1.0, np.sin(X[6]) * np.tan(X[7]), np.cos(X[6]) * np.tan(X[7])], 633 | [0.0, np.cos(X[6]), -np.sin(X[6])], 634 | [0.0, np.sin(X[6]) / np.cos(X[7]), np.cos(X[6]) / np.cos(X[7])]], dtype=np.dtype('f8')) 635 | 636 | x6x7x8_dot = np.dot(H_phi, wbe_b) 637 | 638 | #--------------------- place in first order form -------------------------- 639 | X_dot = np.concatenate((x0x1x2_dot, x3x4x5_dot, x6x7x8_dot)) 640 | 641 | return X_dot 642 | 643 | 644 | # Navigation Equations 645 | # source: 646 | # Christopher Lum - "The Naviation Equations: Computing Position North, East and Down" 647 | # https://www.youtube.com/watch?v=XQZV-YZ7asE 648 | 649 | 650 | @jit 651 | def NED(uvw, phithetapsi): 652 | ''' 653 | compute the NED velocities from: 654 | inputs 655 | uvw: array with u, v, w 656 | phithetapsi: array with phi, theta, psi 657 | 658 | returns 659 | velocities in NED 660 | 661 | remember that h_dot = -Vd 662 | ''' 663 | 664 | u = uvw[0] 665 | v = uvw[1] 666 | w = uvw[2] 667 | phi = phithetapsi[0] 668 | the = phithetapsi[1] 669 | psi = phithetapsi[2] 670 | c1v = np.array([[np.cos(psi), np.sin(psi), 0.0], 671 | [-np.sin(psi), np.cos(psi), 0.0], 672 | [0.0, 0.0, 1.0]]) 673 | 674 | c21 = np.array([[np.cos(the), 0.0, -np.sin(the)], 675 | [0.0, 1.0, 0.0], 676 | [np.sin(the), 0.0, np.cos(the)]]) 677 | 678 | cb2 = np.array([[1.0, 0.0, 0.0], 679 | [0.0, np.cos(phi), np.sin(phi)], 680 | [0.0, -np.sin(phi), np.cos(phi)]]) 681 | 682 | cbv = np.dot(cb2, np.dot(c21,c1v)) #numba does not support np.matmul 683 | return np.dot(cbv.T, uvw) 684 | 685 | 686 | # # # # # Model integration # # # # # 687 | 688 | # # # wrappers 689 | # Scipy's "integrate.ode" does not accept a numba/@jit compiled function 690 | # therefore, we need to create dummy wrappers 691 | 692 | def RCAM_model_wrapper(t, X, U, rho): 693 | return RCAM_model(X, U, rho) 694 | 695 | def NED_wrapper(t, X, NED): 696 | return NED 697 | 698 | def latlonh_dot_wrapper(t, X, V_NED, lat, h): 699 | return latlonh_dot(V_NED, lat, h) 700 | 701 | 702 | # # # integrators 703 | def ss_integrator(t_ini:float, X0:np.ndarray, U:np.ndarray, rho:float): 704 | 705 | ''' 706 | single step integrator 707 | returns scipy object, initialized 708 | ''' 709 | 710 | RK_integrator = integrate.ode(RCAM_model_wrapper) 711 | RK_integrator.set_integrator('dopri5') 712 | RK_integrator.set_f_params(control_sat(U), rho) 713 | RK_integrator.set_initial_value(X0, t_ini) 714 | return RK_integrator 715 | 716 | def time_span_int(t_ini:float, t_fin:float, dt:float, X0:np.ndarray, U:np.ndarray, rho:float) -> np.ndarray: 717 | ''' 718 | function to integrate the model in a time span, with FIXED dt 719 | 720 | inputs: 721 | t_ini: initial time in seconds 722 | t-fin: final time in seconds 723 | dt: delta time between steps, in seconds 724 | X0: initial states 725 | U: controls positions 726 | outputs: 727 | t_vector: time vector 728 | result_array: states integrated for all time steps in time vector 729 | ''' 730 | 731 | t_vector = np.arange(np.datetime64('2011-06-15T00:00'), np.datetime64('2011-06-15T00:00') + np.timedelta64(t_fin, 's'),np.timedelta64(int(dt*1000),'ms'), dtype='datetime64') 732 | 733 | RK_integrator = integrate.ode(RCAM_model_wrapper) 734 | RK_integrator.set_integrator('dopri5') 735 | RK_integrator.set_f_params(control_sat(U), rho) 736 | RK_integrator.set_initial_value(X0, t_ini) 737 | collector = [] 738 | 739 | for _ in t_vector: 740 | RK_integrator.integrate(RK_integrator.t + dt) 741 | aux = np.insert(RK_integrator.y, 0, RK_integrator.t) 742 | collector.append(aux) 743 | result_array = np.array(collector) 744 | return(t_vector, result_array) 745 | 746 | def latlonh_int(t_ini:float, latlonh0:np.ndarray, V_NED): 747 | 748 | ''' 749 | single step integrator for lat/long/height 750 | returns scipy object, initialized 751 | ''' 752 | 753 | RK_integrator = integrate.ode(latlonh_dot_wrapper) 754 | RK_integrator.set_integrator('dopri5') 755 | RK_integrator.set_f_params(V_NED, latlonh0[0], latlonh0[2]) 756 | RK_integrator.set_initial_value(latlonh0, t_ini) 757 | return RK_integrator 758 | 759 | 760 | # # Trimmer 761 | def trim_functional2(Z:np.ndarray, VA_trim, gamma_trim, v_trim, phi_trim, psi_trim, rho_trim) -> np.dtype('f8'): 762 | ''' 763 | functional to calculate a cost for minimizer (used to find trim point) 764 | no constraints yet 765 | inputs: 766 | Z: lumped vector of X (states) and U (control) 767 | trim targets: 768 | VA_trim: airspeed [m/s] 769 | gamma_trim: climb gradient [rad] 770 | v-trim: side speed [m/s] 771 | phi_trim: roll angle [rad] 772 | psi_trim: course angle [rad] 773 | 774 | **** 775 | method 776 | Q.T*H*Q 777 | with H = diagonal matrix of "1"s (equal weights for all states) 778 | 779 | returns: 780 | cost [float] 781 | ''' 782 | 783 | X = Z[:9] 784 | U = Z[9:] 785 | 786 | X_dot = RCAM_model(X, control_sat(U), rho_trim) 787 | V_NED_current = NED(X_dot[:3], X_dot[3:6]) 788 | 789 | VA_current = VA(X[:3]) 790 | 791 | gamma_current = X[7] - np.arctan2(X[2], X[0]) # only valid for wings level case 792 | 793 | Q = np.concatenate((X_dot, [VA_current - VA_trim], [gamma_current - gamma_trim], [X[1] - v_trim], [X[6] - phi_trim], [X[8] - psi_trim])) 794 | diag_ones = np.ones(Q.shape[0]) 795 | H = np.diag(diag_ones) 796 | 797 | return np.dot(np.dot(Q.T, H), Q) 798 | 799 | def trim_model(VA_trim=85.0, gamma_trim=0.0, v_trim=0.0, phi_trim=0.0, psi_trim=0.0, rho_trim=1.225, 800 | X0=np.array([85, 0, 0, 0, 0, 0, 0, 0.1, 0]), 801 | U0=np.array([0, -0.1, 0, 0.08, 0.08])) -> np.ndarray: 802 | ''' 803 | uses scipy minimize on functional to find trim point 804 | ''' 805 | 806 | print(f'trimming with X0 = {X0}') 807 | print(f'trimming with U0 = {U0}') 808 | X0[0] = VA_trim 809 | Z0 = np.concatenate((X0, U0)) 810 | 811 | result = minimize(trim_functional2, Z0, args=(VA_trim, gamma_trim, v_trim, phi_trim, psi_trim, rho_trim), 812 | method='L-BFGS-B', options={'disp':False, 'maxiter':5000,\ 813 | 'gtol':1e-25, 'ftol':1e-25, \ 814 | 'maxls':4000}) 815 | 816 | return result.x, result.message 817 | 818 | 819 | # # Init 820 | def initialize(VA_t=85.0, gamma_t=0.0, latlon=np.zeros(2), altitude=10000, psi_t=0.0): 821 | ''' 822 | this initializes the integrators at a straight and level flight condition 823 | inputs: 824 | VA_t: true airspeed at trim (m/s) 825 | gamma_t: flight path angle at trim (rad) 826 | latlon: initial lat and long (rad) 827 | altitude: trim altitude (ft) 828 | psi_t: initial heading (rad) 829 | outputs: 830 | AC_integrator: aircraft integrator object 831 | X0: initial states found at trim point 832 | U0: initial commands found at trim point 833 | latlonh_integrator: navigation equation scipy object integrator 834 | ''' 835 | ft2m = 0.3048 836 | t0 = 0.0 #intial time for integrators 837 | 838 | print(f'initializing model with altitude {altitude} ft, rho={get_rho(altitude)}') 839 | 840 | alt_m = altitude * ft2m 841 | latlonh0 = np.array([latlon[0]*deg2rad, latlon[1]*deg2rad, alt_m]) 842 | 843 | # trim model 844 | res4, res4_status = trim_model(VA_trim=VA_t, gamma_trim=gamma_t, v_trim=t0, 845 | phi_trim=0.0, psi_trim=psi_t*deg2rad, rho_trim=get_rho(altitude)) 846 | print(res4_status) 847 | X0 = res4[:9] 848 | U0 = res4[9:] 849 | print(f'initial states: {X0}') 850 | print(f'initial inputs: {U0}') 851 | 852 | # initialize integrators 853 | AC_integrator = ss_integrator(t0, X0, U0, get_rho(altitude)) 854 | 855 | NED0 = NED(X0[:3], X0[6:]) #uvw and phithetapsi 856 | 857 | latlonh_integrator = latlonh_int(t0, latlonh0, NED0) 858 | 859 | return AC_integrator, X0, U0, latlonh_integrator 860 | 861 | 862 | 863 | if __name__ == "__main__": 864 | 865 | deg2rad = np.pi / 180 # from degrees to radians 866 | 867 | # Network socket to communicate with FlightGear 868 | UDP_IP = "127.0.0.1" 869 | UDP_PORT = 5500 870 | UDP_IP2 = "192.168.0.163" 871 | UDP_PORT2 = 5501 872 | sock = socket.socket(socket.AF_INET, # Internet 873 | socket.SOCK_DGRAM) # UDP 874 | sock2 = socket.socket(socket.AF_INET, # Internet 875 | socket.SOCK_DGRAM) # UDP 876 | 877 | pygame.init() # automatically initializes joystick also 878 | 879 | joystick_count = pygame.joystick.get_count() 880 | if joystick_count == 0: 881 | print('connect joystick first') 882 | exit() 883 | 884 | print(f'found {joystick_count} joysticks connected.') 885 | this_joy = pygame.joystick.Joystick(0) 886 | print(f'{this_joy.get_name()}, axes={this_joy.get_numaxes()}') 887 | 888 | signals_header = ['u', 'v', 'w', 'p', 'q', 'r', 'phi', 'theta', 'psi', 'lat', 'lon', 'h', 'V_N', 'V_E', 'V_D', 'dA', 'dE', 'dR', 'dT1', 'dT2'] 889 | 890 | ############################################################################ 891 | # INITIAL CONDITIONS 892 | 893 | # Altitude 894 | init_alt = 2100 #ft 895 | 896 | # IAS 897 | v_trim = 140 * kt2ms 898 | 899 | # Gamma 900 | gamma_trim = 0.0 * deg2rad 901 | 902 | # Lat/Lon 903 | #init_latlon = np.array([37.6213, -122.3790]) #in degrees - the func initialize transforms to radians internally 904 | #init_latlon = np.array([-21.7632, -48.4051]) #in degrees - SBGP 905 | init_latlon = np.array([47.2548, 11.2963]) #in degrees - LOWI short final TFB 906 | 907 | # Heading 908 | init_psi = 82.0 909 | 910 | ############################################################################ 911 | 912 | # instantiate FG comms object and initialize it 913 | my_fgFDM = fgFDM() 914 | my_fgFDM.set('latitude', init_latlon[0], units='degrees') 915 | my_fgFDM.set('longitude', init_latlon[1], units='degrees') 916 | my_fgFDM.set('altitude', init_alt, units='meters') 917 | my_fgFDM.set('agl', init_alt, units='meters') 918 | my_fgFDM.set('num_engines', 2) 919 | my_fgFDM.set('num_tanks', 1) 920 | my_fgFDM.set('num_wheels', 3) 921 | my_fgFDM.set('cur_time', int(time.perf_counter()), units='seconds') 922 | 923 | #----------------- control limits / saturation --------------------- 924 | U_limits = [(-25 * deg2rad, 25 * deg2rad), 925 | (-25 * deg2rad, 10 * deg2rad), 926 | (-30 * deg2rad, 30 * deg2rad), 927 | (0.5 * deg2rad, 10 * deg2rad), 928 | (0.5 * deg2rad, 10 * deg2rad)] 929 | 930 | 931 | ####################################################################################### 932 | # SIMULATION OPTIONS 933 | 934 | # start time 935 | t0 = 0 936 | # total simulation time 937 | tf = 10 * 60 #minutes 938 | # simulation loop frame rate throttling 939 | simFR = 400 # (Hz) 940 | # frames per second to be sent out to FG 941 | fgFR = 60 # (Hz) 942 | 943 | wind_speed = np.array([0, 0, 0]) # (m/s), NED 944 | wind_stddev = np.array([1, 1, 0]) # 945 | 946 | ####################################################################################### 947 | 948 | # initializations 949 | # data collectors 950 | collector = [] 951 | t_vector_collector = [] 952 | prev_uvw = np.array([0,0,0]) 953 | current_uvw = np.array([0,0,0]) 954 | 955 | 956 | # aircraft initialization (includes trimming) 957 | this_AC_int, X1, U1, this_latlonh_int = initialize(VA_t=v_trim, gamma_t=gamma_trim, latlon=init_latlon, altitude=init_alt, psi_t=init_psi) 958 | U_man = U1.copy() 959 | 960 | 961 | # frame variables 962 | current_alt = init_alt 963 | current_latlon = init_latlon 964 | frame_count = 0 965 | 966 | send_frame_trigger = False 967 | fg_time_adder = 0 # counts the time between integration steps to trigger sending out a frame to FlightGear 968 | 969 | fgdt = 1 / (fgFR + 1) # (s) fg frame period 970 | 971 | run_sim_loop = False 972 | 973 | simdt = 1 / simFR # (s) desired simulation time step 974 | sim_time_adder = 0 # counts the time between integration steps to trigger next simulation frame 975 | dt = 0 # actual integration time step 976 | prev_dt = dt 977 | 978 | grav_accel = 9.81 # m/s 979 | 980 | exit_signal = 0 # if joystick button #1 is pressed, ends simulation 981 | 982 | 983 | # main loop 984 | 985 | while this_AC_int.t <= tf and exit_signal == 0: 986 | # get clock 987 | start = time.perf_counter() 988 | 989 | if run_sim_loop: 990 | 991 | 992 | _ = pygame.event.get() 993 | 994 | # get density, inputs 995 | current_rho = get_rho(current_alt * m2ft) 996 | U_man, U1, exit_signal = get_joy_inputs(this_joy, U1, simFR) 997 | U_man = control_sat(U_man) 998 | 999 | # set current integration step commands, density and integrate aircraft states 1000 | prev_uvw = current_uvw 1001 | this_AC_int.set_f_params(U_man, current_rho) 1002 | this_AC_int.integrate(this_AC_int.t + dt) 1003 | current_uvw = this_AC_int.y[0:3] 1004 | 1005 | # integrate navigation equations 1006 | current_NED = NED(this_AC_int.y[:3], this_AC_int.y[6:]) 1007 | this_wind = add_wind(wind_speed, wind_stddev) 1008 | this_latlonh_int.set_f_params(current_NED + this_wind, current_latlon[0], current_alt) 1009 | this_latlonh_int.integrate(this_latlonh_int.t + dt) #in radians 1010 | 1011 | # store current state and time vector 1012 | current_latlon = this_latlonh_int.y[0:2] 1013 | current_alt = this_latlonh_int.y[2] 1014 | collector.append(np.concatenate((this_AC_int.y, this_latlonh_int.y, current_NED + this_wind, U_man))) 1015 | t_vector_collector.append(this_AC_int.t) 1016 | 1017 | # check for FG frame trigger 1018 | if send_frame_trigger: 1019 | # it is easier to calculate body accelerations instead of reaching into the RCAM function 1020 | if dt == 0: 1021 | body_accels = (current_uvw - prev_uvw) / prev_dt 1022 | else: 1023 | body_accels = (current_uvw - prev_uvw) / dt 1024 | # add gravity 1025 | g_b = np.array([-grav_accel * np.sin(this_AC_int.y[7]), 1026 | grav_accel * np.cos(this_AC_int.y[7]) * np.sin(this_AC_int.y[6]), 1027 | grav_accel * np.cos(this_AC_int.y[7]) * np.cos(this_AC_int.y[6])]) 1028 | body_accels = body_accels + g_b 1029 | body_accels[2] = -body_accels[2] 1030 | 1031 | set_FDM(my_fgFDM, this_AC_int.y, 1032 | control_norm(U_man, U_limits), 1033 | current_latlon, 1034 | current_alt, 1035 | body_accels) 1036 | my_pack = my_fgFDM.pack() 1037 | sock.sendto(my_pack, (UDP_IP, UDP_PORT)) 1038 | sock2.sendto(my_pack, (UDP_IP2, UDP_PORT2)) 1039 | send_frame_trigger = False 1040 | 1041 | 1042 | frame_count += 1 1043 | # DEBUG ONLY - 1044 | # print out stuff every so often 1045 | if (frame_count % 100) == 0: 1046 | #print(f'frame: {frame_count}, time: {this_AC_int.t:0.2f}, theta:{this_AC_int.y[7]:0.6f}, Elev:{this_joy.get_axis(1) * elev_factor}') 1047 | #print(f'frame: {frame_count}, time: {this_AC_int.t:0.2f}, lat:{current_latlon[0]:0.6f}, lon:{current_latlon[1]:0.6f}') 1048 | #print(f'time: {this_AC_int.t:0.2f}, N:{current_NED[0]:0.3f}, E:{current_NED[1]:0.3f}, D:{current_NED[2]:0.3f}') 1049 | print(f'time: {this_AC_int.t:0.1f}s, Vcas_2fg:{my_fgFDM.get("vcas"):0.1f}KCAS, elev={U1[1]:0.3f} ail={U1[0]:0.3f}, T1/T2={U1[3]:0.3f},{U1[4]:0.3f}') 1050 | 1051 | 1052 | 1053 | # reset integrator timestep counter 1054 | prev_dt = dt 1055 | dt = 0 1056 | run_sim_loop = False 1057 | 1058 | #check/set frame triggers 1059 | if fg_time_adder >= fgdt: 1060 | fg_time_adder = 0 1061 | dt = sim_time_adder 1062 | send_frame_trigger = True 1063 | 1064 | if sim_time_adder >= simdt: 1065 | dt = sim_time_adder 1066 | sim_time_adder = 0 1067 | run_sim_loop = True 1068 | 1069 | 1070 | end = time.perf_counter() 1071 | this_frame_dt = end - start 1072 | fg_time_adder += this_frame_dt 1073 | sim_time_adder += this_frame_dt 1074 | 1075 | 1076 | # save data to disk 1077 | save2disk('test_data.csv', x_data=np.array(t_vector_collector), y_data=np.array(collector), header=signals_header, skip=0) 1078 | #fig1 = make_plots(x_data=np.array(t_vector_collector), y_data=np.array(collector), header=signals_header, skip=1) 1079 | #plt.show() 1080 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # PSim-RCAM 2 | Python implementation of the non-linear 6DOF GARTEUR RCAM aircraft flight dynamics model. 3 | 4 | Group for Aeronautical Research and Technology Europe (GARTEUR) - Research Civil Aircraft Model (RCAM). 5 | http://garteur.org/wp-content/reports/FM/FM_AG-08_TP-088-3.pdf 6 | 7 | The excellent tutorials by Christopher Lum (for Matlab/Simulink) were used as guides: 8 |

9 | 1 - Equations/Modeling: https://www.youtube.com/watch?v=bFFAL9lI2IQ 10 |

11 | 2 - Matlab implementation: https://www.youtube.com/watch?v=m5sEln5bWuM 12 | 13 | The program runs the integration loop at a user defined frame-rate, adjusting the integration steps to the available computing cycles to render real-time data to FlightGear. 14 | 15 | Output is sent to FlightGear (FG), over UDP, at a user specified frame rate. 16 | The FG interface uses the class implemented by Andrew Tridgel: 17 |

18 | fgFDM: https://github.com/ArduPilot/pymavlink/blob/master/fgFDM.py 19 | 20 | Currently, the UDP address is set to the local machine. 21 | 22 | Run this program in one terminal and from a separate terminal, start FG with one of the following commands (depending on the aircraft addons installed): 23 | 24 | fgfs --airport=KSFO --runway=28R --aircraft=ufo --native-fdm=socket,in,60,,5500,udp --fdm=null 25 | 26 | fgfs --airport=KSFO --runway=28R --aircraft=Embraer170 --aircraft-dir=./FlightGear/Aircraft/E-jet-family/ --native-fdm=socket,in,60,,5500,udp --fdm=null 27 | 28 | fgfs --airport=KSFO --runway=28R --aircraft=757-200-RB211 --aircraft-dir=~/.fgfs/Aircraft/org.flightgear.fgaddon.stable_2020/Aircraft/757-200 --native-fdm=socket,in,60,,5500,udp --fdm=null 29 | 30 | fgfs --airport=KSFO --runway=28R --aircraft=757-200-RB211 --aircraft-dir=~/.fgfs/Aircraft/org.flightgear.fgaddon.stable_2020/Aircraft/757-200 --native-fdm=socket,in,60,,5500,udp --fdm=null --enable-hud --turbulence=0.5 --in-air --enable-rembrandt 31 | 32 | REQUIRES a joystick to work. Tested with Logitech USB Stick. 33 | -------------------------------------------------------------------------------- /fgDFM.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # parse and construct FlightGear NET FDM packets 3 | # Andrew Tridgell, November 2011 4 | # released under GNU GPL version 2 or later 5 | # https://github.com/ArduPilot/pymavlink/blob/master/fgFDM.py 6 | 7 | #from builtins import range 8 | #from builtins import object 9 | import math 10 | import struct 11 | 12 | class fgFDMError(Exception): 13 | '''fgFDM error class''' 14 | def __init__(self, msg): 15 | Exception.__init__(self, msg) 16 | self.message = 'fgFDMError: ' + msg 17 | 18 | class fgFDMVariable(object): 19 | '''represent a single fgFDM variable''' 20 | def __init__(self, index, arraylength, units): 21 | self.index = index 22 | self.arraylength = arraylength 23 | self.units = units 24 | 25 | class fgFDMVariableList(object): 26 | '''represent a list of fgFDM variable''' 27 | def __init__(self): 28 | self.vars = {} 29 | self._nextidx = 0 30 | 31 | def add(self, varname, arraylength=1, units=None): 32 | self.vars[varname] = fgFDMVariable(self._nextidx, arraylength, units=units) 33 | self._nextidx += arraylength 34 | 35 | class fgFDM(object): 36 | '''a flightgear native FDM parser/generator''' 37 | def __init__(self): 38 | '''init a fgFDM object''' 39 | self.FG_NET_FDM_VERSION = 24 40 | self.pack_string = '>I 4x 3d 6f 11f 3f 2f I 4I 4f 4f 4f 4f 4f 4f 4f 4f 4f I 4f I 3I 3f 3f 3f I i f 10f' 41 | self.values = [0]*98 42 | 43 | self.FG_MAX_ENGINES = 4 44 | self.FG_MAX_WHEELS = 3 45 | self.FG_MAX_TANKS = 4 46 | 47 | # supported unit mappings 48 | self.unitmap = { 49 | ('radians', 'degrees') : math.degrees(1), 50 | ('rps', 'dps') : math.degrees(1), 51 | ('feet', 'meters') : 0.3048, 52 | ('fps', 'mps') : 0.3048, 53 | ('knots', 'mps') : 0.514444444, 54 | ('knots', 'fps') : 0.514444444/0.3048, 55 | ('fpss', 'mpss') : 0.3048, 56 | ('seconds', 'minutes') : 60, 57 | ('seconds', 'hours') : 3600, 58 | } 59 | 60 | # build a mapping between variable name and index in the values array 61 | # note that the order of this initialisation is critical - it must 62 | # match the wire structure 63 | self.mapping = fgFDMVariableList() 64 | self.mapping.add('version') 65 | 66 | # position 67 | self.mapping.add('longitude', units='radians') # geodetic (radians) 68 | self.mapping.add('latitude', units='radians') # geodetic (radians) 69 | self.mapping.add('altitude', units='meters') # above sea level (meters) 70 | self.mapping.add('agl', units='meters') # above ground level (meters) 71 | 72 | # attitude 73 | self.mapping.add('phi', units='radians') # roll (radians) 74 | self.mapping.add('theta', units='radians') # pitch (radians) 75 | self.mapping.add('psi', units='radians') # yaw or true heading (radians) 76 | self.mapping.add('alpha', units='radians') # angle of attack (radians) 77 | self.mapping.add('beta', units='radians') # side slip angle (radians) 78 | 79 | # Velocities 80 | self.mapping.add('phidot', units='rps') # roll rate (radians/sec) 81 | self.mapping.add('thetadot', units='rps') # pitch rate (radians/sec) 82 | self.mapping.add('psidot', units='rps') # yaw rate (radians/sec) 83 | self.mapping.add('vcas', units='fps') # calibrated airspeed 84 | self.mapping.add('climb_rate', units='fps') # feet per second 85 | self.mapping.add('v_north', units='fps') # north velocity in local/body frame, fps 86 | self.mapping.add('v_east', units='fps') # east velocity in local/body frame, fps 87 | self.mapping.add('v_down', units='fps') # down/vertical velocity in local/body frame, fps 88 | self.mapping.add('v_wind_body_north', units='fps') # north velocity in local/body frame 89 | self.mapping.add('v_wind_body_east', units='fps') # east velocity in local/body frame 90 | self.mapping.add('v_wind_body_down', units='fps') # down/vertical velocity in local/body 91 | 92 | # Accelerations 93 | self.mapping.add('A_X_pilot', units='fpss') # X accel in body frame ft/sec^2 94 | self.mapping.add('A_Y_pilot', units='fpss') # Y accel in body frame ft/sec^2 95 | self.mapping.add('A_Z_pilot', units='fpss') # Z accel in body frame ft/sec^2 96 | 97 | # Stall 98 | self.mapping.add('stall_warning') # 0.0 - 1.0 indicating the amount of stall 99 | self.mapping.add('slip_deg', units='degrees') # slip ball deflection 100 | 101 | # Engine status 102 | self.mapping.add('num_engines') # Number of valid engines 103 | self.mapping.add('eng_state', self.FG_MAX_ENGINES) # Engine state (off, cranking, running) 104 | self.mapping.add('rpm', self.FG_MAX_ENGINES) # Engine RPM rev/min 105 | self.mapping.add('fuel_flow', self.FG_MAX_ENGINES) # Fuel flow gallons/hr 106 | self.mapping.add('fuel_px', self.FG_MAX_ENGINES) # Fuel pressure psi 107 | self.mapping.add('egt', self.FG_MAX_ENGINES) # Exhuast gas temp deg F 108 | self.mapping.add('cht', self.FG_MAX_ENGINES) # Cylinder head temp deg F 109 | self.mapping.add('mp_osi', self.FG_MAX_ENGINES) # Manifold pressure 110 | self.mapping.add('tit', self.FG_MAX_ENGINES) # Turbine Inlet Temperature 111 | self.mapping.add('oil_temp', self.FG_MAX_ENGINES) # Oil temp deg F 112 | self.mapping.add('oil_px', self.FG_MAX_ENGINES) # Oil pressure psi 113 | 114 | # Consumables 115 | self.mapping.add('num_tanks') # Max number of fuel tanks 116 | self.mapping.add('fuel_quantity', self.FG_MAX_TANKS) 117 | 118 | # Gear status 119 | self.mapping.add('num_wheels') 120 | self.mapping.add('wow', self.FG_MAX_WHEELS) 121 | self.mapping.add('gear_pos', self.FG_MAX_WHEELS) 122 | self.mapping.add('gear_steer', self.FG_MAX_WHEELS) 123 | self.mapping.add('gear_compression', self.FG_MAX_WHEELS) 124 | 125 | # Environment 126 | self.mapping.add('cur_time', units='seconds') # current unix time 127 | self.mapping.add('warp', units='seconds') # offset in seconds to unix time 128 | self.mapping.add('visibility', units='meters') # visibility in meters (for env. effects) 129 | 130 | # Control surface positions (normalized values) 131 | self.mapping.add('elevator') 132 | self.mapping.add('elevator_trim_tab') 133 | self.mapping.add('left_flap') 134 | self.mapping.add('right_flap') 135 | self.mapping.add('left_aileron') 136 | self.mapping.add('right_aileron') 137 | self.mapping.add('rudder') 138 | self.mapping.add('nose_wheel') 139 | self.mapping.add('speedbrake') 140 | self.mapping.add('spoilers') 141 | 142 | self._packet_size = struct.calcsize(self.pack_string) 143 | 144 | self.set('version', self.FG_NET_FDM_VERSION) 145 | 146 | if len(self.values) != self.mapping._nextidx: 147 | raise fgFDMError('Invalid variable list in initialisation') 148 | 149 | def packet_size(self): 150 | '''return expected size of FG FDM packets''' 151 | return self._packet_size 152 | 153 | def convert(self, value, fromunits, tounits): 154 | '''convert a value from one set of units to another''' 155 | if fromunits == tounits: 156 | return value 157 | if (fromunits,tounits) in self.unitmap: 158 | return value * self.unitmap[(fromunits,tounits)] 159 | if (tounits,fromunits) in self.unitmap: 160 | return value / self.unitmap[(tounits,fromunits)] 161 | raise fgFDMError("unknown unit mapping (%s,%s)" % (fromunits, tounits)) 162 | 163 | 164 | def units(self, varname): 165 | '''return the default units of a variable''' 166 | if not varname in self.mapping.vars: 167 | raise fgFDMError('Unknown variable %s' % varname) 168 | return self.mapping.vars[varname].units 169 | 170 | 171 | def variables(self): 172 | '''return a list of available variables''' 173 | return sorted(list(self.mapping.vars.keys()), 174 | key = lambda v : self.mapping.vars[v].index) 175 | 176 | 177 | def get(self, varname, idx=0, units=None): 178 | '''get a variable value''' 179 | if not varname in self.mapping.vars: 180 | raise fgFDMError('Unknown variable %s' % varname) 181 | if idx >= self.mapping.vars[varname].arraylength: 182 | raise fgFDMError('index of %s beyond end of array idx=%u arraylength=%u' % ( 183 | varname, idx, self.mapping.vars[varname].arraylength)) 184 | value = self.values[self.mapping.vars[varname].index + idx] 185 | if units: 186 | value = self.convert(value, self.mapping.vars[varname].units, units) 187 | return value 188 | 189 | def set(self, varname, value, idx=0, units=None): 190 | '''set a variable value''' 191 | if not varname in self.mapping.vars: 192 | raise fgFDMError('Unknown variable %s' % varname) 193 | if idx >= self.mapping.vars[varname].arraylength: 194 | raise fgFDMError('index of %s beyond end of array idx=%u arraylength=%u' % ( 195 | varname, idx, self.mapping.vars[varname].arraylength)) 196 | if units: 197 | value = self.convert(value, units, self.mapping.vars[varname].units) 198 | # avoid range errors when packing into 4 byte floats 199 | if math.isinf(value) or math.isnan(value) or math.fabs(value) > 3.4e38: 200 | value = 0 201 | self.values[self.mapping.vars[varname].index + idx] = value 202 | 203 | def parse(self, buf): 204 | '''parse a FD FDM buffer''' 205 | try: 206 | t = struct.unpack(self.pack_string, buf) 207 | except struct.error as msg: 208 | raise fgFDMError('unable to parse - %s' % msg) 209 | self.values = list(t) 210 | 211 | def pack(self): 212 | '''pack a FD FDM buffer from current values''' 213 | for i in range(len(self.values)): 214 | if math.isnan(self.values[i]): 215 | self.values[i] = 0 216 | return struct.pack(self.pack_string, *self.values) --------------------------------------------------------------------------------