src/FDM/LaRCsim/ls_geodesy.c

   1 /***************************************************************************
   2
   3         TITLE:  ls_geodesy
   4
   5 ----------------------------------------------------------------------------
   6
   7         FUNCTION:       Converts geocentric coordinates to geodetic positions
   8
   9 ----------------------------------------------------------------------------
  10
  11         MODULE STATUS:  developmental
  12
  13 ----------------------------------------------------------------------------
  14
  15         GENEALOGY:      Written as part of LaRCSim project by E. B. Jackson
  16
  17 ----------------------------------------------------------------------------
  18
  19         DESIGNED BY:    E. B. Jackson
  20
  21         CODED BY:       E. B. Jackson
  22
  23         MAINTAINED BY:  E. B. Jackson
  24
  25 ----------------------------------------------------------------------------
  26
  27         MODIFICATION HISTORY:
  28
  29         DATE    PURPOSE                                         BY
  30
  31         930208  Modified to avoid singularity near polar region.        EBJ
  32         930602  Moved backwards calcs here from ls_step.                EBJ
  33         931214  Changed erroneous Latitude and Altitude variables to
  34                 *lat_geod and *alt in routine ls_geoc_to_geod.          EBJ
  35         940111  Changed header files from old ls_eom.h style to ls_types,
  36                 and ls_constants.  Also replaced old DATA type with new
  37                 SCALAR type.                                            EBJ
  38
  39         CURRENT RCS HEADER:
  40
  41 $Header$
  42 $Log$
  43 Revision 1.3  2001/03/24 05:03:12  curt
  44 SG-ified logstream.
  45
  46 Revision 1.2  2000/10/23 22:34:54  curt
  47 I tested:
  48 LaRCsim c172 on-ground and in-air starts, reset: all work
  49 UIUC Cessna172 on-ground and in-air starts work as expected, reset
  50 results in an aircraft that is upside down but does not crash FG.   I
  51 don't know what it was like before, so it may well be no change.
  52 JSBSim c172 and X15 in-air starts work fine, resets now work (and are
  53 trimmed), on-ground starts do not -- the c172 ends up on its back.  I
  54 suspect this is no worse than before.
  55
  56 I did not test:
  57 Balloon (the weather code returns nan's for the atmosphere data --this
  58 is in the weather module and apparently is a linux only bug)
  59 ADA (don't know how)
  60 MagicCarpet  (needs work yet)
  61 External (don't know how)
  62
  63 known to be broken:
  64 LaRCsim c172 on-ground starts with a negative terrain altitude (this
  65 happens at KPAO when the scenery is not present).   The FDM inits to
  66 about 50 feet AGL and the model falls to the ground.  It does stay
  67 upright, however, and seems to be fine once it settles out, FWIW.
  68
  69 To do:
  70 --implement set_Model on the bus
  71 --bring Christian's weather data into JSBSim
  72 -- add default method to bus for updating things like the sin and cos of
  73 latitude (for Balloon, MagicCarpet)
  74 -- lots of cleanup
  75
  76 The files:
  77 src/FDM/flight.cxx
  78 src/FDM/flight.hxx
  79 -- all data members now declared protected instead of private.
  80 -- eliminated all but a small set of 'setters', no change to getters.
  81 -- that small set is declared virtual, the default implementation
  82 provided preserves the old behavior
  83 -- all of the vector data members are now initialized.
  84 -- added busdump() method -- SG_LOG's  all the bus data when called,
  85 useful for diagnostics.
  86
  87 src/FDM/ADA.cxx
  88 -- bus data members now directly assigned to
  89
  90 src/FDM/Balloon.cxx
  91 -- bus data members now directly assigned to
  92 -- changed V_equiv_kts to V_calibrated_kts
  93
  94 src/FDM/JSBSim.cxx
  95 src/FDM/JSBSim.hxx
  96 -- bus data members now directly assigned to
  97 -- implemented the FGInterface virtual setters with JSBSim specific
  98 logic
  99 -- changed the static FDMExec to a dynamic fdmex (needed so that the
 100 JSBSim object can be deleted when a model change is called for)
 101 -- implemented constructor and destructor, moved some of the logic
 102 formerly in init() to constructor
 103 -- added logic to bring up FGEngInterface objects and set the RPM and
 104 throttle values.
 105
 106 src/FDM/LaRCsim.cxx
 107 src/FDM/LaRCsim.hxx
 108 -- bus data members now directly assigned to
 109 -- implemented the FGInterface virtual setters with LaRCsim specific
 110 logic, uses LaRCsimIC
 111 -- implemented constructor and destructor, moved some of the logic
 112 formerly in init() to constructor
 113 -- moved default inertias to here from fg_init.cxx
 114 -- eliminated the climb rate calculation.  The equivalent, climb_rate =
 115 -1*vdown, is now in copy_from_LaRCsim().
 116
 117 src/FDM/LaRCsimIC.cxx
 118 src/FDM/LaRCsimIC.hxx
 119 -- similar to FGInitialCondition, this class has all the logic needed to
 120 turn data like Vc and Mach into the more fundamental quantities LaRCsim
 121 needs to initialize.
 122 -- put it in src/FDM since it is a class
 123
 124 src/FDM/MagicCarpet.cxx
 125  -- bus data members now directly assigned to
 126
 127 src/FDM/Makefile.am
 128 -- adds LaRCsimIC.hxx and cxx
 129
 130 src/FDM/JSBSim/FGAtmosphere.h
 131 src/FDM/JSBSim/FGDefs.h
 132 src/FDM/JSBSim/FGInitialCondition.cpp
 133 src/FDM/JSBSim/FGInitialCondition.h
 134 src/FDM/JSBSim/JSBSim.cpp
 135 -- changes to accomodate the new bus
 136
 137 src/FDM/LaRCsim/atmos_62.h
 138 src/FDM/LaRCsim/ls_geodesy.h
 139 -- surrounded prototypes with #ifdef __cplusplus ... #endif , functions
 140 here are needed in LaRCsimIC
 141
 142 src/FDM/LaRCsim/c172_main.c
 143 src/FDM/LaRCsim/cherokee_aero.c
 144 src/FDM/LaRCsim/ls_aux.c
 145 src/FDM/LaRCsim/ls_constants.h
 146 src/FDM/LaRCsim/ls_geodesy.c
 147 src/FDM/LaRCsim/ls_geodesy.h
 148 src/FDM/LaRCsim/ls_step.c
 149 src/FDM/UIUCModel/uiuc_betaprobe.cpp
 150 -- changed PI to LS_PI, eliminates preprocessor naming conflict with
 151 weather module
 152
 153 src/FDM/LaRCsim/ls_interface.c
 154 src/FDM/LaRCsim/ls_interface.h
 155 -- added function ls_set_model_dt()
 156
 157 src/Main/bfi.cxx
 158 -- eliminated calls that set the NED speeds to body components.  They
 159 are no longer needed and confuse the new bus.
 160
 161 src/Main/fg_init.cxx
 162 -- eliminated calls that just brought the bus data up-to-date (e.g.
 163 set_sin_cos_latitude). or set default values.   The bus now handles the
 164 defaults and updates itself when the setters are called (for LaRCsim and
 165 JSBSim).  A default method for doing this needs to be added to the bus.
 166 -- added fgVelocityInit() to set the speed the user asked for.  Both
 167 JSBSim and LaRCsim can now be initialized using any of:
 168 vc,mach, NED components, UVW components.
 169
 170 src/Main/main.cxx
 171 --eliminated call to fgFDMSetGroundElevation, this data is now 'pulled'
 172 onto the bus every update()
 173
 174 src/Main/options.cxx
 175 src/Main/options.hxx
 176 -- added enum to keep track of the speed requested by the user
 177 -- eliminated calls to set NED velocity properties to body speeds, they
 178 are no longer needed.
 179 -- added options for the NED components.
 180
 181 src/Network/garmin.cxx
 182 src/Network/nmea.cxx
 183 --eliminated calls that just brought the bus data up-to-date (e.g.
 184 set_sin_cos_latitude).  The bus now updates itself when the setters are
 185 called (for LaRCsim and JSBSim).  A default method for doing this needs
 186 to be added to the bus.
 187 -- changed set_V_equiv_kts to set_V_calibrated_kts.  set_V_equiv_kts no
 188 longer exists ( get_V_equiv_kts still does, though)
 189
 190 src/WeatherCM/FGLocalWeatherDatabase.cpp
 191 -- commented out the code to put the weather data on the bus, a
 192 different scheme for this is needed.
 193
 194 Revision 1.1.1.1  1999/06/17 18:07:34  curt
 195 Start of 0.7.x branch
 196
 197 Revision 1.1.1.1  1999/04/05 21:32:45  curt
 198 Start of 0.6.x branch.
 199
 200 Revision 1.3  1998/07/08 14:41:37  curt
 201 .
 202
 203 Revision 1.2  1998/01/19 18:40:25  curt
 204 Tons of little changes to clean up the code and to remove fatal errors
 205 when building with the c++ compiler.
 206
 207 Revision 1.1  1997/05/29 00:09:56  curt
 208 Initial Flight Gear revision.
 209
 210  * Revision 1.5  1994/01/11  18:47:05  bjax
 211  * Changed include files to use types and constants, not ls_eom.h
 212  * Also changed DATA type to SCALAR type.
 213  *
 214  * Revision 1.4  1993/12/14  21:06:47  bjax
 215  * Removed global variable references Altitude and Latitude.   EBJ
 216  *
 217  * Revision 1.3  1993/06/02  15:03:40  bjax
 218  * Made new subroutine for calculating geodetic to geocentric; changed name
 219  * of forward conversion routine from ls_geodesy to ls_geoc_to_geod.
 220  *
 221
 222 ----------------------------------------------------------------------------
 223
 224         REFERENCES:
 225
 226                 [ 1]    Stevens, Brian L.; and Lewis, Frank L.: "Aircraft
 227                         Control and Simulation", Wiley and Sons, 1992.
 228                         ISBN 0-471-61397-5
 229
 230
 231 ----------------------------------------------------------------------------
 232
 233         CALLED BY:      ls_aux
 234
 235 ----------------------------------------------------------------------------
 236
 237         CALLS TO:
 238
 239 ----------------------------------------------------------------------------
 240
 241         INPUTS:
 242                 lat_geoc        Geocentric latitude, radians, + = North
 243                 radius          C.G. radius to earth center, ft
 244
 245 ----------------------------------------------------------------------------
 246
 247         OUTPUTS:
 248                 lat_geod        Geodetic latitude, radians, + = North
 249                 alt             C.G. altitude above mean sea level, ft
 250                 sea_level_r     radius from earth center to sea level at
 251                                 local vertical (surface normal) of C.G.
 252
 253 --------------------------------------------------------------------------*/
 254
 255 #include "ls_types.h"
 256 #include "ls_constants.h"
 257 #include "ls_geodesy.h"
 258 #include <math.h>
 259
 260 /* ONE_SECOND is pi/180/60/60, or about 100 feet at earths' equator */
 261 #define ONE_SECOND 4.848136811E-6
 262 #define HALF_PI 0.5*LS_PI
 263
 264
 265 void ls_geoc_to_geod( SCALAR lat_geoc, SCALAR radius, SCALAR *lat_geod,
 266                       SCALAR *alt, SCALAR *sea_level_r )
 267 {
 268         SCALAR  t_lat, x_alpha, mu_alpha, delt_mu, r_alpha, l_point, rho_alpha;
 269         SCALAR  sin_mu_a, denom,delt_lambda, lambda_sl, sin_lambda_sl;
 270
 271         if(   ( (HALF_PI - lat_geoc) < ONE_SECOND )     /* near North pole */
 272            || ( (HALF_PI + lat_geoc) < ONE_SECOND ) )   /* near South pole */
 273           {
 274             *lat_geod = lat_geoc;
 275             *sea_level_r = EQUATORIAL_RADIUS*E;
 276             *alt = radius - *sea_level_r;
 277           }
 278         else
 279           {
 280             t_lat = tan(lat_geoc);
 281             x_alpha = E*EQUATORIAL_RADIUS/sqrt(t_lat*t_lat + E*E);
 282             mu_alpha = atan2(sqrt(RESQ - x_alpha*x_alpha),E*x_alpha);
 283             if (lat_geoc < 0) mu_alpha = - mu_alpha;
 284             sin_mu_a = sin(mu_alpha);
 285             delt_lambda = mu_alpha - lat_geoc;
 286             r_alpha = x_alpha/cos(lat_geoc);
 287             l_point = radius - r_alpha;
 288             *alt = l_point*cos(delt_lambda);
 289             denom = sqrt(1-EPS*EPS*sin_mu_a*sin_mu_a);
 290             rho_alpha = EQUATORIAL_RADIUS*(1-EPS)/
 291               (denom*denom*denom);
 292             delt_mu = atan2(l_point*sin(delt_lambda),rho_alpha + *alt);
 293             *lat_geod = mu_alpha - delt_mu;
 294             lambda_sl = atan( E*E * tan(*lat_geod) ); /* SL geoc. latitude */
 295             sin_lambda_sl = sin( lambda_sl );
 296             *sea_level_r = sqrt(RESQ
 297                            /(1 + ((1/(E*E))-1)*sin_lambda_sl*sin_lambda_sl));
 298           }
 299 }
 300
 301
 302 void ls_geod_to_geoc( SCALAR lat_geod, SCALAR alt,
 303                       SCALAR *sl_radius, SCALAR *lat_geoc )
 304 {
 305     SCALAR lambda_sl, sin_lambda_sl, cos_lambda_sl, sin_mu, cos_mu, px, py;
 306
 307     lambda_sl = atan( E*E * tan(lat_geod) ); /* sea level geocentric latitude */
 308     sin_lambda_sl = sin( lambda_sl );
 309     cos_lambda_sl = cos( lambda_sl );
 310     sin_mu = sin(lat_geod);     /* Geodetic (map makers') latitude */
 311     cos_mu = cos(lat_geod);
 312     *sl_radius = sqrt(RESQ
 313         /(1 + ((1/(E*E))-1)*sin_lambda_sl*sin_lambda_sl));
 314     py = *sl_radius*sin_lambda_sl + alt*sin_mu;
 315     px = *sl_radius*cos_lambda_sl + alt*cos_mu;
 316     *lat_geoc = atan2( py, px );
 317 }