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