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
  44 namespace JSBSim {
  45
  46 static const char *IdSrc = "$Id$";
  47 static const char *IdHdr = ID_MAGNETOMETER;
  48
  49 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  50 CLASS IMPLEMENTATION
  51 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  52
  53
  54 FGMagnetometer::FGMagnetometer(FGFCS* fcs, Element* element) : FGSensor(fcs, element),
  55                                                                FGSensorOrientation(element),
  56                                                                counter(0),
  57                                                                INERTIAL_UPDATE_RATE(1000)
  58 {
  59   Propagate = fcs->GetExec()->GetPropagate();
  60   MassBalance = fcs->GetExec()->GetMassBalance();
  61   Inertial = fcs->GetExec()->GetInertial();
  62
  63   Element* location_element = element->FindElement("location");
  64   if (location_element) vLocation = location_element->FindElementTripletConvertTo("IN");
  65   else {cerr << "No location given for magnetometer. " << endl; exit(-1);}
  66
  67   vRadius = MassBalance->StructuralToBody(vLocation);
  68
  69   //assuming date wont significantly change over a flight to affect mag field
  70   //would be better to get the date from the sim if its simulated...
  71   time_t rawtime;
  72   time( &rawtime );
  73   tm * ptm = gmtime ( &rawtime );
  74
  75   int year = ptm->tm_year;
  76   if(year>100)
  77   {
  78     year-= 100;
  79   }
  80   //the months here are zero based TODO find out if the function expects 1s based
  81   date = (yymmdd_to_julian_days(ptm->tm_year,ptm->tm_mon,ptm->tm_mday));//Julian 1950-2049 yy,mm,dd
  82   updateInertialMag();
  83
  84   Debug(0);
  85 }
  86 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  87
  88 FGMagnetometer::~FGMagnetometer()
  89 {
  90   Debug(1);
  91 }
  92
  93 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  94 void FGMagnetometer::updateInertialMag(void)
  95 {
  96   counter++;
  97   if (counter > INERTIAL_UPDATE_RATE)//dont need to update every iteration
  98   {
  99       counter = 0;
 100
 101       usedLat = (Propagate->GetGeodLatitudeRad());//radians, N and E lat and long are positive, S and W negative
 102       usedLon = (Propagate->GetLongitude());//radians
 103       usedAlt = (Propagate->GetGeodeticAltitude()*fttom*0.001);//km
 104
 105       //this should be done whenever the position changes significantly (in nTesla)
 106       calc_magvar( usedLat,
 107                    usedLon,
 108                    usedAlt,
 109                    date,
 110                    field );
 111   }
 112 }
 113
 114 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 115
 116 bool FGMagnetometer::Run(void )
 117 {
 118   // There is no input assumed. This is a dedicated magnetic field sensor.
 119
 120   vRadius = MassBalance->StructuralToBody(vLocation);
 121
 122   updateInertialMag();
 123
 124   // Inertial magnetic field rotated to the body frame
 125   vMag = Propagate->GetTl2b() * FGColumnVector3(field[3], field[4], field[5]);
 126
 127   // Allow for sensor orientation
 128   vMag = mT * vMag;
 129
 130   Input = vMag(axis);
 131
 132   ProcessSensorSignal();
 133
 134   return true;
 135 }
 136
 137 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 138 //    The bitmasked value choices are as follows:
 139 //    unset: In this case (the default) JSBSim would only print
 140 //       out the normally expected messages, essentially echoing
 141 //       the config files as they are read. If the environment
 142 //       variable is not set, debug_lvl is set to 1 internally
 143 //    0: This requests JSBSim not to output any messages
 144 //       whatsoever.
 145 //    1: This value explicity requests the normal JSBSim
 146 //       startup messages
 147 //    2: This value asks for a message to be printed out when
 148 //       a class is instantiated
 149 //    4: When this value is set, a message is displayed when a
 150 //       FGModel object executes its Run() method
 151 //    8: When this value is set, various runtime state variables
 152 //       are printed out periodically
 153 //    16: When set various parameters are sanity checked and
 154 //       a message is printed out when they go out of bounds
 155
 156 void FGMagnetometer::Debug(int from)
 157 {
 158   string ax[4] = {"none", "X", "Y", "Z"};
 159
 160   if (debug_lvl <= 0) return;
 161
 162   if (debug_lvl & 1) { // Standard console startup message output
 163     if (from == 0) { // Constructor
 164       cout << "        Axis: " << ax[axis] << endl;
 165     }
 166   }
 167   if (debug_lvl & 2 ) { // Instantiation/Destruction notification
 168     if (from == 0) cout << "Instantiated: FGMagnetometer" << endl;
 169     if (from == 1) cout << "Destroyed:    FGMagnetometer" << endl;
 170   }
 171   if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
 172   }
 173   if (debug_lvl & 8 ) { // Runtime state variables
 174   }
 175   if (debug_lvl & 16) { // Sanity checking
 176   }
 177   if (debug_lvl & 64) {
 178     if (from == 0) { // Constructor
 179       cout << IdSrc << endl;
 180       cout << IdHdr << endl;
 181     }
 182   }
 183 }
 184 }