]> git.mxchange.org Git - flightgear.git/blob - src/FDM/JSBSim/models/flight_control/FGMagnetometer.cpp
Merge commit 'refs/merge-requests/1551' of git://gitorious.org/fg/flightgear into...
[flightgear.git] / src / FDM / JSBSim / models / flight_control / FGMagnetometer.cpp
1 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
2
3  Module:       FGMagnetometer.cpp
4  Author:       Matthew Chave
5  Date started: August 2009
6
7  ------------- Copyright (C) 2009 -------------
8
9  This program is free software; you can redistribute it and/or modify it under
10  the terms of the GNU Lesser General Public License as published by the Free Software
11  Foundation; either version 2 of the License, or (at your option) any later
12  version.
13
14  This program is distributed in the hope that it will be useful, but WITHOUT
15  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
16  FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public License for more
17  details.
18
19  You should have received a copy of the GNU Lesser General Public License along with
20  this program; if not, write to the Free Software Foundation, Inc., 59 Temple
21  Place - Suite 330, Boston, MA  02111-1307, USA.
22
23  Further information about the GNU Lesser General Public License can also be found on
24  the world wide web at http://www.gnu.org.
25
26 FUNCTIONAL DESCRIPTION
27 --------------------------------------------------------------------------------
28
29 HISTORY
30 --------------------------------------------------------------------------------
31
32 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
33 COMMENTS, REFERENCES,  and NOTES
34 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
35
36 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
37 INCLUDES
38 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
39
40 #include "FGMagnetometer.h"
41 #include "simgear/magvar/coremag.hxx"
42 #include <ctime>
43 #include <cstdlib>
44 #include <iostream>
45
46 using namespace std;
47
48 namespace JSBSim {
49
50 static const char *IdSrc = "$Id: FGMagnetometer.cpp,v 1.5 2009/10/24 22:59:30 jberndt Exp $";
51 static const char *IdHdr = ID_MAGNETOMETER;
52
53 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
54 CLASS IMPLEMENTATION
55 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
56
57
58 FGMagnetometer::FGMagnetometer(FGFCS* fcs, Element* element) : FGSensor(fcs, element),
59                                                                FGSensorOrientation(element),
60                                                                counter(0),
61                                                                INERTIAL_UPDATE_RATE(1000)
62 {
63   Propagate = fcs->GetExec()->GetPropagate();
64   MassBalance = fcs->GetExec()->GetMassBalance();
65   Inertial = fcs->GetExec()->GetInertial();
66   
67   Element* location_element = element->FindElement("location");
68   if (location_element) vLocation = location_element->FindElementTripletConvertTo("IN");
69   else {cerr << "No location given for magnetometer. " << endl; exit(-1);}
70
71   vRadius = MassBalance->StructuralToBody(vLocation);
72
73   //assuming date wont significantly change over a flight to affect mag field
74   //would be better to get the date from the sim if its simulated...
75   time_t rawtime;
76   time( &rawtime );
77   tm * ptm = gmtime ( &rawtime );
78
79   int year = ptm->tm_year;
80   if(year>100)
81   {
82     year-= 100;
83   }
84   //the months here are zero based TODO find out if the function expects 1s based
85   date = (yymmdd_to_julian_days(ptm->tm_year,ptm->tm_mon,ptm->tm_mday));//Julian 1950-2049 yy,mm,dd
86   updateInertialMag();
87
88   Debug(0);
89 }
90 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
91
92 FGMagnetometer::~FGMagnetometer()
93 {
94   Debug(1);
95 }
96
97 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
98 void FGMagnetometer::updateInertialMag(void)
99 {
100   counter++;
101   if (counter > INERTIAL_UPDATE_RATE)//dont need to update every iteration
102   {
103       counter = 0;
104
105       usedLat = (Propagate->GetGeodLatitudeRad());//radians, N and E lat and long are positive, S and W negative
106       usedLon = (Propagate->GetLongitude());//radians
107       usedAlt = (Propagate->GetGeodeticAltitude()*fttom*0.001);//km
108
109       //this should be done whenever the position changes significantly (in nTesla)
110       calc_magvar( usedLat,
111                    usedLon,
112                    usedAlt,
113                    date,
114                    field );
115   }
116 }
117
118 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
119
120 bool FGMagnetometer::Run(void )
121 {
122   // There is no input assumed. This is a dedicated magnetic field sensor.
123   
124   vRadius = MassBalance->StructuralToBody(vLocation);
125
126   updateInertialMag();
127
128   // Inertial magnetic field rotated to the body frame
129   vMag = Propagate->GetTl2b() * FGColumnVector3(field[3], field[4], field[5]);
130
131   // Allow for sensor orientation
132   vMag = mT * vMag;
133   
134   Input = vMag(axis);
135
136   ProcessSensorSignal();
137
138   return true;
139 }
140
141 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
142 //    The bitmasked value choices are as follows:
143 //    unset: In this case (the default) JSBSim would only print
144 //       out the normally expected messages, essentially echoing
145 //       the config files as they are read. If the environment
146 //       variable is not set, debug_lvl is set to 1 internally
147 //    0: This requests JSBSim not to output any messages
148 //       whatsoever.
149 //    1: This value explicity requests the normal JSBSim
150 //       startup messages
151 //    2: This value asks for a message to be printed out when
152 //       a class is instantiated
153 //    4: When this value is set, a message is displayed when a
154 //       FGModel object executes its Run() method
155 //    8: When this value is set, various runtime state variables
156 //       are printed out periodically
157 //    16: When set various parameters are sanity checked and
158 //       a message is printed out when they go out of bounds
159
160 void FGMagnetometer::Debug(int from)
161 {
162   string ax[4] = {"none", "X", "Y", "Z"};
163
164   if (debug_lvl <= 0) return;
165
166   if (debug_lvl & 1) { // Standard console startup message output
167     if (from == 0) { // Constructor
168       cout << "        Axis: " << ax[axis] << endl;
169     }
170   }
171   if (debug_lvl & 2 ) { // Instantiation/Destruction notification
172     if (from == 0) cout << "Instantiated: FGMagnetometer" << endl;
173     if (from == 1) cout << "Destroyed:    FGMagnetometer" << endl;
174   }
175   if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
176   }
177   if (debug_lvl & 8 ) { // Runtime state variables
178   }
179   if (debug_lvl & 16) { // Sanity checking
180   }
181   if (debug_lvl & 64) {
182     if (from == 0) { // Constructor
183       cout << IdSrc << endl;
184       cout << IdHdr << endl;
185     }
186   }
187 }
188 }