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