#include "Glue.hpp"
#include "RigidBody.hpp"
#include "Surface.hpp"
+#include "Rotorpart.hpp"
+#include "Rotorblade.hpp"
#include "Thruster.hpp"
-
#include "Airplane.hpp"
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;
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()
return ((Tank*)_tanks.get(tank))->density;
}
+float Airplane::getTankCapacity(int tank)
+{
+ return ((Tank*)_tanks.get(tank))->cap;
+}
+
void Airplane::setWeight(float weight)
{
_emptyWeight = weight;
_vstabs.add(vstab);
}
+void Airplane::addRotor(Rotor* rotor)
+{
+ _rotors.add(rotor);
+}
+
void Airplane::addFuselage(float* front, float* back, float width,
float taper, float mid)
{
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);
}
}
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; i<r->numRotorparts(); 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; i<r->numRotorblades(); 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
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;
// 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.
((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
// 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);
{
setupState(_cruiseAoA, _cruiseSpeed, &_cruiseState);
_model.setState(&_cruiseState);
- _model.setAir(_cruiseP, _cruiseT);
+ _model.setAir(_cruiseP, _cruiseT,
+ Atmosphere::calcStdDensity(_cruiseP, _cruiseT));
// The control configuration
_controls.reset();
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);
{
setupState(_approachAoA, _approachSpeed, &_approachState);
_model.setState(&_approachState);
- _model.setAir(_approachP, _approachT);
+ _model.setAir(_approachP, _approachT,
+ Atmosphere::calcStdDensity(_approachP, _approachT));
// The control configuration
_controls.reset();
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);
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);
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);
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:
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
_cruiseAoA -= ARCMIN;
_model.getBody()->getAccel(tmp);
+ Math::tmul33(_cruiseState.orient, tmp, tmp);
float clift1 = _cruiseWeight * tmp[2];
// Do the same with the tail incidence
_tail->setIncidence(_tailIncidence);
_model.getBody()->getAngularAccel(tmp);
+ Math::tmul33(_cruiseState.orient, tmp, tmp);
float pitch1 = tmp[1];
// Now calculate:
_approachElevator.val -= ELEVDIDDLE;
_model.getBody()->getAngularAccel(tmp);
+ Math::tmul33(_approachState.orient, tmp, tmp);
double apitch1 = tmp[1];
float elevDelta = -apitch0 * (ELEVDIDDLE/(apitch1-apitch0));
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;
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
projects / flightgear.git / blobdiff