src/FDM/YASim/PropEngine.cpp

   1 #include "Math.hpp"
   2 #include "Propeller.hpp"
   3 #include "PistonEngine.hpp"
   4 #include "PropEngine.hpp"
   5 namespace yasim {
   6
   7 PropEngine::PropEngine(Propeller* prop, PistonEngine* eng, float moment)
   8 {
   9     // Start off at 500rpm, because the start code doesn't exist yet
  10     _omega = 52.3;
  11     _dir[0] = 1; _dir[1] = 0; _dir[2] = 0;
  12
  13     _variable = false;
  14
  15     _prop = prop;
  16     _eng = eng;
  17     _moment = moment;
  18 }
  19
  20 PropEngine::~PropEngine()
  21 {
  22     delete _prop;
  23     delete _eng;
  24 }
  25
  26 void PropEngine::setAdvance(float advance)
  27 {
  28     _advance = Math::clamp(advance, 0, 1);
  29 }
  30
  31 void PropEngine::setVariableProp(float min, float max)
  32 {
  33     _variable = true;
  34     _minOmega = min;
  35     _maxOmega = max;
  36 }
  37
  38 float PropEngine::getOmega()
  39 {
  40     return _omega;
  41 }
  42
  43 void PropEngine::getThrust(float* out)
  44 {
  45     for(int i=0; i<3; i++) out[i] = _thrust[i];
  46 }
  47
  48 void PropEngine::getTorque(float* out)
  49 {
  50     for(int i=0; i<3; i++) out[i] = _torque[i];
  51 }
  52
  53 void PropEngine::getGyro(float* out)
  54 {
  55     for(int i=0; i<3; i++) out[i] = _gyro[i];
  56 }
  57
  58 float PropEngine::getFuelFlow()
  59 {
  60     return _fuelFlow;
  61 }
  62
  63 void PropEngine::stabilize()
  64 {
  65     float speed = -Math::dot3(_wind, _dir);
  66     _eng->setThrottle(_throttle);
  67     _eng->setMixture(_mixture);
  68
  69     if(_variable) {
  70         _omega = _minOmega + _advance * (_maxOmega - _minOmega);
  71         _prop->modPitch(1e6); // Start at maximum pitch and move down
  72     } else {
  73         _omega = 52;
  74     }
  75
  76     bool goingUp = false;
  77     float step = 10;
  78     while(true) {
  79         float etau, ptau, dummy;
  80         _prop->calc(_rho, speed, _omega, &dummy, &ptau);
  81         _eng->calc(_P, _T, _omega, &etau, &dummy);
  82         float tdiff = etau - ptau;
  83
  84         if(Math::abs(tdiff/_moment) < 0.1)
  85             break;
  86
  87         if(tdiff > 0) {
  88             if(!goingUp) step *= 0.5;
  89             goingUp = true;
  90             if(!_variable)  _omega += step;
  91             else            _prop->modPitch(1+(step*0.005));
  92         } else {
  93             if(goingUp) step *= 0.5;
  94             goingUp = false;
  95             if(!_variable)  _omega -= step;
  96             else            _prop->modPitch(1-(step*0.005));
  97         }
  98     }
  99 }
 100
 101 void PropEngine::integrate(float dt)
 102 {
 103     float speed = -Math::dot3(_wind, _dir);
 104
 105     float propTorque, engTorque, thrust;
 106
 107     _eng->setThrottle(_throttle);
 108     _eng->setMixture(_mixture);
 109
 110     _prop->calc(_rho, speed, _omega,
 111                 &thrust, &propTorque);
 112     _eng->calc(_P, _T, _omega, &engTorque, &_fuelFlow);
 113
 114     // Turn the thrust into a vector and save it
 115     Math::mul3(thrust, _dir, _thrust);
 116
 117     // Euler-integrate the RPM.  This doesn't need the full-on
 118     // Runge-Kutta stuff.
 119     float rotacc = (engTorque-propTorque)/Math::abs(_moment);
 120     _omega += dt * rotacc;
 121
 122     // Clamp to a 500 rpm idle.  This should probably be settable, and
 123     // needs to go away when the startup code gets written.
 124     if(_omega < 52.3) _omega = 52.3;
 125
 126     // FIXME: Integrate the propeller governor here, when that gets
 127     // implemented...
 128
 129     // Store the total angular momentum into _gyro
 130     Math::mul3(_omega*_moment, _dir, _gyro);
 131
 132     // Accumulate the engine torque, it acts on the body as a whole.
 133     // (Note: engine torque, not propeller torque.  They can be
 134     // different, but the difference goes to accelerating the
 135     // rotation.  It is the engine torque that is felt at the shaft
 136     // and works on the body.)
 137     float tau = _moment < 0 ? engTorque : -engTorque;
 138     Math::mul3(tau, _dir, _torque);
 139
 140     // Play with the variable propeller, but only if the propeller is
 141     // vaguely stable alread (accelerating less than 100 rpm/s)
 142     if(_variable && Math::abs(rotacc) < 20) {
 143         float target = _minOmega + _advance*(_maxOmega-_minOmega);
 144         float mod = 1.04;
 145         if(target > _omega) mod = 1/mod;
 146         float diff = Math::abs(target - _omega);
 147         if(diff < 1) mod = 1 + (mod-1)*diff;
 148         if(thrust < 0) mod = 1;
 149         _prop->modPitch(mod);
 150     }
 151 }
 152
 153 }; // namespace yasim