MBDyn-1.7.3
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups
instruments.cc
Go to the documentation of this file.
1 /* $Header: /var/cvs/mbdyn/mbdyn/mbdyn-1.0/mbdyn/aero/instruments.cc,v 1.32 2017/01/12 14:45:58 masarati Exp $ */
2 /*
3  * MBDyn (C) is a multibody analysis code.
4  * http://www.mbdyn.org
5  *
6  * Copyright (C) 1996-2017
7  *
8  * Pierangelo Masarati <masarati@aero.polimi.it>
9  * Paolo Mantegazza <mantegazza@aero.polimi.it>
10  *
11  * Dipartimento di Ingegneria Aerospaziale - Politecnico di Milano
12  * via La Masa, 34 - 20156 Milano, Italy
13  * http://www.aero.polimi.it
14  *
15  * Changing this copyright notice is forbidden.
16  *
17  * This program is free software; you can redistribute it and/or modify
18  * it under the terms of the GNU General Public License as published by
19  * the Free Software Foundation (version 2 of the License).
20  *
21  *
22  * This program is distributed in the hope that it will be useful,
23  * but WITHOUT ANY WARRANTY; without even the implied warranty of
24  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
25  * GNU General Public License for more details.
26  *
27  * You should have received a copy of the GNU General Public License
28  * along with this program; if not, write to the Free Software
29  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
30  */
31 
32 #include "mbconfig.h" /* This goes first in every *.c,*.cc file */
33 
34 /* Aircraft Instruments */
35 
36 #include "dataman.h"
37 #include "instruments.h"
38 
40  const DofOwner *pDO,
41  const StructNode* pN, const Mat3x3 &R, flag fOut)
42 : Elem(uLabel, fOut),
43 AerodynamicElem(uLabel, pDO, fOut),
44 pNode(pN),
45 Rh(R)
46 {
47  memset(&dMeasure[0], 0, sizeof(dMeasure));
48 }
49 
51 {
52  NO_OP;
53 }
54 
57 {
58  return Elem::AERODYNAMIC;
59 }
60 
61 void
63 {
64  const Vec3& X(pNode->GetXCurr());
65  const Mat3x3& R(pNode->GetRCurr()*Rh);
66  const Vec3& V(pNode->GetVCurr());
67  const Vec3& Omega(pNode->GetWCurr());
68  Vec3 VV = V;
69  const Vec3 e1(R.GetVec(1));
70  const Vec3 e2(R.GetVec(2));
71  const Vec3 e3(R.GetVec(3));
72 
73  /*
74  * Assumptions:
75  * - world is "flat"
76  * - world "z" is positive upward
77  * (flight mechanics conventions?)
78  * - aircraft "x" is positive forward
79  * - aircraft "z" is positive downward
80  * - aircraft "y" is positive towards the right of the pilot
81  * - pitch is positive nose up
82  * - roll is positive right wing down
83  * - yaw is positive right wing backward
84  */
85 
86  Vec3 VTmp(Zero3);
87  if (fGetAirVelocity(VTmp, X)) {
88  VV -= VTmp;
89  }
90 
91  /* airspeed */
92  dMeasure[AIRSPEED] = VV.Norm();
93 
94  /* groundspeed */
95  VTmp = V;
96  VTmp(3) = 0.;
97  dMeasure[GROUNDSPEED] = VTmp.Norm();
98 
99  /* altitude */
100  dMeasure[ALTITUDE] = X(3);
101 
102  /* attitude */
103  /* FIXME: better asin(e1(3)) ? */
104  dMeasure[ATTITUDE] = std::atan2(e1(3), e1(1));
105 
106  /* bank */
107  /* FIXME: better asin(e2(3)) ? */
108  dMeasure[BANK] = -std::atan2(e2(3), e2(2));
109 
110  /* turn */
111  dMeasure[TURN] = 0.; /* FIXME */
112 
113  /* slip */
114  dMeasure[SLIP] = 0.; /* FIXME */
115 
116  /* vertical speed */
117  dMeasure[VERTICALSPEED] = VV(3);
118 
119  /* angle of attack */
120  VTmp = R.MulTV(VV);
121  dMeasure[AOA] = std::atan2(VTmp(3), VTmp(1));
122 
123  /* heading */
124  /* FIXME: assumes a flat world! N={1,0,0}, W={0,1,0} */
125  dMeasure[HEADING] = -std::atan2(e1(2), e1(1));
126 
127  /* longitude */
128  /* FIXME: ??? */
129  dMeasure[LONGITUDE] = -X(2); // /EARTH_RADIUS?
130 
131  /* latitude */
132  /* FIXME: ??? */
133  dMeasure[LATITUDE] = X(1); // /EARTH_RADIUS?
134 
135  VTmp = R.MulTV(Omega);
136  dMeasure[ROLLRATE] = VTmp(1);
137  dMeasure[PITCHRATE] = VTmp(2);
138  dMeasure[YAWRATE] = VTmp(3);
139 }
140 
141 /* Scrive il contributo dell'elemento al file di restart */
142 std::ostream&
143 AircraftInstruments::Restart(std::ostream& out) const
144 {
145  out << "aircraft instruments: " << GetLabel()
146  << ", " << pNode->GetLabel()
147  << ", orientation, ", Rh.Write(out)
148  << ";" << std::endl;
149 
150  return out;
151 }
152 
153 void
155 {
156  if (bToBeOutput()) {
157  std::ostream& out = OH.Aerodynamic()
158  << std::setw(8) << GetLabel();
159 
160  for (int iCnt = 1; iCnt < LASTMEASURE; iCnt++) {
161  out << " " << dMeasure[iCnt];
162  }
163 
164  out << std::endl;
165  }
166 }
167 
168 /* Dimensioni del workspace */
169 void
170 AircraftInstruments::WorkSpaceDim(integer* piNumRows, integer* piNumCols) const
171 {
172  *piNumRows = 0;
173  *piNumCols = 0;
174 }
175 
176 /* assemblaggio jacobiano */
179  doublereal /* dCoef */ ,
180  const VectorHandler& /* XCurr */ ,
181  const VectorHandler& /* XPrimeCurr */ )
182 {
183  DEBUGCOUTFNAME("AircraftInstruments::AssJac");
184 
185  WorkMat.SetNullMatrix();
186 
187  return WorkMat;
188 }
189 
190 /* assemblaggio residuo */
193  doublereal dCoef,
194  const VectorHandler& XCurr,
195  const VectorHandler& XPrimeCurr)
196 {
197  WorkVec.Resize(0);
198 
199  Update();
200 
201  return WorkVec;
202 }
203 
204 /* Dati privati */
205 unsigned int
207 {
208  return LASTMEASURE-1;
209 }
210 
211 unsigned int
213 {
214  ASSERT(s != NULL);
215 
216  struct {
217  const char *s;
218  int i;
219  } s2i[] = {
220  { "airspeed", AIRSPEED },
221  { "ground" "speed", GROUNDSPEED },
222  { "altitude", ALTITUDE },
223  { "attitude", ATTITUDE },
224  { "bank", BANK },
225  { "turn", TURN },
226  { "slip", SLIP },
227  { "vertical" "speed", VERTICALSPEED},
228  { "angle" "of" "attack", AOA },
229  { "aoa", AOA },
230  { "heading", HEADING },
231  { "longitude", LONGITUDE },
232  { "latitude", LATITUDE },
233  { "rollrate", ROLLRATE },
234  { "pitchrate", PITCHRATE },
235  { "yawrate", YAWRATE },
236  { 0 }
237  };
238 
239  for (int i = 0; s2i[i].s != 0; i++) {
240  if (strcasecmp(s, s2i[i].s) == 0) {
241  return s2i[i].i;
242  }
243  }
244 
245  return 0;
246 }
247 
250 {
251  if (i <= 0 || i >= LASTMEASURE) {
252  silent_cerr("AircraftInstruments(" << GetLabel() << "): "
253  "illegal measure " << i << std::endl);
255  }
256 
257  return dMeasure[i];
258 }
259 
260 void
261 AircraftInstruments::GetConnectedNodes(std::vector<const Node *>& connectedNodes) const
262 {
263  connectedNodes.resize(1);
264  connectedNodes[0] = pNode;
265 }
266 
267 Elem *
269  const DofOwner *pDO, unsigned int uLabel)
270 {
271  Elem *pEl = NULL;
272 
273  const StructNode* pNode = pDM->ReadNode<const StructNode, Node::STRUCTURAL>(HP);
274  Mat3x3 R = Eye3;
275  if (HP.IsKeyWord("orientation")) {
276  if (HP.IsKeyWord("flight" "mechanics")) {
277  R = ::Eye3;
278 
279  } else if (HP.IsKeyWord("aeroelasticity")) {
280  R = Mat3x3(-1., 0., 0., 0., 1., 0., 0., 0., -1.);
281 
282  } else {
283  R = HP.GetRotRel(ReferenceFrame(pNode));
284  }
285  }
286  flag fOut = pDM->fReadOutput(HP, Elem::AERODYNAMIC);
287 
289  AircraftInstruments(uLabel, pDO, pNode, R, fOut));
290 
291  return pEl;
292 }
293 
flag fReadOutput(MBDynParser &HP, const T &t) const
Definition: dataman.h:1064
Mat3x3 GetRotRel(const ReferenceFrame &rf)
Definition: mbpar.cc:1795
const Vec3 Zero3(0., 0., 0.)
long int flag
Definition: mbdyn.h:43
virtual bool bToBeOutput(void) const
Definition: output.cc:890
#define MBDYN_EXCEPT_ARGS
Definition: except.h:63
Definition: matvec3.h:98
#define DEBUGCOUTFNAME(fname)
Definition: myassert.h:256
virtual const Mat3x3 & GetRCurr(void) const
Definition: strnode.h:1012
virtual VariableSubMatrixHandler & AssJac(VariableSubMatrixHandler &WorkMat, doublereal, const VectorHandler &, const VectorHandler &)
Definition: instruments.cc:178
virtual SubVectorHandler & AssRes(SubVectorHandler &WorkVec, doublereal dCoef, const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)
Definition: instruments.cc:192
Elem * ReadAircraftInstruments(DataManager *pDM, MBDynParser &HP, const DofOwner *pDO, unsigned int uLabel)
Definition: instruments.cc:268
virtual void Output(OutputHandler &OH) const
Definition: instruments.cc:154
const Mat3x3 Eye3(1., 0., 0., 0., 1., 0., 0., 0., 1.)
#define NO_OP
Definition: myassert.h:74
virtual void GetConnectedNodes(std::vector< const Node * > &connectedNodes) const
Definition: instruments.cc:261
Vec3 GetVec(unsigned short int i) const
Definition: matvec3.h:893
Vec3 MulTV(const Vec3 &v) const
Definition: matvec3.cc:482
AircraftInstruments(unsigned int uLabel, const DofOwner *pDO, const StructNode *pN, const Mat3x3 &R, flag fOut)
Definition: instruments.cc:39
void SetNullMatrix(void)
Definition: submat.h:1159
virtual bool IsKeyWord(const char *sKeyWord)
Definition: parser.cc:910
doublereal dMeasure[LASTMEASURE]
Definition: instruments.h:66
virtual const Vec3 & GetWCurr(void) const
Definition: strnode.h:1030
#define ASSERT(expression)
Definition: colamd.c:977
virtual std::ostream & Restart(std::ostream &out) const
Definition: instruments.cc:143
#define SAFENEWWITHCONSTRUCTOR(pnt, item, constructor)
Definition: mynewmem.h:698
virtual const Vec3 & GetXCurr(void) const
Definition: strnode.h:310
const StructNode * pNode
Definition: instruments.h:63
Definition: elem.h:75
Type
Definition: elem.h:91
virtual doublereal dGetPrivData(unsigned int i) const
Definition: instruments.cc:249
virtual ~AircraftInstruments(void)
Definition: instruments.cc:50
virtual Elem::Type GetElemType(void) const
Definition: instruments.cc:56
virtual const Vec3 & GetVCurr(void) const
Definition: strnode.h:322
virtual flag fGetAirVelocity(Vec3 &Velocity, const Vec3 &X) const
Definition: aerodyn.cc:725
GradientExpression< BinaryExpr< FuncAtan2, LhsExpr, RhsExpr > > atan2(const GradientExpression< LhsExpr > &u, const GradientExpression< RhsExpr > &v)
Definition: gradient.h:2962
double doublereal
Definition: colamd.c:52
virtual void WorkSpaceDim(integer *piNumRows, integer *piNumCols) const
Definition: instruments.cc:170
long int integer
Definition: colamd.c:51
std::ostream & Write(std::ostream &out, const char *sFill=" ", const char *sFill2=NULL) const
Definition: matvec3.cc:722
unsigned int GetLabel(void) const
Definition: withlab.cc:62
Node * ReadNode(MBDynParser &HP, Node::Type type) const
Definition: dataman3.cc:2309
virtual void Resize(integer iNewSize)=0
Mat3x3 R
virtual unsigned int iGetPrivDataIdx(const char *s) const
Definition: instruments.cc:212
virtual unsigned int iGetNumPrivData(void) const
Definition: instruments.cc:206
std::ostream & Aerodynamic(void) const
Definition: output.h:485