X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FFDM%2FYASim%2FAirplane.cpp;h=09707c7f7bc6fa2fc25f638d643f1a4f2ff65fca;hb=ce91286e19d6d66b316811d04b5b66b8b768827b;hp=64997d2cdcf749bc22a1944178fbd1902c97d4e3;hpb=7e63c0f6e8fb5a1517343a8d1a5c9b13b4952f90;p=flightgear.git diff --git a/src/FDM/YASim/Airplane.cpp b/src/FDM/YASim/Airplane.cpp index 64997d2cd..09707c7f7 100644 --- a/src/FDM/YASim/Airplane.cpp +++ b/src/FDM/YASim/Airplane.cpp @@ -5,8 +5,9 @@ #include "Glue.hpp" #include "RigidBody.hpp" #include "Surface.hpp" +#include "Rotorpart.hpp" +#include "Rotorblade.hpp" #include "Thruster.hpp" - #include "Airplane.hpp" namespace yasim { @@ -15,6 +16,18 @@ namespace yasim { inline float norm(float f) { return f<1 ? 1/f : f; } inline float abs(float f) { return f<0 ? -f : f; } +// Solver threshold. How close to the solution are we trying +// to get? Trying too hard can result in oscillations about +// the correct solution, which is bad. Stick this in as a +// compile time constant for now, and consider making it +// settable per-model. +const float STHRESH = 1; + +// How slowly do we change values in the solver. Too slow, and +// the solution converges very slowly. Too fast, and it can +// oscillate. +const float SOLVE_TWEAK = 0.3226; + Airplane::Airplane() { _emptyWeight = 0; @@ -61,8 +74,37 @@ void Airplane::iterate(float dt) updateGearState(); _model.iterate(); +} - // FIXME: Consume fuel +void Airplane::consumeFuel(float dt) +{ + // This is a really simple implementation that assumes all engines + // draw equally from all tanks in proportion to the amount of fuel + // stored there. Needs to be fixed, but that has to wait for a + // decision as to what the property interface will look like. + int i, outOfFuel = 0; + float fuelFlow = 0, totalFuel = 0.00001; // <-- overflow protection + for(i=0; i<_thrusters.size(); i++) + fuelFlow += ((ThrustRec*)_thrusters.get(i))->thruster->getFuelFlow(); + for(i=0; i<_tanks.size(); i++) + totalFuel += ((Tank*)_tanks.get(i))->fill; + for(i=0; i<_tanks.size(); i++) { + Tank* t = (Tank*)_tanks.get(i); + t->fill -= dt * fuelFlow * (t->fill/totalFuel); + if(t->fill <= 0) { + t->fill = 0; + outOfFuel = 1; + } + } + if(outOfFuel) + for(int i=0; i<_thrusters.size(); i++) + ((ThrustRec*)_thrusters.get(i))->thruster->setFuelState(false); + + // Set the tank masses on the RigidBody + for(i=0; i<_tanks.size(); i++) { + Tank* t = (Tank*)_tanks.get(i); + _model.getBody()->setMass(t->handle, t->fill); + } } ControlMap* Airplane::getControlMap() @@ -189,6 +231,11 @@ float Airplane::getFuelDensity(int tank) return ((Tank*)_tanks.get(tank))->density; } +float Airplane::getTankCapacity(int tank) +{ + return ((Tank*)_tanks.get(tank))->cap; +} + void Airplane::setWeight(float weight) { _emptyWeight = weight; @@ -209,6 +256,11 @@ void Airplane::addVStab(Wing* vstab) _vstabs.add(vstab); } +void Airplane::addRotor(Rotor* rotor) +{ + _rotors.add(rotor); +} + void Airplane::addFuselage(float* front, float* back, float width, float taper, float mid) { @@ -298,6 +350,7 @@ void Airplane::setFuelFraction(float frac) int i; for(i=0; i<_tanks.size(); i++) { Tank* t = (Tank*)_tanks.get(i); + t->fill = frac * t->cap; _model.getBody()->setMass(t->handle, t->cap * frac); } } @@ -396,6 +449,43 @@ float Airplane::compileWing(Wing* w) return wgt; } +float Airplane::compileRotor(Rotor* r) +{ + // Todo: add rotor to model!!! + // Todo: calc and add mass!!! + r->compile(); + _model.addRotor(r); + + float wgt = 0; + int i; + for(i=0; inumRotorparts(); i++) { + Rotorpart* s = (Rotorpart*)r->getRotorpart(i); + + _model.addRotorpart(s); + + float mass = s->getWeight(); + mass = mass * Math::sqrt(mass); + float pos[3]; + s->getPosition(pos); + _model.getBody()->addMass(mass, pos); + wgt += mass; + } + + for(i=0; inumRotorblades(); i++) { + Rotorblade* b = (Rotorblade*)r->getRotorblade(i); + + _model.addRotorblade(b); + + float mass = b->getWeight(); + mass = mass * Math::sqrt(mass); + float pos[3]; + b->getPosition(pos); + _model.getBody()->addMass(mass, pos); + wgt += mass; + } + return wgt; +} + float Airplane::compileFuselage(Fuselage* f) { // The front and back are contact points @@ -529,17 +619,19 @@ void Airplane::compile() float aeroWgt = 0; // The Wing objects - aeroWgt += compileWing(_wing); - aeroWgt += compileWing(_tail); + if (_wing) + aeroWgt += compileWing(_wing); + if (_tail) + aeroWgt += compileWing(_tail); int i; - for(i=0; i<_vstabs.size(); i++) { + for(i=0; i<_vstabs.size(); i++) aeroWgt += compileWing((Wing*)_vstabs.get(i)); - } + for(i=0; i<_rotors.size(); i++) + aeroWgt += compileRotor((Rotor*)_rotors.get(i)); // The fuselage(s) - for(i=0; i<_fuselages.size(); i++) { + for(i=0; i<_fuselages.size(); i++) aeroWgt += compileFuselage((Fuselage*)_fuselages.get(i)); - } // Count up the absolute weight we have float nonAeroWgt = _ballast; @@ -581,11 +673,14 @@ void Airplane::compile() // Ground effect float gepos[3]; - float gespan = _wing->getGroundEffect(gepos); - _model.setGroundEffect(gepos, gespan, 0.3f); + float gespan = 0; + if(_wing) + gespan = _wing->getGroundEffect(gepos); + _model.setGroundEffect(gepos, gespan, 0.15f); solveGear(); - solve(); + if(_wing && _tail) solve(); + else solveHelicopter(); // Do this after solveGear, because it creates "gear" objects that // we don't want to affect. @@ -618,9 +713,9 @@ void Airplane::solveGear() ((GearRec*)_gears.get(i))->wgt /= total; // The force at max compression should be sufficient to stop a - // plane moving downwards at 3x the approach descent rate. Assume + // plane moving downwards at 2x the approach descent rate. Assume // a 3 degree approach. - float descentRate = 3.0f*_approachSpeed/19.1f; + float descentRate = 2.0f*_approachSpeed/19.1f; // Spread the kinetic energy according to the gear weights. This // will results in an equal compression fraction (not distance) of @@ -637,10 +732,11 @@ void Airplane::solveGear() // Energy in a spring: e = 0.5 * k * len^2 float k = 2 * e / (len*len); - gr->gear->setSpring(k); + gr->gear->setSpring(k * gr->gear->getSpring()); // Critically damped (too damped, too!) - gr->gear->setDamping(2*Math::sqrt(k*_approachWeight*gr->wgt)); + gr->gear->setDamping(2*Math::sqrt(k*_approachWeight*gr->wgt) + * gr->gear->getDamping()); // These are pretty generic gr->gear->setStaticFriction(0.8f); @@ -667,7 +763,8 @@ void Airplane::runCruise() { setupState(_cruiseAoA, _cruiseSpeed, &_cruiseState); _model.setState(&_cruiseState); - _model.setAir(_cruiseP, _cruiseT); + _model.setAir(_cruiseP, _cruiseT, + Atmosphere::calcStdDensity(_cruiseP, _cruiseT)); // The control configuration _controls.reset(); @@ -691,13 +788,15 @@ void Airplane::runCruise() for(i=0; i<_thrusters.size(); i++) { Thruster* t = ((ThrustRec*)_thrusters.get(i))->thruster; t->setWind(wind); - t->setAir(_cruiseP, _cruiseT); + t->setAir(_cruiseP, _cruiseT, + Atmosphere::calcStdDensity(_cruiseP, _cruiseT)); } stabilizeThrust(); updateGearState(); // Precompute thrust in the model, and calculate aerodynamic forces + _model.getBody()->recalc(); _model.getBody()->reset(); _model.initIteration(); _model.calcForces(&_cruiseState); @@ -707,7 +806,8 @@ void Airplane::runApproach() { setupState(_approachAoA, _approachSpeed, &_approachState); _model.setState(&_approachState); - _model.setAir(_approachP, _approachT); + _model.setAir(_approachP, _approachT, + Atmosphere::calcStdDensity(_approachP, _approachT)); // The control configuration _controls.reset(); @@ -731,13 +831,15 @@ void Airplane::runApproach() for(i=0; i<_thrusters.size(); i++) { Thruster* t = ((ThrustRec*)_thrusters.get(i))->thruster; t->setWind(wind); - t->setAir(_approachP, _approachT); + t->setAir(_approachP, _approachT, + Atmosphere::calcStdDensity(_approachP, _approachT)); } stabilizeThrust(); updateGearState(); // Precompute thrust in the model, and calculate aerodynamic forces + _model.getBody()->recalc(); _model.getBody()->reset(); _model.initIteration(); _model.calcForces(&_approachState); @@ -745,10 +847,12 @@ void Airplane::runApproach() void Airplane::applyDragFactor(float factor) { - float applied = Math::sqrt(factor); + float applied = Math::pow(factor, SOLVE_TWEAK); _dragFactor *= applied; - _wing->setDragScale(_wing->getDragScale() * applied); - _tail->setDragScale(_tail->getDragScale() * applied); + if(_wing) + _wing->setDragScale(_wing->getDragScale() * applied); + if(_tail) + _tail->setDragScale(_tail->getDragScale() * applied); int i; for(i=0; i<_vstabs.size(); i++) { Wing* w = (Wing*)_vstabs.get(i); @@ -762,10 +866,12 @@ void Airplane::applyDragFactor(float factor) void Airplane::applyLiftRatio(float factor) { - float applied = Math::sqrt(factor); + float applied = Math::pow(factor, SOLVE_TWEAK); _liftRatio *= applied; - _wing->setLiftRatio(_wing->getLiftRatio() * applied); - _tail->setLiftRatio(_tail->getLiftRatio() * applied); + if(_wing) + _wing->setLiftRatio(_wing->getLiftRatio() * applied); + if(_tail) + _tail->setLiftRatio(_tail->getLiftRatio() * applied); int i; for(i=0; i<_vstabs.size(); i++) { Wing* w = (Wing*)_vstabs.get(i); @@ -794,20 +900,11 @@ void Airplane::solve() float tmp[3]; _solutionIterations = 0; _failureMsg = 0; + while(1) { -#if 0 - printf("%d %f %f %f %f %f\n", //DEBUG - _solutionIterations, - 1000*_dragFactor, - _liftRatio, - _cruiseAoA, - _tailIncidence, - _approachElevator.val); -#endif - - if(_solutionIterations++ > 10000) { + if(_solutionIterations++ > 10000) { _failureMsg = "Solution failed to converge after 10000 iterations"; - return; + return; } // Run an iteration at cruise, and extract the needed numbers: @@ -817,19 +914,23 @@ void Airplane::solve() float thrust = tmp[0]; _model.getBody()->getAccel(tmp); + Math::tmul33(_cruiseState.orient, tmp, tmp); float xforce = _cruiseWeight * tmp[0]; float clift0 = _cruiseWeight * tmp[2]; _model.getBody()->getAngularAccel(tmp); + Math::tmul33(_cruiseState.orient, tmp, tmp); float pitch0 = tmp[1]; // Run an approach iteration, and do likewise runApproach(); _model.getBody()->getAngularAccel(tmp); + Math::tmul33(_approachState.orient, tmp, tmp); double apitch0 = tmp[1]; _model.getBody()->getAccel(tmp); + Math::tmul33(_approachState.orient, tmp, tmp); float alift = _approachWeight * tmp[2]; // Modify the cruise AoA a bit to get a derivative @@ -838,6 +939,7 @@ void Airplane::solve() _cruiseAoA -= ARCMIN; _model.getBody()->getAccel(tmp); + Math::tmul33(_cruiseState.orient, tmp, tmp); float clift1 = _cruiseWeight * tmp[2]; // Do the same with the tail incidence @@ -846,6 +948,7 @@ void Airplane::solve() _tail->setIncidence(_tailIncidence); _model.getBody()->getAngularAccel(tmp); + Math::tmul33(_cruiseState.orient, tmp, tmp); float pitch1 = tmp[1]; // Now calculate: @@ -870,6 +973,7 @@ void Airplane::solve() _approachElevator.val -= ELEVDIDDLE; _model.getBody()->getAngularAccel(tmp); + Math::tmul33(_approachState.orient, tmp, tmp); double apitch1 = tmp[1]; float elevDelta = -apitch0 * (ELEVDIDDLE/(apitch1-apitch0)); @@ -881,30 +985,30 @@ void Airplane::solve() applyLiftRatio(liftFactor); // DON'T do the following until the above are sane - if(normFactor(dragFactor) > 1.0001 - || normFactor(liftFactor) > 1.0001) + if(normFactor(dragFactor) > STHRESH*1.0001 + || normFactor(liftFactor) > STHRESH*1.0001) { continue; } // OK, now we can adjust the minor variables: - _cruiseAoA += 0.5f*aoaDelta; - _tailIncidence += 0.5f*tailDelta; + _cruiseAoA += SOLVE_TWEAK*aoaDelta; + _tailIncidence += SOLVE_TWEAK*tailDelta; _cruiseAoA = clamp(_cruiseAoA, -0.175f, 0.175f); _tailIncidence = clamp(_tailIncidence, -0.175f, 0.175f); - if(norm(dragFactor) < 1.00001 && - norm(liftFactor) < 1.00001 && - abs(aoaDelta) < .000017 && - abs(tailDelta) < .000017) + if(abs(xforce/_cruiseWeight) < STHRESH*0.0001 && + abs(alift/_approachWeight) < STHRESH*0.0001 && + abs(aoaDelta) < STHRESH*.000017 && + abs(tailDelta) < STHRESH*.000017) { // If this finaly value is OK, then we're all done - if(abs(elevDelta) < 0.0001) + if(abs(elevDelta) < STHRESH*0.0001) break; // Otherwise, adjust and do the next iteration - _approachElevator.val += 0.8 * elevDelta; + _approachElevator.val += SOLVE_TWEAK * elevDelta; if(abs(_approachElevator.val) > 1) { _failureMsg = "Insufficient elevator to trim for approach"; break; @@ -926,4 +1030,18 @@ void Airplane::solve() return; } } + +void Airplane::solveHelicopter() +{ + _solutionIterations = 0; + _failureMsg = 0; + + applyDragFactor(Math::pow(15.7/1000, 1/SOLVE_TWEAK)); + applyLiftRatio(Math::pow(104, 1/SOLVE_TWEAK)); + setupState(0,0, &_cruiseState); + _model.setState(&_cruiseState); + _controls.reset(); + _model.getBody()->reset(); +} + }; // namespace yasim