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                                                                counter(0),
  56                                                                INERTIAL_UPDATE_RATE(1000)
  57 {
  58   Propagate = fcs->GetExec()->GetPropagate();
  59   MassBalance = fcs->GetExec()->GetMassBalance();
  60   Inertial = fcs->GetExec()->GetInertial();
  61
  62   Element* location_element = element->FindElement("location");
  63   if (location_element) vLocation = location_element->FindElementTripletConvertTo("IN");
  64   else {cerr << "No location given for magnetometer. " << endl; exit(-1);}
  65
  66   vRadius = MassBalance->StructuralToBody(vLocation);
  67
  68   Element* orient_element = element->FindElement("orientation");
  69   if (orient_element) vOrient = orient_element->FindElementTripletConvertTo("RAD");
  70   else {cerr << "No orientation given for magnetometer. " << endl;}
  71
  72   Element* axis_element = element->FindElement("axis");
  73   if (axis_element) {
  74     string sAxis = element->FindElementValue("axis");
  75     if (sAxis == "X" || sAxis == "x") {
  76       axis = 1;
  77     } else if (sAxis == "Y" || sAxis == "y") {
  78       axis = 2;
  79     } else if (sAxis == "Z" || sAxis == "z") {
  80       axis = 3;
  81     } else {
  82       cerr << "  Incorrect/no axis specified for magnetometer; assuming X axis" << endl;
  83       axis = 1;
  84     }
  85   }
  86
  87   CalculateTransformMatrix();
  88
  89   //assuming date wont significantly change over a flight to affect mag field
  90   //would be better to get the date from the sim if its simulated...
  91   time_t rawtime;
  92   time( &rawtime );
  93   tm * ptm = gmtime ( &rawtime );
  94
  95   int year = ptm->tm_year;
  96   if(year>100)
  97   {
  98     year-= 100;
  99   }
 100   //the months here are zero based TODO find out if the function expects 1s based
 101   date = (yymmdd_to_julian_days(ptm->tm_year,ptm->tm_mon,ptm->tm_mday));//Julian 1950-2049 yy,mm,dd
 102   updateInertialMag();
 103
 104   Debug(0);
 105 }
 106 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 107
 108 FGMagnetometer::~FGMagnetometer()
 109 {
 110   Debug(1);
 111 }
 112
 113 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 114 void FGMagnetometer::updateInertialMag(void)
 115 {
 116   counter++;
 117   if(counter > INERTIAL_UPDATE_RATE)//dont need to update every iteration
 118   {
 119       counter = 0;
 120
 121       usedLat = (Propagate->GetGeodLatitudeRad());//radians, N and E lat and long are positive, S and W negative
 122       usedLon = (Propagate->GetLongitude());//radians
 123       usedAlt = (Propagate->GetGeodeticAltitude()*fttom*0.001);//km
 124
 125       //this should be done whenever the position changes significantly (in nTesla)
 126       double magvar = calc_magvar( usedLat,
 127                                  usedLon,
 128                                  usedAlt,
 129                                  date,
 130                                  field );
 131   }
 132 }
 133
 134 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 135
 136 bool FGMagnetometer::Run(void )
 137 {
 138   // There is no input assumed. This is a dedicated acceleration sensor.
 139
 140   vRadius = MassBalance->StructuralToBody(vLocation);
 141
 142
 143   updateInertialMag();
 144   //Inertial magnetic field rotated to the body frame
 145   vMag = Propagate->GetTl2b() * FGColumnVector3(field[3], field[4], field[5]);
 146
 147   //allow for sensor orientation
 148   vMag = mT * vMag;
 149
 150   Input = vMag(axis);
 151
 152   Output = Input; // perfect magnetometer
 153
 154   // Degrade signal as specified
 155
 156   if (fail_stuck) {
 157     Output = PreviousOutput;
 158     return true;
 159   }
 160
 161   if (lag != 0.0)            Lag();       // models magnetometer lag
 162   if (noise_variance != 0.0) Noise();     // models noise
 163   if (drift_rate != 0.0)     Drift();     // models drift over time
 164   if (bias != 0.0)           Bias();      // models a finite bias
 165   if (gain != 0.0)           Gain();      // models a gain
 166
 167   if (fail_low)  Output = -HUGE_VAL;
 168   if (fail_high) Output =  HUGE_VAL;
 169
 170   if (bits != 0)             Quantize();  // models quantization degradation
 171 //  if (delay != 0.0)          Delay();     // models system signal transport latencies
 172
 173   Clip(); // Is it right to clip an magnetometer?
 174   return true;
 175 }
 176
 177 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 178
 179 void FGMagnetometer::CalculateTransformMatrix(void)
 180 {
 181   double cp,sp,cr,sr,cy,sy;
 182
 183   cp=cos(vOrient(ePitch)); sp=sin(vOrient(ePitch));
 184   cr=cos(vOrient(eRoll));  sr=sin(vOrient(eRoll));
 185   cy=cos(vOrient(eYaw));   sy=sin(vOrient(eYaw));
 186
 187
 188   mT(1,1) =  cp*cy;
 189   mT(1,2) =  cp*sy;
 190   mT(1,3) = -sp;
 191
 192   mT(2,1) = sr*sp*cy - cr*sy;
 193   mT(2,2) = sr*sp*sy + cr*cy;
 194   mT(2,3) = sr*cp;
 195
 196   mT(3,1) = cr*sp*cy + sr*sy;
 197   mT(3,2) = cr*sp*sy - sr*cy;
 198   mT(3,3) = cr*cp;
 199
 200
 201   // This transform is different than for FGForce, where we want a native nozzle
 202   // force in body frame. Here we calculate the body frame accel and want it in
 203   // the transformed magnetometer frame. So, the next line is commented out.
 204   // mT = mT.Inverse();
 205 }
 206
 207 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 208 //    The bitmasked value choices are as follows:
 209 //    unset: In this case (the default) JSBSim would only print
 210 //       out the normally expected messages, essentially echoing
 211 //       the config files as they are read. If the environment
 212 //       variable is not set, debug_lvl is set to 1 internally
 213 //    0: This requests JSBSim not to output any messages
 214 //       whatsoever.
 215 //    1: This value explicity requests the normal JSBSim
 216 //       startup messages
 217 //    2: This value asks for a message to be printed out when
 218 //       a class is instantiated
 219 //    4: When this value is set, a message is displayed when a
 220 //       FGModel object executes its Run() method
 221 //    8: When this value is set, various runtime state variables
 222 //       are printed out periodically
 223 //    16: When set various parameters are sanity checked and
 224 //       a message is printed out when they go out of bounds
 225
 226 void FGMagnetometer::Debug(int from)
 227 {
 228   string ax[4] = {"none", "X", "Y", "Z"};
 229
 230   if (debug_lvl <= 0) return;
 231
 232   if (debug_lvl & 1) { // Standard console startup message output
 233     if (from == 0) { // Constructor
 234       cout << "        Axis: " << ax[axis] << endl;
 235     }
 236   }
 237   if (debug_lvl & 2 ) { // Instantiation/Destruction notification
 238     if (from == 0) cout << "Instantiated: FGMagnetometer" << endl;
 239     if (from == 1) cout << "Destroyed:    FGMagnetometer" << endl;
 240   }
 241   if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
 242   }
 243   if (debug_lvl & 8 ) { // Runtime state variables
 244   }
 245   if (debug_lvl & 16) { // Sanity checking
 246   }
 247   if (debug_lvl & 64) {
 248     if (from == 0) { // Constructor
 249       cout << IdSrc << endl;
 250       cout << IdHdr << endl;
 251     }
 252   }
 253 }
 254 }