JSBSim Flight Dynamics Model  1.0 (02 March 2017)
An Open Source Flight Dynamics and Control Software Library in C++
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 
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
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  size_t 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 (unsigned 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 (unsigned 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 (unsigned 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 (unsigned int i=0;i<getSize();i++) array[i] = m_components[i]->getDeriv();
174  }
175  void set(std::vector<double> vals)
176  {
177  for (unsigned 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 (unsigned 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 (unsigned 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 (unsigned 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 (unsigned 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
void SetDePos(int form, double pos)
Sets the elevator position.
Definition: FGFCS.cpp:228
FGAccelerations * GetAccelerations(void)
Returns the FGAccelerations pointer.
Definition: FGFDMExec.h:347
void SetThetaRadIC(double theta)
Sets the initial pitch angle.
void SetVEastFpsIC(double ve)
Sets the initial local axis east velocity.
FGInitialCondition * GetIC(void)
Returns a pointer to the FGInitialCondition object.
Definition: FGFDMExec.h:385
void SetVtrueFpsIC(double vt)
Sets the initial true airspeed.
void SetLatitudeRadIC(double lat)
Sets the initial latitude.
double GetDeCmd(void) const
Gets the elevator command.
Definition: FGFCS.h:222
void SetVNorthFpsIC(double vn)
Sets the initial local axis north velocity.
double GetBetaDegIC(void) const
Gets the initial sideslip angle.
double GetDePos(int form=ofRad) const
Gets the elevator position.
Definition: FGFCS.h:300
void SetDrPos(int form, double pos)
Sets the rudder position.
Definition: FGFCS.cpp:247
double GetDaLPos(int form=ofRad) const
Gets the left aileron position.
Definition: FGFCS.h:290
void SetPhiRadIC(double phi)
Sets the initial roll angle.
const FGColumnVector3 & GetPQRi(void) const
Retrieves the body angular rates vector, relative to the ECI (inertial) frame.
Definition: FGPropagate.h:232
FGAuxiliary * GetAuxiliary(void)
Returns the FGAuxiliary pointer.
Definition: FGFDMExec.h:371
FGEngine * GetEngine(unsigned int index) const
Retrieves an engine object pointer from the list of engines.
Definition: FGPropulsion.h:140
void SetQRadpsIC(double Q)
Sets the initial body axis pitch rate.
double GetThrottlePos(int engine) const
Gets the throttle position.
Definition: FGFCS.cpp:377
void DisableOutput(void)
Disables data logging to all outputs.
Definition: FGFDMExec.h:474
double GetAltitudeASL(void) const
Returns the current altitude above sea level.
Definition: FGPropagate.h:337
const FGColumnVector3 & GetUVWdot(void) const
Retrieves the body axis acceleration.
void SetThrottleCmd(int engine, double cmd)
Sets the throttle command for the specified engine.
Definition: FGFCS.cpp:323
void SetDaCmd(double cmd)
Sets the aileron command.
Definition: FGFCS.h:379
void EnableOutput(void)
Enables data logging to all outputs.
Definition: FGFDMExec.h:476
bool Run(bool Holding)
Runs the Flight Controls model; called by the Executive Can pass in a value indicating if the executi...
Definition: FGFCS.cpp:161
void SetDaRPos(int form, double pos)
Sets the right aileron position.
Definition: FGFCS.cpp:209
void SetFlightPathAngleRadIC(double gamma)
Sets the initial flight path angle.
const FGColumnVector3 & GetUVW(void) const
Retrieves the body frame vehicle velocity vector.
Definition: FGPropagate.h:204
const FGColumnVector3 & GetPQR(void) const
Retrieves the body angular rates vector, relative to the ECEF frame.
Definition: FGPropagate.h:218
void Initialize(FGInitialCondition *FGIC)
Initializes the simulation with initial conditions.
Definition: FGFDMExec.cpp:591
void SetDaLPos(int form, double pos)
Sets the left aileron position.
Definition: FGFCS.cpp:190
double Setsim_time(double cur_time)
Sets the current sim time.
Definition: FGFDMExec.h:551
void Setdt(double delta_t)
Sets the integration time step for the simulation executive.
Definition: FGFDMExec.h:559
double GetThrottleCmd(int engine) const
Gets the throttle command.
Definition: FGFCS.cpp:359
int GetDebugLevel(void) const
Retrieves the current debug level setting.
Definition: FGFDMExec.h:585
void SetRRadpsIC(double R)
Sets the initial body axis yaw rate.
void SetVgroundFpsIC(double vg)
Sets the initial ground speed.
double GetPsiRadIC(void) const
Gets the initial heading angle.
const FGMatrix33 & GetTb2l(void) const
Retrieves the body-to-local transformation matrix.
Definition: FGPropagate.h:474
const FGColumnVector3 & GetVel(void) const
Retrieves the velocity vector.
Definition: FGPropagate.h:192
void SetDrCmd(double cmd)
Sets the rudder command.
Definition: FGFCS.h:387
const FGColumnVector3 & GetEuler(void) const
Retrieves the Euler angles that define the vehicle orientation.
Definition: FGPropagate.h:260
void SetDeCmd(double cmd)
Sets the elevator command.
Definition: FGFCS.h:383
void SetVDownFpsIC(double vd)
Sets the initial local axis down velocity.
double GetDaCmd(void) const
Gets the aileron command.
Definition: FGFCS.h:218
FGFCS * GetFCS(void)
Returns the FGFCS pointer.
Definition: FGFDMExec.h:351
double GetSimTime(void) const
Returns the cumulative simulation time in seconds.
Definition: FGFDMExec.h:533
void SetAlphaRadIC(double alpha)
Sets the initial angle of attack.
double Gethdot(void) const
Returns the current altitude rate.
Definition: FGPropagate.h:425
double GetAlphaRadIC(void) const
Gets the initial angle of attack.
void SetThrottlePos(int engine, double cmd)
Sets the actual throttle setting for the specified engine.
Definition: FGFCS.cpp:341
double GetDeltaT(void) const
Returns the simulation delta T.
Definition: FGFDMExec.h:536
const FGColumnVector3 & GetPQRdot(void) const
Retrieves the body axis angular acceleration vector.
FGPropulsion * GetPropulsion(void)
Returns the FGPropulsion pointer.
Definition: FGFDMExec.h:353
bool Run(void)
This function executes each scheduled model in succession.
Definition: FGFDMExec.cpp:310
void SetPRadpsIC(double P)
Sets the initial body axis roll rate.
void SetLongitudeRadIC(double lon)
Sets the initial longitude.
double GetThetaRadIC(void) const
Gets the initial pitch angle.
Encapsulates the JSBSim simulation executive.
Definition: FGFDMExec.h:189
void SetBetaRadIC(double beta)
Sets the initial sideslip angle.
double GetDrCmd(void) const
Gets the rudder command.
Definition: FGFCS.h:226
double GetVground(void) const
Gets the ground speed in feet per second.
Definition: FGAuxiliary.h:217
void SetAltitudeASLFtIC(double altitudeASL)
Sets the altitude above sea level initial condition in feet.
void SetPsiRadIC(double psi)
Sets the initial heading angle.
double GetVt(void) const
Gets the magnitude of total vehicle velocity including wind effects in feet per second.
Definition: FGAuxiliary.h:211
FGPropagate * GetPropagate(void)
Returns the FGPropagate pointer.
Definition: FGFDMExec.h:369
double GetDrPos(int form=ofRad) const
Gets the rudder position.
Definition: FGFCS.h:305
unsigned int GetNumEngines(void) const
Retrieves the number of engines defined for the aircraft.
Definition: FGPropulsion.h:134