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$";
  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 }