src/FDM/YASim/FGFDM.cpp

   1 #include <stdio.h>
   2 #include <stdlib.h>
   3
   4 #include <Main/fg_props.hxx>
   5
   6 #include "Jet.hpp"
   7 #include "Gear.hpp"
   8 #include "Atmosphere.hpp"
   9 #include "PropEngine.hpp"
  10 #include "Propeller.hpp"
  11 #include "PistonEngine.hpp"
  12
  13 #include "FGFDM.hpp"
  14 namespace yasim {
  15
  16 // Some conversion factors
  17 static const float KTS2MPS = 0.514444444444;
  18 static const float FT2M = 0.3048;
  19 static const float DEG2RAD = 0.0174532925199;
  20 static const float RPM2RAD = 0.10471975512;
  21 static const float LBS2N = 4.44822;
  22 static const float LBS2KG = 0.45359237;
  23 static const float CM2GALS = 264.172037284;
  24 static const float HP2W = 745.700;
  25
  26 // Stubs, so that this can be compiled without the FlightGear
  27 // binary.  What's the best way to handle this?
  28
  29 //     float fgGetFloat(char* name, float def) { return 0; }
  30 //     void fgSetFloat(char* name, float val) {}
  31
  32 FGFDM::FGFDM()
  33 {
  34     _nextEngine = 0;
  35 }
  36
  37 FGFDM::~FGFDM()
  38 {
  39     for(int i=0; i<_axes.size(); i++) {
  40         AxisRec* a = (AxisRec*)_axes.get(i);
  41         delete[] a->name;
  42         delete a;
  43     }
  44     for(int i=0; i<_pistons.size(); i++) {
  45         EngRec* er = (EngRec*)_pistons.get(i);
  46         delete[] er->prefix;
  47         delete (PropEngine*)er->eng;
  48         delete er;
  49     }
  50     for(int i=0; i<_jets.size(); i++) {
  51         EngRec* er = (EngRec*)_pistons.get(i);
  52         delete[] er->prefix;
  53         delete (Jet*)er->eng;
  54         delete er;
  55     }
  56     for(int i=0; i<_weights.size(); i++) {
  57         WeightRec* wr = (WeightRec*)_weights.get(i);
  58         delete[] wr->prop;
  59         delete wr;
  60     }
  61
  62 }
  63
  64 void FGFDM::iterate(float dt)
  65 {
  66     getExternalInput(dt);
  67     _airplane.iterate(dt);
  68     setOutputProperties();
  69 }
  70
  71 Airplane* FGFDM::getAirplane()
  72 {
  73     return &_airplane;
  74 }
  75
  76 void FGFDM::init()
  77 {
  78     // We don't want to use these ties (we set the values ourselves,
  79     // and this works only for the first piston engine anyway).
  80     fgUntie("/engines/engine[0]/rpm");
  81     fgUntie("/engines/engine[0]/egt-degf");
  82     fgUntie("/engines/engine[0]/cht-degf");
  83     fgUntie("/engines/engine[0]/oil-temperature-degf");
  84     fgUntie("/engines/engine[0]/mp-osi");
  85     fgUntie("/engines/engine[0]/fuel-flow-gph");
  86     fgUntie("/engines/engine[0]/running");
  87     fgUntie("/engines/engine[0]/cranking");
  88     fgUntie("/consumables/fuel/tank[0]/level-gal_us");
  89     fgUntie("/consumables/fuel/tank[1]/level-gal_us");
  90
  91     // Set these to sane values.  We don't support engine start yet.
  92     fgSetBool("/engines/engine[0]/running", true);
  93     fgSetBool("/engines/engine[0]/cranking", false);
  94
  95     // Allows the user to start with something other than full fuel
  96     _airplane.setFuelFraction(fgGetFloat("/yasim/fuel-fraction", 1));
  97
  98     // This has a nasty habit of being false at startup.  That's not
  99     // good.
 100     fgSetBool("/controls/gear-down", true);
 101 }
 102
 103 // Not the worlds safest parser.  But it's short & sweet.
 104 void FGFDM::startElement(const char* name, const XMLAttributes &atts)
 105 {
 106     XMLAttributes* a = (XMLAttributes*)&atts;
 107     float v[3];
 108     char buf[64];
 109
 110     if(eq(name, "airplane")) {
 111         _airplane.setWeight(attrf(a, "mass") * LBS2KG);
 112     } else if(eq(name, "approach")) {
 113         float spd = attrf(a, "speed") * KTS2MPS;
 114         float alt = attrf(a, "alt", 0) * FT2M;
 115         float aoa = attrf(a, "aoa", 0) * DEG2RAD;
 116         _airplane.setApproach(spd, alt, aoa);
 117         _cruiseCurr = false;
 118     } else if(eq(name, "cruise")) {
 119         float spd = attrf(a, "speed") * KTS2MPS;
 120         float alt = attrf(a, "alt") * FT2M;
 121         _airplane.setCruise(spd, alt);
 122         _cruiseCurr = true;
 123     } else if(eq(name, "cockpit")) {
 124         v[0] = attrf(a, "x");
 125         v[1] = attrf(a, "y");
 126         v[2] = attrf(a, "z");
 127         _airplane.setPilotPos(v);
 128     } else if(eq(name, "wing")) {
 129         _airplane.setWing(parseWing(a, name));
 130     } else if(eq(name, "hstab")) {
 131         _airplane.setTail(parseWing(a, name));
 132     } else if(eq(name, "vstab")) {
 133         _airplane.addVStab(parseWing(a, name));
 134     } else if(eq(name, "propeller")) {
 135         parsePropeller(a);
 136     } else if(eq(name, "jet")) {
 137         Jet* j = new Jet();
 138         _currObj = j;
 139         v[0] = attrf(a, "x");
 140         v[1] = attrf(a, "y");
 141         v[2] = attrf(a, "z");
 142         float mass = attrf(a, "mass") * LBS2KG;
 143         j->setDryThrust(attrf(a, "thrust") * LBS2N);
 144         j->setPosition(v);
 145         _airplane.addThruster(j, mass, v);
 146         sprintf(buf, "/engines/engine[%d]", _nextEngine++);
 147         EngRec* er = new EngRec();
 148         er->eng = j;
 149         er->prefix = dup(buf);
 150         _jets.add(er);
 151     } else if(eq(name, "gear")) {
 152         Gear* g = new Gear();
 153         _currObj = g;
 154         v[0] = attrf(a, "x");
 155         v[1] = attrf(a, "y");
 156         v[2] = attrf(a, "z");
 157         g->setPosition(v);
 158         v[0] = 0;
 159         v[1] = 0;
 160         v[2] = attrf(a, "compression", 1);
 161         g->setCompression(v);
 162         g->setStaticFriction(attrf(a, "sfric", 0.8));
 163         g->setDynamicFriction(attrf(a, "dfric", 0.7));
 164         float transitionTime = attrf(a, "retract-time", 0);
 165         _airplane.addGear(g, transitionTime);
 166     } else if(eq(name, "fuselage")) {
 167         float b[3];
 168         v[0] = attrf(a, "ax");
 169         v[1] = attrf(a, "ay");
 170         v[2] = attrf(a, "az");
 171         b[0] = attrf(a, "bx");
 172         b[1] = attrf(a, "by");
 173         b[2] = attrf(a, "bz");
 174         _airplane.addFuselage(v, b, attrf(a, "width"));
 175     } else if(eq(name, "tank")) {
 176         v[0] = attrf(a, "x");
 177         v[1] = attrf(a, "y");
 178         v[2] = attrf(a, "z");
 179         float density = 6.0; // gasoline, in lbs/gal
 180         if(a->hasAttribute("jet")) density = 6.72;
 181         density *= LBS2KG/CM2GALS;
 182         _airplane.addTank(v, attrf(a, "capacity") * LBS2KG, density);
 183     } else if(eq(name, "ballast")) {
 184         v[0] = attrf(a, "x");
 185         v[1] = attrf(a, "y");
 186         v[2] = attrf(a, "z");
 187         _airplane.addBallast(v, attrf(a, "mass") * LBS2KG);
 188     } else if(eq(name, "weight")) {
 189         parseWeight(a);
 190     } else if(eq(name, "stall")) {
 191         Wing* w = (Wing*)_currObj;
 192         w->setStall(attrf(a, "aoa") * DEG2RAD);
 193         w->setStallWidth(attrf(a, "width", 2) * DEG2RAD);
 194         w->setStallPeak(attrf(a, "peak", 1.5));
 195     } else if(eq(name, "flap0")) {
 196         ((Wing*)_currObj)->setFlap0(attrf(a, "start"), attrf(a, "end"),
 197                                     attrf(a, "lift"), attrf(a, "drag"));
 198     } else if(eq(name, "flap1")) {
 199         ((Wing*)_currObj)->setFlap1(attrf(a, "start"), attrf(a, "end"),
 200                                     attrf(a, "lift"), attrf(a, "drag"));
 201     } else if(eq(name, "slat")) {
 202         ((Wing*)_currObj)->setSlat(attrf(a, "start"), attrf(a, "end"),
 203                                    attrf(a, "aoa"), attrf(a, "drag"));
 204     } else if(eq(name, "spoiler")) {
 205         ((Wing*)_currObj)->setSpoiler(attrf(a, "start"), attrf(a, "end"),
 206                                       attrf(a, "lift"), attrf(a, "drag"));
 207     } else if(eq(name, "actionpt")) {
 208         v[0] = attrf(a, "x");
 209         v[1] = attrf(a, "y");
 210         v[2] = attrf(a, "z");
 211         ((Thruster*)_currObj)->setPosition(v);
 212     } else if(eq(name, "dir")) {
 213         v[0] = attrf(a, "x");
 214         v[1] = attrf(a, "y");
 215         v[2] = attrf(a, "z");
 216         ((Thruster*)_currObj)->setDirection(v);
 217     } else if(eq(name, "control")) {
 218         const char* axis = a->getValue("axis");
 219         if(a->hasAttribute("output")) {
 220             // assert: output type must match _currObj type!
 221             const char* output = a->getValue("output");
 222             int opt = 0;
 223             opt |= a->hasAttribute("split") ? ControlMap::OPT_SPLIT : 0;
 224             opt |= a->hasAttribute("invert") ? ControlMap::OPT_INVERT : 0;
 225             opt |= a->hasAttribute("square") ? ControlMap::OPT_SQUARE : 0;
 226             _airplane.getControlMap()->addMapping(parseAxis(axis),
 227                                                   parseOutput(output),
 228                                                   _currObj,
 229                                                   opt);
 230         } else {
 231             // assert: must be under a "cruise" or "approach" tag
 232             float value = attrf(a, "value", 0);
 233             if(_cruiseCurr)
 234                 _airplane.addCruiseControl(parseAxis(axis), value);
 235             else
 236                 _airplane.addApproachControl(parseAxis(axis), value);
 237         }
 238     } else {
 239         *(int*)0=0; // unexpected tag, boom
 240     }
 241 }
 242
 243 void FGFDM::getExternalInput(float dt)
 244 {
 245     // The control axes
 246     ControlMap* cm = _airplane.getControlMap();
 247     cm->reset();
 248     for(int i=0; i<_axes.size(); i++) {
 249         AxisRec* a = (AxisRec*)_axes.get(i);
 250         float val = fgGetFloat(a->name, 0);
 251         cm->setInput(a->handle, val);
 252     }
 253     cm->applyControls();
 254
 255     // Weights
 256     for(int i=0; i<_weights.size(); i++) {
 257         WeightRec* wr = (WeightRec*)_weights.get(i);
 258         _airplane.setWeight(wr->handle, fgGetFloat(wr->prop));
 259     }
 260
 261     // Gear state
 262     _airplane.setGearState(fgGetBool("/controls/gear-down"), dt);
 263 }
 264
 265 void FGFDM::setOutputProperties()
 266 {
 267     char buf[256];
 268     for(int i=0; i<_airplane.numTanks(); i++) {
 269         sprintf(buf, "/consumables/fuel/tank[%d]/level-gal_us", i);
 270         fgSetFloat(buf,
 271                    CM2GALS*_airplane.getFuel(i)/_airplane.getFuelDensity(i));
 272     }
 273
 274     for(int i=0; i<_pistons.size(); i++) {
 275         EngRec* er = (EngRec*)_pistons.get(i);
 276         PropEngine* p = (PropEngine*)er->eng;
 277
 278         sprintf(buf, "%s/rpm", er->prefix);
 279         fgSetFloat(buf, p->getOmega() * (30/3.15149265358979));
 280
 281         sprintf(buf, "%s/fuel-flow-gph", er->prefix);
 282         fgSetFloat(buf, p->getFuelFlow() * (3600*2.2/5)); // FIXME, wrong
 283     }
 284
 285     for(int i=0; i<_jets.size(); i++) {
 286         EngRec* er = (EngRec*)_jets.get(i);
 287         Jet* j = (Jet*)er->eng;
 288
 289         sprintf(buf, "%s/fuel-flow-gph", er->prefix);
 290         fgSetFloat(buf, j->getFuelFlow() * (3600*2.2/6)); // FIXME, wrong
 291     }
 292 }
 293
 294 Wing* FGFDM::parseWing(XMLAttributes* a, const char* type)
 295 {
 296     Wing* w = new Wing();
 297
 298     float defDihed = 0;
 299     if(eq(type, "vstab"))
 300         defDihed = 90;
 301     else
 302         w->setMirror(true);
 303
 304     float pos[3];
 305     pos[0] = attrf(a, "x");
 306     pos[1] = attrf(a, "y");
 307     pos[2] = attrf(a, "z");
 308     w->setBase(pos);
 309
 310     w->setLength(attrf(a, "length"));
 311     w->setChord(attrf(a, "chord"));
 312     w->setSweep(attrf(a, "sweep", 0) * DEG2RAD);
 313     w->setTaper(attrf(a, "taper", 1));
 314     w->setDihedral(attrf(a, "dihedral", defDihed) * DEG2RAD);
 315     w->setCamber(attrf(a, "camber", 0));
 316     w->setIncidence(attrf(a, "incidence", 0) * DEG2RAD);
 317
 318     float effect = attrf(a, "effectiveness", 1);
 319     w->setDragScale(w->getDragScale()*effect);
 320
 321     _currObj = w;
 322     return w;
 323 }
 324
 325 void FGFDM::parsePropeller(XMLAttributes* a)
 326 {
 327     float cg[3];
 328     cg[0] = attrf(a, "x");
 329     cg[1] = attrf(a, "y");
 330     cg[2] = attrf(a, "z");
 331     float mass = attrf(a, "mass") * LBS2KG;
 332     float moment = attrf(a, "moment");
 333     float radius = attrf(a, "radius");
 334     float speed = attrf(a, "cruise-speed") * KTS2MPS;
 335     float omega = attrf(a, "cruise-rpm") * RPM2RAD;
 336     float rho = Atmosphere::getStdDensity(attrf(a, "alt") * FT2M);
 337     float power = attrf(a, "takeoff-power") * HP2W;
 338     float omega0 = attrf(a, "takeoff-rpm") * RPM2RAD;
 339
 340     // FIXME: this is a hack.  Find a better way to ask the engine how
 341     // much power it can produce under cruise conditions.
 342     float cruisePower = (power * (rho/Atmosphere::getStdDensity(0))
 343                          * (omega/omega0));
 344
 345     Propeller* prop = new Propeller(radius, speed, omega, rho, cruisePower,
 346                                     omega0, power);
 347     PistonEngine* eng = new PistonEngine(power, omega0);
 348     PropEngine* thruster = new PropEngine(prop, eng, moment);
 349     _airplane.addThruster(thruster, mass, cg);
 350
 351     char buf[64];
 352     sprintf(buf, "/engines/engine[%d]", _nextEngine++);
 353     EngRec* er = new EngRec();
 354     er->eng = thruster;
 355     er->prefix = dup(buf);
 356     _pistons.add(er);
 357
 358     _currObj = thruster;
 359 }
 360
 361 // Turns a string axis name into an integer for use by the
 362 // ControlMap.  Creates a new axis if this one hasn't been defined
 363 // yet.
 364 int FGFDM::parseAxis(const char* name)
 365 {
 366     for(int i=0; i<_axes.size(); i++) {
 367         AxisRec* a = (AxisRec*)_axes.get(i);
 368         if(eq(a->name, name))
 369             return a->handle;
 370     }
 371
 372     // Not there, make a new one.
 373     AxisRec* a = new AxisRec();
 374     a->name = dup(name);
 375     a->handle = _airplane.getControlMap()->newInput();
 376     _axes.add(a);
 377     return a->handle;
 378 }
 379
 380 int FGFDM::parseOutput(const char* name)
 381 {
 382     if(eq(name, "THROTTLE"))  return ControlMap::THROTTLE;
 383     if(eq(name, "MIXTURE"))   return ControlMap::MIXTURE;
 384     if(eq(name, "REHEAT"))    return ControlMap::REHEAT;
 385     if(eq(name, "PROP"))      return ControlMap::PROP;
 386     if(eq(name, "BRAKE"))     return ControlMap::BRAKE;
 387     if(eq(name, "STEER"))     return ControlMap::STEER;
 388     if(eq(name, "EXTEND"))    return ControlMap::EXTEND;
 389     if(eq(name, "INCIDENCE")) return ControlMap::INCIDENCE;
 390     if(eq(name, "FLAP0"))     return ControlMap::FLAP0;
 391     if(eq(name, "FLAP1"))     return ControlMap::FLAP1;
 392     if(eq(name, "SLAT"))      return ControlMap::SLAT;
 393     if(eq(name, "SPOILER"))   return ControlMap::SPOILER;
 394     // error here...
 395     return -1;
 396 }
 397
 398 void FGFDM::parseWeight(XMLAttributes* a)
 399 {
 400     WeightRec* wr = new WeightRec();
 401
 402     float v[3];
 403     v[0] = attrf(a, "x");
 404     v[1] = attrf(a, "y");
 405     v[2] = attrf(a, "z");
 406
 407     wr->prop = dup(a->getValue("mass-prop"));
 408     wr->size = attrf(a, "size", 0);
 409     wr->handle = _airplane.addWeight(v, wr->size);
 410
 411     _weights.add(wr);
 412 }
 413
 414 bool FGFDM::eq(const char* a, const char* b)
 415 {
 416     // Figure it out for yourself. :)
 417     while(*a && *b && *a++ == *b++);
 418     return !(*a || *b);
 419 }
 420
 421 char* FGFDM::dup(const char* s)
 422 {
 423     int len=0;
 424     while(s[len++]);
 425     char* s2 = new char[len+1];
 426     char* p = s2;
 427     while((*p++ = *s++));
 428     s2[len] = 0;
 429     return s2;
 430 }
 431
 432 int FGFDM::attri(XMLAttributes* atts, char* attr)
 433 {
 434     if(!atts->hasAttribute(attr)) *(int*)0=0; // boom
 435     return attri(atts, attr, 0);
 436 }
 437
 438 int FGFDM::attri(XMLAttributes* atts, char* attr, int def)
 439 {
 440     const char* val = atts->getValue(attr);
 441     if(val == 0) return def;
 442     else         return atol(val);
 443 }
 444
 445 float FGFDM::attrf(XMLAttributes* atts, char* attr)
 446 {
 447     if(!atts->hasAttribute(attr)) *(int*)0=0; // boom
 448     return attrf(atts, attr, 0);
 449 }
 450
 451 float FGFDM::attrf(XMLAttributes* atts, char* attr, float def)
 452 {
 453     const char* val = atts->getValue(attr);
 454     if(val == 0) return def;
 455     else         return (float)atof(val);
 456 }
 457
 458 }; // namespace yasim