← Back to branch summary

~ubuntu-branches/debian/sid/flightgear/sid

« back to all changes in this revision

Viewing changes to src/FDM/JSBSim/math/FGStateSpace.h

  • Committer: Package Import Robot
  • Author(s): Markus Wanner, Markus Wanner, Rebecca Palmer
  • Date: 2014-01-21 22:31:02 UTC
  • mfrom: (1.3.1) (15.1.2 experimental)
  • Revision ID: package-import@ubuntu.com-20140121223102-cjw7g9le25acd119
Tags: 3.0.0~git20140204+c99ea4-1
http://bugs.debian.org/724686
http://bugs.debian.org/556362
http://bugs.debian.org/698308
http://bugs.debian.org/574173
http://bugs.debian.org/738436
http://bugs.debian.org/682947
http://bugs.debian.org/713924
http://bugs.debian.org/252899
[ Markus Wanner ]
* Upload to unstable.
* Adjust B-D to allow building on kfreebsd-*. Closes: #724686.
* Add a lintian-overrides on autotools; we use cmake.
* Upstream corrected the fgfs manpage. Closes: #556362.
* Drop unnecessary man page for gl-info. Closes: #698308.
* Drop README.Linux: it's outdated to the point of uselessness.
  Closes: #574173.
* Add an upper limit of libsimgear-dev versions that flightgear can be
  built with. Closes: #738436.
* Drop the libsvn-dev dependency, neither flightgear nor simgear depend
  on libsvn, anymore. Closes: #682947.
* List icons in debian/install rather than copying around from rules.
* Update menu entry for flightgear, add one for fgcom; add .xpm icons.
  Closes: #713924.
* flightgear.desktop: add German translation
* Bump Standards-Version to 3.9.5; no changes needed.

