src/FDM/YASim/YASim.cxx

   1
   2 #include <simgear/debug/logstream.hxx>
   3 #include <simgear/math/sg_geodesy.hxx>
   4 #include <simgear/misc/sg_path.hxx>
   5 #include <simgear/scene/model/placement.hxx>
   6 #include <simgear/xml/easyxml.hxx>
   7
   8 #include <Main/globals.hxx>
   9 #include <Main/fg_props.hxx>
  10
  11 #include "FGFDM.hpp"
  12 #include "Atmosphere.hpp"
  13 #include "Math.hpp"
  14 #include "Airplane.hpp"
  15 #include "Model.hpp"
  16 #include "Integrator.hpp"
  17 #include "Glue.hpp"
  18 #include "Gear.hpp"
  19 #include "PropEngine.hpp"
  20 #include "PistonEngine.hpp"
  21
  22 #include "YASim.hxx"
  23
  24 using namespace yasim;
  25
  26 static const float YASIM_PI = 3.14159265358979323846;
  27 static const float RAD2DEG = 180/YASIM_PI;
  28 static const float PI2 = YASIM_PI*2;
  29 static const float RAD2RPM = 9.54929658551;
  30 static const float M2FT = 3.2808399;
  31 static const float FT2M = 0.3048;
  32 static const float MPS2KTS = 3600.0/1852.0;
  33 static const float CM2GALS = 264.172037284; // gallons/cubic meter
  34 static const float KG2LBS = 2.20462262185;
  35 static const float W2HP = 1.3416e-3;
  36 static const float INHG2PA = 3386.389;
  37 static const float SLUG2KG = 14.59390;
  38
  39 YASim::YASim(double dt)
  40 {
  41 //     set_delta_t(dt);
  42     _fdm = new FGFDM();
  43
  44     _dt = dt;
  45
  46     _fdm->getAirplane()->getModel()->getIntegrator()->setInterval(_dt);
  47 }
  48
  49 void YASim::report()
  50 {
  51     Airplane* a = _fdm->getAirplane();
  52
  53     float aoa = a->getCruiseAoA() * RAD2DEG;
  54     float tail = -1 * a->getTailIncidence() * RAD2DEG;
  55     float drag = 1000 * a->getDragCoefficient();
  56
  57     SG_LOG(SG_FLIGHT,SG_INFO,"YASim solution results:");
  58     SG_LOG(SG_FLIGHT,SG_INFO,"       Iterations: "<<a->getSolutionIterations());
  59     SG_LOG(SG_FLIGHT,SG_INFO," Drag Coefficient: "<< drag);
  60     SG_LOG(SG_FLIGHT,SG_INFO,"       Lift Ratio: "<<a->getLiftRatio());
  61     SG_LOG(SG_FLIGHT,SG_INFO,"       Cruise AoA: "<< aoa);
  62     SG_LOG(SG_FLIGHT,SG_INFO,"   Tail Incidence: "<< tail);
  63     SG_LOG(SG_FLIGHT,SG_INFO,"Approach Elevator: "<<a->getApproachElevator());
  64
  65
  66     float cg[3];
  67     char buf[256];
  68     a->getModel()->getBody()->getCG(cg);
  69     sprintf(buf, "            CG: %.3f, %.3f, %.3f", cg[0], cg[1], cg[2]);
  70     SG_LOG(SG_FLIGHT, SG_INFO, buf);
  71
  72     if(a->getFailureMsg()) {
  73         SG_LOG(SG_FLIGHT, SG_ALERT, "YASim SOLUTION FAILURE:");
  74         SG_LOG(SG_FLIGHT, SG_ALERT, a->getFailureMsg());
  75         exit(1);
  76     }
  77 }
  78
  79 void YASim::bind()
  80 {
  81     // Run the superclass bind to set up a bunch of property ties
  82     FGInterface::bind();
  83
  84     // Now UNtie the ones that we are going to set ourselves.
  85     fgUntie("/consumables/fuel/tank[0]/level-gal_us");
  86     fgUntie("/consumables/fuel/tank[1]/level-gal_us");
  87
  88     char buf[256];
  89     for(int i=0; i<_fdm->getAirplane()->getModel()->numThrusters(); i++) {
  90         sprintf(buf, "/engines/engine[%d]/fuel-flow-gph", i); fgUntie(buf);
  91         sprintf(buf, "/engines/engine[%d]/rpm", i);           fgUntie(buf);
  92         sprintf(buf, "/engines/engine[%d]/mp-osi", i);        fgUntie(buf);
  93         sprintf(buf, "/engines/engine[%d]/egt-degf", i);      fgUntie(buf);
  94     }
  95 }
  96
  97 void YASim::init()
  98 {
  99     Airplane* a = _fdm->getAirplane();
 100     Model* m = a->getModel();
 101
 102     // Superclass hook
 103     common_init();
 104
 105     m->setCrashed(false);
 106
 107     // Figure out the initial speed type
 108     string speed_set = fgGetString("/sim/presets/speed-set", "UVW");
 109     if (speed_set == "NED")
 110         _speed_set = NED;
 111     else if (speed_set == "UVW")
 112         _speed_set = UVW;
 113     else if (speed_set == "knots")
 114         _speed_set = KNOTS;
 115     else if (speed_set == "mach")
 116         _speed_set = MACH;
 117     else {
 118         _speed_set = UVW;
 119         SG_LOG(SG_FLIGHT, SG_ALERT, "Unknown speed type " << speed_set);
 120     }
 121
 122     // Build a filename and parse it
 123     SGPath f(fgGetString("/sim/aircraft-dir"));
 124     f.append(fgGetString("/sim/aero"));
 125     f.concat(".xml");
 126     readXML(f.str(), *_fdm);
 127
 128     // Compile it into a real airplane, and tell the user what they got
 129     a->compile();
 130     report();
 131
 132     _fdm->init();
 133
 134     // Create some FG{Eng|Gear}Interface objects
 135     int i;
 136     for(i=0; i<a->numGear(); i++) {
 137         Gear* g = a->getGear(i);
 138         SGPropertyNode * node = fgGetNode("gear/gear", i, true);
 139         float pos[3];
 140         g->getPosition(pos);
 141         node->setDoubleValue("xoffset-in", pos[0]);
 142         node->setDoubleValue("yoffset-in", pos[1]);
 143         node->setDoubleValue("zoffset-in", pos[2]);
 144     }
 145
 146     // Are we at ground level?  If so, lift the plane up so the gear
 147     // clear the ground.
 148     double runway_altitude = get_Runway_altitude();
 149     if(get_Altitude() - runway_altitude < 50) {
 150         fgSetBool("/controls/gear/gear-down", false);
 151         float minGearZ = 1e18;
 152         for(i=0; i<a->numGear(); i++) {
 153             Gear* g = a->getGear(i);
 154             float pos[3];
 155             g->getPosition(pos);
 156             if(pos[2] < minGearZ)
 157                 minGearZ = pos[2];
 158         }
 159         _set_Altitude(runway_altitude - minGearZ*M2FT);
 160         fgSetBool("/controls/gear/gear-down", true);
 161     }
 162
 163     // Blank the state, and copy in ours
 164     State s;
 165     m->setState(&s);
 166     copyToYASim(true);
 167
 168     _fdm->getExternalInput();
 169     _fdm->getAirplane()->initEngines();
 170
 171     set_inited(true);
 172 }
 173
 174 void YASim::update(double dt)
 175 {
 176     if (is_suspended())
 177         return;
 178
 179     int iterations = _calc_multiloop(dt);
 180
 181     // If we're crashed, then we don't care
 182     if(_fdm->getAirplane()->getModel()->isCrashed()) {
 183         if(!fgGetBool("/sim/crashed"))
 184             fgSetBool("/sim/crashed", true);
 185         return;
 186     }
 187
 188     int i;
 189     for(i=0; i<iterations; i++) {
 190         copyToYASim(false);
 191         _fdm->iterate(_dt);
 192         copyFromYASim();
 193     }
 194 }
 195
 196 void YASim::copyToYASim(bool copyState)
 197 {
 198     // Physical state
 199     float lat = get_Latitude();
 200     float lon = get_Longitude();
 201     float alt = get_Altitude() * FT2M;
 202     float roll = get_Phi();
 203     float pitch = get_Theta();
 204     float hdg = get_Psi();
 205
 206     // Environment
 207     float wind[3];
 208     wind[0] = get_V_north_airmass() * FT2M * -1.0;
 209     wind[1] = get_V_east_airmass() * FT2M * -1.0;
 210     wind[2] = get_V_down_airmass() * FT2M * -1.0;
 211
 212     // Get ground elevation
 213     double ground = fgGetDouble("/position/ground-elev-m");
 214     // cout << "YASIM: ground = " << ground << endl;
 215
 216     float pressure = fgGetFloat("/environment/pressure-inhg") * INHG2PA;
 217     float temp = fgGetFloat("/environment/temperature-degc") + 273.15;
 218     float dens = fgGetFloat("/environment/density-slugft3")
 219         * SLUG2KG * M2FT*M2FT*M2FT;
 220
 221     // Convert and set:
 222     Model* model = _fdm->getAirplane()->getModel();
 223     State s;
 224     float xyz2ned[9];
 225     Glue::xyz2nedMat(lat, lon, xyz2ned);
 226
 227     // position
 228     sgGeodToCart(lat, lon, alt, s.pos);
 229
 230     // orientation
 231     Glue::euler2orient(roll, pitch, hdg, s.orient);
 232     Math::mmul33(s.orient, xyz2ned, s.orient);
 233
 234     // Velocity
 235     string speed_set = fgGetString("/sim/presets/speed-set", "UVW");
 236     float v[3];
 237     bool needCopy = false;
 238     switch (_speed_set) {
 239     case NED:
 240         v[0] = get_V_north() * FT2M * -1.0;
 241         v[1] = get_V_east() * FT2M * -1.0;
 242         v[2] = get_V_down() * FT2M * -1.0;
 243         break;
 244     case UVW:
 245         v[0] = get_uBody() * FT2M;
 246         v[1] = get_vBody() * FT2M;
 247         v[2] = get_wBody() * FT2M;
 248         Math::tmul33(s.orient, v, v);
 249         break;
 250     case KNOTS:
 251         v[0] = Atmosphere::spdFromVCAS(get_V_calibrated_kts()/MPS2KTS,
 252                                        pressure, temp);
 253         v[1] = 0;
 254         v[2] = 0;
 255         Math::tmul33(s.orient, v, v);
 256         needCopy = true;
 257         break;
 258     case MACH:
 259         v[0] = Atmosphere::spdFromMach(get_Mach_number(), temp);
 260         v[1] = 0;
 261         v[2] = 0;
 262         Math::tmul33(s.orient, v, v);
 263         needCopy = true;
 264         break;
 265     default:
 266         v[0] = 0;
 267         v[1] = 0;
 268         v[2] = 0;
 269         break;
 270     }
 271     if (!copyState)
 272         _speed_set = UVW;       // change to this after initial setting
 273     Math::set3(v, s.v);
 274
 275     if(copyState || needCopy)
 276         model->setState(&s);
 277
 278     // wind
 279     Math::tmul33(xyz2ned, wind, wind);
 280     model->setWind(wind);
 281
 282     // ground.  Calculate a cartesian coordinate for the ground under
 283     // us, find the (geodetic) up vector normal to the ground, then
 284     // use that to find the final (radius) term of the plane equation.
 285     double xyz[3], gplane[3]; float up[3];
 286     sgGeodToCart(lat, lon, ground, xyz);
 287     Glue::geodUp(lat, lon, up); // FIXME, needless reverse computation...
 288     int i;
 289     for(i=0; i<3; i++) gplane[i] = up[i];
 290     double rad = gplane[0]*xyz[0] + gplane[1]*xyz[1] + gplane[2]*xyz[2];
 291     model->setGroundPlane(gplane, rad);
 292
 293     // air
 294     model->setAir(pressure, temp, dens);
 295 }
 296
 297 // All the settables:
 298 //
 299 // These are set below:
 300 // _set_Accels_Local
 301 // _set_Accels_Body
 302 // _set_Accels_CG_Body
 303 // _set_Accels_Pilot_Body
 304 // _set_Accels_CG_Body_N
 305 // _set_Velocities_Local
 306 // _set_Velocities_Ground
 307 // _set_Velocities_Wind_Body
 308 // _set_Omega_Body
 309 // _set_Euler_Rates
 310 // _set_Euler_Angles
 311 // _set_V_rel_wind
 312 // _set_V_ground_speed
 313 // _set_V_equiv_kts
 314 // _set_V_calibrated_kts
 315 // _set_Alpha
 316 // _set_Beta
 317 // _set_Mach_number
 318 // _set_Climb_Rate
 319 // _set_Tank1Fuel
 320 // _set_Tank2Fuel
 321 // _set_Altitude_AGL
 322 // _set_Geodetic_Position
 323 // _set_Runway_altitude
 324
 325 // Ignoring these, because they're unused:
 326 // _set_Geocentric_Position
 327 // _set_Geocentric_Rates
 328 // _set_Cos_phi
 329 // _set_Cos_theta
 330 // _set_Earth_position_angle (WTF?)
 331 // _set_Gamma_vert_rad
 332 // _set_Inertias
 333 // _set_T_Local_to_Body
 334 // _set_CG_Position
 335 // _set_Sea_Level_Radius
 336
 337 // Externally set via the weather code:
 338 // _set_Velocities_Local_Airmass
 339 // _set_Density
 340 // _set_Static_pressure
 341 // _set_Static_temperature
 342 void YASim::copyFromYASim()
 343 {
 344     Airplane* airplane = _fdm->getAirplane();
 345     Model* model = airplane->getModel();
 346     State* s = model->getState();
 347
 348     // position
 349     double lat, lon, alt;
 350     sgCartToGeod(s->pos, &lat, &lon, &alt);
 351     _set_Geodetic_Position(lat, lon, alt*M2FT);
 352
 353     // UNUSED
 354     //_set_Geocentric_Position(Glue::geod2geocLat(lat), lon, alt*M2FT);
 355
 356     _set_Altitude_AGL(model->getAGL() * M2FT);
 357
 358     // useful conversion matrix
 359     float xyz2ned[9];
 360     Glue::xyz2nedMat(lat, lon, xyz2ned);
 361
 362     // velocity
 363     float v[3];
 364     Math::vmul33(xyz2ned, s->v, v);
 365     _set_Velocities_Local(M2FT*v[0], M2FT*v[1], M2FT*v[2]);
 366     _set_V_ground_speed(Math::sqrt(M2FT*v[0]*M2FT*v[0] +
 367                                    M2FT*v[1]*M2FT*v[1]));
 368     _set_Climb_Rate(-M2FT*v[2]);
 369
 370     // The HUD uses this, but inverts down (?!)
 371     _set_Velocities_Ground(M2FT*v[0], M2FT*v[1], -M2FT*v[2]);
 372
 373     // _set_Geocentric_Rates(M2FT*v[0], M2FT*v[1], M2FT*v[2]); // UNUSED
 374
 375     // Airflow velocity.
 376     float wind[3];
 377     wind[0] = get_V_north_airmass() * FT2M * -1.0;  // Wind in NED
 378     wind[1] = get_V_east_airmass() * FT2M * -1.0;
 379     wind[2] = get_V_down_airmass() * FT2M * -1.0;
 380     Math::tmul33(xyz2ned, wind, wind);              // Wind in global
 381     Math::sub3(s->v, wind, v);                      // V - wind in global
 382     Math::vmul33(s->orient, v, v);               // to body coordinates
 383     _set_Velocities_Wind_Body(v[0]*M2FT, -v[1]*M2FT, -v[2]*M2FT);
 384     _set_V_rel_wind(Math::mag3(v)*M2FT); // units?
 385
 386     float P = fgGetDouble("/environment/pressure-inhg") * INHG2PA;
 387     float T = fgGetDouble("/environment/temperature-degC") + 273.15;
 388     float D = fgGetFloat("/environment/density-slugft3")
 389         *SLUG2KG * M2FT*M2FT*M2FT;
 390     _set_V_equiv_kts(Atmosphere::calcVEAS(v[0], P, T, D)*MPS2KTS);
 391     _set_V_calibrated_kts(Atmosphere::calcVCAS(v[0], P, T)*MPS2KTS);
 392     _set_Mach_number(Atmosphere::calcMach(v[0], T));
 393
 394     // acceleration
 395     Math::vmul33(xyz2ned, s->acc, v);
 396     _set_Accels_Local(M2FT*v[0], M2FT*v[1], M2FT*v[2]);
 397
 398     Math::vmul33(s->orient, s->acc, v);
 399     _set_Accels_Body(M2FT*v[0], -M2FT*v[1], -M2FT*v[2]);
 400     _set_Accels_CG_Body(M2FT*v[0], -M2FT*v[1], -M2FT*v[2]);
 401
 402     _fdm->getAirplane()->getPilotAccel(v);
 403     _set_Accels_Pilot_Body(-M2FT*v[0], M2FT*v[1], M2FT*v[2]);
 404
 405     // There is no property for pilot G's, but I need it for a panel
 406     // instrument.  Hack this in here, and REMOVE IT WHEN IT FINDS A
 407     // REAL HOME!
 408     fgSetFloat("/accelerations/pilot-g", -v[2]/9.8);
 409
 410     // The one appears (!) to want inverted pilot acceleration
 411     // numbers, in G's...
 412     Math::mul3(1.0/9.8, v, v);
 413     _set_Accels_CG_Body_N(v[0], -v[1], -v[2]);
 414
 415     // orientation
 416     float alpha, beta;
 417     Glue::calcAlphaBeta(s, &alpha, &beta);
 418     _set_Alpha(alpha);
 419     _set_Beta(beta);
 420
 421     float tmp[9];
 422     Math::trans33(xyz2ned, tmp);
 423     Math::mmul33(s->orient, tmp, tmp);
 424     float roll, pitch, hdg;
 425     Glue::orient2euler(tmp, &roll, &pitch, &hdg);
 426     // make heading positive value
 427     if(hdg < 0.0) hdg += PI2;
 428     _set_Euler_Angles(roll, pitch, hdg);
 429
 430     // rotation
 431     Math::vmul33(s->orient, s->rot, v);
 432     _set_Omega_Body(v[0], -v[1], -v[2]);
 433
 434     Glue::calcEulerRates(s, &roll, &pitch, &hdg);
 435     _set_Euler_Rates(roll, pitch, hdg);
 436
 437     // Fill out our engine and gear objects
 438     int i;
 439     for(i=0; i<airplane->numGear(); i++) {
 440         Gear* g = airplane->getGear(i);
 441         SGPropertyNode * node = fgGetNode("gear/gear", i, true);
 442         node->setBoolValue("has-brake", g->getBrake() != 0);
 443         node->setBoolValue("wow", g->getCompressFraction() != 0);
 444         node->setFloatValue("compression-norm", g->getCompressFraction());
 445     }
 446 }