#include "Glue.hpp"
#include "RigidBody.hpp"
#include "Surface.hpp"
+#include "Rotorpart.hpp"
+#include "Rotorblade.hpp"
#include "Thruster.hpp"
-
#include "Airplane.hpp"
+
namespace yasim {
-// FIXME: hook gear extension into the force calculation somehow...
+// gadgets
+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()
{
_wing = 0;
_tail = 0;
_ballast = 0;
- _cruiseRho = 0;
+ _cruiseP = 0;
+ _cruiseT = 0;
_cruiseSpeed = 0;
_cruiseWeight = 0;
- _approachRho = 0;
+ _approachP = 0;
+ _approachT = 0;
_approachSpeed = 0;
_approachAoA = 0;
_approachWeight = 0;
Airplane::~Airplane()
{
- for(int i=0; i<_fuselages.size(); i++)
+ int i;
+ for(i=0; i<_fuselages.size(); i++)
delete (Fuselage*)_fuselages.get(i);
- for(int i=0; i<_tanks.size(); i++)
+ for(i=0; i<_tanks.size(); i++)
delete (Tank*)_tanks.get(i);
- for(int i=0; i<_thrusters.size(); i++)
+ for(i=0; i<_thrusters.size(); i++)
delete (ThrustRec*)_thrusters.get(i);
- for(int i=0; i<_gears.size(); i++)
+ for(i=0; i<_gears.size(); i++)
delete (GearRec*)_gears.get(i);
- for(int i=0; i<_surfs.size(); i++)
+ for(i=0; i<_surfs.size(); i++)
delete (Surface*)_surfs.get(i);
+ for(i=0; i<_contacts.size(); i++)
+ delete[] (float*)_contacts.get(i);
}
void Airplane::iterate(float dt)
{
+ // The gear might have moved. Change their aerodynamics.
+ updateGearState();
+
_model.iterate();
+}
- // Consume fuel
- // FIXME
+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()
// Gravity
Glue::geodUp(s->pos, out);
- Math::mul3(-9.8, out, out);
+ Math::mul3(-9.8f, out, out);
// The regular acceleration
float tmp[3];
void Airplane::setPilotPos(float* pos)
{
- for(int i=0; i<3; i++) _pilotPos[i] = pos[i];
+ int i;
+ for(i=0; i<3; i++) _pilotPos[i] = pos[i];
}
void Airplane::getPilotPos(float* out)
{
- for(int i=0; i<3; i++) out[i] = _pilotPos[i];
+ int i;
+ for(i=0; i<3; i++) out[i] = _pilotPos[i];
}
int Airplane::numGear()
return ((GearRec*)_gears.get(g))->gear;
}
-void Airplane::setGearState(bool down, float dt)
+void Airplane::updateGearState()
{
for(int i=0; i<_gears.size(); i++) {
GearRec* gr = (GearRec*)_gears.get(i);
- if(gr->time == 0) {
- // Non-extensible
- gr->gear->setExtension(1);
- gr->surf->setXDrag(1);
- gr->surf->setYDrag(1);
- gr->surf->setZDrag(1);
- continue;
- }
-
- float diff = dt / gr->time;
- if(!down) diff = -diff;
- float ext = gr->gear->getExtension() + diff;
- if(ext < 0) ext = 0;
- if(ext > 1) ext = 1;
+ float ext = gr->gear->getExtension();
- gr->gear->setExtension(ext);
gr->surf->setXDrag(ext);
gr->surf->setYDrag(ext);
gr->surf->setZDrag(ext);
void Airplane::setApproach(float speed, float altitude, float aoa)
{
_approachSpeed = speed;
- _approachRho = Atmosphere::getStdDensity(altitude);
+ _approachP = Atmosphere::getStdPressure(altitude);
+ _approachT = Atmosphere::getStdTemperature(altitude);
_approachAoA = aoa;
}
void Airplane::setCruise(float speed, float altitude)
{
_cruiseSpeed = speed;
- _cruiseRho = Atmosphere::getStdDensity(altitude);
+ _cruiseP = Atmosphere::getStdPressure(altitude);
+ _cruiseT = Atmosphere::getStdTemperature(altitude);
_cruiseAoA = 0;
_tailIncidence = 0;
}
+void Airplane::setElevatorControl(int control)
+{
+ _approachElevator.control = control;
+ _approachElevator.val = 0;
+ _approachControls.add(&_approachElevator);
+}
+
void Airplane::addApproachControl(int control, float val)
{
Control* c = new Control();
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::addFuselage(float* front, float* back, float width)
+void Airplane::addRotor(Rotor* rotor)
+{
+ _rotors.add(rotor);
+}
+
+void Airplane::addFuselage(float* front, float* back, float width,
+ float taper, float mid)
{
Fuselage* f = new Fuselage();
- for(int i=0; i<3; i++) {
+ int i;
+ for(i=0; i<3; i++) {
f->front[i] = front[i];
f->back[i] = back[i];
}
f->width = width;
+ f->taper = taper;
+ f->mid = mid;
_fuselages.add(f);
}
int Airplane::addTank(float* pos, float cap, float density)
{
Tank* t = new Tank();
- for(int i=0; i<3; i++) t->pos[i] = pos[i];
+ int i;
+ for(i=0; i<3; i++) t->pos[i] = pos[i];
t->cap = cap;
t->fill = cap;
t->density = density;
return _tanks.add(t);
}
-void Airplane::addGear(Gear* gear, float transitionTime)
+void Airplane::addGear(Gear* gear)
{
GearRec* g = new GearRec();
g->gear = gear;
g->surf = 0;
- g->time = transitionTime;
_gears.add(g);
}
ThrustRec* t = new ThrustRec();
t->thruster = thruster;
t->mass = mass;
- for(int i=0; i<3; i++) t->cg[i] = cg[i];
+ int i;
+ for(i=0; i<3; i++) t->cg[i] = cg[i];
_thrusters.add(t);
}
wr->handle = _model.getBody()->addMass(0, pos);
wr->surf = new Surface();
+ wr->surf->setPosition(pos);
wr->surf->setTotalDrag(size*size);
_model.addSurface(wr->surf);
_surfs.add(wr->surf);
void Airplane::setFuelFraction(float frac)
{
- for(int i=0; i<_tanks.size(); i++) {
+ 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);
}
}
s->v[0] = speed; s->v[1] = 0; s->v[2] = 0;
- for(int i=0; i<3; i++)
+ int i;
+ for(i=0; i<3; i++)
s->pos[i] = s->rot[i] = s->acc[i] = s->racc[i] = 0;
// Put us 1m above the origin, or else the gravity computation in
s->pos[2] = 1;
}
+void Airplane::addContactPoint(float* pos)
+{
+ float* cp = new float[3];
+ cp[0] = pos[0];
+ cp[1] = pos[1];
+ cp[2] = pos[2];
+ _contacts.add(cp);
+}
+
float Airplane::compileWing(Wing* w)
{
+ // The tip of the wing is a contact point
+ float tip[3];
+ w->getTip(tip);
+ addContactPoint(tip);
+ if(w->isMirrored()) {
+ tip[1] *= -1;
+ addContactPoint(tip);
+ }
+
// Make sure it's initialized. The surfaces will pop out with
// total drag coefficients equal to their areas, which is what we
// want.
w->compile();
float wgt = 0;
- for(int i=0; i<w->numSurfaces(); i++) {
+ int i;
+ for(i=0; i<w->numSurfaces(); i++) {
Surface* s = (Surface*)w->getSurface(i);
+
+ float td = s->getTotalDrag();
+ s->setTotalDrag(td);
+
_model.addSurface(s);
+ float mass = w->getSurfaceWeight(i);
+ mass = mass * Math::sqrt(mass);
+ float pos[3];
+ s->getPosition(pos);
+ _model.getBody()->addMass(mass, pos);
+ wgt += mass;
+ }
+ 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(w->getSurfaceWeight(i), pos);
- wgt += w->getSurfaceWeight(i);
+ _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
+ addContactPoint(f->front);
+ addContactPoint(f->back);
+
float wgt = 0;
float fwd[3];
Math::sub3(f->front, f->back, fwd);
float len = Math::mag3(fwd);
float wid = f->width;
int segs = (int)Math::ceil(len/wid);
- float segWgt = wid*wid/segs;
- for(int j=0; j<segs; j++) {
- float frac = (j+0.5) / segs;
+ float segWgt = len*wid/segs;
+ int j;
+ for(j=0; j<segs; j++) {
+ float frac = (j+0.5f) / segs;
+
+ float scale = 1;
+ if(frac < f->mid)
+ scale = f->taper+(1-f->taper) * (frac / f->mid);
+ else
+ scale = f->taper+(1-f->taper) * (frac - f->mid) / (1 - f->mid);
+
+ // Where are we?
float pos[3];
Math::mul3(frac, fwd, pos);
Math::add3(f->back, pos, pos);
- _model.getBody()->addMass(segWgt, pos);
- wgt += segWgt;
+
+ // _Mass_ weighting goes as surface area^(3/2)
+ float mass = scale*segWgt * Math::sqrt(scale*segWgt);
+ _model.getBody()->addMass(mass, pos);
+ wgt += mass;
// Make a Surface too
Surface* s = new Surface();
float sideDrag = len/wid;
s->setYDrag(sideDrag);
s->setZDrag(sideDrag);
- s->setTotalDrag(segWgt);
+ s->setTotalDrag(scale*segWgt);
// FIXME: fails for fuselages aligned along the Y axis
float o[9];
Math::unit3(fwd, x);
y[0] = 0; y[1] = 1; y[2] = 0;
Math::cross3(x, y, z);
+ Math::unit3(z, z);
+ Math::cross3(z, x, y);
s->setOrientation(o);
_model.addSurface(s);
// Put the surface at the half-way point on the gear strut, give
// it a drag coefficient equal to a square of the same dimension
- // (gear are really draggy) and make it symmetric.
+ // (gear are really draggy) and make it symmetric. Assume that
+ // the "length" of the gear is 3x the compression distance
float pos[3], cmp[3];
g->getCompression(cmp);
- float length = Math::mag3(cmp);
+ float length = 3 * Math::mag3(cmp);
g->getPosition(pos);
Math::mul3(0.5, cmp, cmp);
Math::add3(pos, cmp, pos);
_surfs.add(s);
}
+void Airplane::compileContactPoints()
+{
+ // Figure it will compress by 20cm
+ float comp[3];
+ float DIST = 0.2f;
+ comp[0] = 0; comp[1] = 0; comp[2] = DIST;
+
+ // Give it a spring constant such that at full compression it will
+ // hold up 10 times the planes mass. That's about right. Yeah.
+ float mass = _model.getBody()->getTotalMass();
+ float spring = (1/DIST) * 9.8f * 10.0f * mass;
+ float damp = 2 * Math::sqrt(spring * mass);
+
+ int i;
+ for(i=0; i<_contacts.size(); i++) {
+ float *cp = (float*)_contacts.get(i);
+
+ Gear* g = new Gear();
+ g->setPosition(cp);
+
+ g->setCompression(comp);
+ g->setSpring(spring);
+ g->setDamping(damp);
+ g->setBrake(1);
+
+ // I made these up
+ g->setStaticFriction(0.6f);
+ g->setDynamicFriction(0.5f);
+
+ _model.addGear(g);
+ }
+}
+
void Airplane::compile()
{
double ground[3];
float aeroWgt = 0;
// The Wing objects
- aeroWgt += compileWing(_wing);
- aeroWgt += compileWing(_tail);
- for(int i=0; i<_vstabs.size(); i++) {
+ if (_wing)
+ aeroWgt += compileWing(_wing);
+ if (_tail)
+ aeroWgt += compileWing(_tail);
+ int 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(int 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;
- for(int i=0; i<_thrusters.size(); i++)
+ for(i=0; i<_thrusters.size(); i++)
nonAeroWgt += ((ThrustRec*)_thrusters.get(i))->mass;
// Rescale to the specified empty weight
float wscale = (_emptyWeight-nonAeroWgt)/aeroWgt;
- for(int i=firstMass; i<body->numMasses(); i++)
+ for(i=firstMass; i<body->numMasses(); i++)
body->setMass(i, body->getMass(i)*wscale);
// Add the thruster masses
- for(int i=0; i<_thrusters.size(); i++) {
+ for(i=0; i<_thrusters.size(); i++) {
ThrustRec* t = (ThrustRec*)_thrusters.get(i);
body->addMass(t->mass, t->cg);
}
// Add the tanks, empty for now.
float totalFuel = 0;
- for(int i=0; i<_tanks.size(); i++) {
+ for(i=0; i<_tanks.size(); i++) {
Tank* t = (Tank*)_tanks.get(i);
t->handle = body->addMass(0, t->pos);
totalFuel += t->cap;
}
- _cruiseWeight = _emptyWeight + totalFuel*0.5;
- _approachWeight = _emptyWeight + totalFuel*0.2;
+ _cruiseWeight = _emptyWeight + totalFuel*0.5f;
+ _approachWeight = _emptyWeight + totalFuel*0.2f;
body->recalc();
// Add surfaces for the landing gear.
- for(int i=0; i<_gears.size(); i++)
+ for(i=0; i<_gears.size(); i++)
compileGear((GearRec*)_gears.get(i));
// The Thruster objects
- for(int i=0; i<_thrusters.size(); i++) {
+ for(i=0; i<_thrusters.size(); i++) {
ThrustRec* tr = (ThrustRec*)_thrusters.get(i);
tr->handle = _model.addThruster(tr->thruster);
}
+ // Ground effect
+ float gepos[3];
+ float gespan = 0;
+ if(_wing)
+ gespan = _wing->getGroundEffect(gepos);
+ _model.setGroundEffect(gepos, gespan, 0.15f);
+
solveGear();
- solve();
+ if(_wing && _tail) solve();
+ else solveHelicopter();
- // Drop the gear (use a really big dt)
- setGearState(true, 1000000);
+ // Do this after solveGear, because it creates "gear" objects that
+ // we don't want to affect.
+ compileContactPoints();
}
void Airplane::solveGear()
// "buffer" to keep things from blowing up with aircraft with a
// single gear very near the c.g. (AV-8, for example).
float total = 0;
- for(int i=0; i<_gears.size(); i++) {
+ int i;
+ for(i=0; i<_gears.size(); i++) {
GearRec* gr = (GearRec*)_gears.get(i);
Gear* g = gr->gear;
g->getPosition(pos);
Math::sub3(cg, pos, pos);
- gr->wgt = 1/(0.5+Math::sqrt(pos[0]*pos[0] + pos[1]*pos[1]));
+ gr->wgt = 1.0f/(0.5f+Math::sqrt(pos[0]*pos[0] + pos[1]*pos[1]));
total += gr->wgt;
}
// Renormalize so they sum to 1
- for(int i=0; i<_gears.size(); i++)
+ for(i=0; i<_gears.size(); i++)
((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*_approachSpeed/19.1;
+ 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
// each gear.
- float energy = 0.5*_approachWeight*descentRate*descentRate;
+ float energy = 0.5f*_approachWeight*descentRate*descentRate;
- for(int i=0; i<_gears.size(); i++) {
+ for(i=0; i<_gears.size(); i++) {
GearRec* gr = (GearRec*)_gears.get(i);
float e = energy * gr->wgt;
float comp[3];
// 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.8);
- gr->gear->setDynamicFriction(0.7);
+ gr->gear->setStaticFriction(0.8f);
+ gr->gear->setDynamicFriction(0.7f);
}
}
-void Airplane::stabilizeThrust()
+void Airplane::initEngines()
{
- float thrust[3], tmp[3];
- float last = 0;
- while(1) {
- thrust[0] = thrust[1] = thrust[2] = 0;
- for(int i=0; i<_thrusters.size(); i++) {
- Thruster* t = _model.getThruster(i);
- t->integrate(0.033);
- t->getThrust(tmp);
- Math::add3(thrust, tmp, thrust);
- }
-
- float t = Math::mag3(thrust);
- float ratio = (t+0.1)/(last+0.1);
- if(ratio < 1.00001 && ratio > 0.99999)
- break;
-
- last = t;
+ for(int i=0; i<_thrusters.size(); i++) {
+ ThrustRec* tr = (ThrustRec*)_thrusters.get(i);
+ tr->thruster->init();
}
}
+void Airplane::stabilizeThrust()
+{
+ int i;
+ for(i=0; i<_thrusters.size(); i++)
+ _model.getThruster(i)->stabilize();
+}
+
void Airplane::runCruise()
{
setupState(_cruiseAoA, _cruiseSpeed, &_cruiseState);
_model.setState(&_cruiseState);
- _model.setAirDensity(_cruiseRho);
+ _model.setAir(_cruiseP, _cruiseT,
+ Atmosphere::calcStdDensity(_cruiseP, _cruiseT));
// The control configuration
_controls.reset();
- for(int i=0; i<_cruiseControls.size(); i++) {
+ int i;
+ for(i=0; i<_cruiseControls.size(); i++) {
Control* c = (Control*)_cruiseControls.get(i);
_controls.setInput(c->control, c->val);
}
- _controls.applyControls();
+ _controls.applyControls(1000000); // Huge dt value
// The local wind
float wind[3];
Math::mul3(-1, _cruiseState.v, wind);
Math::vmul33(_cruiseState.orient, wind, wind);
- // Gear are up (if they're non-retractable, this is a noop)
- setGearState(false, 100000);
-
// Cruise is by convention at 50% tank capacity
setFuelFraction(0.5);
// Set up the thruster parameters and iterate until the thrust
// stabilizes.
- for(int i=0; i<_thrusters.size(); i++) {
+ for(i=0; i<_thrusters.size(); i++) {
Thruster* t = ((ThrustRec*)_thrusters.get(i))->thruster;
t->setWind(wind);
- t->setDensity(_cruiseRho);
+ 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.setAirDensity(_approachRho);
+ _model.setAir(_approachP, _approachT,
+ Atmosphere::calcStdDensity(_approachP, _approachT));
// The control configuration
_controls.reset();
- for(int i=0; i<_approachControls.size(); i++) {
+ int i;
+ for(i=0; i<_approachControls.size(); i++) {
Control* c = (Control*)_approachControls.get(i);
_controls.setInput(c->control, c->val);
}
- _controls.applyControls();
+ _controls.applyControls(1000000);
// The local wind
float wind[3];
Math::vmul33(_approachState.orient, wind, wind);
// Approach is by convention at 20% tank capacity
- setFuelFraction(0.2);
-
- // Gear are down
- setGearState(true, 100000);
+ setFuelFraction(0.2f);
// Run the thrusters until they get to a stable setting. FIXME:
// this is lots of wasted work.
- for(int i=0; i<_thrusters.size(); i++) {
+ for(i=0; i<_thrusters.size(); i++) {
Thruster* t = ((ThrustRec*)_thrusters.get(i))->thruster;
t->setWind(wind);
- t->setDensity(_approachRho);
+ 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);
- for(int i=0; i<_vstabs.size(); i++) {
+ 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);
w->setDragScale(w->getDragScale() * applied);
}
- for(int i=0; i<_surfs.size(); i++) {
+ for(i=0; i<_surfs.size(); i++) {
Surface* s = (Surface*)_surfs.get(i);
s->setTotalDrag(s->getTotalDrag() * applied);
}
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);
- for(int i=0; i<_vstabs.size(); i++) {
+ 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);
w->setLiftRatio(w->getLiftRatio() * applied);
}
void Airplane::solve()
{
- static const float ARCMIN = 0.0002909;
+ static const float ARCMIN = 0.0002909f;
float tmp[3];
_solutionIterations = 0;
_failureMsg = 0;
+
while(1) {
- 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:
- float awgt = 9.8 * _approachWeight;
+ float awgt = 9.8f * _approachWeight;
float dragFactor = thrust / (thrust-xforce);
float liftFactor = awgt / (awgt+alift);
float tailDelta = -pitch0 * (ARCMIN/(pitch1-pitch0));
// Sanity:
- if(dragFactor <= 0) {
- _failureMsg = "Zero or negative drag adjustment.";
- return;
- } else if(liftFactor <= 0) {
- _failureMsg = "Zero or negative lift adjustment.";
- return;
- }
+ if(dragFactor <= 0 || liftFactor <= 0)
+ break;
+
+ // And the elevator control in the approach. This works just
+ // like the tail incidence computation (it's solving for the
+ // same thing -- pitching moment -- by diddling a different
+ // variable).
+ const float ELEVDIDDLE = 0.001f;
+ _approachElevator.val += ELEVDIDDLE;
+ runApproach();
+ _approachElevator.val -= ELEVDIDDLE;
+
+ _model.getBody()->getAngularAccel(tmp);
+ Math::tmul33(_approachState.orient, tmp, tmp);
+ double apitch1 = tmp[1];
+ float elevDelta = -apitch0 * (ELEVDIDDLE/(apitch1-apitch0));
+
+ // Now apply the values we just computed. Note that the
+ // "minor" variables are deferred until we get the lift/drag
+ // numbers in the right ballpark.
- // And apply:
applyDragFactor(dragFactor);
applyLiftRatio(liftFactor);
// DON'T do the following until the above are sane
- if(normFactor(dragFactor) > 1.1
- || normFactor(liftFactor) > 1.1)
+ if(normFactor(dragFactor) > STHRESH*1.0001
+ || normFactor(liftFactor) > STHRESH*1.0001)
{
continue;
}
- // OK, now we can adjust the minor variables
- _cruiseAoA += 0.5*aoaDelta;
- _tailIncidence += 0.5*tailDelta;
+ // OK, now we can adjust the minor variables:
+ _cruiseAoA += SOLVE_TWEAK*aoaDelta;
+ _tailIncidence += SOLVE_TWEAK*tailDelta;
- _cruiseAoA = clamp(_cruiseAoA, -.174, .174);
- _tailIncidence = clamp(_tailIncidence, -.174, .174);
+ _cruiseAoA = clamp(_cruiseAoA, -0.175f, 0.175f);
+ _tailIncidence = clamp(_tailIncidence, -0.175f, 0.175f);
- if(dragFactor < 1.00001 && liftFactor < 1.00001 &&
- aoaDelta < .000017 && tailDelta < .000017)
+ if(abs(xforce/_cruiseWeight) < STHRESH*0.0001 &&
+ abs(alift/_approachWeight) < STHRESH*0.0001 &&
+ abs(aoaDelta) < STHRESH*.000017 &&
+ abs(tailDelta) < STHRESH*.000017)
{
- break;
+ // If this finaly value is OK, then we're all done
+ if(abs(elevDelta) < STHRESH*0.0001)
+ break;
+
+ // Otherwise, adjust and do the next iteration
+ _approachElevator.val += SOLVE_TWEAK * elevDelta;
+ if(abs(_approachElevator.val) > 1) {
+ _failureMsg = "Insufficient elevator to trim for approach";
+ break;
+ }
}
}
} else if(_liftRatio < 1e-04 || _liftRatio > 1e4) {
_failureMsg = "Lift ratio beyond reasonable bounds.";
return;
- } else if(Math::abs(_cruiseAoA) >= .174) {
+ } else if(Math::abs(_cruiseAoA) >= .17453293) {
_failureMsg = "Cruise AoA > 10 degrees";
return;
- } else if(Math::abs(_tailIncidence) >= .174) {
+ } else if(Math::abs(_tailIncidence) >= .17453293) {
_failureMsg = "Tail incidence > 10 degrees";
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