[ Rebecca Palmer ]
* New upstream release.
* Install the icons (based on code by Saikrishna Arcot).  (Not a
  complete fix for LP908153 as it only sets the menu/Dash icon, not the
  running window's icon, but better than nothing).
* Disable screensaver while running. Closes: LP#793599. Add required
  libdbus-1-dev dependency.
* Remove outdated README.Debian.
* Terrasync now works after just ticking the box. Closes: #252899.
* Always set Terrasync directory.

Show diffs side-by-side

added added

removed removed

Lines of Context:
src/FDM/JSBSim/math/FGStateSpace.h
 
1
/*
 
2
 * FGStateSpace.h
 
3
 * Copyright (C) James Goppert 2010 <james.goppert@gmail.com>
 
4
 *
 
5
 * FGStateSpace.h is free software: you can redistribute it and/or modify it
 
6
 * under the terms of the GNU Lesser General Public License as published by the
 
7
 * Free Software Foundation, either version 2 of the License, or
 
8
 * (at your option) any later version.
 
9
 *
 
10
 * FGStateSpace.h is distributed in the hope that it will be useful, but
 
11
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 
12
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
 
13
 * See the GNU Lesser General Public License for more details.
 
14
 *
 
15
 * You should have received a copy of the GNU Lesser General Public License along
 
16
 * with this program.  If not, see <http://www.gnu.org/licenses/>.
 
17
 */
 
18
 
 
19
#ifndef JSBSim_FGStateSpace_H
 
20
#define JSBSim_FGStateSpace_H
 
21
 
 
22
#include "FGFDMExec.h"
 
23
#include "models/FGPropulsion.h"
 
24
#include "models/FGAccelerations.h"
 
25
#include "models/propulsion/FGEngine.h"
 
26
#include "models/propulsion/FGThruster.h"
 
27
#include "models/propulsion/FGTurbine.h"
 
28
#include "models/propulsion/FGTurboProp.h"
 
29
#include "models/FGAuxiliary.h"
 
30
#include "models/FGFCS.h"
 
31
#include <fstream>
 
32
#include <iostream>
 
33
#include <limits>
 
34
 
 
35
namespace JSBSim
 
36
{
 
37
 
 
38
class FGStateSpace
 
39
{
 
40
public:
 
41
 
 
42
    // component class
 
43
    class Component
 
44
    {
 
45
    protected:
 
46
        FGStateSpace * m_stateSpace;
 
47
        FGFDMExec * m_fdm;
 
48
        std::string m_name, m_unit;
 
49
    public:
 
50
        Component(const std::string & name, const std::string & unit) :
 
51
                m_stateSpace(), m_fdm(), m_name(name), m_unit(unit) {};
 
52
        virtual ~Component() {};
 
53
        virtual double get() const = 0;
 
54
        virtual void set(double val) = 0;
 
55
        virtual double getDeriv() const
 
56
        {
 
57
            // by default should calculate using finite difference approx
 
58
            std::vector<double> x0 = m_stateSpace->x.get();
 
59
            double f0 = get();
 
60
            double dt0 = m_fdm->GetDeltaT();
 
61
            double time0 = m_fdm->GetSimTime();
 
62
            m_fdm->Setdt(1./120.);
 
63
            m_fdm->DisableOutput();
 
64
            m_fdm->Run();
 
65
            double f1 = get();
 
66
            m_stateSpace->x.set(x0);
 
67
            if (m_fdm->GetDebugLevel() > 1)
 
68
            {
 
69
                std::cout << std::scientific
 
70
                          << "name: " << m_name
 
71
                          << "\nf1: " << f0
 
72
                          << "\nf2: " << f1
 
73
                          << "\ndt: " << m_fdm->GetDeltaT()
 
74
                          << "\tdf/dt: " << (f1-f0)/m_fdm->GetDeltaT()
 
75
                          << std::fixed << std::endl;
 
76
            }
 
77
            double deriv = (f1-f0)/m_fdm->GetDeltaT();
 
78
            m_fdm->Setdt(dt0); // restore original value
 
79
            m_fdm->Setsim_time(time0);
 
80
            m_fdm->EnableOutput();
 
81
            return deriv;
 
82
        }
 
83
        void setStateSpace(FGStateSpace * stateSpace)
 
84
        {
 
85
            m_stateSpace = stateSpace;
 
86
        }
 
87
        void setFdm(FGFDMExec * fdm)
 
88
        {
 
89
            m_fdm = fdm;
 
90
        }
 
91
        const std::string & getName() const
 
92
        {
 
93
            return m_name;
 
94
        }
 
95
        const std::string & getUnit() const
 
96
        {
 
97
            return m_unit;
 
98
        }
 
99
    };
 
100
 
 
101
    // component vector class
 
102
    class ComponentVector
 
103
    {
 
104
    public:
 
105
        ComponentVector(FGFDMExec * fdm, FGStateSpace * stateSpace) :
 
106
                m_stateSpace(stateSpace), m_fdm(fdm), m_components() {}
 
107
        ComponentVector & operator=(ComponentVector & componentVector)
 
108
        {
 
109
            m_stateSpace = componentVector.m_stateSpace;
 
110
            m_fdm = componentVector.m_fdm;
 
111
            m_components = componentVector.m_components;
 
112
            return *this;
 
113
        }
 
114
        ComponentVector(const ComponentVector & componentVector) :
 
115
                m_stateSpace(componentVector.m_stateSpace),
 
116
                m_fdm(componentVector.m_fdm),
 
117
                m_components(componentVector.m_components)
 
118
        {
 
119
        }
 
120
        void add(Component * comp)
 
121
        {
 
122
            comp->setStateSpace(m_stateSpace);
 
123
            comp->setFdm(m_fdm);
 
124
            m_components.push_back(comp);
 
125
        }
 
126
        int getSize() const
 
127
        {
 
128
            return m_components.size();
 
129
        }
 
130
        Component * getComp(int i) const
 
131
        {
 
132
            return m_components[i];
 
133
        };
 
134
        Component * getComp(int i)
 
135
        {
 
136
            return m_components[i];
 
137
        };
 
138
        double get(int i) const
 
139
        {
 
140
            return m_components[i]->get();
 
141
        };
 
142
        void set(int i, double val)
 
143
        {
 
144
            m_components[i]->set(val);
 
145
            m_stateSpace->run();
 
146
        };
 
147
        double get(int i)
 
148
        {
 
149
            return m_components[i]->get();
 
150
        };
 
151
        std::vector<double> get() const
 
152
        {
 
153
            std::vector<double> val;
 
154
            for (int i=0;i<getSize();i++) val.push_back(m_components[i]->get());
 
155
            return val;
 
156
        }
 
157
        void get(double * array) const
 
158
        {
 
159
            for (int i=0;i<getSize();i++) array[i] = m_components[i]->get();
 
160
        }
 
161
        double getDeriv(int i)
 
162
        {
 
163
            return m_components[i]->getDeriv();
 
164
        };
 
165
        std::vector<double> getDeriv() const
 
166
        {
 
167
            std::vector<double> val;
 
168
            for (int i=0;i<getSize();i++) val.push_back(m_components[i]->getDeriv());
 
169
            return val;
 
170
        }
 
171
        void getDeriv(double * array) const
 
172
        {
 
173
            for (int i=0;i<getSize();i++) array[i] = m_components[i]->getDeriv();
 
174
        }
 
175
        void set(std::vector<double> vals)
 
176
        {
 
177
            for (int i=0;i<getSize();i++) m_components[i]->set(vals[i]);
 
178
            m_stateSpace->run();
 
179
        }
 
180
        void set(double * array)
 
181
        {
 
182
            for (int i=0;i<getSize();i++) m_components[i]->set(array[i]);
 
183
            m_stateSpace->run();
 
184
        }
 
185
        std::string getName(int i) const
 
186
        {
 
187
            return m_components[i]->getName();
 
188
        };
 
189
        std::vector<std::string> getName() const
 
190
        {
 
191
            std::vector<std::string> name;
 
192
            for (int i=0;i<getSize();i++) name.push_back(m_components[i]->getName());
 
193
            return name;
 
194
        }
 
195
        std::string getUnit(int i) const
 
196
        {
 
197
            return m_components[i]->getUnit();
 
198
        };
 
199
        std::vector<std::string> getUnit() const
 
200
        {
 
201
            std::vector<std::string> unit;
 
202
            for (int i=0;i<getSize();i++) unit.push_back(m_components[i]->getUnit());
 
203
            return unit;
 
204
        }
 
205
        void clear() {
 
206
            m_components.clear();
 
207
        }
 
208
    private:
 
209
        FGStateSpace * m_stateSpace;
 
210
        FGFDMExec * m_fdm;
 
211
        std::vector<Component *> m_components;
 
212
    };
 
213
 
 
214
    // component vectors
 
215
    ComponentVector x, u, y;
 
216
 
 
217
    // constructor
 
218
    FGStateSpace(FGFDMExec * fdm) : x(fdm,this), u(fdm,this), y(fdm,this), m_fdm(fdm) {};
 
219
 
 
220
    void setFdm(FGFDMExec * fdm) { m_fdm = fdm; }
 
221
 
 
222
    void run() {
 
223
        // initialize
 
224
        m_fdm->Initialize(m_fdm->GetIC());
 
225
        for (unsigned int i=0; i<m_fdm->GetPropulsion()->GetNumEngines(); i++) {
 
226
            m_fdm->GetPropulsion()->GetEngine(i)->InitRunning();
 
227
        }
 
228
 
 
229
        // wait for stable state
 
230
        double cost = stateSum();
 
231
        for(int i=0;i<1000;i++) {
 
232
            m_fdm->GetPropulsion()->GetSteadyState();
 
233
            m_fdm->SetTrimStatus(true);
 
234
            m_fdm->DisableOutput();
 
235
            m_fdm->SuspendIntegration();
 
236
            m_fdm->Run();
 
237
            m_fdm->SetTrimStatus(false);
 
238
            m_fdm->EnableOutput();
 
239
            m_fdm->ResumeIntegration();
 
240
 
 
241
            double costNew = stateSum();
 
242
            double dcost = fabs(costNew - cost);
 
243
            if (dcost < std::numeric_limits<double>::epsilon()) {
 
244
                if(m_fdm->GetDebugLevel() > 1) {
 
245
                    std::cout << "cost convergd, i: " << i << std::endl;
 
246
                }
 
247
                break;
 
248
            }
 
249
            if (i > 1000) {
 
250
                if(m_fdm->GetDebugLevel() > 1) {
 
251
                    std::cout << "cost failed to converge, dcost: " 
 
252
                        << std::scientific
 
253
                        << dcost << std::endl;
 
254
                }
 
255
                break;
 
256
            }
 
257
            cost = costNew;
 
258
        }
 
259
    }
 
260
 
 
261
    double stateSum() {
 
262
        double sum = 0;
 
263
        for (int i=0;i<x.getSize();i++) sum += x.get(i);
 
264
        return sum;
 
265
    }
 
266
 
 
267
    void clear() {
 
268
        x.clear();
 
269
        u.clear();
 
270
        y.clear();
 
271
    }
 
272
 
 
273
    // deconstructor
 
274
    virtual ~FGStateSpace() {};
 
275
 
 
276
    // linearization function
 
277
    void linearize(std::vector<double> x0, std::vector<double> u0, std::vector<double> y0,
 
278
                   std::vector< std::vector<double> > & A,
 
279
                   std::vector< std::vector<double> > & B,
 
280
                   std::vector< std::vector<double> > & C,
 
281
                   std::vector< std::vector<double> > & D);
 
282
 
 
283
 
 
284
private:
 
285
 
 
286
    // compute numerical jacobian of a matrix
 
287
    void numericalJacobian(std::vector< std::vector<double> > & J, ComponentVector & y,
 
288
                           ComponentVector & x, const std::vector<double> & y0,
 
289
                           const std::vector<double> & x0, double h=1e-5, bool computeYDerivative = false);
 
290
 
 
291
    // flight dynamcis model
 
292
    FGFDMExec * m_fdm;
 
293
 
 
294
public:
 
295
 
 
296
    // components
 
297
 
 
298
    class Vt : public Component
 
299
    {
 
300
    public:
 
301
        Vt() : Component("Vt","ft/s") {};
 
302
        double get() const
 
303
        {
 
304
            return m_fdm->GetAuxiliary()->GetVt();
 
305
        }
 
306
        void set(double val)
 
307
        {
 
308
            m_fdm->GetIC()->SetVtrueFpsIC(val);
 
309
        }
 
310
        double getDeriv() const
 
311
        {
 
312
 
 
313
            return (m_fdm->GetPropagate()->GetUVW(1)*m_fdm->GetAccelerations()->GetUVWdot(1) +
 
314
                    m_fdm->GetPropagate()->GetUVW(2)*m_fdm->GetAccelerations()->GetUVWdot(2) +
 
315
                    m_fdm->GetPropagate()->GetUVW(3)*m_fdm->GetAccelerations()->GetUVWdot(3))/
 
316
                   m_fdm->GetAuxiliary()->GetVt(); // from lewis, vtrue dot
 
317
        }
 
318
 
 
319
    };
 
320
 
 
321
    class VGround : public Component
 
322
    {
 
323
    public:
 
324
        VGround() : Component("VGround","ft/s") {};
 
325
        double get() const
 
326
        {
 
327
            return m_fdm->GetAuxiliary()->GetVground();
 
328
        }
 
329
        void set(double val)
 
330
        {
 
331
            m_fdm->GetIC()->SetVgroundFpsIC(val);
 
332
        }
 
333
    };
 
334
 
 
335
    class AccelX : public Component
 
336
    {
 
337
    public:
 
338
        AccelX() : Component("AccelX","ft/s^2") {};
 
339
        double get() const
 
340
        {
 
341
            return m_fdm->GetAuxiliary()->GetPilotAccel(1);
 
342
        }
 
343
        void set(double val)
 
344
        {
 
345
            // XXX: not possible to implement currently
 
346
        }
 
347
    };
 
348
 
 
349
    class AccelY : public Component
 
350
    {
 
351
    public:
 
352
        AccelY() : Component("AccelY","ft/s^2") {};
 
353
        double get() const
 
354
        {
 
355
            return m_fdm->GetAuxiliary()->GetPilotAccel(2);
 
356
        }
 
357
        void set(double val)
 
358
        {
 
359
            // XXX: not possible to implement currently
 
360
        }
 
361
    };
 
362
 
 
363
    class AccelZ : public Component
 
364
    {
 
365
    public:
 
366
        AccelZ() : Component("AccelZ","ft/s^2") {};
 
367
        double get() const
 
368
        {
 
369
            return m_fdm->GetAuxiliary()->GetPilotAccel(3);
 
370
        }
 
371
        void set(double val)
 
372
        {
 
373
            // XXX: not possible to implement currently
 
374
        }
 
375
    };
 
376
 
 
377
    class Alpha : public Component
 
378
    {
 
379
    public:
 
380
        Alpha() : Component("Alpha","rad") {};
 
381
        double get() const
 
382
        {
 
383
            return m_fdm->GetAuxiliary()->Getalpha();
 
384
        }
 
385
        void set(double val)
 
386
        {
 
387
            double beta = m_fdm->GetIC()->GetBetaDegIC();
 
388
            double psi = m_fdm->GetIC()->GetPsiRadIC();
 
389
            double theta = m_fdm->GetIC()->GetThetaRadIC();
 
390
            m_fdm->GetIC()->SetAlphaRadIC(val);
 
391
            m_fdm->GetIC()->SetBetaRadIC(beta);
 
392
            m_fdm->GetIC()->SetPsiRadIC(psi);
 
393
            m_fdm->GetIC()->SetThetaRadIC(theta);
 
394
        }
 
395
        double getDeriv() const
 
396
        {
 
397
            return m_fdm->GetAuxiliary()->Getadot();
 
398
        }
 
399
    };
 
400
 
 
401
    class Theta : public Component
 
402
    {
 
403
    public:
 
404
        Theta() : Component("Theta","rad") {};
 
405
        double get() const
 
406
        {
 
407
            return m_fdm->GetPropagate()->GetEuler(2);
 
408
        }
 
409
        void set(double val)
 
410
        {
 
411
                        m_fdm->GetIC()->SetFlightPathAngleRadIC(val-m_fdm->GetIC()->GetAlphaRadIC());
 
412
            //m_fdm->GetIC()->SetThetaRadIC(val);
 
413
        }
 
414
        double getDeriv() const
 
415
        {
 
416
            return m_fdm->GetAuxiliary()->GetEulerRates(2);
 
417
        }
 
418
    };
 
419
 
 
420
    class Q : public Component
 
421
    {
 
422
    public:
 
423
        Q() : Component("Q","rad/s") {};
 
424
        double get() const
 
425
        {
 
426
            return m_fdm->GetPropagate()->GetPQR(2);
 
427
        }
 
428
        void set(double val)
 
429
        {
 
430
            m_fdm->GetIC()->SetQRadpsIC(val);
 
431
        }
 
432
        double getDeriv() const
 
433
        {
 
434
            return m_fdm->GetAccelerations()->GetPQRdot(2);
 
435
        }
 
436
    };
 
437
 
 
438
    class Alt : public Component
 
439
    {
 
440
    public:
 
441
        Alt() : Component("Alt","ft") {};
 
442
        double get() const
 
443
        {
 
444
            return m_fdm->GetPropagate()->GetAltitudeASL();
 
445
        }
 
446
        void set(double val)
 
447
        {
 
448
            m_fdm->GetIC()->SetAltitudeASLFtIC(val);
 
449
        }
 
450
        double getDeriv() const
 
451
        {
 
452
            return m_fdm->GetPropagate()->Gethdot();
 
453
        }
 
454
    };
 
455
 
 
456
    class Beta : public Component
 
457
    {
 
458
    public:
 
459
        Beta() : Component("Beta","rad") {};
 
460
        double get() const
 
461
        {
 
462
            return m_fdm->GetAuxiliary()->Getbeta();
 
463
        }
 
464
        void set(double val)
 
465
        {
 
466
            double psi = m_fdm->GetIC()->GetPsiRadIC();
 
467
            m_fdm->GetIC()->SetBetaRadIC(val);
 
468
            m_fdm->GetIC()->SetPsiRadIC(psi);
 
469
        }
 
470
        double getDeriv() const
 
471
        {
 
472
            return m_fdm->GetAuxiliary()->Getbdot();
 
473
        }
 
474
    };
 
475
 
 
476
    class Phi : public Component
 
477
    {
 
478
    public:
 
479
        Phi() : Component("Phi","rad") {};
 
480
        double get() const
 
481
        {
 
482
            return m_fdm->GetPropagate()->GetEuler(1);
 
483
        }
 
484
        void set(double val)
 
485
        {
 
486
            m_fdm->GetIC()->SetPhiRadIC(val);
 
487
        }
 
488
        double getDeriv() const
 
489
        {
 
490
            return m_fdm->GetAuxiliary()->GetEulerRates(1);
 
491
        }
 
492
    };
 
493
 
 
494
    class P : public Component
 
495
    {
 
496
    public:
 
497
        P() : Component("P","rad/s") {};
 
498
        double get() const
 
499
        {
 
500
            return m_fdm->GetPropagate()->GetPQR(1);
 
501
        }
 
502
        void set(double val)
 
503
        {
 
504
            m_fdm->GetIC()->SetPRadpsIC(val);
 
505
        }
 
506
        double getDeriv() const
 
507
        {
 
508
            return m_fdm->GetAccelerations()->GetPQRdot(1);
 
509
        }
 
510
    };
 
511
 
 
512
    class R : public Component
 
513
    {
 
514
    public:
 
515
        R() : Component("R","rad/s") {};
 
516
        double get() const
 
517
        {
 
518
            return m_fdm->GetPropagate()->GetPQR(3);
 
519
        }
 
520
        void set(double val)
 
521
        {
 
522
            m_fdm->GetIC()->SetRRadpsIC(val);
 
523
        }
 
524
        double getDeriv() const
 
525
        {
 
526
            return m_fdm->GetAccelerations()->GetPQRdot(3);
 
527
        }
 
528
    };
 
529
 
 
530
    class Psi : public Component
 
531
    {
 
532
    public:
 
533
        Psi() : Component("Psi","rad") {};
 
534
        double get() const
 
535
        {
 
536
            return m_fdm->GetPropagate()->GetEuler(3);
 
537
        }
 
538
        void set(double val)
 
539
        {
 
540
            m_fdm->GetIC()->SetPsiRadIC(val);
 
541
        }
 
542
        double getDeriv() const
 
543
        {
 
544
            return m_fdm->GetAuxiliary()->GetEulerRates(3);
 
545
        }
 
546
    };
 
547
 
 
548
    class ThrottleCmd : public Component
 
549
    {
 
550
    public:
 
551
        ThrottleCmd() : Component("ThtlCmd","norm") {};
 
552
        double get() const
 
553
        {
 
554
            return m_fdm->GetFCS()->GetThrottleCmd(0);
 
555
        }
 
556
        void set(double val)
 
557
        {
 
558
            for (unsigned int i=0;i<m_fdm->GetPropulsion()->GetNumEngines();i++)
 
559
                m_fdm->GetFCS()->SetThrottleCmd(i,val);
 
560
            m_fdm->GetFCS()->Run(true);
 
561
        }
 
562
    };
 
563
 
 
564
    class ThrottlePos : public Component
 
565
    {
 
566
    public:
 
567
        ThrottlePos() : Component("ThtlPos","norm") {};
 
568
        double get() const
 
569
        {
 
570
            return m_fdm->GetFCS()->GetThrottlePos(0);
 
571
        }
 
572
        void set(double val)
 
573
        {
 
574
            for (unsigned int i=0;i<m_fdm->GetPropulsion()->GetNumEngines();i++)
 
575
                m_fdm->GetFCS()->SetThrottlePos(i,val);
 
576
        }
 
577
    };
 
578
 
 
579
    class DaCmd : public Component
 
580
    {
 
581
    public:
 
582
        DaCmd() : Component("DaCmd","norm") {};
 
583
        double get() const
 
584
        {
 
585
            return m_fdm->GetFCS()->GetDaCmd();
 
586
        }
 
587
        void set(double val)
 
588
        {
 
589
            m_fdm->GetFCS()->SetDaCmd(val);
 
590
            m_fdm->GetFCS()->Run(true);
 
591
        }
 
592
    };
 
593
 
 
594
    class DaPos : public Component
 
595
    {
 
596
    public:
 
597
        DaPos() : Component("DaPos","norm") {};
 
598
        double get() const
 
599
        {
 
600
            return m_fdm->GetFCS()->GetDaLPos();
 
601
        }
 
602
        void set(double val)
 
603
        {
 
604
            m_fdm->GetFCS()->SetDaLPos(ofRad,val);
 
605
            m_fdm->GetFCS()->SetDaRPos(ofRad,val); // TODO: check if this is neg.
 
606
        }
 
607
    };
 
608
 
 
609
    class DeCmd : public Component
 
610
    {
 
611
    public:
 
612
        DeCmd() : Component("DeCmd","norm") {};
 
613
        double get() const
 
614
        {
 
615
            return m_fdm->GetFCS()->GetDeCmd();
 
616
        }
 
617
        void set(double val)
 
618
        {
 
619
            m_fdm->GetFCS()->SetDeCmd(val);
 
620
            m_fdm->GetFCS()->Run(true);
 
621
        }
 
622
    };
 
623
 
 
624
    class DePos : public Component
 
625
    {
 
626
    public:
 
627
        DePos() : Component("DePos","norm") {};
 
628
        double get() const
 
629
        {
 
630
            return m_fdm->GetFCS()->GetDePos();
 
631
        }
 
632
        void set(double val)
 
633
        {
 
634
            m_fdm->GetFCS()->SetDePos(ofRad,val);
 
635
        }
 
636
    };
 
637
 
 
638
    class DrCmd : public Component
 
639
    {
 
640
    public:
 
641
        DrCmd() : Component("DrCmd","norm") {};
 
642
        double get() const
 
643
        {
 
644
            return m_fdm->GetFCS()->GetDrCmd();
 
645
        }
 
646
        void set(double val)
 
647
        {
 
648
            m_fdm->GetFCS()->SetDrCmd(val);
 
649
            m_fdm->GetFCS()->Run(true);
 
650
        }
 
651
    };
 
652
 
 
653
    class DrPos : public Component
 
654
    {
 
655
    public:
 
656
        DrPos() : Component("DrPos","norm") {};
 
657
        double get() const
 
658
        {
 
659
            return m_fdm->GetFCS()->GetDrPos();
 
660
        }
 
661
        void set(double val)
 
662
        {
 
663
            m_fdm->GetFCS()->SetDrPos(ofRad,val);
 
664
        }
 
665
    };
 
666
 
 
667
    class Rpm0 : public Component
 
668
    {
 
669
    public:
 
670
        Rpm0() : Component("Rpm0","rev/min") {};
 
671
        double get() const
 
672
        {
 
673
            return m_fdm->GetPropulsion()->GetEngine(0)->GetThruster()->GetRPM();
 
674
        }
 
675
        void set(double val)
 
676
        {
 
677
            m_fdm->GetPropulsion()->GetEngine(0)->GetThruster()->SetRPM(val);
 
678
        }
 
679
    };
 
680
 
 
681
    class Rpm1 : public Component
 
682
    {
 
683
    public:
 
684
        Rpm1() : Component("Rpm1","rev/min") {};
 
685
        double get() const
 
686
        {
 
687
            return m_fdm->GetPropulsion()->GetEngine(1)->GetThruster()->GetRPM();
 
688
        }
 
689
        void set(double val)
 
690
        {
 
691
            m_fdm->GetPropulsion()->GetEngine(1)->GetThruster()->SetRPM(val);
 
692
        }
 
693
    };
 
694
 
 
695
    class Rpm2 : public Component
 
696
    {
 
697
    public:
 
698
        Rpm2() : Component("Rpm2","rev/min") {};
 
699
        double get() const
 
700
        {
 
701
            return m_fdm->GetPropulsion()->GetEngine(2)->GetThruster()->GetRPM();
 
702
        }
 
703
        void set(double val)
 
704
        {
 
705
            m_fdm->GetPropulsion()->GetEngine(2)->GetThruster()->SetRPM(val);
 
706
        }
 
707
    };
 
708
 
 
709
    class Rpm3 : public Component
 
710
    {
 
711
    public:
 
712
        Rpm3() : Component("Rpm3","rev/min") {};
 
713
        double get() const
 
714
        {
 
715
            return m_fdm->GetPropulsion()->GetEngine(3)->GetThruster()->GetRPM();
 
716
        }
 
717
        void set(double val)
 
718
        {
 
719
            m_fdm->GetPropulsion()->GetEngine(3)->GetThruster()->SetRPM(val);
 
720
        }
 
721
    };
 
722
 
 
723
    class PropPitch : public Component
 
724
    {
 
725
    public:
 
726
        PropPitch() : Component("Prop Pitch","deg") {};
 
727
        double get() const
 
728
        {
 
729
            return m_fdm->GetPropulsion()->GetEngine(0)->GetThruster()->GetPitch();
 
730
        }
 
731
        void set(double val)
 
732
        {
 
733
            for (unsigned int i=0;i<m_fdm->GetPropulsion()->GetNumEngines();i++)
 
734
                m_fdm->GetPropulsion()->GetEngine(i)->GetThruster()->SetPitch(val);
 
735
        }
 
736
    };
 
737
 
 
738
    class Longitude : public Component
 
739
    {
 
740
    public:
 
741
        Longitude() : Component("Longitude","rad") {};
 
742
        double get() const
 
743
        {
 
744
            return m_fdm->GetPropagate()->GetLongitude();
 
745
        }
 
746
        void set(double val)
 
747
        {
 
748
            m_fdm->GetIC()->SetLongitudeRadIC(val);
 
749
        }
 
750
        double getDeriv() const
 
751
        {
 
752
            return m_fdm->GetPropagate()->GetVel(2)/(cos(m_fdm->GetPropagate()->GetLatitude())*m_fdm->GetPropagate()->GetRadius());
 
753
        }
 
754
    };
 
755
 
 
756
    class Latitude : public Component
 
757
    {
 
758
    public:
 
759
        Latitude() : Component("Latitude","rad") {};
 
760
        double get() const
 
761
        {
 
762
            return m_fdm->GetPropagate()->GetLatitude();
 
763
        }
 
764
        void set(double val)
 
765
        {
 
766
            m_fdm->GetIC()->SetLatitudeRadIC(val);
 
767
        }
 
768
        double getDeriv() const
 
769
        {
 
770
            return m_fdm->GetPropagate()->GetVel(1)/(m_fdm->GetPropagate()->GetRadius());
 
771
        }
 
772
    };
 
773
 
 
774
    class Pi : public Component
 
775
    {
 
776
    public:
 
777
        Pi() : Component("P inertial","rad/s") {};
 
778
        double get() const
 
779
        {
 
780
            return m_fdm->GetPropagate()->GetPQRi(1);
 
781
        }
 
782
        void set(double val)
 
783
        {
 
784
            //Set PQR from PQRi
 
785
            //VState.vPQR = VState.vPQRi - Ti2b * vOmegaEarth;
 
786
            m_fdm->GetIC()->SetQRadpsIC(val + \
 
787
                    (m_fdm->GetPropagate()->GetPQR(1) - m_fdm->GetPropagate()->GetPQRi(1)));
 
788
        }
 
789
        double getDeriv() const
 
790
        {
 
791
            return m_fdm->GetAccelerations()->GetPQRdot(1);
 
792
        }
 
793
    };
 
794
 
 
795
    class Qi : public Component
 
796
    {
 
797
    public:
 
798
        Qi() : Component("Q inertial","rad/s") {};
 
799
        double get() const
 
800
        {
 
801
            return m_fdm->GetPropagate()->GetPQRi(2);
 
802
        }
 
803
        void set(double val)
 
804
        {
 
805
            //Set PQR from PQRi
 
806
            //VState.vPQR = VState.vPQRi - Ti2b * vOmegaEarth;
 
807
            m_fdm->GetIC()->SetQRadpsIC(val + \
 
808
                    (m_fdm->GetPropagate()->GetPQR(2) - m_fdm->GetPropagate()->GetPQRi(2)));
 
809
        }
 
810
        double getDeriv() const
 
811
        {
 
812
            return m_fdm->GetAccelerations()->GetPQRdot(2);
 
813
        }
 
814
    };
 
815
 
 
816
    class Ri : public Component
 
817
    {
 
818
    public:
 
819
        Ri() : Component("R inertial","rad/s") {};
 
820
        double get() const
 
821
        {
 
822
            return m_fdm->GetPropagate()->GetPQRi(3);
 
823
        }
 
824
        void set(double val)
 
825
        {
 
826
            //Set PQR from PQRi
 
827
            //VState.vPQR = VState.vPQRi - Ti2b * vOmegaEarth;
 
828
            m_fdm->GetIC()->SetQRadpsIC(val + \
 
829
                    (m_fdm->GetPropagate()->GetPQR(3) - m_fdm->GetPropagate()->GetPQRi(3)));
 
830
        }
 
831
        double getDeriv() const
 
832
        {
 
833
            return m_fdm->GetAccelerations()->GetPQRdot(3);
 
834
        }
 
835
    };
 
836
 
 
837
    class Vn : public Component
 
838
    {
 
839
    public:
 
840
        Vn() : Component("Vel north","feet/s") {};
 
841
        double get() const
 
842
        {
 
843
            return m_fdm->GetPropagate()->GetVel(1);
 
844
        }
 
845
        void set(double val)
 
846
        {
 
847
            m_fdm->GetIC()->SetVNorthFpsIC(val);
 
848
        }
 
849
        double getDeriv() const
 
850
        {
 
851
            //get NED accel from body accel
 
852
            return (m_fdm->GetPropagate()->GetTb2l()*m_fdm->GetAccelerations()->GetUVWdot())(1);
 
853
        }
 
854
    };
 
855
 
 
856
    class Ve : public Component
 
857
    {
 
858
    public:
 
859
        Ve() : Component("Vel east","feet/s") {};
 
860
        double get() const
 
861
        {
 
862
            return m_fdm->GetPropagate()->GetVel(2);
 
863
        }
 
864
        void set(double val)
 
865
        {
 
866
            m_fdm->GetIC()->SetVEastFpsIC(val);
 
867
        }
 
868
        double getDeriv() const
 
869
        {
 
870
            //get NED accel from body accel
 
871
            return (m_fdm->GetPropagate()->GetTb2l()*m_fdm->GetAccelerations()->GetUVWdot())(2);
 
872
        }
 
873
    };
 
874
 
 
875
    class Vd : public Component
 
876
    {
 
877
    public:
 
878
        Vd() : Component("Vel down","feet/s") {};
 
879
        double get() const
 
880
        {
 
881
            return m_fdm->GetPropagate()->GetVel(3);
 
882
        }
 
883
        void set(double val)
 
884
        {
 
885
            m_fdm->GetIC()->SetVDownFpsIC(val);
 
886
        }
 
887
        double getDeriv() const
 
888
        {
 
889
            //get NED accel from body accel
 
890
            return (m_fdm->GetPropagate()->GetTb2l()*m_fdm->GetAccelerations()->GetUVWdot())(3);
 
891
        }
 
892
    };
 
893
 
 
894
    class COG : public Component
 
895
    {
 
896
    public:
 
897
        COG() : Component("Course Over Ground","rad") {};
 
898
        double get() const
 
899
        {
 
900
            //cog = atan2(Ve,Vn)
 
901
            return atan2(m_fdm->GetPropagate()->GetVel(2),m_fdm->GetPropagate()->GetVel(1));
 
902
        }
 
903
        void set(double val)
 
904
        {
 
905
            //set Vn and Ve according to vGround and COG
 
906
            m_fdm->GetIC()->SetVNorthFpsIC(m_fdm->GetAuxiliary()->GetVground()*cos(val));
 
907
            m_fdm->GetIC()->SetVEastFpsIC(m_fdm->GetAuxiliary()->GetVground()*sin(val));
 
908
        }
 
909
        double getDeriv() const
 
910
        {
 
911
            double Vn = m_fdm->GetPropagate()->GetVel(1);
 
912
            double Vndot = (m_fdm->GetPropagate()->GetTb2l()*m_fdm->GetAccelerations()->GetUVWdot())(1);
 
913
            double Ve = m_fdm->GetPropagate()->GetVel(2);
 
914
            double Vedot = (m_fdm->GetPropagate()->GetTb2l()*m_fdm->GetAccelerations()->GetUVWdot())(2); 
 
915
 
 
916
            //dCOG/dt = dCOG/dVe*dVe/dt + dCOG/dVn*dVn/dt
 
917
            return Vn/(Vn*Vn+Ve*Ve)*Vedot - Ve/(Vn*Vn+Ve*Ve)*Vndot;
 
918
        }
 
919
    };
 
920
 
 
921
};
 
922
 
 
923
// stream output
 
924
std::ostream &operator<<(std::ostream &out, const FGStateSpace::Component &c );
 
925
std::ostream &operator<<(std::ostream &out, const FGStateSpace::ComponentVector &v );
 
926
std::ostream &operator<<(std::ostream &out, const FGStateSpace &ss );
 
927
std::ostream &operator<<( std::ostream &out, const std::vector< std::vector<double> > &vec2d );
 
928
std::ostream &operator<<( std::ostream &out, const std::vector<double> &vec );
 
929
 
 
930
} // JSBSim
 
931
 
 
932
#endif
 
933
 
 
934
// vim:ts=4:sw=4