diff --git a/src/FDM/Makefile.am b/src/FDM/Makefile.am index c711e0b85..696f8d72e 100644 --- a/src/FDM/Makefile.am +++ b/src/FDM/Makefile.am @@ -1,4 +1,4 @@ -SUBDIRS = Balloon JSBSim LaRCsim UIUCModel +SUBDIRS = Balloon JSBSim LaRCsim UIUCModel YASim noinst_LIBRARIES = libFlight.a diff --git a/src/FDM/YASim/Airplane.cpp b/src/FDM/YASim/Airplane.cpp new file mode 100644 index 000000000..93a822a3e --- /dev/null +++ b/src/FDM/YASim/Airplane.cpp @@ -0,0 +1,799 @@ +#include "Atmosphere.hpp" +#include "ControlMap.hpp" +#include "Gear.hpp" +#include "Math.hpp" +#include "Glue.hpp" +#include "RigidBody.hpp" +#include "Surface.hpp" +#include "Thruster.hpp" + +#include "Airplane.hpp" +namespace yasim { + +// FIXME: hook gear extension into the force calculation somehow... + +Airplane::Airplane() +{ + _emptyWeight = 0; + _pilotPos[0] = _pilotPos[1] = _pilotPos[2] = 0; + _wing = 0; + _tail = 0; + _ballast = 0; + _cruiseRho = 0; + _cruiseSpeed = 0; + _cruiseWeight = 0; + _approachRho = 0; + _approachSpeed = 0; + _approachAoA = 0; + _approachWeight = 0; + + _dragFactor = 1; + _liftRatio = 1; + _cruiseAoA = 0; + _tailIncidence = 0; +} + +Airplane::~Airplane() +{ + for(int i=0; i<_fuselages.size(); i++) + delete (Fuselage*)_fuselages.get(i); + for(int i=0; i<_tanks.size(); i++) + delete (Tank*)_tanks.get(i); + for(int i=0; i<_thrusters.size(); i++) + delete (ThrustRec*)_thrusters.get(i); + for(int i=0; i<_gears.size(); i++) + delete (GearRec*)_gears.get(i); + for(int i=0; i<_surfs.size(); i++) + delete (Surface*)_surfs.get(i); +} + +void Airplane::iterate(float dt) +{ + _model.iterate(); + + // Consume fuel + // FIXME +} + +ControlMap* Airplane::getControlMap() +{ + return &_controls; +} + +Model* Airplane::getModel() +{ + return &_model; +} + +void Airplane::getPilotAccel(float* out) +{ + State* s = _model.getState(); + + // Gravity + Glue::geodUp(s->pos, out); + Math::mul3(-9.8, out, out); + + // The regular acceleration + float tmp[3]; + Math::mul3(-1, s->acc, tmp); + Math::add3(tmp, out, out); + + // Convert to aircraft coordinates + Math::vmul33(s->orient, out, out); + + // FIXME: rotational & centripetal acceleration needed +} + +void Airplane::setPilotPos(float* pos) +{ + for(int 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 Airplane::numGear() +{ + return _gears.size(); +} + +Gear* Airplane::getGear(int g) +{ + return ((GearRec*)_gears.get(g))->gear; +} + +void Airplane::setGearState(bool down, float dt) +{ + 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; + + gr->gear->setExtension(ext); + gr->surf->setXDrag(ext); + gr->surf->setYDrag(ext); + gr->surf->setZDrag(ext); + } +} + +void Airplane::setApproach(float speed, float altitude) +{ + // The zero AoA will become a calculated stall AoA in compile() + setApproach(speed, altitude, 0); +} + +void Airplane::setApproach(float speed, float altitude, float aoa) +{ + _approachSpeed = speed; + _approachRho = Atmosphere::getStdDensity(altitude); + _approachAoA = aoa; +} + +void Airplane::setCruise(float speed, float altitude) +{ + _cruiseSpeed = speed; + _cruiseRho = Atmosphere::getStdDensity(altitude); + _cruiseAoA = 0; + _tailIncidence = 0; +} + +void Airplane::addApproachControl(int control, float val) +{ + Control* c = new Control(); + c->control = control; + c->val = val; + _approachControls.add(c); +} + +void Airplane::addCruiseControl(int control, float val) +{ + Control* c = new Control(); + c->control = control; + c->val = val; + _cruiseControls.add(c); +} + +int Airplane::numTanks() +{ + return _tanks.size(); +} + +float Airplane::getFuel(int tank) +{ + return ((Tank*)_tanks.get(tank))->fill; +} + +float Airplane::getFuelDensity(int tank) +{ + return ((Tank*)_tanks.get(tank))->density; +} + +void Airplane::setWeight(float weight) +{ + _emptyWeight = weight; +} + +void Airplane::setWing(Wing* wing) +{ + _wing = wing; +} + +void Airplane::setTail(Wing* tail) +{ + _tail = tail; +} + +void Airplane::addVStab(Wing* vstab) +{ + _vstabs.add(vstab); +} + +void Airplane::addFuselage(float* front, float* back, float width) +{ + Fuselage* f = new Fuselage(); + for(int i=0; i<3; i++) { + f->front[i] = front[i]; + f->back[i] = back[i]; + } + f->width = width; + _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]; + t->cap = cap; + t->fill = cap; + t->density = density; + t->handle = 0xffffffff; + return _tanks.add(t); +} + +void Airplane::addGear(Gear* gear, float transitionTime) +{ + GearRec* g = new GearRec(); + g->gear = gear; + g->surf = 0; + g->time = transitionTime; + _gears.add(g); +} + +void Airplane::addThruster(Thruster* thruster, float mass, float* cg) +{ + ThrustRec* t = new ThrustRec(); + t->thruster = thruster; + t->mass = mass; + for(int i=0; i<3; i++) t->cg[i] = cg[i]; + _thrusters.add(t); +} + +void Airplane::addBallast(float* pos, float mass) +{ + _model.getBody()->addMass(mass, pos); + _ballast += mass; +} + +int Airplane::addWeight(float* pos, float size) +{ + WeightRec* wr = new WeightRec(); + wr->handle = _model.getBody()->addMass(0, pos); + + wr->surf = new Surface(); + wr->surf->setTotalDrag(size*size); + _model.addSurface(wr->surf); + _surfs.add(wr->surf); + + return _weights.add(wr); +} + +void Airplane::setWeight(int handle, float mass) +{ + WeightRec* wr = (WeightRec*)_weights.get(handle); + + _model.getBody()->setMass(wr->handle, mass); + + // Kill the aerodynamic drag if the mass is exactly zero. This is + // how we simulate droppable stores. + if(mass == 0) { + wr->surf->setXDrag(0); + wr->surf->setYDrag(0); + wr->surf->setZDrag(0); + } else { + wr->surf->setXDrag(1); + wr->surf->setYDrag(1); + wr->surf->setZDrag(1); + } +} + +void Airplane::setFuelFraction(float frac) +{ + for(int i=0; i<_tanks.size(); i++) { + Tank* t = (Tank*)_tanks.get(i); + _model.getBody()->setMass(t->handle, t->cap * frac); + } +} + +float Airplane::getDragCoefficient() +{ + return _dragFactor; +} + +float Airplane::getLiftRatio() +{ + return _liftRatio; +} + +float Airplane::getCruiseAoA() +{ + return _cruiseAoA; +} + +float Airplane::getTailIncidence() +{ + return _tailIncidence; +} + +char* Airplane::getFailureMsg() +{ + return _failureMsg; +} + +int Airplane::getSolutionIterations() +{ + return _solutionIterations; +} + +void Airplane::setupState(float aoa, float speed, State* s) +{ + float cosAoA = Math::cos(aoa); + float sinAoA = Math::sin(aoa); + s->orient[0] = cosAoA; s->orient[1] = 0; s->orient[2] = sinAoA; + s->orient[3] = 0; s->orient[4] = 1; s->orient[5] = 0; + s->orient[6] = -sinAoA; s->orient[7] = 0; s->orient[8] = cosAoA; + + s->v[0] = speed; s->v[1] = 0; s->v[2] = 0; + + for(int 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 + // Model goes nuts + s->pos[2] = 1; +} + +float Airplane::compileWing(Wing* w) +{ + // 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++) { + Surface* s = (Surface*)w->getSurface(i); + _model.addSurface(s); + + float pos[3]; + s->getPosition(pos); + _model.getBody()->addMass(w->getSurfaceWeight(i), pos); + wgt += w->getSurfaceWeight(i); + } + return wgt; +} + +float Airplane::compileFuselage(Fuselage* f) +{ + 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 pos[3]; + Math::mul3(frac, fwd, pos); + Math::add3(f->back, pos, pos); + _model.getBody()->addMass(segWgt, pos); + wgt += segWgt; + + // Make a Surface too + Surface* s = new Surface(); + s->setPosition(pos); + float sideDrag = len/wid; + s->setYDrag(sideDrag); + s->setZDrag(sideDrag); + s->setTotalDrag(segWgt); + + // FIXME: fails for fuselages aligned along the Y axis + float o[9]; + float *x=o, *y=o+3, *z=o+6; // nicknames for the axes + Math::unit3(fwd, x); + y[0] = 0; y[1] = 1; y[2] = 0; + Math::cross3(x, y, z); + s->setOrientation(o); + + _model.addSurface(s); + _surfs.add(s); + } + return wgt; +} + +// FIXME: should probably add a mass for the gear, too +void Airplane::compileGear(GearRec* gr) +{ + Gear* g = gr->gear; + + // Make a Surface object for the aerodynamic behavior + Surface* s = new Surface(); + gr->surf = 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. + float pos[3], cmp[3]; + g->getCompression(cmp); + float length = Math::mag3(cmp); + g->getPosition(pos); + Math::mul3(0.5, cmp, cmp); + Math::add3(pos, cmp, pos); + + s->setPosition(pos); + s->setTotalDrag(length*length); + + _model.addGear(g); + _model.addSurface(s); + _surfs.add(s); +} + +void Airplane::compile() +{ + double ground[3]; + ground[0] = 0; ground[1] = 0; ground[2] = 1; + _model.setGroundPlane(ground, -100000); + + RigidBody* body = _model.getBody(); + int firstMass = body->numMasses(); + + // Generate the point masses for the plane. Just use unitless + // numbers for a first pass, then go back through and rescale to + // make the weight right. + float aeroWgt = 0; + + // The Wing objects + aeroWgt += compileWing(_wing); + aeroWgt += compileWing(_tail); + for(int i=0; i<_vstabs.size(); i++) { + aeroWgt += compileWing((Wing*)_vstabs.get(i)); + } + + // The fuselage(s) + for(int 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++) + 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++) + body->setMass(i, body->getMass(i)*wscale); + + // Add the thruster masses + for(int 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++) { + 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; + + body->recalc(); + + // Add surfaces for the landing gear. + for(int i=0; i<_gears.size(); i++) + compileGear((GearRec*)_gears.get(i)); + + // The Thruster objects + for(int i=0; i<_thrusters.size(); i++) { + ThrustRec* tr = (ThrustRec*)_thrusters.get(i); + tr->handle = _model.addThruster(tr->thruster); + } + + solveGear(); + solve(); + + // Drop the gear (use a really big dt) + setGearState(true, 1000000); +} + +void Airplane::solveGear() +{ + float cg[3], pos[3]; + _model.getBody()->getCG(cg); + + // Calculate spring constant weightings for the gear. Weight by + // the inverse of the distance to the c.g. in the XY plane, which + // should be correct for most gear arrangements. Add 50cm of + // "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++) { + 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])); + total += gr->wgt; + } + + // Renormalize so they sum to 1 + for(int 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 + // a 3 degree approach. + float descentRate = 3*_approachSpeed/19.1; + + // 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; + + for(int i=0; i<_gears.size(); i++) { + GearRec* gr = (GearRec*)_gears.get(i); + float e = energy * gr->wgt; + float comp[3]; + gr->gear->getCompression(comp); + float len = Math::mag3(comp); + + // Energy in a spring: e = 0.5 * k * len^2 + float k = 2 * e / (len*len); + + gr->gear->setSpring(k); + + // Critically damped (too damped, too!) + gr->gear->setDamping(2*Math::sqrt(k*_approachWeight*gr->wgt)); + + // These are pretty generic + gr->gear->setStaticFriction(0.8); + gr->gear->setDynamicFriction(0.7); + } +} + +void Airplane::stabilizeThrust() +{ + 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; + } +} + +void Airplane::runCruise() +{ + setupState(_cruiseAoA, _cruiseSpeed, &_cruiseState); + _model.setState(&_cruiseState); + _model.setAirDensity(_cruiseRho); + + // The control configuration + _controls.reset(); + for(int i=0; i<_cruiseControls.size(); i++) { + Control* c = (Control*)_cruiseControls.get(i); + _controls.setInput(c->control, c->val); + } + _controls.applyControls(); + + // 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++) { + Thruster* t = ((ThrustRec*)_thrusters.get(i))->thruster; + t->setWind(wind); + t->setDensity(_cruiseRho); + } + stabilizeThrust(); + + // Precompute thrust in the model, and calculate aerodynamic forces + _model.getBody()->reset(); + _model.initIteration(); + _model.calcForces(&_cruiseState); +} + +void Airplane::runApproach() +{ + setupState(_approachAoA, _approachSpeed, &_approachState); + _model.setState(&_approachState); + _model.setAirDensity(_approachRho); + + // The control configuration + _controls.reset(); + for(int i=0; i<_approachControls.size(); i++) { + Control* c = (Control*)_approachControls.get(i); + _controls.setInput(c->control, c->val); + } + _controls.applyControls(); + + // The local wind + float wind[3]; + Math::mul3(-1, _approachState.v, wind); + Math::vmul33(_approachState.orient, wind, wind); + + // Approach is by convention at 20% tank capacity + setFuelFraction(0.2); + + // Gear are down + setGearState(true, 100000); + + // 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++) { + Thruster* t = ((ThrustRec*)_thrusters.get(i))->thruster; + t->setWind(wind); + t->setDensity(_approachRho); + } + stabilizeThrust(); + + // Precompute thrust in the model, and calculate aerodynamic forces + _model.getBody()->reset(); + _model.initIteration(); + _model.calcForces(&_approachState); +} + +void Airplane::applyDragFactor(float factor) +{ + float applied = Math::sqrt(factor); + _dragFactor *= applied; + _wing->setDragScale(_wing->getDragScale() * applied); + _tail->setDragScale(_tail->getDragScale() * applied); + for(int 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++) { + Surface* s = (Surface*)_surfs.get(i); + s->setTotalDrag(s->getTotalDrag() * applied); + } +} + +void Airplane::applyLiftRatio(float factor) +{ + float applied = Math::sqrt(factor); + _liftRatio *= applied; + _wing->setLiftRatio(_wing->getLiftRatio() * applied); + _tail->setLiftRatio(_tail->getLiftRatio() * applied); + for(int i=0; i<_vstabs.size(); i++) { + Wing* w = (Wing*)_vstabs.get(i); + w->setLiftRatio(w->getLiftRatio() * applied); + } +} + +float Airplane::clamp(float val, float min, float max) +{ + if(val < min) return min; + if(val > max) return max; + return val; +} + +float Airplane::normFactor(float f) +{ + if(f < 0) f = -f; + if(f < 1) f = 1/f; + return f; +} + +void Airplane::solve() +{ + static const float ARCMIN = 0.0002909; + + float tmp[3]; + _solutionIterations = 0; + _failureMsg = 0; + while(1) { + if(_solutionIterations++ > 10000) { + _failureMsg = "Solution failed to converge after 10000 iterations"; + return; + } + + // Run an iteration at cruise, and extract the needed numbers: + runCruise(); + + _model.getThrust(tmp); + float thrust = tmp[0]; + + _model.getBody()->getAccel(tmp); + float xforce = _cruiseWeight * tmp[0]; + float clift0 = _cruiseWeight * tmp[2]; + + _model.getBody()->getAngularAccel(tmp); + float pitch0 = tmp[1]; + + // Run an approach iteration, and do likewise + runApproach(); + + _model.getBody()->getAccel(tmp); + float alift = _approachWeight * tmp[2]; + + // Modify the cruise AoA a bit to get a derivative + _cruiseAoA += ARCMIN; + runCruise(); + _cruiseAoA -= ARCMIN; + + _model.getBody()->getAccel(tmp); + float clift1 = _cruiseWeight * tmp[2]; + + // Do the same with the tail incidence + _tail->setIncidence(_tailIncidence + ARCMIN); + runCruise(); + _tail->setIncidence(_tailIncidence); + + _model.getBody()->getAngularAccel(tmp); + float pitch1 = tmp[1]; + + // Now calculate: + float awgt = 9.8 * _approachWeight; + + float dragFactor = thrust / (thrust-xforce); + float liftFactor = awgt / (awgt+alift); + float aoaDelta = -clift0 * (ARCMIN/(clift1-clift0)); + 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; + } + + // 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) + { + continue; + } + + // OK, now we can adjust the minor variables + _cruiseAoA += 0.5*aoaDelta; + _tailIncidence += 0.5*tailDelta; + + _cruiseAoA = clamp(_cruiseAoA, -.174, .174); + _tailIncidence = clamp(_tailIncidence, -.174, .174); + + if(dragFactor < 1.00001 && liftFactor < 1.00001 && + aoaDelta < .000017 && tailDelta < .000017) + { + break; + } + } + + if(_dragFactor < 1e-06 || _dragFactor > 1e6) { + _failureMsg = "Drag factor beyond reasonable bounds."; + return; + } else if(_liftRatio < 1e-04 || _liftRatio > 1e4) { + _failureMsg = "Lift ratio beyond reasonable bounds."; + return; + } else if(Math::abs(_cruiseAoA) >= .174) { + _failureMsg = "Cruise AoA > 10 degrees"; + return; + } else if(Math::abs(_tailIncidence) >= .174) { + _failureMsg = "Tail incidence > 10 degrees"; + return; + } +} +}; // namespace yasim diff --git a/src/FDM/YASim/Airplane.hpp b/src/FDM/YASim/Airplane.hpp new file mode 100644 index 000000000..250d1e15e --- /dev/null +++ b/src/FDM/YASim/Airplane.hpp @@ -0,0 +1,135 @@ +#ifndef _AIRPLANE_HPP +#define _AIRPLANE_HPP + +#include "ControlMap.hpp" +#include "Model.hpp" +#include "Wing.hpp" +#include "util/Vector.hpp" + +namespace yasim { + +class Gear; +class Thruster; + +class Airplane { +public: + Airplane(); + ~Airplane(); + + void iterate(float dt); + + ControlMap* getControlMap(); + Model* getModel(); + + void setPilotPos(float* pos); + void getPilotPos(float* out); + + void getPilotAccel(float* out); + + void setWeight(float weight); + + void setWing(Wing* wing); + void setTail(Wing* tail); + void addVStab(Wing* vstab); + + void addFuselage(float* front, float* back, float width); + int addTank(float* pos, float cap, float fuelDensity); + void addGear(Gear* g, float transitionTime); + void addThruster(Thruster* t, float mass, float* cg); + void addBallast(float* pos, float mass); + + int addWeight(float* pos, float size); + void setWeight(int handle, float mass); + + void setApproach(float speed, float altitude); + void setApproach(float speed, float altitude, float aoa); + void setCruise(float speed, float altitude); + + void addApproachControl(int control, float val); + void addCruiseControl(int control, float val); + + int numGear(); + Gear* getGear(int g); + void setGearState(bool down, float dt); + + int numTanks(); + void setFuelFraction(float frac); // 0-1, total amount of fuel + float getFuel(int tank); // in kg! + float getFuelDensity(int tank); // kg/m^3 + + void compile(); // generate point masses & such, then solve + + // Solution output values + int getSolutionIterations(); + float getDragCoefficient(); + float getLiftRatio(); + float getCruiseAoA(); + float getTailIncidence(); + char* getFailureMsg(); + +private: + struct Tank { float pos[3]; float cap; float fill; + float density; int handle; }; + struct Fuselage { float front[3], back[3], width; }; + struct GearRec { Gear* gear; Surface* surf; float wgt; float time; }; + struct ThrustRec { Thruster* thruster; + int handle; float cg[3]; float mass; }; + struct Control { int control; float val; }; + struct WeightRec { int handle; Surface* surf; }; + + void runCruise(); + void runApproach(); + void setupState(float aoa, float speed, State* s); + void solveGear(); + void solve(); + float compileWing(Wing* w); + float compileFuselage(Fuselage* f); + void compileGear(GearRec* gr); + void stabilizeThrust(); + void applyDragFactor(float factor); + void applyLiftRatio(float factor); + float clamp(float val, float min, float max); + float normFactor(float f); + + Model _model; + ControlMap _controls; + + float _emptyWeight; + float _pilotPos[3]; + + Wing* _wing; + Wing* _tail; + + Vector _fuselages; + Vector _vstabs; + Vector _tanks; + Vector _thrusters; + float _ballast; + + Vector _gears; + Vector _weights; + Vector _surfs; // NON-wing Surfaces + + Vector _cruiseControls; + State _cruiseState; + float _cruiseRho; + float _cruiseSpeed; + float _cruiseWeight; + + Vector _approachControls; + State _approachState; + float _approachRho; + float _approachSpeed; + float _approachAoA; + float _approachWeight; + + int _solutionIterations; + float _dragFactor; + float _liftRatio; + float _cruiseAoA; + float _tailIncidence; + char* _failureMsg; +}; + +}; // namespace yasim +#endif // _AIRPLANE_HPP diff --git a/src/FDM/YASim/Atmosphere.cpp b/src/FDM/YASim/Atmosphere.cpp new file mode 100644 index 000000000..c331b2faf --- /dev/null +++ b/src/FDM/YASim/Atmosphere.cpp @@ -0,0 +1,120 @@ +#include "Math.hpp" +#include "Atmosphere.hpp" +namespace yasim { + +// Copied from McCormick, who got it from "The ARDC Model Atmosphere" +// Note that there's an error in the text in the first entry, +// McCormick lists 299.16/101325/1.22500, but those don't agree with +// R=287. I chose to correct the temperature to 288.20, since 79F is +// pretty hot for a "standard" atmosphere. +// meters kelvin kPa kg/m^3 +float Atmosphere::data[][4] = {{ 0, 288.20, 101325, 1.22500 }, + { 900, 282.31, 90971, 1.12260 }, + { 1800, 276.46, 81494, 1.02690 }, + { 2700, 270.62, 72835, 0.93765 }, + { 3600, 264.77, 64939, 0.85445 }, + { 4500, 258.93, 57752, 0.77704 }, + { 5400, 253.09, 51226, 0.70513 }, + { 6300, 247.25, 45311, 0.63845 }, + { 7200, 241.41, 39963, 0.57671 }, + { 8100, 235.58, 35140, 0.51967 }, + { 9000, 229.74, 30800, 0.46706 }, + { 9900, 223.91, 26906, 0.41864 }, + { 10800, 218.08, 23422, 0.37417 }, + { 11700, 216.66, 20335, 0.32699 }, + { 12600, 216.66, 17654, 0.28388 }, + { 13500, 216.66, 15327, 0.24646 }, + { 14400, 216.66, 13308, 0.21399 }, + { 15300, 216.66, 11555, 0.18580 }, + { 16200, 216.66, 10033, 0.16133 }, + { 17100, 216.66, 8712, 0.14009 }, + { 18000, 216.66, 7565, 0.12165 }, + { 18900, 216.66, 6570, 0.10564 }}; + +float Atmosphere::getStdTemperature(float alt) +{ + return getRecord(alt, 1); +} + +float Atmosphere::getStdPressure(float alt) +{ + return getRecord(alt, 2); +} + +float Atmosphere::getStdDensity(float alt) +{ + return getRecord(alt, 3); +} + +float Atmosphere::calcVEAS(float spd, float pressure, float temp) +{ + return 0; //FIXME +} + +float Atmosphere::calcVCAS(float spd, float pressure, float temp) +{ + // Stolen shamelessly from JSBSim. Constants that appear: + // 2/5 == gamma-1 + // 5/12 == 1/(gamma+1) + // 4/5 == 2*(gamma-1) + // 14/5 == 2*gamma + // 28/5 == 4*gamma + // 144/25 == (gamma+1)^2 + + float m2 = calcMach(spd, temp); + m2 = m2*m2; // mach^2 + + float cp; // pressure coefficient + if(m2 < 1) { + // (1+(mach^2)/5)^(gamma/(gamma-1)) + cp = Math::pow(1+0.2*m2, 3.5); + } else { + float tmp0 = ((144/25.) * m2) / (28/5.*m2 - 4/5.); + float tmp1 = ((14/5.) * m2 - (2/5.)) * (5/12.); + cp = Math::pow(tmp0, 3.5) * tmp1; + } + + // Conditions at sea level + float p0 = getStdPressure(0); + float rho0 = getStdDensity(0); + + float tmp = Math::pow((pressure/p0)*(cp-1) + 1, (2/7.)); + return Math::sqrt((7*p0/rho0)*(tmp-1)); +} + +float Atmosphere::calcDensity(float pressure, float temp) +{ + // P = rho*R*T, R == 287 kPa*m^3 per kg*kelvin for air + return pressure / (287 * temp); +} + +float Atmosphere::calcMach(float spd, float temp) +{ + return spd / Math::sqrt(1.4 * 287 * temp); +} + +float Atmosphere::getRecord(float alt, int recNum) +{ + int hi = (sizeof(data) / (4*sizeof(float))) - 1; + int lo = 0; + + // safety valve, clamp to the edges of the table + if(alt < data[0][0]) hi=1; + else if(alt > data[hi][0]) lo = hi-1; + + // binary search + while(1) { + if(hi-lo == 1) break; + int mid = (hi+lo)>>1; + if(alt < data[mid][0]) hi = mid; + else lo = mid; + } + + // interpolate + float frac = (alt - data[lo][0])/(data[hi][0] - data[lo][0]); + float a = data[lo][recNum]; + float b = data[hi][recNum]; + return a + frac * (b-a); +} + +}; // namespace yasim diff --git a/src/FDM/YASim/Atmosphere.hpp b/src/FDM/YASim/Atmosphere.hpp new file mode 100644 index 000000000..2e9c0512f --- /dev/null +++ b/src/FDM/YASim/Atmosphere.hpp @@ -0,0 +1,23 @@ +#ifndef _ATMOSPHERE_HPP +#define _ATMOSPHERE_HPP + +namespace yasim { + +class Atmosphere { +public: + static float getStdTemperature(float alt); + static float getStdPressure(float alt); + static float getStdDensity(float alt); + + static float calcVCAS(float spd, float pressure, float temp); + static float calcVEAS(float spd, float pressure, float temp); + static float calcMach(float spd, float temp); + static float calcDensity(float pressure, float temp); + +private: + static float getRecord(float alt, int idx); + static float data[][4]; +}; + +}; // namespace yasim +#endif // _ATMOSPHERE_HPP diff --git a/src/FDM/YASim/BodyEnvironment.hpp b/src/FDM/YASim/BodyEnvironment.hpp new file mode 100644 index 000000000..3d6d38efc --- /dev/null +++ b/src/FDM/YASim/BodyEnvironment.hpp @@ -0,0 +1,59 @@ +#ifndef _BODYENVIRONMENT_HPP +#define _BODYENVIRONMENT_HPP + +#include "RigidBody.hpp" + +namespace yasim { + +// The values for position and orientation within the global reference +// frame, along with their first and second time derivatives. The +// orientation is stored as a matrix for simplicity. Note that +// because it is orthonormal, its inverse is simply its transpose. +// You can get local->global transformations by calling Math::tmul33() +// and using the same matrix. +struct State { + double pos[3]; // position + float orient[9]; // global->local xform matrix + float v[3]; // velocity + float rot[3]; // rotational velocity + float acc[3]; // acceleration + float racc[3]; // rotational acceleration + + // Simple initialization + State() { + for(int i=0; i<3; i++) { + pos[i] = v[i] = rot[i] = acc[i] = racc[i] = 0; + for(int j=0; j<3; j++) + orient[3*i+j] = i==j ? 1 : 0; + } + } +}; + +// +// Objects implementing this interface are responsible for calculating +// external forces on a RigidBody object. These will then be used by +// an Integrator to decide on a new solution to the state equations, +// which will be reported to the BodyEnvironment for further action. +// +class BodyEnvironment +{ +public: + // This method inspects the "environment" in which a RigidBody + // exists, calculates forces and torques on the body, and sets + // them via addForce()/addTorque(). Because this method is called + // multiple times ("trials") as part of a Runge-Kutta integration, + // this is NOT the place to make decisions about anything but the + // forces on the object. Note that the acc and racc fields of the + // passed-in State object are undefined! (They are calculed BY + // this method). + virtual void calcForces(State* state) = 0; + + // Called when the RK4 integrator has determined a "real" new + // point on the curve of life. Any side-effect producing checks + // of body state vs. the environment can happen here (crashes, + // etc...). + virtual void newState(State* state) = 0; +}; + +}; // namespace yasim +#endif // _BODYENVIRONMENT_HPP diff --git a/src/FDM/YASim/ControlMap.cpp b/src/FDM/YASim/ControlMap.cpp new file mode 100644 index 000000000..e738b5b08 --- /dev/null +++ b/src/FDM/YASim/ControlMap.cpp @@ -0,0 +1,130 @@ +#include "Jet.hpp" +#include "Thruster.hpp" +#include "Gear.hpp" +#include "Wing.hpp" +#include "Math.hpp" + +#include "ControlMap.hpp" +namespace yasim { + +ControlMap::~ControlMap() +{ + for(int i=0; i<_inputs.size(); i++) { + Vector* v = (Vector*)_inputs.get(i); + for(int j=0; j<v->size(); j++) + delete (MapRec*)v->get(j); + delete v; + } + + for(int i=0; i<_outputs.size(); i++) { + OutRec* o = (OutRec*)_outputs.get(i); + delete[] o->values; + delete o; + } +} + +int ControlMap::newInput() +{ + Vector* v = new Vector(); + return _inputs.add(v); +} + +void ControlMap::addMapping(int input, int type, void* object, int options) +{ + // See if the output object already exists + OutRec* out = 0; + for(int i=0; i<_outputs.size(); i++) { + OutRec* o = (OutRec*)_outputs.get(i); + if(o->object == object && o->type == type) { + out = o; + break; + } + } + + // Create one if it doesn't + if(out == 0) { + out = new OutRec(); + out->type = type; + out->object = object; + out->n = 0; + out->values = 0; + _outputs.add(out); + } + + // Make space for the new input value + int idx = out->n++; + delete[] out->values; + out->values = new float[out->n]; + + // Add the new option tag + out->options.add((void*)options); + + // Make a new input record + MapRec* map = new MapRec(); + map->out = out; + map->idx = idx; + + // And add it to the approproate vector. + Vector* maps = (Vector*)_inputs.get(input); + maps->add(map); +} + +void ControlMap::reset() +{ + // Set all the values to zero + for(int i=0; i<_outputs.size(); i++) { + OutRec* o = (OutRec*)_outputs.get(i); + for(int j=0; j<o->n; j++) + o->values[j] = 0; + } +} + +void ControlMap::setInput(int input, float value) +{ + Vector* maps = (Vector*)_inputs.get(input); + for(int i=0; i<maps->size(); i++) { + MapRec* map = (MapRec*)maps->get(i); + map->out->values[map->idx] = value; + } +} + +void ControlMap::applyControls() +{ + for(int outrec=0; outrec<_outputs.size(); outrec++) { + OutRec* o = (OutRec*)_outputs.get(outrec); + + // Generate a summed value. Note the check for "split" + // control axes like ailerons. + float lval = 0, rval = 0; + for(int i=0; i<o->n; i++) { + float val = o->values[i]; + int opt = (int)o->options.get(i); + if(opt & OPT_SQUARE) + val = val * Math::abs(val); + if(opt & OPT_INVERT) + val = -val; + lval += val; + if(opt & OPT_SPLIT) + rval -= val; + else + rval += val; + } + + void* obj = o->object; + switch(o->type) { + case THROTTLE: ((Thruster*)obj)->setThrottle(lval); break; + case MIXTURE: ((Thruster*)obj)->setMixture(lval); break; + case PROP: ((Thruster*)obj)->setPropAdvance(lval); break; + case REHEAT: ((Jet*)obj)->setReheat(lval); break; + case BRAKE: ((Gear*)obj)->setBrake(lval); break; + case STEER: ((Gear*)obj)->setRotation(lval); break; + case EXTEND: ((Gear*)obj)->setExtension(lval); break; + case SLAT: ((Wing*)obj)->setSlat(lval); break; + case FLAP0: ((Wing*)obj)->setFlap0(lval, rval); break; + case FLAP1: ((Wing*)obj)->setFlap1(lval, rval); break; + case SPOILER: ((Wing*)obj)->setSpoiler(lval, rval); break; + } + } +} + +}; // namespace yasim diff --git a/src/FDM/YASim/ControlMap.hpp b/src/FDM/YASim/ControlMap.hpp new file mode 100644 index 000000000..0534298cf --- /dev/null +++ b/src/FDM/YASim/ControlMap.hpp @@ -0,0 +1,54 @@ +#ifndef _CONTROL_MAP_HPP +#define _CONTROL_MAP_HPP + +#include "util/Vector.hpp" + +namespace yasim { + +class ControlMap { +public: + ~ControlMap(); + + enum OutputType { THROTTLE, MIXTURE, REHEAT, PROP, + BRAKE, STEER, EXTEND, + INCIDENCE, FLAP0, FLAP1, SLAT, SPOILER }; + + static const int OPT_SPLIT = 0x01; + static const int OPT_INVERT = 0x02; + static const int OPT_SQUARE = 0x04; + + // Returns a new, not-yet-used "input handle" for addMapping and + // setInput. This typically corresponds to one user axis. + int newInput(); + + // Adds a mapping to between input handle and a particular setting + // on an output object. The value of output MUST match the type + // of object! + void addMapping(int input, int output, void* object, int options=0); + + // Resets our accumulated input values. Call before any + // setInput() invokations. + void reset(); + + // Sets the specified input (as returned by newInput) to the + // specified value. + void setInput(int input, float value); + + // Calculates and applies the settings received since the last reset(). + void applyControls(); + +private: + struct OutRec { int type; void* object; int n; + float* values; Vector options; }; + struct MapRec { OutRec* out; int idx; }; + + // A list of (sub)Vectors containing a bunch of MapRec objects for + // each input handle. + Vector _inputs; + + // An unordered list of output settings. + Vector _outputs; +}; + +}; // namespace yasim +#endif // _CONTROL_MAP_HPP diff --git a/src/FDM/YASim/FGFDM.cpp b/src/FDM/YASim/FGFDM.cpp new file mode 100644 index 000000000..f606b15fa --- /dev/null +++ b/src/FDM/YASim/FGFDM.cpp @@ -0,0 +1,458 @@ +#include <stdio.h> +#include <stdlib.h> + +#include <Main/fg_props.hxx> + +#include "Jet.hpp" +#include "Gear.hpp" +#include "Atmosphere.hpp" +#include "PropEngine.hpp" +#include "Propeller.hpp" +#include "PistonEngine.hpp" + +#include "FGFDM.hpp" +namespace yasim { + +// Some conversion factors +static const float KTS2MPS = 0.514444444444; +static const float FT2M = 0.3048; +static const float DEG2RAD = 0.0174532925199; +static const float RPM2RAD = 0.10471975512; +static const float LBS2N = 4.44822; +static const float LBS2KG = 0.45359237; +static const float CM2GALS = 264.172037284; +static const float HP2W = 745.700; + +// Stubs, so that this can be compiled without the FlightGear +// binary. What's the best way to handle this? + +// float fgGetFloat(char* name, float def) { return 0; } +// void fgSetFloat(char* name, float val) {} + +FGFDM::FGFDM() +{ + _nextEngine = 0; +} + +FGFDM::~FGFDM() +{ + for(int i=0; i<_axes.size(); i++) { + AxisRec* a = (AxisRec*)_axes.get(i); + delete[] a->name; + delete a; + } + for(int i=0; i<_pistons.size(); i++) { + EngRec* er = (EngRec*)_pistons.get(i); + delete[] er->prefix; + delete (PropEngine*)er->eng; + delete er; + } + for(int i=0; i<_jets.size(); i++) { + EngRec* er = (EngRec*)_pistons.get(i); + delete[] er->prefix; + delete (Jet*)er->eng; + delete er; + } + for(int i=0; i<_weights.size(); i++) { + WeightRec* wr = (WeightRec*)_weights.get(i); + delete[] wr->prop; + delete wr; + } + +} + +void FGFDM::iterate(float dt) +{ + getExternalInput(dt); + _airplane.iterate(dt); + setOutputProperties(); +} + +Airplane* FGFDM::getAirplane() +{ + return &_airplane; +} + +void FGFDM::init() +{ + // We don't want to use these ties (we set the values ourselves, + // and this works only for the first piston engine anyway). + fgUntie("/engines/engine[0]/rpm"); + fgUntie("/engines/engine[0]/egt-degf"); + fgUntie("/engines/engine[0]/cht-degf"); + fgUntie("/engines/engine[0]/oil-temperature-degf"); + fgUntie("/engines/engine[0]/mp-osi"); + fgUntie("/engines/engine[0]/fuel-flow-gph"); + fgUntie("/engines/engine[0]/running"); + fgUntie("/engines/engine[0]/cranking"); + fgUntie("/consumables/fuel/tank[0]/level-gal_us"); + fgUntie("/consumables/fuel/tank[1]/level-gal_us"); + + // Set these to sane values. We don't support engine start yet. + fgSetBool("/engines/engine[0]/running", true); + fgSetBool("/engines/engine[0]/cranking", false); + + // Allows the user to start with something other than full fuel + _airplane.setFuelFraction(fgGetFloat("/yasim/fuel-fraction", 1)); + + // This has a nasty habit of being false at startup. That's not + // good. + fgSetBool("/controls/gear-down", true); +} + +// Not the worlds safest parser. But it's short & sweet. +void FGFDM::startElement(const char* name, const XMLAttributes &atts) +{ + XMLAttributes* a = (XMLAttributes*)&atts; + float v[3]; + char buf[64]; + + if(eq(name, "airplane")) { + _airplane.setWeight(attrf(a, "mass") * LBS2KG); + } else if(eq(name, "approach")) { + float spd = attrf(a, "speed") * KTS2MPS; + float alt = attrf(a, "alt", 0) * FT2M; + float aoa = attrf(a, "aoa", 0) * DEG2RAD; + _airplane.setApproach(spd, alt, aoa); + _cruiseCurr = false; + } else if(eq(name, "cruise")) { + float spd = attrf(a, "speed") * KTS2MPS; + float alt = attrf(a, "alt") * FT2M; + _airplane.setCruise(spd, alt); + _cruiseCurr = true; + } else if(eq(name, "cockpit")) { + v[0] = attrf(a, "x"); + v[1] = attrf(a, "y"); + v[2] = attrf(a, "z"); + _airplane.setPilotPos(v); + } else if(eq(name, "wing")) { + _airplane.setWing(parseWing(a, name)); + } else if(eq(name, "hstab")) { + _airplane.setTail(parseWing(a, name)); + } else if(eq(name, "vstab")) { + _airplane.addVStab(parseWing(a, name)); + } else if(eq(name, "propeller")) { + parsePropeller(a); + } else if(eq(name, "jet")) { + Jet* j = new Jet(); + _currObj = j; + v[0] = attrf(a, "x"); + v[1] = attrf(a, "y"); + v[2] = attrf(a, "z"); + float mass = attrf(a, "mass") * LBS2KG; + j->setDryThrust(attrf(a, "thrust") * LBS2N); + j->setPosition(v); + _airplane.addThruster(j, mass, v); + sprintf(buf, "/engines/engine[%d]", _nextEngine++); + EngRec* er = new EngRec(); + er->eng = j; + er->prefix = dup(buf); + _jets.add(er); + } else if(eq(name, "gear")) { + Gear* g = new Gear(); + _currObj = g; + v[0] = attrf(a, "x"); + v[1] = attrf(a, "y"); + v[2] = attrf(a, "z"); + g->setPosition(v); + v[0] = 0; + v[1] = 0; + v[2] = attrf(a, "compression", 1); + g->setCompression(v); + g->setStaticFriction(attrf(a, "sfric", 0.8)); + g->setDynamicFriction(attrf(a, "dfric", 0.7)); + float transitionTime = attrf(a, "retract-time", 0); + _airplane.addGear(g, transitionTime); + } else if(eq(name, "fuselage")) { + float b[3]; + v[0] = attrf(a, "ax"); + v[1] = attrf(a, "ay"); + v[2] = attrf(a, "az"); + b[0] = attrf(a, "bx"); + b[1] = attrf(a, "by"); + b[2] = attrf(a, "bz"); + _airplane.addFuselage(v, b, attrf(a, "width")); + } else if(eq(name, "tank")) { + v[0] = attrf(a, "x"); + v[1] = attrf(a, "y"); + v[2] = attrf(a, "z"); + float density = 6.0; // gasoline, in lbs/gal + if(a->hasAttribute("jet")) density = 6.72; + density *= LBS2KG/CM2GALS; + _airplane.addTank(v, attrf(a, "capacity") * LBS2KG, density); + } else if(eq(name, "ballast")) { + v[0] = attrf(a, "x"); + v[1] = attrf(a, "y"); + v[2] = attrf(a, "z"); + _airplane.addBallast(v, attrf(a, "mass") * LBS2KG); + } else if(eq(name, "weight")) { + parseWeight(a); + } else if(eq(name, "stall")) { + Wing* w = (Wing*)_currObj; + w->setStall(attrf(a, "aoa") * DEG2RAD); + w->setStallWidth(attrf(a, "width", 2) * DEG2RAD); + w->setStallPeak(attrf(a, "peak", 1.5)); + } else if(eq(name, "flap0")) { + ((Wing*)_currObj)->setFlap0(attrf(a, "start"), attrf(a, "end"), + attrf(a, "lift"), attrf(a, "drag")); + } else if(eq(name, "flap1")) { + ((Wing*)_currObj)->setFlap1(attrf(a, "start"), attrf(a, "end"), + attrf(a, "lift"), attrf(a, "drag")); + } else if(eq(name, "slat")) { + ((Wing*)_currObj)->setSlat(attrf(a, "start"), attrf(a, "end"), + attrf(a, "aoa"), attrf(a, "drag")); + } else if(eq(name, "spoiler")) { + ((Wing*)_currObj)->setSpoiler(attrf(a, "start"), attrf(a, "end"), + attrf(a, "lift"), attrf(a, "drag")); + } else if(eq(name, "actionpt")) { + v[0] = attrf(a, "x"); + v[1] = attrf(a, "y"); + v[2] = attrf(a, "z"); + ((Thruster*)_currObj)->setPosition(v); + } else if(eq(name, "dir")) { + v[0] = attrf(a, "x"); + v[1] = attrf(a, "y"); + v[2] = attrf(a, "z"); + ((Thruster*)_currObj)->setDirection(v); + } else if(eq(name, "control")) { + const char* axis = a->getValue("axis"); + if(a->hasAttribute("output")) { + // assert: output type must match _currObj type! + const char* output = a->getValue("output"); + int opt = 0; + opt |= a->hasAttribute("split") ? ControlMap::OPT_SPLIT : 0; + opt |= a->hasAttribute("invert") ? ControlMap::OPT_INVERT : 0; + opt |= a->hasAttribute("square") ? ControlMap::OPT_SQUARE : 0; + _airplane.getControlMap()->addMapping(parseAxis(axis), + parseOutput(output), + _currObj, + opt); + } else { + // assert: must be under a "cruise" or "approach" tag + float value = attrf(a, "value", 0); + if(_cruiseCurr) + _airplane.addCruiseControl(parseAxis(axis), value); + else + _airplane.addApproachControl(parseAxis(axis), value); + } + } else { + *(int*)0=0; // unexpected tag, boom + } +} + +void FGFDM::getExternalInput(float dt) +{ + // The control axes + ControlMap* cm = _airplane.getControlMap(); + cm->reset(); + for(int i=0; i<_axes.size(); i++) { + AxisRec* a = (AxisRec*)_axes.get(i); + float val = fgGetFloat(a->name, 0); + cm->setInput(a->handle, val); + } + cm->applyControls(); + + // Weights + for(int i=0; i<_weights.size(); i++) { + WeightRec* wr = (WeightRec*)_weights.get(i); + _airplane.setWeight(wr->handle, fgGetFloat(wr->prop)); + } + + // Gear state + _airplane.setGearState(fgGetBool("/controls/gear-down"), dt); +} + +void FGFDM::setOutputProperties() +{ + char buf[256]; + for(int i=0; i<_airplane.numTanks(); i++) { + sprintf(buf, "/consumables/fuel/tank[%d]/level-gal_us", i); + fgSetFloat(buf, + CM2GALS*_airplane.getFuel(i)/_airplane.getFuelDensity(i)); + } + + for(int i=0; i<_pistons.size(); i++) { + EngRec* er = (EngRec*)_pistons.get(i); + PropEngine* p = (PropEngine*)er->eng; + + sprintf(buf, "%s/rpm", er->prefix); + fgSetFloat(buf, p->getOmega() * (30/3.15149265358979)); + + sprintf(buf, "%s/fuel-flow-gph", er->prefix); + fgSetFloat(buf, p->getFuelFlow() * (3600*2.2/5)); // FIXME, wrong + } + + for(int i=0; i<_jets.size(); i++) { + EngRec* er = (EngRec*)_jets.get(i); + Jet* j = (Jet*)er->eng; + + sprintf(buf, "%s/fuel-flow-gph", er->prefix); + fgSetFloat(buf, j->getFuelFlow() * (3600*2.2/6)); // FIXME, wrong + } +} + +Wing* FGFDM::parseWing(XMLAttributes* a, const char* type) +{ + Wing* w = new Wing(); + + float defDihed = 0; + if(eq(type, "vstab")) + defDihed = 90; + else + w->setMirror(true); + + float pos[3]; + pos[0] = attrf(a, "x"); + pos[1] = attrf(a, "y"); + pos[2] = attrf(a, "z"); + w->setBase(pos); + + w->setLength(attrf(a, "length")); + w->setChord(attrf(a, "chord")); + w->setSweep(attrf(a, "sweep", 0) * DEG2RAD); + w->setTaper(attrf(a, "taper", 1)); + w->setDihedral(attrf(a, "dihedral", defDihed) * DEG2RAD); + w->setCamber(attrf(a, "camber", 0)); + w->setIncidence(attrf(a, "incidence", 0) * DEG2RAD); + + float effect = attrf(a, "effectiveness", 1); + w->setDragScale(w->getDragScale()*effect); + + _currObj = w; + return w; +} + +void FGFDM::parsePropeller(XMLAttributes* a) +{ + float cg[3]; + cg[0] = attrf(a, "x"); + cg[1] = attrf(a, "y"); + cg[2] = attrf(a, "z"); + float mass = attrf(a, "mass") * LBS2KG; + float moment = attrf(a, "moment"); + float radius = attrf(a, "radius"); + float speed = attrf(a, "cruise-speed") * KTS2MPS; + float omega = attrf(a, "cruise-rpm") * RPM2RAD; + float rho = Atmosphere::getStdDensity(attrf(a, "alt") * FT2M); + float power = attrf(a, "takeoff-power") * HP2W; + float omega0 = attrf(a, "takeoff-rpm") * RPM2RAD; + + // FIXME: this is a hack. Find a better way to ask the engine how + // much power it can produce under cruise conditions. + float cruisePower = (power * (rho/Atmosphere::getStdDensity(0)) + * (omega/omega0)); + + Propeller* prop = new Propeller(radius, speed, omega, rho, cruisePower, + omega0, power); + PistonEngine* eng = new PistonEngine(power, omega0); + PropEngine* thruster = new PropEngine(prop, eng, moment); + _airplane.addThruster(thruster, mass, cg); + + char buf[64]; + sprintf(buf, "/engines/engine[%d]", _nextEngine++); + EngRec* er = new EngRec(); + er->eng = thruster; + er->prefix = dup(buf); + _pistons.add(er); + + _currObj = thruster; +} + +// Turns a string axis name into an integer for use by the +// ControlMap. Creates a new axis if this one hasn't been defined +// yet. +int FGFDM::parseAxis(const char* name) +{ + for(int i=0; i<_axes.size(); i++) { + AxisRec* a = (AxisRec*)_axes.get(i); + if(eq(a->name, name)) + return a->handle; + } + + // Not there, make a new one. + AxisRec* a = new AxisRec(); + a->name = dup(name); + a->handle = _airplane.getControlMap()->newInput(); + _axes.add(a); + return a->handle; +} + +int FGFDM::parseOutput(const char* name) +{ + if(eq(name, "THROTTLE")) return ControlMap::THROTTLE; + if(eq(name, "MIXTURE")) return ControlMap::MIXTURE; + if(eq(name, "REHEAT")) return ControlMap::REHEAT; + if(eq(name, "PROP")) return ControlMap::PROP; + if(eq(name, "BRAKE")) return ControlMap::BRAKE; + if(eq(name, "STEER")) return ControlMap::STEER; + if(eq(name, "EXTEND")) return ControlMap::EXTEND; + if(eq(name, "INCIDENCE")) return ControlMap::INCIDENCE; + if(eq(name, "FLAP0")) return ControlMap::FLAP0; + if(eq(name, "FLAP1")) return ControlMap::FLAP1; + if(eq(name, "SLAT")) return ControlMap::SLAT; + if(eq(name, "SPOILER")) return ControlMap::SPOILER; + // error here... + return -1; +} + +void FGFDM::parseWeight(XMLAttributes* a) +{ + WeightRec* wr = new WeightRec(); + + float v[3]; + v[0] = attrf(a, "x"); + v[1] = attrf(a, "y"); + v[2] = attrf(a, "z"); + + wr->prop = dup(a->getValue("mass-prop")); + wr->size = attrf(a, "size", 0); + wr->handle = _airplane.addWeight(v, wr->size); + + _weights.add(wr); +} + +bool FGFDM::eq(const char* a, const char* b) +{ + // Figure it out for yourself. :) + while(*a && *b && *a++ == *b++); + return !(*a || *b); +} + +char* FGFDM::dup(const char* s) +{ + int len=0; + while(s[len++]); + char* s2 = new char[len+1]; + char* p = s2; + while((*p++ = *s++)); + s2[len] = 0; + return s2; +} + +int FGFDM::attri(XMLAttributes* atts, char* attr) +{ + if(!atts->hasAttribute(attr)) *(int*)0=0; // boom + return attri(atts, attr, 0); +} + +int FGFDM::attri(XMLAttributes* atts, char* attr, int def) +{ + const char* val = atts->getValue(attr); + if(val == 0) return def; + else return atol(val); +} + +float FGFDM::attrf(XMLAttributes* atts, char* attr) +{ + if(!atts->hasAttribute(attr)) *(int*)0=0; // boom + return attrf(atts, attr, 0); +} + +float FGFDM::attrf(XMLAttributes* atts, char* attr, float def) +{ + const char* val = atts->getValue(attr); + if(val == 0) return def; + else return (float)atof(val); +} + +}; // namespace yasim diff --git a/src/FDM/YASim/FGFDM.hpp b/src/FDM/YASim/FGFDM.hpp new file mode 100644 index 000000000..1df02b653 --- /dev/null +++ b/src/FDM/YASim/FGFDM.hpp @@ -0,0 +1,69 @@ +#ifndef _FGFDM_HPP +#define _FGFDM_HPP + +#include <simgear/easyxml.hxx> + +#include "Airplane.hpp" +#include "util/Vector.hpp" + +namespace yasim { + +class Wing; + +// This class forms the "glue" to the FlightGear codebase. It handles +// parsing of XML airplane files, interfacing to the properties +// system, and providing data for the use of the FGInterface object. +class FGFDM : public XMLVisitor { +public: + FGFDM(); + ~FGFDM(); + void init(); + void iterate(float dt); + + Airplane* getAirplane(); + + // XML parsing callback from XMLVisitor + virtual void startElement(const char* name, const XMLAttributes &atts); + +private: + struct AxisRec { char* name; int handle; }; + struct EngRec { char* prefix; void* eng; }; + struct WeightRec { char* prop; float size; int handle; }; + + void getExternalInput(float dt); + void setOutputProperties(); + + Wing* parseWing(XMLAttributes* a, const char* name); + int parseAxis(const char* name); + int parseOutput(const char* name); + void parseWeight(XMLAttributes* a); + void parsePropeller(XMLAttributes* a); + bool eq(const char* a, const char* b); + char* dup(const char* s); + int attri(XMLAttributes* atts, char* attr); + int attri(XMLAttributes* atts, char* attr, int def); + float attrf(XMLAttributes* atts, char* attr); + float attrf(XMLAttributes* atts, char* attr, float def); + + // The core Airplane object we manage. + Airplane _airplane; + + // The list of "axes" that we expect to find as input. These are + // typically property names. + Vector _axes; + + // Settable weights + Vector _weights; + + // Engine types. Contains an EngRec structure. + Vector _jets; + Vector _pistons; + + // Parsing temporaries + void* _currObj; + bool _cruiseCurr; + int _nextEngine; +}; + +}; // namespace yasim +#endif // _FGFDM_HPP diff --git a/src/FDM/YASim/Gear.cpp b/src/FDM/YASim/Gear.cpp new file mode 100644 index 000000000..056bc61a4 --- /dev/null +++ b/src/FDM/YASim/Gear.cpp @@ -0,0 +1,240 @@ +#include "Math.hpp" +#include "RigidBody.hpp" + +#include "Gear.hpp" +namespace yasim { + +Gear::Gear() +{ + for(int i=0; i<3; i++) + _pos[i] = _cmpr[i] = 0; + _spring = 1; + _damp = 0; + _sfric = 0.8; + _dfric = 0.7; + _brake = 0; + _rot = 0; + _extension = 1; +} + +void Gear::setPosition(float* position) +{ + for(int i=0; i<3; i++) _pos[i] = position[i]; +} + +void Gear::setCompression(float* compression) +{ + for(int i=0; i<3; i++) _cmpr[i] = compression[i]; +} + +void Gear::setSpring(float spring) +{ + _spring = spring; +} + +void Gear::setDamping(float damping) +{ + _damp = damping; +} + +void Gear::setStaticFriction(float sfric) +{ + _sfric = sfric; +} + +void Gear::setDynamicFriction(float dfric) +{ + _dfric = dfric; +} + +void Gear::setBrake(float brake) +{ + _brake = brake; +} + +void Gear::setRotation(float rotation) +{ + _rot = rotation; +} + +void Gear::setExtension(float extension) +{ + _extension = extension; +} + +void Gear::getPosition(float* out) +{ + for(int i=0; i<3; i++) out[i] = _pos[i]; +} + +void Gear::getCompression(float* out) +{ + for(int i=0; i<3; i++) out[i] = _cmpr[i]; +} + +float Gear::getSpring() +{ + return _spring; +} + +float Gear::getDamping() +{ + return _damp; +} + +float Gear::getStaticFriction() +{ + return _sfric; +} + +float Gear::getDynamicFriction() +{ + return _dfric; +} + +float Gear::getBrake() +{ + return _brake; +} + +float Gear::getRotation() +{ + return _rot; +} + +float Gear::getExtension() +{ + return _extension; +} + +void Gear::getForce(float* force, float* contact) +{ + Math::set3(_force, force); + Math::set3(_contact, contact); +} + +float Gear::getWoW() +{ + return _wow; +} + +float Gear::getCompressFraction() +{ + return _frac; +} + +void Gear::calcForce(RigidBody* body, float* v, float* rot, float* ground) +{ + // Init the return values + for(int i=0; i<3; i++) _force[i] = _contact[i] = 0; + + // Don't bother if it's not down + if(_extension < 1) + return; + + float tmp[3]; + + // First off, make sure that the gear "tip" is below the ground. + // If it's not, there's no force. + float a = ground[3] - Math::dot3(_pos, ground); + if(a > 0) + return; + + // Now a is the distance from the tip to ground, so make b the + // distance from the base to ground. We can get the fraction + // (0-1) of compression from a/(a-b). Note the minus sign -- stuff + // above ground is negative. + Math::add3(_cmpr, _pos, tmp); + float b = ground[3] - Math::dot3(tmp, ground); + + // Calculate the point of ground _contact. + _frac = a/(a-b); + if(b < 0) _frac = 1; + for(int i=0; i<3; i++) + _contact[i] = _pos[i] + _frac*_cmpr[i]; + + // Turn _cmpr into a unit vector and a magnitude + float cmpr[3]; + float clen = Math::mag3(_cmpr); + Math::mul3(1/clen, _cmpr, cmpr); + + // Now get the velocity of the point of contact + float cv[3]; + body->pointVelocity(_contact, rot, cv); + Math::add3(cv, v, cv); + + // Finally, we can start adding up the forces. First the + // spring compression (note the clamping of _frac to 1): + _frac = (_frac > 1) ? 1 : _frac; + float fmag = _frac*clen*_spring; + Math::mul3(fmag, cmpr, _force); + + // Then the damping. Use the only the velocity into the ground + // (projection along "ground") projected along the compression + // axis. So Vdamp = ground*(ground dot cv) dot cmpr + Math::mul3(Math::dot3(ground, cv), ground, tmp); + float dv = Math::dot3(cmpr, tmp); + float damp = _damp * dv; + if(damp > fmag) damp = fmag; // can't pull the plane down! + if(damp < -fmag) damp = -fmag; // sanity + Math::mul3(-damp, cmpr, tmp); + Math::add3(_force, tmp, _force); + + _wow = fmag - damp; + + // Wheels are funky. Split the velocity along the ground plane + // into rolling and skidding components. Assuming small angles, + // we generate "forward" and "left" unit vectors (the compression + // goes "up") for the gear, make a "steer" direction from these, + // and then project it onto the ground plane. Project the + // velocity onto the ground plane too, and extract the "steer" + // component. The remainder is the skid velocity. + + float gup[3]; // "up" unit vector from the ground + Math::set3(ground, gup); + Math::mul3(-1, gup, gup); + + float xhat[] = {1,0,0}; + float steer[3], skid[3]; + Math::cross3(gup, xhat, skid); // up cross xhat =~ skid + Math::unit3(skid, skid); // == skid + + Math::cross3(skid, gup, steer); // skid cross up == steer + + if(_rot != 0) { + // Correct for a (small) rotation + Math::mul3(_rot, steer, tmp); + Math::add3(tmp, skid, skid); + Math::unit3(skid, skid); + Math::cross3(skid, gup, steer); + } + + float vsteer = Math::dot3(cv, steer); + float vskid = Math::dot3(cv, skid); + float wgt = Math::dot3(_force, gup); // force into the ground + + float fsteer = _brake * calcFriction(wgt, vsteer); + float fskid = calcFriction(wgt, vskid); + if(vsteer > 0) fsteer = -fsteer; + if(vskid > 0) fskid = -fskid; + + // Phoo! All done. Add it up and get out of here. + Math::mul3(fsteer, steer, tmp); + Math::add3(tmp, _force, _force); + + Math::mul3(fskid, skid, tmp); + Math::add3(tmp, _force, _force); +} + +float Gear::calcFriction(float wgt, float v) +{ + // How slow is stopped? 50 cm/second? + const float STOP = 0.5; + const float iSTOP = 1/STOP; + v = Math::abs(v); + if(v < STOP) return v*iSTOP * wgt * _sfric; + else return wgt * _dfric; +} + +}; // namespace yasim + diff --git a/src/FDM/YASim/Gear.hpp b/src/FDM/YASim/Gear.hpp new file mode 100644 index 000000000..6e432b6dd --- /dev/null +++ b/src/FDM/YASim/Gear.hpp @@ -0,0 +1,82 @@ +#ifndef _GEAR_HPP +#define _GEAR_HPP + +namespace yasim { + +class RigidBody; + +// A landing gear has the following parameters: +// +// position: a point in the aircraft's local coordinate system where the +// fully-extended wheel will be found. +// compression: a vector from position where a fully-compressed wheel +// will be, also in aircraft-local coordinates. +// spring coefficient: force coefficient along the compression axis, in +// Newtons per meter. +// damping coefficient: force per compression speed, in Ns/m +// static coefficient of friction: force along the ground plane exerted +// by a motionless wheel. A unitless scalar. +// dynamic coefficient of friction: force along the ground plane exerted +// by a sliding/skidding wheel. +// braking fraction: fraction of the dynamic friction that will be +// actually exerted by a rolling wheel +// rotation: the angle from "forward" by which the wheel is rotated +// around its compression axis. In radians. +// +class Gear { +public: + Gear(); + + // Externally set values + void setPosition(float* position); + void setCompression(float* compression); + void setSpring(float spring); + void setDamping(float damping); + void setStaticFriction(float sfric); + void setDynamicFriction(float dfric); + void setBrake(float brake); + void setRotation(float rotation); + void setExtension(float extension); + + void getPosition(float* out); + void getCompression(float* out); + float getSpring(); + float getDamping(); + float getStaticFriction(); + float getDynamicFriction(); + float getBrake(); + float getRotation(); + float getExtension(); + + // Takes a velocity of the aircraft relative to ground, a rotation + // vector, and a ground plane (all specified in local coordinates) + // and make a force and point of application (i.e. ground contact) + // available via getForce(). + void calcForce(RigidBody* body, float* v, float* rot, float* ground); + + // Computed values: total force, weight-on-wheels (force normal to + // ground) and compression fraction. + void getForce(float* force, float* contact); + float getWoW(); + float getCompressFraction(); + +private: + float calcFriction(float wgt, float v); + + float _pos[3]; + float _cmpr[3]; + float _spring; + float _damp; + float _sfric; + float _dfric; + float _brake; + float _rot; + float _extension; + float _force[3]; + float _contact[3]; + float _wow; + float _frac; +}; + +}; // namespace yasim +#endif // _GEAR_HPP diff --git a/src/FDM/YASim/Glue.cpp b/src/FDM/YASim/Glue.cpp new file mode 100644 index 000000000..119a1b161 --- /dev/null +++ b/src/FDM/YASim/Glue.cpp @@ -0,0 +1,235 @@ +#include "Math.hpp" +#include "Glue.hpp" +namespace yasim { + +void Glue::calcAlphaBeta(State* s, float* alpha, float* beta) +{ + // Convert the velocity to the aircraft frame. + float v[3]; + Math::vmul33(s->orient, s->v, v); + + // By convention, positive alpha is an up pitch, and a positive + // beta is yawed to the right. + *alpha = -Math::atan2(v[2], v[0]); + *beta = Math::atan2(v[1], v[0]); +} + +void Glue::calcEulerRates(State* s, float* roll, float* pitch, float* hdg) +{ + // This one is easy, the projection of the rotation vector around + // the "up" axis. + float up[3]; + geodUp(s->pos, up); + *hdg = -Math::dot3(up, s->rot); // negate for "NED" conventions + + // A bit harder: the X component of the rotation vector expressed + // in airframe coordinates. + float lr[3]; + Math::vmul33(s->orient, s->rot, lr); + *roll = lr[0]; + + // Hardest: the component of rotation along the direction formed + // by the cross product of (and thus perpendicular to) the + // aircraft's forward vector (i.e. the first three elements of the + // orientation matrix) and the "up" axis. + float pitchAxis[3]; + Math::cross3(s->orient, up, pitchAxis); + Math::unit3(pitchAxis, pitchAxis); + *pitch = Math::dot3(pitchAxis, s->rot); +} + +void Glue::xyz2geoc(double* xyz, double* lat, double* lon, double* alt) +{ + double x=xyz[0], y=xyz[1], z=xyz[2]; + + // Cylindrical radius from the polar axis + double rcyl = Math::sqrt(x*x + y*y); + + // In geocentric coordinates, these are just the angles in + // cartesian space. + *lon = Math::atan2(y, x); + *lat = Math::atan2(z, rcyl); + + // To get XYZ coordinate of "ground", we "squash" the cylindric + // radius into a coordinate system where the earth is a sphere, + // find the fraction of the xyz vector that is above ground. + double rsquash = SQUASH * rcyl; + double frac = POLRAD/Math::sqrt(rsquash*rsquash + z*z); + double len = Math::sqrt(x*x + y*y + z*z); + + *alt = len * (1-frac); +} + +void Glue::geoc2xyz(double lat, double lon, double alt, double* out) +{ + // Generate a unit vector + double rcyl = Math::cos(lat); + double x = rcyl*Math::cos(lon); + double y = rcyl*Math::sin(lon); + double z = Math::sin(lat); + + // Convert to "squashed" space, renormalize the unit vector, + // multiply by the polar radius, and back convert to get us the + // point of intersection of the unit vector with the surface. + // Then just add the altitude. + double rtmp = rcyl*SQUASH; + double renorm = POLRAD/Math::sqrt(rtmp*rtmp + z*z); + double ztmp = z*renorm; + rtmp *= renorm*STRETCH; + double len = Math::sqrt(rtmp*rtmp + ztmp*ztmp); + len += alt; + + out[0] = x*len; + out[1] = y*len; + out[2] = z*len; +} + +double Glue::geod2geocLat(double lat) +{ + double r = Math::cos(lat)*STRETCH*STRETCH; + double z = Math::sin(lat); + return Math::atan2(z, r); +} + +double Glue::geoc2geodLat(double lat) +{ + double r = Math::cos(lat)*SQUASH*SQUASH; + double z = Math::sin(lat); + return Math::atan2(z, r); +} + +void Glue::xyz2geod(double* xyz, double* lat, double* lon, double* alt) +{ + xyz2geoc(xyz, lat, lon, alt); + *lat = geoc2geodLat(*lat); +} + +void Glue::geod2xyz(double lat, double lon, double alt, double* out) +{ + lat = geod2geocLat(lat); + geoc2xyz(lat, lon, alt, out); +} + +void Glue::xyz2nedMat(double lat, double lon, float* out) +{ + // Shorthand for our output vectors: + float *north = out, *east = out+3, *down = out+6; + + float slat = Math::sin(lat); + float clat = Math::cos(lat); + float slon = Math::sin(lon); + float clon = Math::cos(lon); + + north[0] = -clon * slat; + north[1] = -slon * slat; + north[2] = clat; + + east[0] = -slon; + east[1] = clon; + east[2] = 0; + + down[0] = -clon * clat; + down[1] = -slon * clat; + down[2] = -slat; +} + +void Glue::euler2orient(float roll, float pitch, float hdg, float* out) +{ + // To translate a point in aircraft space to the output "NED" + // frame, first negate the Y and Z axes (ugh), then roll around + // the X axis, then pitch around Y, then point to the correct + // heading about Z. Expressed as a matrix multiplication, then, + // the transformation from aircraft to local is HPRN. And our + // desired output is the inverse (i.e. transpose) of that. Since + // all rotations are 2D, they have a simpler form than a generic + // rotation and are done out longhand below for efficiency. + + // Init to the identity matrix + for(int i=0; i<3; i++) + for(int j=0; j<3; j++) + out[3*i+j] = (i==j) ? 1 : 0; + + // Negate Y and Z + out[4] = out[8] = -1; + + float s = Math::sin(roll); + float c = Math::cos(roll); + for(int col=0; col<3; col++) { + float y=out[col+3], z=out[col+6]; + out[col+3] = c*y - s*z; + out[col+6] = s*y + c*z; + } + + s = Math::sin(pitch); + c = Math::cos(pitch); + for(int col=0; col<3; col++) { + float x=out[col], z=out[col+6]; + out[col] = c*x + s*z; + out[col+6] = c*z - s*x; + } + + s = Math::sin(hdg); + c = Math::cos(hdg); + for(int col=0; col<3; col++) { + float x=out[col], y=out[col+3]; + out[col] = c*x - s*y; + out[col+3] = s*x + c*y; + } + + // Invert: + Math::trans33(out, out); +} + +void Glue::orient2euler(float* o, float* roll, float* pitch, float* hdg) +{ + // The airplane's "pointing" direction in NED coordinates is vx, + // and it's y (left-right) axis is vy. + float vx[3], vy[3]; + vx[0]=o[0], vx[1]=o[1], vx[2]=o[2]; + vy[0]=o[3], vy[1]=o[4], vy[2]=o[5]; + + // The heading is simply the rotation of the projection onto the + // XY plane + *hdg = Math::atan2(vx[1], vx[0]); + + // The pitch is the angle between the XY plane and vx, remember + // that rotations toward positive Z are _negative_ + float projmag = Math::sqrt(vx[0]*vx[0]+vx[1]*vx[1]); + *pitch = -Math::atan2(vx[2], projmag); + + // Roll is a bit harder. Construct an "unrolled" orientation, + // where the X axis is the same as the "rolled" one, and the Y + // axis is parallel to the XY plane. These two can give you an + // "unrolled" Z axis as their cross product. Now, take the "vy" + // axis, which points out the left wing. The projections of this + // along the "unrolled" YZ plane will give you the roll angle via + // atan(). + float* ux = vx; + float uy[3], uz[3]; + + uz[0] = 0; uz[1] = 0; uz[2] = 1; + Math::cross3(uz, ux, uy); + Math::unit3(uy, uy); + Math::cross3(ux, uy, uz); + + float py = -Math::dot3(vy, uy); + float pz = -Math::dot3(vy, uz); + *roll = Math::atan2(pz, py); +} + +void Glue::geodUp(double* pos, float* out) +{ + double lat, lon, alt; + xyz2geod(pos, &lat, &lon, &alt); + + float slat = Math::sin(lat); + float clat = Math::cos(lat); + float slon = Math::sin(lon); + float clon = Math::cos(lon); + out[0] = clon * clat; + out[1] = slon * clat; + out[2] = slat; +} + +}; // namespace yasim + diff --git a/src/FDM/YASim/Glue.hpp b/src/FDM/YASim/Glue.hpp new file mode 100644 index 000000000..fe6dbf702 --- /dev/null +++ b/src/FDM/YASim/Glue.hpp @@ -0,0 +1,60 @@ +#ifndef _GLUE_HPP +#define _GLUE_HPP + +#include "BodyEnvironment.hpp" + +namespace yasim { + +// The XYZ coordinate system has Z as the earth's axis, the Y axis +// pointing out the equator at zero longitude, and the X axis pointing +// out the middle of the western hemisphere. +class Glue { +public: + static void calcAlphaBeta(State* s, float* alpha, float* beta); + + // Calculates the instantaneous rotation velocities about each + // axis. + static void calcEulerRates(State* s, + float* roll, float* pitch, float* hdg); + + static void xyz2geoc(double* xyz, + double* lat, double* lon, double* alt); + static void geoc2xyz(double lat, double lon, double alt, + double* out); + static void xyz2geod(double* xyz, + double* lat, double* lon, double* alt); + static void geod2xyz(double lat, double lon, double alt, + double* out); + + static double geod2geocLat(double lat); + static double geoc2geodLat(double lat); + + // Returns a global to "local" (north, east, down) matrix. Note + // that the latitude passed in is geoDETic. + static void xyz2nedMat(double lat, double lon, float* out); + + // Conversion between a euler triplet and a matrix that transforms + // "local" (north/east/down) coordinates to the aircraft frame. + static void euler2orient(float roll, float pitch, float hdg, + float* out); + static void orient2euler(float* o, + float* roll, float* pitch, float* hdg); + + // Returns a geodetic (i.e. gravitational, "level", etc...) "up" + // vector for the specified xyz position. + static void geodUp(double* pos, float* out); + +private: + + // WGS84 numbers + static const double EQURAD = 6378137; // equatorial radius + static const double STRETCH = 1.003352810665; // equ./polar radius + + // Derived from the above + static const double SQUASH = 0.99665839311; // 1/STRETCH + static const double POLRAD = 6356823.77346; // EQURAD*SQUASH + static const double iPOLRAD = 1.57311266701e-07; // 1/POLRAD +}; + +}; // namespace yasim +#endif // _GLUE_HPP diff --git a/src/FDM/YASim/Integrator.cpp b/src/FDM/YASim/Integrator.cpp new file mode 100644 index 000000000..872f902c4 --- /dev/null +++ b/src/FDM/YASim/Integrator.cpp @@ -0,0 +1,311 @@ +#include "Math.hpp" +#include "Integrator.hpp" +namespace yasim { + +void Integrator::setBody(RigidBody* body) +{ + _body = body; +} + +void Integrator::setEnvironment(BodyEnvironment* env) +{ + _env = env; +} + +void Integrator::setInterval(float dt) +{ + _dt = dt; +} + +float Integrator::getInterval() +{ + return _dt; +} + +void Integrator::setState(State* s) +{ + _s = *s; +} + +State* Integrator::getState() +{ + return &_s; +} + +// Transforms a "local" vector to a "global" vector (not coordinate!) +// using the specified orientation. +void Integrator::l2gVector(float* orient, float* v, float* out) +{ + Math::tmul33(_s.orient, v, out); +} + +// Updates a position vector for a body c.g. motion with velocity v +// over time dt, from orientation o0 to o1. Because the position +// references the local coordinate origin, but the velocity is that of +// the c.g., this gets a bit complicated. +void Integrator::extrapolatePosition(double* pos, float* v, float dt, + float* o1, float* o2) +{ + // Remember that it's the c.g. that's moving, so account for + // changes in orientation. The motion of the coordinate + // frame will be l2gOLD(cg) + deltaCG - l2gNEW(cg) + float cg[3], tmp[3]; + + _body->getCG(cg); + l2gVector(o1, cg, cg); // cg = l2gOLD(cg) ("cg0") + Math::set3(v, tmp); // tmp = vel + Math::mul3(dt, tmp, tmp); // = vel*dt ("deltaCG") + Math::add3(cg, tmp, tmp); // = cg0 + deltaCG + _body->getCG(cg); + l2gVector(o2, cg, cg); // cg = l2gNEW(cg) ("cg1") + Math::sub3(tmp, cg, tmp); // tmp = cg0 + deltaCG - cg1 + + pos[0] += tmp[0]; // p1 = p0 + (cg0+deltaCG-cg1) + pos[1] += tmp[1]; // (positions are doubles, so we + pos[2] += tmp[2]; // can't use Math::add3) +} + +#if 0 +// A straight euler integration, for reference. Don't use. +void Integrator::calcNewInterval() +{ + float tmp[3]; + State s = _s; + + float dt = _dt / 4; + + orthonormalize(_s.orient); + for(int i=0; i<4; i++) { + _body->reset(); + _env->calcForces(&s); + + _body->getAccel(s.acc); + l2gVector(_s.orient, s.acc, s.acc); + + _body->getAngularAccel(s.racc); + l2gVector(_s.orient, s.racc, s.racc); + + float rotmat[9]; + rotMatrix(s.rot, dt, rotmat); + Math::mmul33(_s.orient, rotmat, s.orient); + + extrapolatePosition(s.pos, s.v, dt, _s.orient, s.orient); + + Math::mul3(dt, s.acc, tmp); + Math::add3(tmp, s.v, s.v); + + Math::mul3(dt, s.racc, tmp); + Math::add3(tmp, s.rot, s.rot); + + _s = s; + } + + _env->newState(&_s); +} +#endif + +void Integrator::calcNewInterval() +{ + // In principle, these could be changed for something other than + // a 4th order integration. I doubt if anyone cares. + const static int NITER=4; + static float TIMESTEP[] = { 1.0, 0.5, 0.5, 1.0 }; + static float WEIGHTS[] = { 6.0, 3.0, 3.0, 6.0 }; + + // Scratch pads: + double pos[NITER][3]; float vel[NITER][3]; float acc[NITER][3]; + float ori[NITER][9]; float rot[NITER][3]; float rac[NITER][3]; + + float *currVel = _s.v; + float *currAcc = _s.acc; + float *currRot = _s.rot; + float *currRac = _s.racc; + + // First off, sanify the initial orientation + orthonormalize(_s.orient); + + for(int i=0; i<NITER; i++) { + // + // extrapolate forward based on current values of the + // derivatives and the ORIGINAL values of the + // position/orientation. + // + float dt = _dt * TIMESTEP[i]; + float tmp[3]; + + // "add" rotation to orientation (generate a rotation matrix) + float rotmat[9]; + rotMatrix(currRot, dt, rotmat); + Math::mmul33(_s.orient, rotmat, ori[i]); + + // add velocity to (original!) position + for(int j=0; j<3; j++) pos[i][j] = _s.pos[j]; + extrapolatePosition(pos[i], currVel, dt, _s.orient, ori[i]); + + // add acceleration to (original!) velocity + Math::set3(currAcc, tmp); + Math::mul3(dt, tmp, tmp); + Math::add3(_s.v, tmp, vel[i]); + + // add rotational acceleration to rotation + Math::set3(currRac, tmp); + Math::mul3(dt, tmp, tmp); + Math::add3(_s.rot, tmp, rot[i]); + + // + // Tell the environment to generate new forces on the body, + // extract the accelerations, and convert to vectors in the + // global frame. + // + _body->reset(); + + // FIXME. Copying into a state object is clumsy! The + // per-coordinate arrays should be changed into a single array + // of State objects. Ick. + State stmp; + for(int j=0; j<3; j++) { + stmp.pos[j] = pos[i][j]; + stmp.v[j] = vel[i][j]; + stmp.rot[j] = rot[i][j]; + } + for(int j=0; j<9; j++) + stmp.orient[j] = ori[i][j]; + _env->calcForces(&stmp); + + _body->getAccel(acc[i]); + _body->getAngularAccel(rac[i]); + l2gVector(_s.orient, acc[i], acc[i]); + l2gVector(_s.orient, rac[i], rac[i]); + + // + // Save the resulting derivatives for the next iteration + // + currVel = vel[i]; currAcc = acc[i]; + currRot = rot[i]; currRac = rac[i]; + } + + // Average the resulting derivatives together according to their + // weights. Yes, we're "averaging" rotations, which isn't + // stricly correct -- rotations live in a non-cartesian space. + // But the space is "locally" cartesian. + State derivs; + float tot = 0; + for(int i=0; i<NITER; i++) { + float wgt = WEIGHTS[i]; + tot += wgt; + for(int j=0; j<3; j++) { + derivs.v[j] += wgt*vel[i][j]; derivs.rot[j] += wgt*rot[i][j]; + derivs.acc[j] += wgt*acc[i][j]; derivs.racc[j] += wgt*rac[i][j]; + } + } + float itot = 1/tot; + for(int i=0; i<3; i++) { + derivs.v[i] *= itot; derivs.rot[i] *= itot; + derivs.acc[i] *= itot; derivs.racc[i] *= itot; + } + + // And finally extrapolate once more, using the averaged + // derivatives, to the final position and orientation. This code + // is essentially identical to the position extrapolation step + // inside the loop. + + // save the starting orientation + float orient0[9]; + for(int i=0; i<9; i++) orient0[i] = _s.orient[i]; + + float rotmat[9]; + rotMatrix(derivs.rot, _dt, rotmat); + Math::mmul33(orient0, rotmat, _s.orient); + + extrapolatePosition(_s.pos, derivs.v, _dt, orient0, _s.orient); + + float tmp[3]; + Math::mul3(_dt, derivs.acc, tmp); + Math::add3(_s.v, tmp, _s.v); + + Math::mul3(_dt, derivs.racc, tmp); + Math::add3(_s.rot, tmp, _s.rot); + + for(int i=0; i<3; i++) { + _s.acc[i] = derivs.acc[i]; + _s.racc[i] = derivs.racc[i]; + } + + // Tell the environment about our decision + _env->newState(&_s); +} + +// Generates a matrix that rotates about axis r through an angle equal +// to (|r| * dt). That is, a rotation effected by rotating with rate +// r for dt "seconds" (or whatever time unit is in use). +// Implementation shamelessly cribbed from the OpenGL specification. +// +// NOTE: we're actually returning the _transpose_ of the rotation +// matrix! This is becuase we store orientations as global-to-local +// transformations. Thus, we want to rotate the ROWS of the old +// matrix to get the new one. +void Integrator::rotMatrix(float* r, float dt, float* out) +{ + // Normalize the vector, and extract the angle through which we'll + // rotate. + float mag = Math::mag3(r); + float angle = dt*mag; + + // Tiny rotations result in degenerate (zero-length) rotation + // vectors, so clamp to an identity matrix. 1e-06 radians + // per 1/30th of a second (typical dt unit) corresponds to one + // revolution per 2.4 days, or significantly less than the + // coriolis rotation. And it's still preserves half the floating + // point precision of a radian-per-iteration rotation. + if(angle < 1e-06) { + out[0] = 1; out[1] = 0; out[2] = 0; + out[3] = 0; out[4] = 1; out[5] = 0; + out[6] = 0; out[7] = 0; out[8] = 1; + return; + } + + float runit[3]; + Math::mul3(1/mag, r, runit); + + float s = Math::sin(angle); + float c = Math::cos(angle); + float c1 = 1-c; + float c1rx = c1*runit[0]; + float c1ry = c1*runit[1]; + float xx = c1rx*runit[0]; + float xy = c1rx*runit[1]; + float xz = c1rx*runit[2]; + float yy = c1ry*runit[1]; + float yz = c1ry*runit[2]; + float zz = c1*runit[2]*runit[2]; + float xs = runit[0]*s; + float ys = runit[1]*s; + float zs = runit[2]*s; + + out[0] = xx+c ; out[3] = xy-zs; out[6] = xz+ys; + out[1] = xy+zs; out[4] = yy+c ; out[7] = yz-xs; + out[2] = xz-ys; out[5] = yz+xs; out[8] = zz+c ; +} + +// Does a Gram-Schmidt orthonormalization on the rows of a +// global-to-local orientation matrix. The order of normalization +// here is x, z, y. This is because of the convention of "x" being +// the forward vector and "z" being up in the body frame. These two +// vectors are the most important to keep correct. +void Integrator::orthonormalize(float* m) +{ + // The 1st, 2nd and 3rd rows of the matrix store the global frame + // equivalents of the x, y, and z unit vectors in the local frame. + float *x = m, *y = m+3, *z = m+6; + + Math::unit3(x, x); // x = x/|x| + + float v[3]; + Math::mul3(Math::dot3(x, z), z, v); // v = z(x dot z) + Math::sub3(z, v, z); // z = z - z*(x dot z) + Math::unit3(z, z); // z = z/|z| + + Math::cross3(z, x, y); // y = z cross x +} + +}; // namespace yasim diff --git a/src/FDM/YASim/Integrator.hpp b/src/FDM/YASim/Integrator.hpp new file mode 100644 index 000000000..323177ced --- /dev/null +++ b/src/FDM/YASim/Integrator.hpp @@ -0,0 +1,58 @@ +#ifndef _INTEGRATOR_HPP +#define _INTEGRATOR_HPP + +#include "BodyEnvironment.hpp" +#include "RigidBody.hpp" + +namespace yasim { + +// +// These objects are responsible for extracting force data from a +// BodyEnvironment object, using a RigidBody object to calculate +// accelerations, and then tying that all together into a new +// "solution" of position/orientation/etc... for the body. The method +// used is a fourth-order Runge-Kutta integration. +// +class Integrator +{ +public: + // Sets the RigidBody that will be integrated. + void setBody(RigidBody* body); + + // Sets the BodyEnvironment object used to calculate the second + // derivatives. + void setEnvironment(BodyEnvironment* env); + + // Sets the time interval between steps. Units can be anything, + // but they must match the other values (if you specify speed in + // m/s, then angular acceleration had better be in 1/s^2 and the + // time interval should be in seconds, etc...) + void setInterval(float dt); + float getInterval(); + + // The current state, i.e. initial conditions for the next + // integration iteration. Note that the acceleration parameters + // in the State object are ignored. + State* getState(); + void setState(State* s); + + // Do a 4th order Runge-Kutta integration over one time interval. + // This is the top level of the simulation. + void calcNewInterval(); + +private: + void orthonormalize(float* m); + void rotMatrix(float* r, float dt, float* out); + void l2gVector(float* orient, float* v, float* out); + void extrapolatePosition(double* pos, float* v, float dt, + float* o1, float* o2); + + BodyEnvironment* _env; + RigidBody* _body; + float _dt; + + State _s; +}; + +}; // namespace yasim +#endif // _INTEGRATOR_HPP diff --git a/src/FDM/YASim/Jet.cpp b/src/FDM/YASim/Jet.cpp new file mode 100644 index 000000000..b4169b69b --- /dev/null +++ b/src/FDM/YASim/Jet.cpp @@ -0,0 +1,59 @@ +#include "Atmosphere.hpp" +#include "Math.hpp" +#include "Jet.hpp" +namespace yasim { + +Jet::Jet() +{ + _rho0 = Atmosphere::getStdDensity(0); + _thrust = 0; + _reheat = 0; +} + +Thruster* Jet::clone() +{ + Jet* j = new Jet(); + j->_thrust = _thrust; + j->_rho0 = _rho0; + return j; +} + +void Jet::setDryThrust(float thrust) +{ + _thrust = thrust; +} + +void Jet::setReheat(float reheat) +{ + _reheat = reheat; +} + +void Jet::getThrust(float* out) +{ + float t = _thrust * _throttle * (_rho / _rho0); + Math::mul3(t, _dir, out); +} + +void Jet::getTorque(float* out) +{ + out[0] = out[1] = out[2] = 0; + return; +} + +void Jet::getGyro(float* out) +{ + out[0] = out[1] = out[2] = 0; + return; +} + +float Jet::getFuelFlow() +{ + return 0; +} + +void Jet::integrate(float dt) +{ + return; +} + +}; // namespace yasim diff --git a/src/FDM/YASim/Jet.hpp b/src/FDM/YASim/Jet.hpp new file mode 100644 index 000000000..c51e30276 --- /dev/null +++ b/src/FDM/YASim/Jet.hpp @@ -0,0 +1,32 @@ +#ifndef _JET_HPP +#define _JET_HPP + +#include "Thruster.hpp" + +namespace yasim { + +// Incredibly dumb placeholder for a Jet implementation. But it does +// what's important, which is provide thrust. +class Jet : public Thruster { +public: + Jet(); + + virtual Thruster* clone(); + + void setDryThrust(float thrust); + void setReheat(float reheat); + + virtual void getThrust(float* out); + virtual void getTorque(float* out); + virtual void getGyro(float* out); + virtual float getFuelFlow(); + virtual void integrate(float dt); + +private: + float _thrust; + float _rho0; + float _reheat; +}; + +}; // namespace yasim +#endif // _JET_HPP diff --git a/src/FDM/YASim/Makefile.am b/src/FDM/YASim/Makefile.am new file mode 100644 index 000000000..c4f7fdcd5 --- /dev/null +++ b/src/FDM/YASim/Makefile.am @@ -0,0 +1,9 @@ +noinst_LIBRARIES = libYASim.a + +libYASim_a_SOURCES = YASim.cxx Airplane.cpp Atmosphere.cpp ControlMap.cpp \ + FGFDM.cpp Gear.cpp Glue.cpp Integrator.cpp Jet.cpp \ + Math.cpp Model.cpp PistonEngine.cpp Propeller.cpp \ + PropEngine.cpp RigidBody.cpp Surface.cpp \ + Thruster.cpp Wing.cpp + +INCLUDES += -I$(top_srcdir) -I$(top_srcdir)/src diff --git a/src/FDM/YASim/Math.cpp b/src/FDM/YASim/Math.cpp new file mode 100644 index 000000000..0af57a03f --- /dev/null +++ b/src/FDM/YASim/Math.cpp @@ -0,0 +1,237 @@ +#include <math.h> + +#include "Math.hpp" +namespace yasim { + +float Math::abs(float f) +{ + return ::fabs(f); +} + +float Math::sqrt(float f) +{ + return ::sqrt(f); +} + +float Math::pow(float base, float exp) +{ + return ::pow(base, exp); +} + +float Math::ceil(float f) +{ + return ::ceil(f); +} + +float Math::cos(float f) +{ + return ::cos(f); +} + +float Math::sin(float f) +{ + return ::sin(f); +} + +float Math::tan(float f) +{ + return ::tan(f); +} + +float Math::atan2(float y, float x) +{ + return ::atan2(y, x); +} + +double Math::abs(double f) +{ + return ::fabs(f); +} + +double Math::sqrt(double f) +{ + return ::sqrt(f); +} + +double Math::pow(double base, double exp) +{ + return ::pow(base, exp); +} + +double Math::ceil(double f) +{ + return ::ceil(f); +} + +double Math::cos(double f) +{ + return ::cos(f); +} + +double Math::sin(double f) +{ + return ::sin(f); +} + +double Math::tan(double f) +{ + return ::tan(f); +} + +double Math::atan2(double y, double x) +{ + return ::atan2(y, x); +} + +void Math::set3(float* v, float* out) +{ + out[0] = v[0]; + out[1] = v[1]; + out[2] = v[2]; +} + +float Math::dot3(float* a, float* b) +{ + return a[0]*b[0] + a[1]*b[1] + a[2]*b[2]; +} + +void Math::mul3(float scalar, float* v, float* out) +{ + out[0] = scalar * v[0]; + out[1] = scalar * v[1]; + out[2] = scalar * v[2]; +} + +void Math::add3(float* a, float* b, float* out) +{ + out[0] = a[0] + b[0]; + out[1] = a[1] + b[1]; + out[2] = a[2] + b[2]; +} + +void Math::sub3(float* a, float* b, float* out) +{ + out[0] = a[0] - b[0]; + out[1] = a[1] - b[1]; + out[2] = a[2] - b[2]; +} + +float Math::mag3(float* v) +{ + return sqrt(dot3(v, v)); +} + + +void Math::unit3(float* v, float* out) +{ + float imag = 1/mag3(v); + mul3(imag, v, out); +} + +void Math::cross3(float* a, float* b, float* out) +{ + float ax=a[0], ay=a[1], az=a[2]; + float bx=b[0], by=b[1], bz=b[2]; + out[0] = ay*bz - by*az; + out[1] = az*bx - bz*ax; + out[2] = ax*by - bx*ay; +} + +void Math::mmul33(float* a, float* b, float* out) +{ + float tmp[9]; + tmp[0] = a[0]*b[0] + a[1]*b[3] + a[2]*b[6]; + tmp[3] = a[3]*b[0] + a[4]*b[3] + a[5]*b[6]; + tmp[6] = a[6]*b[0] + a[7]*b[3] + a[8]*b[6]; + + tmp[1] = a[0]*b[1] + a[1]*b[4] + a[2]*b[7]; + tmp[4] = a[3]*b[1] + a[4]*b[4] + a[5]*b[7]; + tmp[7] = a[6]*b[1] + a[7]*b[4] + a[8]*b[7]; + + tmp[2] = a[0]*b[2] + a[1]*b[5] + a[2]*b[8]; + tmp[5] = a[3]*b[2] + a[4]*b[5] + a[5]*b[8]; + tmp[8] = a[6]*b[2] + a[7]*b[5] + a[8]*b[8]; + + for(int i=0; i<9; i++) + out[i] = tmp[i]; +} + +void Math::vmul33(float* m, float* v, float* out) +{ + float x = v[0], y = v[1], z = v[2]; + out[0] = x*m[0] + y*m[1] + z*m[2]; + out[1] = x*m[3] + y*m[4] + z*m[5]; + out[2] = x*m[6] + y*m[7] + z*m[8]; +} + +void Math::tmul33(float* m, float* v, float* out) +{ + float x = v[0], y = v[1], z = v[2]; + out[0] = x*m[0] + y*m[3] + z*m[6]; + out[1] = x*m[1] + y*m[4] + z*m[7]; + out[2] = x*m[2] + y*m[5] + z*m[8]; +} + +void Math::invert33(float* m, float* out) +{ + // Compute the inverse as the adjoint matrix times 1/(det M). + // A, B ... I are the cofactors of a b c + // d e f + // g h i + float a=m[0], b=m[1], c=m[2]; + float d=m[3], e=m[4], f=m[5]; + float g=m[6], h=m[7], i=m[8]; + + float A = (e*i - h*f); + float B = -(d*i - g*f); + float C = (d*h - g*e); + float D = -(b*i - h*c); + float E = (a*i - g*c); + float F = -(a*h - g*b); + float G = (b*f - e*c); + float H = -(a*f - d*c); + float I = (a*e - d*b); + + float id = 1/(a*A + b*B + c*C); + + out[0] = id*A; out[1] = id*D; out[2] = id*G; + out[3] = id*B; out[4] = id*E; out[5] = id*H; + out[6] = id*C; out[7] = id*F; out[8] = id*I; +} + +void Math::trans33(float* m, float* out) +{ + // 0 1 2 Elements 0, 4, and 8 are the same + // 3 4 5 Swap elements 1/3, 2/6, and 5/7 + // 6 7 8 + out[0] = m[0]; + out[4] = m[4]; + out[8] = m[8]; + + float tmp = m[1]; + out[1] = m[3]; + out[3] = tmp; + + tmp = m[2]; + out[2] = m[6]; + out[6] = tmp; + + tmp = m[5]; + out[5] = m[7]; + out[7] = tmp; +} + +void Math::ortho33(float* x, float* y, + float* xOut, float* yOut, float* zOut) +{ + float x0[3], y0[3]; + set3(x, x0); + set3(y, y0); + + unit3(x0, xOut); + cross3(xOut, y0, zOut); + unit3(zOut, zOut); + cross3(zOut, xOut, yOut); +} + +}; // namespace yasim diff --git a/src/FDM/YASim/Math.hpp b/src/FDM/YASim/Math.hpp new file mode 100644 index 000000000..9407c2a9a --- /dev/null +++ b/src/FDM/YASim/Math.hpp @@ -0,0 +1,70 @@ +#ifndef _MATH_HPP +#define _MATH_HPP + +namespace yasim { + +class Math +{ +public: + // Simple wrappers around library routines + static float abs(float f); + static float sqrt(float f); + static float pow(float base, float exp); + static float ceil(float f); + static float sin(float f); + static float cos(float f); + static float tan(float f); + static float atan2(float y, float x); + + // double variants of the above + static double abs(double f); + static double sqrt(double f); + static double pow(double base, double exp); + static double ceil(double f); + static double sin(double f); + static double cos(double f); + static double tan(double f); + static double atan2(double y, double x); + + // Some 3D vector stuff. In all cases, it is permissible for the + // "out" vector to be the same as one of the inputs. + static void set3(float* v, float* out); + static float dot3(float* a, float* b); + static void cross3(float* a, float* b, float* out); + static void mul3(float scalar, float* v, float* out); + static void add3(float* a, float* b, float* out); + static void sub3(float* a, float* b, float* out); + static float mag3(float* v); + static void unit3(float* v, float* out); + + // Matrix array convention: 0 1 2 + // 3 4 5 + // 6 7 8 + + // Multiply two matrices + static void mmul33(float* a, float* b, float* out); + + // Multiply by vector + static void vmul33(float* m, float* v, float* out); + + // Multiply the vector by the matrix transpose. Or pre-multiply the + // matrix by v as a row vector. Same thing. + static void tmul33(float* m, float* v, float* out); + + // Invert matrix + static void invert33(float* m, float* out); + + // Transpose matrix (for an orthonormal orientation matrix, this + // is the same as the inverse). + static void trans33(float* m, float* out); + + // Generates an orthonormal basis: + // xOut becomes the unit vector in the direction of x + // yOut is perpendicular to xOut in the x/y plane + // zOut becomes the unit vector: (xOut cross yOut) + static void ortho33(float* x, float* y, + float* xOut, float* yOut, float* zOut); +}; + +}; // namespace yasim +#endif // _MATH_HPP diff --git a/src/FDM/YASim/Model.cpp b/src/FDM/YASim/Model.cpp new file mode 100644 index 000000000..5cd77e9cb --- /dev/null +++ b/src/FDM/YASim/Model.cpp @@ -0,0 +1,296 @@ +#include <stdio.h> + +#include "Atmosphere.hpp" +#include "Thruster.hpp" +#include "Math.hpp" +#include "RigidBody.hpp" +#include "Integrator.hpp" +#include "Propeller.hpp" +#include "PistonEngine.hpp" +#include "Gear.hpp" +#include "Surface.hpp" +#include "Glue.hpp" + +#include "Model.hpp" +namespace yasim { + +void printState(State* s) +{ + State tmp = *s; + Math::vmul33(tmp.orient, tmp.v, tmp.v); + Math::vmul33(tmp.orient, tmp.acc, tmp.acc); + Math::vmul33(tmp.orient, tmp.rot, tmp.rot); + Math::vmul33(tmp.orient, tmp.racc, tmp.racc); + + printf("\nNEW STATE (LOCAL COORDS)\n"); + printf("pos: %10.2f %10.2f %10.2f\n", tmp.pos[0], tmp.pos[1], tmp.pos[2]); + printf("o: "); + for(int i=0; i<3; i++) { + if(i != 0) printf(" "); + printf("%6.2f %6.2f %6.2f\n", + tmp.orient[3*i+0], tmp.orient[3*i+1], tmp.orient[3*i+2]); + } + printf("v: %6.2f %6.2f %6.2f\n", tmp.v[0], tmp.v[1], tmp.v[2]); + printf("acc: %6.2f %6.2f %6.2f\n", tmp.acc[0], tmp.acc[1], tmp.acc[2]); + printf("rot: %6.2f %6.2f %6.2f\n", tmp.rot[0], tmp.rot[1], tmp.rot[2]); + printf("rac: %6.2f %6.2f %6.2f\n", tmp.racc[0], tmp.racc[1], tmp.racc[2]); +} + +Model::Model() +{ + for(int i=0; i<3; i++) _wind[i] = 0; + + _integrator.setBody(&_body); + _integrator.setEnvironment(this); + + // Default value of 30 Hz + _integrator.setInterval(1.0/30.0); +} + +Model::~Model() +{ + // FIXME: who owns these things? Need a policy +} + +void Model::getThrust(float* out) +{ + float tmp[3]; + out[0] = out[1] = out[2] = 0; + for(int i=0; i<_thrusters.size(); i++) { + Thruster* t = (Thruster*)_thrusters.get(i); + t->getThrust(tmp); + Math::add3(tmp, out, out); + } +} + +void Model::initIteration() +{ + // Precompute torque and angular momentum for the thrusters + for(int i=0; i<3; i++) + _gyro[i] = _torque[i] = 0; + for(int i=0; i<_thrusters.size(); i++) { + Thruster* t = (Thruster*)_thrusters.get(i); + + // Get the wind velocity at the thruster location + float pos[3], v[3]; + t->getPosition(pos); + localWind(pos, _s, v); + + t->setWind(v); + t->setDensity(_rho); + t->integrate(_integrator.getInterval()); + + t->getTorque(v); + Math::add3(v, _torque, _torque); + + t->getGyro(v); + Math::add3(v, _gyro, _gyro); + } +} + +void Model::iterate() +{ + initIteration(); + _body.recalc(); // FIXME: amortize this, somehow + _integrator.calcNewInterval(); +} + +State* Model::getState() +{ + return _s; +} + +void Model::setState(State* s) +{ + _integrator.setState(s); + _s = _integrator.getState(); +} + +RigidBody* Model::getBody() +{ + return &_body; +} + +Integrator* Model::getIntegrator() +{ + return &_integrator; +} + +Surface* Model::getSurface(int handle) +{ + return (Surface*)_surfaces.get(handle); +} + +int Model::addThruster(Thruster* t) +{ + return _thrusters.add(t); +} + +Thruster* Model::getThruster(int handle) +{ + return (Thruster*)_thrusters.get(handle); +} + +void Model::setThruster(int handle, Thruster* t) +{ + _thrusters.set(handle, t); +} + +int Model::addSurface(Surface* surf) +{ + return _surfaces.add(surf); +} + +int Model::addGear(Gear* gear) +{ + return _gears.add(gear); +} + +// The first three elements are a unit vector pointing from the global +// origin to the plane, the final element is the distance from the +// origin (the radius of the earth, in most implementations). So +// (v dot _ground)-_ground[3] gives the distance AGL. +void Model::setGroundPlane(double* planeNormal, double fromOrigin) +{ + for(int i=0; i<3; i++) _ground[i] = planeNormal[i]; + _ground[3] = fromOrigin; +} + +void Model::setAirDensity(float rho) +{ + _rho = rho; +} + +void Model::setAir(float pressure, float temp) +{ + _rho = Atmosphere::calcDensity(pressure, temp); +} + +void Model::setWind(float* wind) +{ + Math::set3(wind, _wind); +} + +void Model::calcForces(State* s) +{ + // Add in the pre-computed stuff. These values aren't part of the + // Runge-Kutta integration (they don't depend on position or + // velocity), and are therefore constant across the four calls to + // calcForces. They get computed before we begin the integration + // step. + _body.setGyro(_gyro); + _body.addTorque(_torque); + for(int i=0; i<_thrusters.size(); i++) { + Thruster* t = (Thruster*)_thrusters.get(i); + float thrust[3], pos[3]; + t->getThrust(thrust); + t->getPosition(pos); + _body.addForce(pos, thrust); + } + + // Gravity, convert to a force, then to local coordinates + float grav[3]; + Glue::geodUp(s->pos, grav); + Math::mul3(-9.8 * _body.getTotalMass(), grav, grav); + Math::vmul33(s->orient, grav, grav); + _body.addForce(grav); + + // Do each surface, remembering that the local velocity at each + // point is different due to rotation. + for(int i=0; i<_surfaces.size(); i++) { + Surface* sf = (Surface*)_surfaces.get(i); + + // Vsurf = wind - velocity + (rot cross (cg - pos)) + float vs[3], pos[3]; + sf->getPosition(pos); + localWind(pos, s, vs); + + float force[3], torque[3]; + sf->calcForce(vs, _rho, force, torque); + _body.addForce(pos, force); + _body.addTorque(torque); + } + + // Get a ground plane in local coordinates. The first three + // elements are the normal vector, the final one is the distance + // from the local origin along that vector to the ground plane + // (negative for objects "above" the ground) + float ground[4]; + ground[3] = localGround(s, ground); + + // Convert the velocity and rotation vectors to local coordinates + float lrot[3], lv[3]; + Math::vmul33(s->orient, s->rot, lrot); + Math::vmul33(s->orient, s->v, lv); + + // The landing gear + for(int i=0; i<_gears.size(); i++) { + float force[3], contact[3]; + Gear* g = (Gear*)_gears.get(i); + g->calcForce(&_body, lv, lrot, ground); + g->getForce(force, contact); + _body.addForce(contact, force); + } +} + +void Model::newState(State* s) +{ + _s = s; + + //printState(s); + + // Some simple collision detection + float ground[4], pos[3], cmpr[3]; + ground[3] = localGround(s, ground); + for(int i=0; i<_gears.size(); i++) { + Gear* g = (Gear*)_gears.get(i); + g->getPosition(pos); + g->getCompression(cmpr); + Math::add3(cmpr, pos, pos); + float dist = ground[3] - Math::dot3(pos, ground); + if(dist < 0) { + printf("CRASH: gear %d\n", i); + *(int*)0=0; + } + } +} + +// Returns a unit "down" vector for the ground in out, and the +// distance from the local origin to the ground as the return value. +// So for a given position V, "dist - (V dot out)" will be the height +// AGL. +float Model::localGround(State* s, float* out) +{ + // Get the ground's "down" vector, this can be in floats, because + // we don't need positioning accuracy. The direction has plenty + // of accuracy after truncation. + out[0] = -(float)_ground[0]; + out[1] = -(float)_ground[1]; + out[2] = -(float)_ground[2]; + Math::vmul33(s->orient, out, out); + + // The distance from the ground to the Aircraft's origin: + double dist = (s->pos[0]*_ground[0] + + s->pos[1]*_ground[1] + + s->pos[2]*_ground[2] - _ground[3]); + + return (float)dist; +} + +// Calculates the airflow direction at the given point and for the +// specified aircraft velocity. +void Model::localWind(float* pos, State* s, float* out) +{ + // Most of the input is in global coordinates. Fix that. + float lwind[3], lrot[3], lv[3]; + Math::vmul33(s->orient, _wind, lwind); + Math::vmul33(s->orient, s->rot, lrot); + Math::vmul33(s->orient, s->v, lv); + + _body.pointVelocity(pos, lrot, out); // rotational velocity + Math::mul3(-1, out, out); // (negated) + Math::add3(lwind, out, out); // + wind + Math::sub3(out, lv, out); // - velocity +} + +}; // namespace yasim diff --git a/src/FDM/YASim/Model.hpp b/src/FDM/YASim/Model.hpp new file mode 100644 index 000000000..a54799d01 --- /dev/null +++ b/src/FDM/YASim/Model.hpp @@ -0,0 +1,80 @@ +#ifndef _MODEL_HPP +#define _MODEL_HPP + +#include "Integrator.hpp" +#include "RigidBody.hpp" +#include "BodyEnvironment.hpp" +#include "util/Vector.hpp" + +namespace yasim { + +// Declare the types whose pointers get passed around here +class Integrator; +class Thruster; +class Surface; +class Gear; + +class Model : public BodyEnvironment { +public: + Model(); + virtual ~Model(); + + RigidBody* getBody(); + Integrator* getIntegrator(); + + State* getState(); + void setState(State* s); + + void iterate(); + + // Externally-managed subcomponents + int addThruster(Thruster* t); + int addSurface(Surface* surf); + int addGear(Gear* gear); + Surface* getSurface(int handle); + Gear* getGear(int handle); + + // Semi-private methods for use by the Airplane solver. + Thruster* getThruster(int handle); + void setThruster(int handle, Thruster* t); + void initIteration(); + void getThrust(float* out); + + // + // Per-iteration settables + // + void setGroundPlane(double* planeNormal, double fromOrigin); + void setWind(float* wind); + void setAirDensity(float rho); + void setAir(float pressure, float temp); + + // BodyEnvironment callbacks + virtual void calcForces(State* s); + virtual void newState(State* s); + +private: + void calcGearForce(Gear* g, float* v, float* rot, float* ground); + float gearFriction(float wgt, float v, Gear* g); + float localGround(State* s, float* out); + void localWind(float* pos, State* s, float* out); + + Integrator _integrator; + RigidBody _body; + + Vector _thrusters; + Vector _surfaces; + Vector _gears; + + double _ground[4]; + float _rho; + float _wind[3]; + + // Accumulators for the total internal gyro and engine torque + float _gyro[3]; + float _torque[3]; + + State* _s; +}; + +}; // namespace yasim +#endif // _MODEL_HPP diff --git a/src/FDM/YASim/PistonEngine.cpp b/src/FDM/YASim/PistonEngine.cpp new file mode 100644 index 000000000..bea871f0b --- /dev/null +++ b/src/FDM/YASim/PistonEngine.cpp @@ -0,0 +1,67 @@ +#include "PistonEngine.hpp" +namespace yasim { + +PistonEngine::PistonEngine(float power, float speed) +{ + // Presume a BSFC (in lb/hour per HP) of 0.45. In SI that becomes + // (2.2 lb/kg, 745.7 W/hp, 3600 sec/hour) 3.69e-07 kg/Ws. + _f0 = power * 3.69e-07; + + _P0 = power; + _omega0 = speed; + + // We must be at sea level under standard conditions + _rho0 = 1.225; // kg/m^3 + + // Further presume that takeoff is (duh) full throttle and + // peak-power, that means that by our efficiency function, we are + // at 11/8 of "ideal" fuel flow. + float realFlow = _f0 * (11.0/8.0); + _mixCoeff = realFlow * 1.1 / _omega0; +} + +void PistonEngine::setThrottle(float t) +{ + _throttle = t; +} + +void PistonEngine::setMixture(float m) +{ + _mixture = m; +} + +void PistonEngine::calc(float density, float speed, + float* torqueOut, float* fuelFlowOut) +{ + // The actual fuel flow + float fuel = _mixture * _mixCoeff * speed; + + // manifold air density + float rho = density * _throttle; + + // How much fuel could be burned with ideal (i.e. uncorrected!) + // combustion. + float burnable = _f0 * (rho/_rho0) * (speed/_omega0); + + // Calculate the fuel that actually burns to produce work. The + // idea is that less than 5/8 of ideal, we get complete + // combustion. We use up all the oxygen at 1 3/8 of ideal (that + // is, you need to waste fuel to use all your O2). In between, + // interpolate. This vaguely matches a curve I copied out of a + // book for a single engine. Shrug. + float burned; + float r = fuel/burnable; + if (burnable == 0) burned = 0; + else if(r < .625) burned = fuel; + else if(r > 1.375) burned = burnable; + else burned = fuel + (burnable-fuel)*(r-.625)*(4.0/3.0); + + // And finally the power is just the reference power scaled by the + // amount of fuel burned. + float power = _P0 * burned/_f0; + + *torqueOut = power/speed; + *fuelFlowOut = fuel; +} + +}; // namespace yasim diff --git a/src/FDM/YASim/PistonEngine.hpp b/src/FDM/YASim/PistonEngine.hpp new file mode 100644 index 000000000..95f43c438 --- /dev/null +++ b/src/FDM/YASim/PistonEngine.hpp @@ -0,0 +1,35 @@ +#ifndef _PISTONENGINE_HPP +#define _PISTONENGINE_HPP + +namespace yasim { + +class PistonEngine { +public: + // Initializes an engine from known "takeoff" parameters. + PistonEngine(float power, float spd); + + void setThrottle(float throttle); + void setMixture(float mixture); + + // Calculates power output and fuel flow, based on a given + // throttle setting (0-1 corresponding to the fraction of + // "available" manifold pressure), mixture (fuel flux per rpm, + // 0-1, where 1 is "max rich", or a little bit more than needed + // for rated power at sea level) + void calc(float density, float speed, + float* powerOut, float* fuelFlowOut); + +private: + float _P0; // reference power setting + float _omega0; // " engine speed + float _rho0; // " manifold air density + float _f0; // "ideal" fuel flow at P0/omega0 + float _mixCoeff; // fuel flow per omega at full mixture + + // Runtime settables: + float _throttle; + float _mixture; +}; + +}; // namespace yasim +#endif // _PISTONENGINE_HPP diff --git a/src/FDM/YASim/PropEngine.cpp b/src/FDM/YASim/PropEngine.cpp new file mode 100644 index 000000000..56ebae51d --- /dev/null +++ b/src/FDM/YASim/PropEngine.cpp @@ -0,0 +1,94 @@ +#include "Math.hpp" +#include "Propeller.hpp" +#include "PistonEngine.hpp" +#include "PropEngine.hpp" +namespace yasim { + +PropEngine::PropEngine(Propeller* prop, PistonEngine* eng, float moment) +{ + // Start off at 500rpm, because the start code doesn't exist yet + _omega = 52.3; + _dir[0] = 1; _dir[1] = 0; _dir[2] = 0; + + _prop = prop; + _eng = eng; + _moment = moment; +} + +PropEngine::~PropEngine() +{ + delete _prop; + delete _eng; +} + +float PropEngine::getOmega() +{ + return _omega; +} + +Thruster* PropEngine::clone() +{ + // FIXME: bloody mess + PropEngine* p = new PropEngine(_prop, _eng, _moment); + cloneInto(p); + p->_prop = new Propeller(*_prop); + p->_eng = new PistonEngine(*_eng); + return p; +} + +void PropEngine::getThrust(float* out) +{ + for(int i=0; i<3; i++) out[i] = _thrust[i]; +} + +void PropEngine::getTorque(float* out) +{ + for(int i=0; i<3; i++) out[i] = _torque[i]; +} + +void PropEngine::getGyro(float* out) +{ + for(int i=0; i<3; i++) out[i] = _gyro[i]; +} + +float PropEngine::getFuelFlow() +{ + return _fuelFlow; +} + +void PropEngine::integrate(float dt) +{ + float speed = -Math::dot3(_wind, _dir); + + float propTorque, engTorque, thrust; + + _eng->setThrottle(_throttle); + _eng->setMixture(_mixture); + + _prop->calc(_rho, speed, _omega, + &thrust, &propTorque); + _eng->calc(_rho, _omega, &engTorque, &_fuelFlow); + + // Turn the thrust into a vector and save it + Math::mul3(thrust, _dir, _thrust); + + // Euler-integrate the RPM. This doesn't need the full-on + // Runge-Kutta stuff. + _omega += dt*(engTorque-propTorque)/Math::abs(_moment); + + // FIXME: Integrate the propeller governor here, when that gets + // implemented... + + // Store the total angular momentum into _gyro + Math::mul3(_omega*_moment, _dir, _gyro); + + // Accumulate the engine torque, it acta on the body as a whole. + // (Note: engine torque, not propeller torque. They can be + // different, but the difference goes to accelerating the + // rotation. It is the engine torque that is felt at the shaft + // and works on the body.) + float tau = _moment < 0 ? engTorque : -engTorque; + Math::mul3(tau, _dir, _torque); +} + +}; // namespace yasim diff --git a/src/FDM/YASim/PropEngine.hpp b/src/FDM/YASim/PropEngine.hpp new file mode 100644 index 000000000..481552a29 --- /dev/null +++ b/src/FDM/YASim/PropEngine.hpp @@ -0,0 +1,42 @@ +#ifndef _PROPENGINE_HPP +#define _PROPENGINE_HPP + +#include "Thruster.hpp" + +namespace yasim { + +class Propeller; +class PistonEngine; + +class PropEngine : public Thruster { +public: + PropEngine(Propeller* prop, PistonEngine* eng, float moment); + virtual ~PropEngine(); + + virtual Thruster* clone(); + + // Dynamic output + virtual void getThrust(float* out); + virtual void getTorque(float* out); + virtual void getGyro(float* out); + virtual float getFuelFlow(); + + // Runtime instructions + virtual void integrate(float dt); + + float getOmega(); + +private: + float _moment; + Propeller* _prop; + PistonEngine* _eng; + + float _omega; // RPM, in radians/sec + float _thrust[3]; + float _torque[3]; + float _gyro[3]; + float _fuelFlow; +}; + +}; // namespace yasim +#endif // _PROPENGINE_HPP diff --git a/src/FDM/YASim/Propeller.cpp b/src/FDM/YASim/Propeller.cpp new file mode 100644 index 000000000..bce03ab8b --- /dev/null +++ b/src/FDM/YASim/Propeller.cpp @@ -0,0 +1,89 @@ +#include "Atmosphere.hpp" +#include "Math.hpp" +#include "Propeller.hpp" +namespace yasim { + +Propeller::Propeller(float radius, float v, float omega, + float rho, float power, float omega0, + float power0) +{ + // Initialize numeric constants: + _lambdaPeak = Math::pow(9.0, -1.0/8.0); + _beta = 1.0/(Math::pow(9.0, -1.0/8.0) - Math::pow(9.0, -9.0/8.0)); + + _r = radius; + _etaC = 0.85; // make this settable? + + _J0 = v/(omega*_lambdaPeak); + + float V2 = v*v + (_r*omega)*(_r*omega); + _F0 = 2*_etaC*power/(rho*v*V2); + + float stallAngle = 0.25; + _lambdaS = _r*(_J0/_r - stallAngle) / _J0; + + // Now compute a correction for zero forward speed to make the + // takeoff performance correct. + float torque0 = power0/omega0; + float thrust, torque; + _takeoffCoef = 1; + calc(Atmosphere::getStdDensity(0), 0, omega0, &thrust, &torque); + _takeoffCoef = torque/torque0; +} + +void Propeller::calc(float density, float v, float omega, + float* thrustOut, float* torqueOut) +{ + float tipspd = _r*omega; + float V2 = v*v + tipspd*tipspd; + + // Clamp v (forward velocity) to zero, now that we've used it to + // calculate V (propeller "speed") + if(v < 0) v = 0; + + float J = v/omega; + float lambda = J/_J0; + + float torque = 0; + if(lambda > 1) { + lambda = 1.0/lambda; + torque = (density*V2*_F0*_J0)/(8*_etaC*_beta*(1-_lambdaPeak)); + } + + // There's an undefined point at 1. Just offset by a tiny bit to + // fix (note: the discontinuity is at EXACTLY one, this is about + // the only time in history you'll see me use == on a floating + // point number!) + if(lambda == 1) lambda = 0.9999; + + // Compute thrust, remembering to clamp lambda to the stall point + float lambda2 = lambda < _lambdaS ? _lambdaS : lambda; + float thrust = (0.5*density*V2*_F0/(1-_lambdaPeak))*(1-lambda2); + + // Calculate lambda^8 + float l8 = lambda*lambda; l8 = l8*l8; l8 = l8*l8; + + // thrust/torque ratio + float gamma = (_etaC*_beta/_J0)*(1-l8); + + // Correct slow speeds to get the takeoff parameters correct + if(lambda < _lambdaPeak) { + // This will interpolate takeoffCoef along a descending from 1 + // at lambda==0 to 0 at the peak, fairing smoothly into the + // flat peak. + float frac = (lambda - _lambdaPeak)/_lambdaPeak; + gamma *= 1 + (_takeoffCoef - 1)*frac*frac; + } + + if(torque > 0) { + torque -= thrust/gamma; + thrust = -thrust; + } else { + torque = thrust/gamma; + } + + *thrustOut = thrust; + *torqueOut = torque; +} + +}; // namespace yasim diff --git a/src/FDM/YASim/Propeller.hpp b/src/FDM/YASim/Propeller.hpp new file mode 100644 index 000000000..eed76450c --- /dev/null +++ b/src/FDM/YASim/Propeller.hpp @@ -0,0 +1,35 @@ +#ifndef _PROPELLER_HPP +#define _PROPELLER_HPP + +namespace yasim { + +// A generic propeller model. See the TeX documentation for +// implementation details, this is too hairy to explain in code +// comments. +class Propeller +{ +public: + // Initializes a propeller with the specified "cruise" numbers + // for airspeed, RPM, power and air density, and two "takeoff" + // numbers for RPM and power (with air speed and density being + // zero and sea level). RPM values are in radians per second, of + // course. + Propeller(float radius, float v, float omega, float rho, float power, + float omega0, float power0); + + void calc(float density, float v, float omega, + float* thrustOut, float* torqueOut); + +private: + float _r; // characteristic radius + float _J0; // zero-thrust advance ratio + float _lambdaS; // "propeller stall" normalized advance ratio + float _F0; // thrust coefficient + float _etaC; // Peak efficiency + float _lambdaPeak; // constant, ~0.759835; + float _beta; // constant, ~1.48058; + float _takeoffCoef; // correction to get the zero-speed torque right +}; + +}; // namespace yasim +#endif // _PROPELLER_HPP diff --git a/src/FDM/YASim/RigidBody.cpp b/src/FDM/YASim/RigidBody.cpp new file mode 100644 index 000000000..5ddded7c2 --- /dev/null +++ b/src/FDM/YASim/RigidBody.cpp @@ -0,0 +1,204 @@ +#include "Math.hpp" +#include "RigidBody.hpp" +namespace yasim { + +RigidBody::RigidBody() +{ + // Allocate space for 16 masses initially. More space will be + // allocated dynamically. + _nMasses = 0; + _massesAlloced = 16; + _masses = new Mass[_massesAlloced]; + _gyro[0] = _gyro[1] = _gyro[2] = 0; + _spin[0] = _spin[1] = _spin[2] = 0; +} + +RigidBody::~RigidBody() +{ + delete[] _masses; +} + +int RigidBody::addMass(float mass, float* pos) +{ + // If out of space, reallocate twice as much + if(_nMasses == _massesAlloced) { + _massesAlloced *= 2; + Mass *m2 = new Mass[_massesAlloced]; + for(int i=0; i<_nMasses; i++) + m2[i] = _masses[i]; + delete[] _masses; + _masses = m2; + } + + _masses[_nMasses].m = mass; + Math::set3(pos, _masses[_nMasses].p); + return _nMasses++; +} + +void RigidBody::setMass(int handle, float mass) +{ + _masses[handle].m = mass; +} + +void RigidBody::setMass(int handle, float mass, float* pos) +{ + _masses[handle].m = mass; + Math::set3(pos, _masses[handle].p); +} + +int RigidBody::numMasses() +{ + return _nMasses; +} + +float RigidBody::getMass(int handle) +{ + return _masses[handle].m; +} + +void RigidBody::getMassPosition(int handle, float* out) +{ + out[0] = _masses[handle].p[0]; + out[1] = _masses[handle].p[1]; + out[2] = _masses[handle].p[2]; +} + +float RigidBody::getTotalMass() +{ + return _totalMass; +} + +// Calcualtes the rotational velocity of a particular point. All +// coordinates are local! +void RigidBody::pointVelocity(float* pos, float* rot, float* out) +{ + Math::sub3(pos, _cg, out); // out = pos-cg + Math::cross3(rot, out, out); // = rot cross (pos-cg) +} + +void RigidBody::setGyro(float* angularMomentum) +{ + Math::set3(angularMomentum, _gyro); +} + +void RigidBody::recalc() +{ + // Calculate the c.g and total mass: + _totalMass = 0; + _cg[0] = _cg[1] = _cg[2] = 0; + for(int i=0; i<_nMasses; i++) { + float m = _masses[i].m; + _totalMass += m; + _cg[0] += m * _masses[i].p[0]; + _cg[1] += m * _masses[i].p[1]; + _cg[2] += m * _masses[i].p[2]; + } + Math::mul3(1/_totalMass, _cg, _cg); + + // Now the inertia tensor: + for(int i=0; i<9; i++) + _I[i] = 0; + + for(int i=0; i<_nMasses; i++) { + float m = _masses[i].m; + + float x = _masses[i].p[0] - _cg[0]; + float y = _masses[i].p[1] - _cg[1]; + float z = _masses[i].p[2] - _cg[2]; + + float xy = m*x*y; float yz = m*y*z; float zx = m*z*x; + float x2 = m*x*x; float y2 = m*y*y; float z2 = m*z*z; + + _I[0] += y2+z2; _I[1] -= xy; _I[2] -= zx; + _I[3] -= xy; _I[4] += x2+z2; _I[5] -= yz; + _I[6] -= zx; _I[7] -= yz; _I[8] += x2+y2; + } + + // And its inverse + Math::invert33(_I, _invI); +} + +void RigidBody::reset() +{ + _torque[0] = _torque[1] = _torque[2] = 0; + _force[0] = _force[1] = _force[2] = 0; +} + +void RigidBody::addForce(float* force) +{ + Math::add3(_force, force, _force); +} + +void RigidBody::addTorque(float* torque) +{ + Math::add3(_torque, torque, _torque); +} + +void RigidBody::addForce(float* pos, float* force) +{ + addForce(force); + + // For a force F at position X, the torque about the c.g C is: + // torque = F cross (C - X) + float v[3], t[3]; + Math::sub3(_cg, pos, v); + Math::cross3(force, v, t); + addTorque(t); +} + +void RigidBody::setBodySpin(float* rotation) +{ + Math::set3(rotation, _spin); +} + +void RigidBody::getCG(float* cgOut) +{ + Math::set3(_cg, cgOut); +} + +void RigidBody::getAccel(float* accelOut) +{ + Math::mul3(1/_totalMass, _force, accelOut); +} + +void RigidBody::getAccel(float* pos, float* accelOut) +{ + getAccel(accelOut); + + // Turn the "spin" vector into a normalized spin axis "a" and a + // radians/sec scalar "rate". + float a[3]; + float rate = Math::mag3(_spin); + Math::set3(_spin, a); + Math::mul3(1/rate, a, a); + + float v[3]; + Math::sub3(_cg, pos, v); // v = cg - pos + Math::mul3(Math::dot3(v, a), a, a); // a = a * (v dot a) + Math::add3(v, a, v); // v = v + a + + // Now v contains the vector from pos to the rotation axis. + // Multiply by the square of the rotation rate to get the linear + // acceleration. + Math::mul3(rate*rate, v, v); + Math::add3(v, accelOut, accelOut); +} + +void RigidBody::getAngularAccel(float* accelOut) +{ + // Compute "tau" as the externally applied torque, plus the + // counter-torque due to the internal gyro. + float tau[3]; // torque + Math::cross3(_gyro, _spin, tau); + Math::add3(_torque, tau, tau); + + // Now work the equation of motion. Use "v" as a notational + // shorthand, as the value isn't an acceleration until the end. + float *v = accelOut; + Math::vmul33(_I, _spin, v); // v = I*omega + Math::cross3(_spin, v, v); // v = omega X I*omega + Math::add3(tau, v, v); // v = tau + (omega X I*omega) + Math::vmul33(_invI, v, v); // v = invI*(tau + (omega X I*omega)) +} + +}; // namespace yasim diff --git a/src/FDM/YASim/RigidBody.hpp b/src/FDM/YASim/RigidBody.hpp new file mode 100644 index 000000000..08e07ef88 --- /dev/null +++ b/src/FDM/YASim/RigidBody.hpp @@ -0,0 +1,123 @@ +#ifndef _RIGIDBODY_HPP +#define _RIGIDBODY_HPP + +namespace yasim { + +// +// A RigidBody object maintains all "internal" state about an object, +// accumulates force and torque information from external sources, and +// calculates the resulting accelerations. +// +// +// Units note: obviously, the choice of mass, time and distance units +// is up to the user. If you provide mass in kilograms, forces in +// newtons, and torques in newton-meters, you'll get your +// accelerations back in m/s^2. The angular units, however, are +// UNIFORMLY radians. Angular velocities are radians per <time unit>, +// the angular acceleration you get back is radians per <time unit>^2, +// and the angular momenta supplied to setGyro must be in radians, +// too. Radians, not degrees. Don't forget. +// +class RigidBody +{ +public: + RigidBody(); + ~RigidBody(); + + // Adds a point mass to the system. Returns a handle so the gyro + // can be later modified via setMass(). + int addMass(float mass, float* pos); + + // Modifies a previously-added point mass (fuel tank running dry, + // gear going up, swing wing swinging, pilot bailing out, etc...) + void setMass(int handle, float mass); + void setMass(int handle, float mass, float* pos); + + int numMasses(); + float getMass(int handle); + void getMassPosition(int handle, float* out); + float getTotalMass(); + + // The velocity, in local coordinates, of the specified point on a + // body rotating about its c.g. with velocity rot. + void pointVelocity(float* pos, float* rot, float* out); + + // Sets the "gyroscope" for the body. This is the total + // "intrinsic" angular momentum of the body; that is, rotations of + // sub-objects, NOT rotation of the whole body within the global + // frame. Because angular momentum is additive in this way, we + // don't need to specify specific gyro objects; just add all their + // momenta together and set it here. + void setGyro(float* angularMomentum); + + + // When masses are moved or changed, this object needs to + // regenerate its internal tables. This step is expensive, so + // it's exposed to the client who can amortize the call across + // multiple changes. + void recalc(); + + // Resets the current force/torque parameters to zero. + void reset(); + + + // Applies a force at the specified position. + void addForce(float* pos, float* force); + + // Applies a force at the center of gravity. + void addForce(float* force); + + // Adds a torque with the specified axis and magnitude + void addTorque(float* torque); + + // Sets the rotation rate of the body (about its c.g.) within the + // surrounding environment. This is needed to compute torque on + // the body due to the centripetal forces involved in the + // rotation. NOTE: the rotation vector, like all other + // coordinates used here, is specified IN THE LOCAL COORDINATE + // SYSTEM. + void setBodySpin(float* rotation); + + + + // Returns the center of gravity of the masses, in the body + // coordinate system. + void getCG(float* cgOut); + + // Returns the acceleration of the body's c.g. relative to the + // rest of the world, specified in local coordinates. + void getAccel(float* accelOut); + + // Returns the acceleration of a specific location in local + // coordinates. If the body is rotating, this will be different + // from the c.g. acceleration due to the centripetal accelerations + // of points not on the rotation axis. + void getAccel(float* pos, float* accelOut); + + // Returns the instantaneous rate of change of the angular + // velocity, as a vector in local coordinates. + void getAngularAccel(float* accelOut); + +private: + struct Mass { float m; float p[3]; }; + + // Internal "rotational structure" + Mass* _masses; + int _nMasses; + int _massesAlloced; + float _totalMass; + float _cg[3]; + float _gyro[3]; + + // Inertia tensor, and its inverse. Computed from the above. + float _I[9]; + float _invI[9]; + + // Externally determined quantities + float _force[3]; + float _torque[3]; + float _spin[3]; +}; + +}; // namespace yasim +#endif // _RIGIDBODY_HPP diff --git a/src/FDM/YASim/Surface.cpp b/src/FDM/YASim/Surface.cpp new file mode 100644 index 000000000..e0a7093a4 --- /dev/null +++ b/src/FDM/YASim/Surface.cpp @@ -0,0 +1,245 @@ +#include "Math.hpp" +#include "Surface.hpp" +namespace yasim { + +Surface::Surface() +{ + // Start in a "sane" mode, so unset stuff doesn't freak us out + _c0 = 1; + _cx = _cy = _cz = 1; + _cz0 = 0; + _peaks[0] = _peaks[1] = 1; + for(int i=0; i<4; i++) + _stalls[i] = _widths[i] = 0; + _orient[0] = 1; _orient[1] = 0; _orient[2] = 0; + _orient[3] = 0; _orient[4] = 1; _orient[5] = 0; + _orient[6] = 0; _orient[7] = 0; _orient[8] = 1; + + _incidence = 0; + _slatPos = _spoilerPos = _flapPos = 0; + _slatDrag = _spoilerDrag = _flapDrag = 1; + _flapLift = 0; + _slatAlpha = 0; + _spoilerLift = 1; +} + +void Surface::setPosition(float* p) +{ + for(int i=0; i<3; i++) _pos[i] = p[i]; +} + +void Surface::getPosition(float* out) +{ + for(int i=0; i<3; i++) out[i] = _pos[i]; +} + +void Surface::setChord(float chord) +{ + _chord = chord; +} + +void Surface::setTotalDrag(float c0) +{ + _c0 = c0; +} + +float Surface::getTotalDrag() +{ + return _c0; +} + +void Surface::setXDrag(float cx) +{ + _cx = cx; +} + +void Surface::setYDrag(float cy) +{ + _cy = cy; +} + +void Surface::setZDrag(float cz) +{ + _cz = cz; +} + +void Surface::setBaseZDrag(float cz0) +{ + _cz0 = cz0; +} + +void Surface::setStallPeak(int i, float peak) +{ + _peaks[i] = peak; +} + +void Surface::setStall(int i, float alpha) +{ + _stalls[i] = alpha; +} + +void Surface::setStallWidth(int i, float width) +{ + _widths[i] = width; +} + +void Surface::setOrientation(float* o) +{ + for(int i=0; i<9; i++) + _orient[i] = o[i]; +} + +void Surface::setIncidence(float angle) +{ + _incidence = angle; +} + +void Surface::setSlatParams(float stallDelta, float dragPenalty) +{ + _slatAlpha = stallDelta; + _slatDrag = dragPenalty; +} + +void Surface::setFlapParams(float liftAdd, float dragPenalty) +{ + _flapLift = liftAdd; + _flapDrag = dragPenalty; +} + +void Surface::setSpoilerParams(float liftPenalty, float dragPenalty) +{ + _spoilerLift = liftPenalty; + _spoilerDrag = dragPenalty; +} + +void Surface::setFlap(float pos) +{ + _flapPos = pos; +} + +void Surface::setSlat(float pos) +{ + _slatPos = pos; +} + +void Surface::setSpoiler(float pos) +{ + _spoilerPos = pos; +} + +// Calculate the aerodynamic force given a wind vector v (in the +// aircraft's "local" coordinates) and an air density rho. Returns a +// torque about the Y axis, too. +void Surface::calcForce(float* v, float rho, float* out, float* torque) +{ + // Split v into magnitude and direction: + float vel = Math::mag3(v); + + // Handle the blowup condition. Zero velocity means zero force by + // definition. + if(vel == 0) { + for(int i=0; i<3; i++) out[i] = torque[i] = 0; + return; + } + + Math::mul3(1/vel, v, out); + + // Convert to the surface's coordinates + Math::vmul33(_orient, out, out); + + // "Rotate" by the incidence angle. Assume small angles, so we + // need to diddle only the Z component, X is relatively unchanged + // by small rotations. + out[2] += _incidence * out[0]; // z' = z + incidence * x + + // Diddle the Z force according to our configuration + float stallMul = stallFunc(out); + stallMul *= 1 + _spoilerPos * (_spoilerLift - 1); + float stallLift = (stallMul - 1) * _cz * out[2]; + float flapLift = _cz * _flapPos * (_flapLift-1); + + out[2] *= _cz; // scaling factor + out[2] += _cz*_cz0; // zero-alpha lift + out[2] += stallLift; + out[2] += flapLift; + + // Airfoil lift (pre-stall and zero-alpha) torques "up" (negative + // torque) around the Y axis, while flap lift pushes down. Both + // forces are considered to act at one third chord from the + // center. Convert to local (i.e. airplane) coordiantes and store + // into "torque". + torque[0] = 0; + torque[1] = 0.33 * _chord * (flapLift - (_cz*_cz0 + stallLift)); + torque[2] = 0; + Math::tmul33(_orient, torque, torque); + + // Diddle X (drag) and Y (side force) in the same manner + out[0] *= _cx * controlDrag(); + out[1] *= _cy; + + // Reverse the incidence rotation to get back to surface + // coordinates. + out[2] -= _incidence * out[0]; + + // Convert back to external coordinates + Math::tmul33(_orient, out, out); + + // Add in the units to make a real force: + float scale = 0.5*rho*vel*vel*_c0; + Math::mul3(scale, out, out); + Math::mul3(scale, torque, torque); +} + +// Returns a multiplier for the "plain" force equations that +// approximates an airfoil's lift/stall curve. +float Surface::stallFunc(float* v) +{ + // Sanity check to treat FPU psychopathology + if(v[0] == 0) return 1; + + float alpha = Math::abs(v[2]/v[0]); + + // Wacky use of indexing, see setStall*() methods. + int fwdBak = v[0] > 0; // set if this is "backward motion" + int posNeg = v[2] < 0; // set if the lift is toward -z + int i = (fwdBak<<1) | posNeg; + + float stallAlpha = _stalls[i]; + if(stallAlpha == 0) + return 1; + + if(i == 0) + stallAlpha += _slatAlpha; + + // Beyond the stall + if(alpha > stallAlpha+_widths[i]) + return 1; + + // (note mask: we want to use the "positive" stall angle here) + float scale = 0.5*_peaks[fwdBak]/_stalls[i&2]; + + // Before the stall + if(alpha <= stallAlpha) + return scale; + + // Inside the stall. Compute a cubic interpolation between the + // pre-stall "scale" value and the post-stall unity. + float frac = (alpha - stallAlpha) / _widths[i]; + frac = frac*frac*(3-2*frac); + + return scale*(1-frac) + frac; +} + +float Surface::controlDrag() +{ + float d = 1; + d *= 1 + _spoilerPos * (_spoilerDrag - 1); + d *= 1 + _slatPos * (_slatDrag - 1); + + // FIXME: flaps should REDUCE drag when the reduce lift! + d *= 1 + Math::abs(_flapPos) * (_flapDrag - 1); + + return d; +} + +}; // namespace yasim diff --git a/src/FDM/YASim/Surface.hpp b/src/FDM/YASim/Surface.hpp new file mode 100644 index 000000000..b1d81c677 --- /dev/null +++ b/src/FDM/YASim/Surface.hpp @@ -0,0 +1,94 @@ +#ifndef _SURFACE_HPP +#define _SURFACE_HPP + +namespace yasim { + +// FIXME: need a "chord" member for calculating moments. Generic +// forces act at the center, but "pre-stall" lift acts towards the +// front, and flaps act (in both lift and drag) toward the back. +class Surface +{ +public: + Surface(); + + // Position of this surface in local coords + void setPosition(float* p); + void getPosition(float* out); + + // Distance scale along the X axis + void setChord(float chord); + + // Slats act to move the stall peak by the specified angle, and + // increase drag by the multiplier specified. + void setSlatParams(float stallDelta, float dragPenalty); + + // Flaps add to lift coefficient, and multiply drag. + void setFlapParams(float liftAdd, float dragPenalty); + + // Spoilers reduce the pre-stall lift, and multiply drag. + void setSpoilerParams(float liftPenalty, float dragPenalty); + + // Positions for the controls, in the range [0:1]. [-1:1] for + // flaps, with positive meaning "force goes towards positive Z" + void setFlap(float pos); + void setSlat(float pos); + void setSpoiler(float pos); + + // local -> Surface coords + void setOrientation(float* o); + + // For variable-incidence control surfaces. The angle is a + // negative rotation about the surface's Y axis, in radians, so + // positive is "up" (i.e. "positive AoA") + void setIncidence(float angle); + + void setTotalDrag(float c0); + float getTotalDrag(); + + void setXDrag(float cx); + void setYDrag(float cy); + void setZDrag(float cz); + + // zero-alpha Z drag ("camber") specified as a fraction of cz + void setBaseZDrag(float cz0); + + // i: 0 == forward, 1 == backwards + void setStallPeak(int i, float peak); + + // i: 0 == fwd/+z, 1 == fwd/-z, 2 == rev/+z, 3 == rev/-z + void setStall(int i, float alpha); + void setStallWidth(int i, float width); + + void calcForce(float* v, float rho, float* forceOut, float* torqueOut); + +private: + float stallFunc(float* v); + float controlDrag(); + + float _chord; // X-axis size + float _c0; // total force coefficient + float _cx; // X-axis force coefficient + float _cy; // Y-axis force coefficient + float _cz; // Z-axis force coefficient + float _cz0; // Z-axis force offset + float _peaks[2]; // Stall peak coefficients (fwd, back) + float _stalls[4]; // Stall angles (fwd/back, pos/neg) + float _widths[4]; // Stall widths " " + float _pos[3]; // position in local coords + float _orient[9]; // local->surface orthonormal matrix + + float _slatAlpha; + float _slatDrag; + float _flapLift; + float _flapDrag; + float _spoilerLift; + float _spoilerDrag; + + float _slatPos; + float _flapPos; + float _spoilerPos; + float _incidence; +}; + +}; // namespace yasim +#endif // _SURFACE_HPP diff --git a/src/FDM/YASim/Thruster.cpp b/src/FDM/YASim/Thruster.cpp new file mode 100644 index 000000000..c288edf5e --- /dev/null +++ b/src/FDM/YASim/Thruster.cpp @@ -0,0 +1,75 @@ +#include "Math.hpp" +#include "Thruster.hpp" +namespace yasim { + +Thruster::Thruster() +{ + _dir[0] = 1; _dir[1] = 0; _dir[2] = 0; + for(int i=0; i<3; i++) _pos[i] = _wind[i] = 0; + _throttle = 0; + _mixture = 0; + _propAdvance = 0; + _rho = 0; +} + +Thruster::~Thruster() +{ +} + +void Thruster::getPosition(float* out) +{ + for(int i=0; i<3; i++) out[i] = _pos[i]; +} + +void Thruster::setPosition(float* pos) +{ + for(int i=0; i<3; i++) _pos[i] = pos[i]; +} + +void Thruster::getDirection(float* out) +{ + for(int i=0; i<3; i++) out[i] = _dir[i]; +} + +void Thruster::setDirection(float* dir) +{ + Math::unit3(dir, _dir); +} + +void Thruster::setThrottle(float throttle) +{ + _throttle = throttle; +} + +void Thruster::setMixture(float mixture) +{ + _mixture = mixture; +} + +void Thruster::setPropAdvance(float propAdvance) +{ + _propAdvance = propAdvance; +} + +void Thruster::setWind(float* wind) +{ + for(int i=0; i<3; i++) _wind[i] = wind[i]; +} + +void Thruster::setDensity(float rho) +{ + _rho = rho; +} + +void Thruster::cloneInto(Thruster* out) +{ + for(int i=0; i<3; i++) { + out->_pos[i] = _pos[i]; + out->_dir[i] = _dir[i]; + } + out->_throttle = _throttle; + out->_mixture = _mixture; + out->_propAdvance = _propAdvance; +} + +}; // namespace yasim diff --git a/src/FDM/YASim/Thruster.hpp b/src/FDM/YASim/Thruster.hpp new file mode 100644 index 000000000..25ceac8cf --- /dev/null +++ b/src/FDM/YASim/Thruster.hpp @@ -0,0 +1,50 @@ +#ifndef _THRUSTER_HPP +#define _THRUSTER_HPP + +namespace yasim { + +class Thruster { +public: + Thruster(); + virtual ~Thruster(); + + // Static data + void getPosition(float* out); + void setPosition(float* pos); + void getDirection(float* out); + void setDirection(float* dir); + + virtual Thruster* clone()=0; + + // Controls + void setThrottle(float throttle); + void setMixture(float mixture); + void setPropAdvance(float advance); + + // Dynamic output + virtual void getThrust(float* out)=0; + virtual void getTorque(float* out)=0; + virtual void getGyro(float* out)=0; + virtual float getFuelFlow()=0; + + // Runtime instructions + void setWind(float* wind); + void setDensity(float rho); + virtual void integrate(float dt)=0; + +protected: + void cloneInto(Thruster* out); + + float _pos[3]; + float _dir[3]; + float _throttle; + float _mixture; + float _propAdvance; + + float _wind[3]; + float _rho; +}; + +}; // namespace yasim +#endif // _THRUSTER_HPP + diff --git a/src/FDM/YASim/Wing.cpp b/src/FDM/YASim/Wing.cpp new file mode 100644 index 000000000..4fa499d0b --- /dev/null +++ b/src/FDM/YASim/Wing.cpp @@ -0,0 +1,399 @@ +#include "Math.hpp" +#include "Surface.hpp" +#include "Wing.hpp" +namespace yasim { + +Wing::Wing() +{ + _mirror = false; + _base[0] = _base[1] = _base[2] = 0; + _length = 0; + _chord = 0; + _taper = 0; + _sweep = 0; + _dihedral = 0; + _stall = 0; + _stallWidth = 0; + _stallPeak = 0; + _camber = 0; + _incidence = 0; + _dragScale = 1; + _liftRatio = 1; + _flap0Start = 0; + _flap0End = 0; + _flap0Lift = 0; + _flap0Drag = 0; + _flap1Start = 0; + _flap1End = 0; + _flap1Lift = 0; + _flap1Drag = 0; + _spoilerStart = 0; + _spoilerEnd = 0; + _spoilerLift = 0; + _spoilerDrag = 0; + _slatStart = 0; + _slatEnd = 0; + _slatAoA = 0; + _slatDrag = 0; +} + +Wing::~Wing() +{ + for(int i=0; i<_surfs.size(); i++) { + SurfRec* s = (SurfRec*)_surfs.get(i); + delete s->surface; + delete s; + } +} + +int Wing::numSurfaces() +{ + return _surfs.size(); +} + +Surface* Wing::getSurface(int n) +{ + return ((SurfRec*)_surfs.get(n))->surface; +} + +float Wing::getSurfaceWeight(int n) +{ + return ((SurfRec*)_surfs.get(n))->weight; +} + +void Wing::setMirror(bool mirror) +{ + _mirror = mirror; +} + +void Wing::setBase(float* base) +{ + for(int i=0; i<3; i++) _base[i] = base[i]; +} + +void Wing::setLength(float length) +{ + _length = length; +} + +void Wing::setChord(float chord) +{ + _chord = chord; +} + +void Wing::setTaper(float taper) +{ + _taper = taper; +} + +void Wing::setSweep(float sweep) +{ + _sweep = sweep; +} + +void Wing::setDihedral(float dihedral) +{ + _dihedral = dihedral; +} + +void Wing::setStall(float aoa) +{ + _stall = aoa; +} + +void Wing::setStallWidth(float angle) +{ + _stallWidth = angle; +} + +void Wing::setStallPeak(float fraction) +{ + _stallPeak = fraction; +} + +void Wing::setCamber(float camber) +{ + _camber = camber; +} + +void Wing::setIncidence(float incidence) +{ + _incidence = incidence; + for(int i=0; i<_surfs.size(); i++) + ((SurfRec*)_surfs.get(i))->surface->setIncidence(incidence); +} + +void Wing::setFlap0(float start, float end, float lift, float drag) +{ + _flap0Start = start; + _flap0End = end; + _flap0Lift = lift; + _flap0Drag = drag; +} + +void Wing::setFlap1(float start, float end, float lift, float drag) +{ + _flap1Start = start; + _flap1End = end; + _flap1Lift = lift; + _flap1Drag = drag; +} + +void Wing::setSlat(float start, float end, float aoa, float drag) +{ + _slatStart = start; + _slatEnd = end; + _slatAoA = aoa; + _slatDrag = drag; +} + +void Wing::setSpoiler(float start, float end, float lift, float drag) +{ + _spoilerStart = start; + _spoilerEnd = end; + _spoilerLift = lift; + _spoilerDrag = drag; +} + +void Wing::setFlap0(float lval, float rval) +{ + for(int i=0; i<_flap0Surfs.size(); i++) { + ((Surface*)_flap0Surfs.get(i))->setFlap(lval); + if(_mirror) ((Surface*)_flap0Surfs.get(++i))->setFlap(rval); + } +} + +void Wing::setFlap1(float lval, float rval) +{ + for(int i=0; i<_flap1Surfs.size(); i++) { + ((Surface*)_flap1Surfs.get(i))->setFlap(lval); + if(_mirror) ((Surface*)_flap1Surfs.get(++i))->setFlap(rval); + } +} + +void Wing::setSpoiler(float lval, float rval) +{ + for(int i=0; i<_spoilerSurfs.size(); i++) { + ((Surface*)_spoilerSurfs.get(i))->setSpoiler(lval); + if(_mirror) ((Surface*)_spoilerSurfs.get(++i))->setSpoiler(rval); + } +} + +void Wing::setSlat(float val) +{ + for(int i=0; i<_slatSurfs.size(); i++) + ((Surface*)_slatSurfs.get(i))->setSlat(val); +} + +void Wing::compile() +{ + // Have we already been compiled? + if(_surfs.size() != 0) return; + + // Assemble the start/end coordinates into an array, sort them, + // and remove duplicates. This gives us the boundaries of our + // segments. + float bounds[8]; + bounds[0] = _flap0Start; bounds[1] = _flap0End; + bounds[2] = _flap1Start; bounds[3] = _flap1End; + bounds[4] = _spoilerStart; bounds[5] = _spoilerEnd; + bounds[6] = _slatStart; bounds[7] = _slatEnd; + + // Sort in increasing order + for(int i=0; i<8; i++) { + int minIdx = i; + float minVal = bounds[i]; + for(int j=i+1; j<8; j++) { + if(bounds[j] < minVal) { + minIdx = j; + minVal = bounds[j]; + } + } + float tmp = bounds[i]; + bounds[i] = minVal; bounds[minIdx] = tmp; + } + + // Uniqify + float last = bounds[0]; + int nbounds = 1; + for(int i=1; i<8; i++) { + if(bounds[i] != last) + bounds[nbounds++] = bounds[i]; + last = bounds[i]; + } + + // Calculate a "nominal" segment length equal to an average chord, + // normalized to lie within 0-1 over the length of the wing. + float segLen = _chord * (0.5*(_taper+1)) / _length; + + // Generating a unit vector pointing out the left wing. + float left[3]; + left[0] = -Math::tan(_sweep); + left[1] = Math::cos(_dihedral); + left[2] = Math::sin(_dihedral); + Math::unit3(left, left); + + // Calculate coordinates for the root and tip of the wing + float root[3], tip[3]; + Math::set3(_base, root); + Math::set3(left, tip); + Math::mul3(_length, tip, tip); + Math::add3(root, tip, tip); + + // The wing's Y axis will be the "left" vector. The Z axis will + // be perpendicular to this and the local (!) X axis, because we + // want motion along the local X axis to be zero AoA (i.e. in the + // wing's XY plane) by definition. Then the local X coordinate is + // just Y cross Z. + float orient[9], rightOrient[9]; + float *x = orient, *y = orient+3, *z = orient+6; + x[0] = 1; x[1] = 0; x[2] = 0; + Math::set3(left, y); + Math::cross3(x, y, z); + Math::unit3(z, z); + Math::cross3(y, z, x); + + if(_mirror) { + // Derive the right side orientation matrix from this one. + for(int i=0; i<9; i++) rightOrient[i] = orient[i]; + + // Negate all Y coordinates, this gets us a valid basis, but + // it's left handed! So... + for(int i=1; i<9; i+=3) rightOrient[i] = -rightOrient[i]; + + // Change the direction of the Y axis to get back to a + // right-handed system. + for(int i=3; i<6; i++) rightOrient[i] = -rightOrient[i]; + } + + // Now go through each boundary and make segments + for(int i=0; i<(nbounds-1); i++) { + float start = bounds[i]; + float end = bounds[i+1]; + float mid = (start+end)/2; + + bool flap0=0, flap1=0, slat=0, spoiler=0; + if(_flap0Start < mid && mid < _flap0End) flap0 = 1; + if(_flap1Start < mid && mid < _flap1End) flap1 = 1; + if(_slatStart < mid && mid < _slatEnd) slat = 1; + if(_spoilerStart < mid && mid < _spoilerEnd) spoiler = 1; + + // FIXME: Should probably detect an error here if both flap0 + // and flap1 are set. Right now flap1 overrides. + + int nSegs = (int)Math::ceil((end-start)/segLen); + float segWid = _length * (end - start)/nSegs; + + for(int j=0; j<nSegs; j++) { + float frac = start + (j+0.5) * (end-start)/nSegs; + float pos[3]; + interp(root, tip, frac, pos); + + float chord = _chord * (1 - (1-_taper)*frac); + + Surface *s = newSurface(pos, orient, chord, + flap0, flap1, slat, spoiler); + + SurfRec *sr = new SurfRec(); + sr->surface = s; + sr->weight = chord * segWid; + s->setTotalDrag(sr->weight); + _surfs.add(sr); + + if(_mirror) { + pos[1] = -pos[1]; + s = newSurface(pos, rightOrient, chord, + flap0, flap1, slat, spoiler); + sr = new SurfRec(); + sr->surface = s; + sr->weight = chord * segWid; + s->setTotalDrag(sr->weight); + _surfs.add(sr); + } + } + } +} + +float Wing::getDragScale() +{ + return _dragScale; +} + +void Wing::setDragScale(float scale) +{ + _dragScale = scale; + for(int i=0; i<_surfs.size(); i++) { + SurfRec* s = (SurfRec*)_surfs.get(i); + s->surface->setTotalDrag(scale * s->weight); + } +} + +void Wing::setLiftRatio(float ratio) +{ + _liftRatio = ratio; + for(int i=0; i<_surfs.size(); i++) + ((SurfRec*)_surfs.get(i))->surface->setZDrag(ratio); +} + +float Wing::getLiftRatio() +{ + return _liftRatio; +} + +Surface* Wing::newSurface(float* pos, float* orient, float chord, + bool flap0, bool flap1, bool slat, bool spoiler) +{ + Surface* s = new Surface(); + + s->setPosition(pos); + s->setOrientation(orient); + s->setChord(chord); + + // Camber is expressed as a fraction of stall peak, so convert. + s->setBaseZDrag(_camber*_stallPeak); + + // The "main" (i.e. normal) stall angle + float stallAoA = _stall - _stallWidth/4; + s->setStall(0, stallAoA); + s->setStallWidth(0, _stallWidth); + s->setStallPeak(0, _stallPeak); + + // The negative AoA stall is the same if we're using an uncambered + // airfoil, otherwise a "little badder". + if(_camber > 0) { + s->setStall(1, stallAoA * 0.8); + s->setStallWidth(1, _stallWidth * 0.5); + } else { + s->setStall(1, stallAoA); + s->setStall(1, _stallWidth); + } + + // The "reverse" stalls are unmeasurable junk. Just use 13deg and + // "sharp". + s->setStallPeak(1, 1); + for(int i=2; i<4; i++) { + s->setStall(i, 0.2267); + s->setStallWidth(i, 1); + } + + if(flap0) s->setFlapParams(_flap0Lift, _flap0Drag); + if(flap1) s->setFlapParams(_flap1Lift, _flap1Drag); + if(slat) s->setSlatParams(_slatAoA, _slatDrag); + if(spoiler) s->setSpoilerParams(_spoilerLift, _spoilerDrag); + + if(flap0) _flap0Surfs.add(s); + if(flap1) _flap1Surfs.add(s); + if(slat) _slatSurfs.add(s); + if(spoiler) _spoilerSurfs.add(s); + + return s; +} + +void Wing::interp(float* v1, float* v2, float frac, float* out) +{ + out[0] = v1[0] + frac*(v2[0]-v1[0]); + out[1] = v1[1] + frac*(v2[1]-v1[1]); + out[2] = v1[2] + frac*(v2[2]-v1[2]); +} + +}; // namespace yasim diff --git a/src/FDM/YASim/Wing.hpp b/src/FDM/YASim/Wing.hpp new file mode 100644 index 000000000..af47840f6 --- /dev/null +++ b/src/FDM/YASim/Wing.hpp @@ -0,0 +1,115 @@ +#ifndef _WING_HPP +#define _WING_HPP + +#include "util/Vector.hpp" + +namespace yasim { + +class Surface; + +// FIXME: need to handle "inverted" controls for mirrored wings. +class Wing { +public: + Wing(); + ~Wing(); + + // Do we mirror ourselves about the XZ plane? + void setMirror(bool mirror); + + // Wing geometry: + void setBase(float* base); // in local coordinates + void setLength(float length); // dist. ALONG wing (not span!) + void setChord(float chord); // at base, measured along X axis + void setTaper(float taper); // fraction, 0-1 + void setSweep(float sweep); // radians + void setDihedral(float dihedral); // radians, positive is "up" + + void setStall(float aoa); + void setStallWidth(float angle); + void setStallPeak(float fraction); + void setCamber(float camber); + void setIncidence(float incidence); + + void setFlap0(float start, float end, float lift, float drag); + void setFlap1(float start, float end, float lift, float drag); + void setSpoiler(float start, float end, float lift, float drag); + void setSlat(float start, float end, float aoa, float drag); + + // Set the control axes for the sub-surfaces + void setFlap0(float lval, float rval); + void setFlap1(float lval, float rval); + void setSpoiler(float lval, float rval); + void setSlat(float val); + + // Compile the thing into a bunch of Surface objects + void compile(); + + // Query the list of Surface objects + int numSurfaces(); + Surface* getSurface(int n); + float getSurfaceWeight(int n); + + // The overall drag coefficient for the wing as a whole. Units are + // arbitrary. + void setDragScale(float scale); + float getDragScale(); + + // The ratio of force along the Z (lift) direction of each wing + // segment to that along the X (drag) direction. + void setLiftRatio(float ratio); + float getLiftRatio(); + +private: + void interp(float* v1, float* v2, float frac, float* out); + Surface* newSurface(float* pos, float* orient, float chord, + bool flap0, bool flap1, bool slat, bool spoiler); + + struct SurfRec { Surface * surface; float weight; }; + + Vector _surfs; + Vector _flap0Surfs; + Vector _flap1Surfs; + Vector _slatSurfs; + Vector _spoilerSurfs; + + bool _mirror; + + float _base[3]; + float _length; + float _chord; + float _taper; + float _sweep; + float _dihedral; + + float _stall; + float _stallWidth; + float _stallPeak; + float _camber; + float _incidence; + + float _dragScale; + float _liftRatio; + + float _flap0Start; + float _flap0End; + float _flap0Lift; + float _flap0Drag; + + float _flap1Start; + float _flap1End; + float _flap1Lift; + float _flap1Drag; + + float _spoilerStart; + float _spoilerEnd; + float _spoilerLift; + float _spoilerDrag; + + float _slatStart; + float _slatEnd; + float _slatAoA; + float _slatDrag; +}; + +}; // namespace yasim +#endif // _WING_HPP diff --git a/src/FDM/YASim/YASim.cxx b/src/FDM/YASim/YASim.cxx new file mode 100644 index 000000000..175f3d1b9 --- /dev/null +++ b/src/FDM/YASim/YASim.cxx @@ -0,0 +1,353 @@ +#include <stdio.h> + +#include <simgear/misc/sg_path.hxx> +#include <simgear/debug/logstream.hxx> +#include <simgear/easyxml.hxx> +#include <Main/globals.hxx> +#include <Main/fg_props.hxx> +#include <Scenery/scenery.hxx> + +#include "FGFDM.hpp" +#include "Atmosphere.hpp" +#include "Math.hpp" +#include "Airplane.hpp" +#include "Model.hpp" +#include "Integrator.hpp" +#include "Glue.hpp" +#include "Gear.hpp" +#include "PropEngine.hpp" + +#include "YASim.hxx" + +using namespace yasim; + +static const float RAD2DEG = 180/3.14159265358979323846; +static const float M2FT = 3.2808399; +static const float FT2M = 0.3048; +static const float MPS2KTS = 3600.0/1852.0; +static const float CM2GALS = 264.172037284; // gallons/cubic meter + +void YASim::printDEBUG() +{ + static int debugCount = 0; + + debugCount++; + if(debugCount >= 3) { + debugCount = 0; + +// printf("RPM %.1f FF %.1f\n", +// fgGetFloat("/engines/engine[0]/rpm"), +// fgGetFloat("/engines/engine[0]/fuel-flow-gph")); + +// printf("gear: %f\n", fgGetFloat("/controls/gear-down")); + +// printf("alpha %5.1f beta %5.1f\n", get_Alpha()*57.3, get_Beta()*57.3); + +// printf("pilot: %f %f %f\n", +// fgGetDouble("/sim/view/pilot/x-offset-m"), +// fgGetDouble("/sim/view/pilot/y-offset-m"), +// fgGetDouble("/sim/view/pilot/z-offset-m")); + } +} + +YASim::YASim(double dt) +{ + set_delta_t(dt); + _fdm = new FGFDM(); + + // Because the integration method is via fourth-order Runge-Kutta, + // we only get an "output" state for every 4 times the internal + // forces are calculated. So divide dt by four to account for + // this, and only run an iteration every fourth time through + // update. + _dt = dt * 4; + _fdm->getAirplane()->getModel()->getIntegrator()->setInterval(_dt); + _updateCount = 0; +} + +void YASim::report() +{ + Airplane* a = _fdm->getAirplane(); + + float aoa = a->getCruiseAoA() * RAD2DEG; + float tail = -1 * a->getTailIncidence() * RAD2DEG; + float drag = 1000 * a->getDragCoefficient(); + + SG_LOG(SG_FLIGHT,SG_INFO,"YASim solution results:"); + SG_LOG(SG_FLIGHT,SG_INFO," Iterations: "<<a->getSolutionIterations()); + SG_LOG(SG_FLIGHT,SG_INFO,"Drag Coefficient: "<< drag); + SG_LOG(SG_FLIGHT,SG_INFO," Lift Ratio: "<<a->getLiftRatio()); + SG_LOG(SG_FLIGHT,SG_INFO," Cruise AoA: "<< aoa); + SG_LOG(SG_FLIGHT,SG_INFO," Tail Incidence: "<< tail); + + float cg[3]; + char buf[256]; + a->getModel()->getBody()->getCG(cg); + sprintf(buf, " CG: %.1f, %.1f, %.1f", cg[0], cg[1], cg[2]); + SG_LOG(SG_FLIGHT, SG_INFO, buf); + + if(a->getFailureMsg()) { + SG_LOG(SG_FLIGHT, SG_ALERT, "YASim SOLUTION FAILURE:"); + SG_LOG(SG_FLIGHT, SG_ALERT, a->getFailureMsg()); + exit(1); + } +} + +void YASim::init() +{ + Airplane* a = _fdm->getAirplane(); + Model* m = a->getModel(); + + // Superclass hook + common_init(); + + // Build a filename and parse it + SGPath f(globals->get_fg_root()); + f.append("Aircraft"); + f.append(fgGetString("/sim/aircraft")); + f.concat(".xml"); + readXML(f.str(), *_fdm); + + // Compile it into a real airplane, and tell the user what they got + a->compile(); + report(); + + _fdm->init(); + + // Lift the plane up so the gear clear the ground + float minGearZ = 1e18; + for(int i=0; i<a->numGear(); i++) { + Gear* g = a->getGear(i); + float pos[3]; + g->getPosition(pos); + if(pos[2] < minGearZ) + minGearZ = pos[2]; + } + _set_Altitude(get_Altitude() - minGearZ*M2FT); + + // The pilot's eyepoint + float pilot[3]; + a->getPilotPos(pilot); + fgSetFloat("/sim/view/pilot/x-offset-m", -pilot[0]); + fgSetFloat("/sim/view/pilot/y-offset-m", -pilot[1]); + fgSetFloat("/sim/view/pilot/z-offset-m", pilot[2]); + + // Blank the state, and copy in ours + State s; + m->setState(&s); + + copyToYASim(true); + set_inited(true); +} + +bool YASim::update(int iterations) +{ + for(int i=0; i<iterations; i++) { + // Remember, update only every 4th call + _updateCount++; + if(_updateCount >= 4) { + + copyToYASim(false); + _fdm->iterate(_dt); + copyFromYASim(); + + printDEBUG(); + + _updateCount = 0; + } + } + + return true; // what does this mean? +} + +void YASim::copyToYASim(bool copyState) +{ + // Physical state + float lat = get_Latitude(); + float lon = get_Longitude(); + float alt = get_Altitude() * FT2M; + float roll = get_Phi(); + float pitch = get_Theta(); + float hdg = get_Psi(); + + // Environment + float wind[3]; + wind[0] = get_V_north_airmass() * FT2M; + wind[1] = get_V_east_airmass() * FT2M; + wind[2] = get_V_down_airmass() * FT2M; + double ground = get_Runway_altitude() * FT2M; + + // You'd this this would work, but it doesn't. These values are + // always zero. Use a "standard" pressure intstead. + // + // float pressure = get_Static_pressure(); + // float temp = get_Static_temperature(); + float pressure = Atmosphere::getStdPressure(alt); + float temp = Atmosphere::getStdTemperature(alt); + + // Convert and set: + Model* model = _fdm->getAirplane()->getModel(); + State s; + float xyz2ned[9]; + Glue::xyz2nedMat(lat, lon, xyz2ned); + + // position + Glue::geod2xyz(lat, lon, alt, s.pos); + + // orientation + Glue::euler2orient(roll, pitch, hdg, s.orient); + Math::mmul33(s.orient, xyz2ned, s.orient); + + // Copy in the existing velocity for now... + Math::set3(model->getState()->v, s.v); + + if(copyState) + model->setState(&s); + + // wind + Math::tmul33(xyz2ned, wind, wind); + model->setWind(wind); + + // ground. Calculate a cartesian coordinate for the ground under + // us, find the (geodetic) up vector normal to the ground, then + // use that to find the final (radius) term of the plane equation. + double xyz[3], gplane[3]; float up[3]; + Glue::geod2xyz(lat, lon, ground, xyz); + Glue::geodUp(xyz, up); // FIXME, needless reverse computation... + for(int i=0; i<3; i++) gplane[i] = up[i]; + double rad = gplane[0]*xyz[0] + gplane[1]*xyz[1] + gplane[2]*xyz[2]; + model->setGroundPlane(gplane, rad); + + // air + model->setAir(pressure, temp); +} + +// All the settables: +// +// These are set below: +// _set_Accels_Local +// _set_Accels_Body +// _set_Accels_CG_Body +// _set_Accels_Pilot_Body +// _set_Accels_CG_Body_N +// _set_Velocities_Local +// _set_Velocities_Ground +// _set_Velocities_Wind_Body +// _set_Omega_Body +// _set_Euler_Rates +// _set_Euler_Angles +// _set_V_rel_wind +// _set_V_ground_speed +// _set_V_equiv_kts +// _set_V_calibrated_kts +// _set_Alpha +// _set_Beta +// _set_Mach_number +// _set_Climb_Rate +// _set_Tank1Fuel +// _set_Tank2Fuel +// _set_Altitude_AGL +// _set_Geodetic_Position +// _set_Runway_altitude + +// Ignoring these, because they're unused: +// _set_Geocentric_Position +// _set_Geocentric_Rates +// _set_Cos_phi +// _set_Cos_theta +// _set_Earth_position_angle (WTF?) +// _set_Gamma_vert_rad +// _set_Inertias +// _set_T_Local_to_Body +// _set_CG_Position +// _set_Sea_Level_Radius + +// Externally set via the weather code: +// _set_Velocities_Local_Airmass +// _set_Density +// _set_Static_pressure +// _set_Static_temperature +void YASim::copyFromYASim() +{ + Model* model = _fdm->getAirplane()->getModel(); + State* s = model->getState(); + + // position + double lat, lon, alt; + Glue::xyz2geod(s->pos, &lat, &lon, &alt); + _set_Geodetic_Position(lat, lon, alt*M2FT); + + // UNUSED + //_set_Geocentric_Position(Glue::geod2geocLat(lat), lon, alt*M2FT); + + // FIXME: there's a normal vector available too, use it. + float groundMSL = scenery.get_cur_elev(); + _set_Runway_altitude(groundMSL*M2FT); + _set_Altitude_AGL((alt - groundMSL)*M2FT); + + // useful conversion matrix + float xyz2ned[9]; + Glue::xyz2nedMat(lat, lon, xyz2ned); + + // velocity + float v[3]; + Math::vmul33(xyz2ned, s->v, v); + _set_Velocities_Local(M2FT*v[0], M2FT*v[1], M2FT*v[2]); + _set_V_ground_speed(Math::sqrt(M2FT*v[0]*M2FT*v[0] + + M2FT*v[1]*M2FT*v[1])); + _set_Climb_Rate(-M2FT*v[2]); + + // The HUD uses this, but inverts down (?!) + _set_Velocities_Ground(M2FT*v[0], M2FT*v[1], -M2FT*v[2]); + + // _set_Geocentric_Rates(M2FT*v[0], M2FT*v[1], M2FT*v[2]); // UNUSED + + Math::vmul33(s->orient, s->v, v); + _set_Velocities_Wind_Body(v[0]*M2FT, -v[1]*M2FT, -v[2]*M2FT); + _set_V_rel_wind(Math::mag3(v)*M2FT); // units? + + // These don't work, use a dummy based on altitude + // float P = get_Static_pressure(); + // float T = get_Static_temperature(); + float P = Atmosphere::getStdPressure(alt); + float T = Atmosphere::getStdTemperature(alt); + _set_V_equiv_kts(Atmosphere::calcVEAS(v[0], P, T)*MPS2KTS); + _set_V_calibrated_kts(Atmosphere::calcVCAS(v[0], P, T)*MPS2KTS); + _set_Mach_number(Atmosphere::calcMach(v[0], T)); + + // acceleration + Math::vmul33(xyz2ned, s->acc, v); + _set_Accels_Local(M2FT*v[0], M2FT*v[1], M2FT*v[2]); + + Math::vmul33(s->orient, s->acc, v); + _set_Accels_Body(M2FT*v[0], -M2FT*v[1], -M2FT*v[2]); + _set_Accels_CG_Body(M2FT*v[0], -M2FT*v[1], -M2FT*v[2]); + + _fdm->getAirplane()->getPilotAccel(v); + _set_Accels_Pilot_Body(M2FT*v[0], -M2FT*v[1], -M2FT*v[2]); + + // The one appears (!) to want inverted pilot acceleration + // numbers, in G's... + Math::mul3(1.0/9.8, v, v); + _set_Accels_CG_Body_N(v[0], -v[1], -v[2]); + + // orientation + float alpha, beta; + Glue::calcAlphaBeta(s, &alpha, &beta); + _set_Alpha(alpha); + _set_Beta(beta); + + float tmp[9]; + Math::trans33(xyz2ned, tmp); + Math::mmul33(s->orient, tmp, tmp); + float roll, pitch, hdg; + Glue::orient2euler(tmp, &roll, &pitch, &hdg); + _set_Euler_Angles(roll, pitch, hdg); + + // rotation + Math::vmul33(s->orient, s->rot, v); + _set_Omega_Body(v[0], -v[1], -v[2]); + + Glue::calcEulerRates(s, &roll, &pitch, &hdg); + _set_Euler_Rates(roll, pitch, hdg); +} diff --git a/src/FDM/YASim/YASim.hxx b/src/FDM/YASim/YASim.hxx new file mode 100644 index 000000000..30a4409c6 --- /dev/null +++ b/src/FDM/YASim/YASim.hxx @@ -0,0 +1,30 @@ +#ifndef _YASIM_HXX +#define _YASIM_HXX + +#include <FDM/flight.hxx> + +namespace yasim { class FGFDM; }; + +class YASim : public FGInterface { +public: + YASim(double dt); + + // Load externally set stuff into the FDM + virtual void init(); + + // Run an iteration + virtual bool update(int iterations); + + private: + void report(); + void copyFromYASim(); + void copyToYASim(bool copyState); + + void printDEBUG(); + + yasim::FGFDM* _fdm; + int _updateCount; + float _dt; +}; + +#endif // _YASIM_HXX