diff --git a/src/FDM/YASim/Airplane.cpp b/src/FDM/YASim/Airplane.cpp
index 2665eb1f9..2c7e898af 100644
--- a/src/FDM/YASim/Airplane.cpp
+++ b/src/FDM/YASim/Airplane.cpp
@@ -17,7 +17,6 @@
 namespace yasim {
 
 // gadgets
-inline float norm(float f) { return f<1 ? 1/f : f; }
 inline float abs(float f) { return f<0 ? -f : f; }
 
 // Solver threshold.  How close to the solution are we trying
@@ -57,6 +56,9 @@ Airplane::Airplane()
     _tailIncidence = 0;
 
     _failureMsg = 0;
+    _wingsN = 0;
+    _cgMaxX = -1e6;
+    _cgMinX = 1e6;
 }
 
 Airplane::~Airplane()
@@ -113,16 +115,6 @@ void Airplane::calcFuelWeights()
     }
 }
 
-ControlMap* Airplane::getControlMap()
-{
-    return &_controls;
-}
-
-Model* Airplane::getModel()
-{
-    return &_model;
-}
-
 void Airplane::getPilotAccel(float* out)
 {
     State* s = _model.getState();
@@ -145,43 +137,6 @@ void Airplane::getPilotAccel(float* out)
     // FIXME: rotational & centripetal acceleration needed
 }
 
-void Airplane::setPilotPos(float* pos)
-{
-    int i;
-    for(i=0; i<3; i++) _pilotPos[i] = pos[i];
-}
-
-void Airplane::getPilotPos(float* out)
-{
-    int i;
-    for(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;
-}
-
-Hook* Airplane::getHook()
-{
-    return _model.getHook();
-}
-
-Launchbar* Airplane::getLaunchbar()
-{
-    return _model.getLaunchbar();
-}
-
-Rotorgear* Airplane::getRotorgear()
-{
-    return _model.getRotorgear();
-}
-
 void Airplane::updateGearState()
 {
     for(int i=0; i<_gears.size(); i++) {
@@ -247,51 +202,6 @@ void Airplane::addSolutionWeight(bool approach, int idx, float wgt)
     _solveWeights.add(w);
 }
 
-int Airplane::numTanks()
-{
-    return _tanks.size();
-}
-
-float Airplane::getFuel(int tank)
-{
-    return ((Tank*)_tanks.get(tank))->fill;
-}
-
-float Airplane::setFuel(int tank, float fuel)
-{
-    return ((Tank*)_tanks.get(tank))->fill = fuel;
-}
-
-float Airplane::getFuelDensity(int tank)
-{
-    return ((Tank*)_tanks.get(tank))->density;
-}
-
-float Airplane::getTankCapacity(int tank)
-{
-    return ((Tank*)_tanks.get(tank))->cap;
-}
-
-void Airplane::setWeight(float weight)
-{
-    _emptyWeight = weight;
-}
-
-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,
                            float taper, float mid, 
                            float cx, float cy, float cz, float idrag)
@@ -330,21 +240,10 @@ void Airplane::addGear(Gear* gear)
     g->gear = gear;
     g->surf = 0;
     _gears.add(g);
-}
-
-void Airplane::addHook(Hook* hook)
-{
-    _model.addHook(hook);
-}
-
-void Airplane::addHitch(Hitch* hitch)
-{
-    _model.addHitch(hitch);
-}
-
-void Airplane::addLaunchbar(Launchbar* launchbar)
-{
-    _model.addLaunchbar(launchbar);
+    float pos[3];
+    g->gear->getPosition(pos);
+    if (pos[0] > _cgMaxX) _cgMaxX = pos[0];
+    if (pos[0] < _cgMinX) _cgMinX = pos[0];
 }
 
 void Airplane::addThruster(Thruster* thruster, float mass, float* cg)
@@ -357,12 +256,14 @@ void Airplane::addThruster(Thruster* thruster, float mass, float* cg)
     _thrusters.add(t);
 }
 
+/// Use ballast to redistribute mass, this is NOT added to empty weight.
 void Airplane::addBallast(float* pos, float mass)
 {
-    _model.getBody()->addMass(mass, pos);
+    _model.getBody()->addMass(mass, pos, true);
     _ballast += mass;
 }
 
+/// Masses configurable at runtime, e.g. cargo, pax
 int Airplane::addWeight(float* pos, float size)
 {
     WeightRec* wr = new WeightRec();
@@ -377,6 +278,7 @@ int Airplane::addWeight(float* pos, float size)
     return _weights.add(wr);
 }
 
+/// Change weight of a previously added mass point
 void Airplane::setWeight(int handle, float mass)
 {
     WeightRec* wr = (WeightRec*)_weights.get(handle);
@@ -398,45 +300,14 @@ void Airplane::setWeight(int handle, float mass)
 
 void Airplane::setFuelFraction(float frac)
 {
-    int i;
-    for(i=0; i<_tanks.size(); i++) {
+    for(int i=0; i<_tanks.size(); i++) {
         Tank* t = (Tank*)_tanks.get(i);
         t->fill = frac * t->cap;
         _model.getBody()->setMass(t->handle, t->cap * frac);
     }
 }
 
-float Airplane::getDragCoefficient()
-{
-    return _dragFactor;
-}
-
-float Airplane::getLiftRatio()
-{
-    return _liftRatio;
-}
-
-float Airplane::getCruiseAoA()
-{
-    return _cruiseAoA;
-}
-
-float Airplane::getTailIncidence()
-{
-    return _tailIncidence;
-}
-
-const char* Airplane::getFailureMsg()
-{
-    return _failureMsg;
-}
-
-int Airplane::getSolutionIterations()
-{
-    return _solutionIterations;
-}
-
-void Airplane::setupState(float aoa, float speed, float gla, State* s)
+void Airplane::setupState(const float aoa, const float speed, const float gla, State* s)
 {
     float cosAoA = Math::cos(aoa);
     float sinAoA = Math::sin(aoa);
@@ -444,10 +315,10 @@ void Airplane::setupState(float aoa, float speed, float gla, State* s)
     s->orient[3] =       0; s->orient[4] = 1; s->orient[5] =      0;
     s->orient[6] = -sinAoA; s->orient[7] = 0; s->orient[8] = cosAoA;
 
+    //? what is gla? v[1]=y, v[2]=z? should sin go to v2 instead v1?
     s->v[0] = speed*Math::cos(gla); s->v[1] = -speed*Math::sin(gla); s->v[2] = 0;
 
-    int i;
-    for(i=0; i<3; i++)
+    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
@@ -455,39 +326,59 @@ void Airplane::setupState(float aoa, float speed, float gla, State* s)
     s->pos[2] = 1;
 }
 
+/**
+ * @brief add contact point for crash detection 
+ * used to add wingtips and fuselage nose and tail
+ * 
+ * @param pos ...
+ * @return void
+ */
+
 void Airplane::addContactPoint(float* pos)
 {
     ContactRec* c = new ContactRec;
     c->gear = 0;
-    c->p[0] = pos[0];
-    c->p[1] = pos[1];
-    c->p[2] = pos[2];
+    Math::set3(pos, c->p);
     _contacts.add(c);
 }
 
 float Airplane::compileWing(Wing* w)
 {
-    // The tip of the wing is a contact point
-    float tip[3];
-    w->getTip(tip);
-    addContactPoint(tip);
-    if(w->isMirrored()) {
-        tip[1] *= -1;
-        addContactPoint(tip);
-    }
-
     // Make sure it's initialized.  The surfaces will pop out with
     // total drag coefficients equal to their areas, which is what we
     // want.
     w->compile();
 
-    float wgt = 0;
-    int i;
-    for(i=0; i<w->numSurfaces(); i++) {
-        Surface* s = (Surface*)w->getSurface(i);
+    // The tip of the wing is a contact point
+    float tip[3];
+    // need compile() before getTip()!
+    w->getTip(tip);
+    addContactPoint(tip);
+    if(w->isMirrored()) {
+        tip[1] *= -1;
+        addContactPoint(tip);
+        tip[1] *= -1; //undo mirror
+    }
+    if (_wingsN != 0) {
+      _wingsN->getNode("tip-x", true)->setFloatValue(tip[0]);
+      _wingsN->getNode("tip-y", true)->setFloatValue(tip[1]);
+      _wingsN->getNode("tip-z", true)->setFloatValue(tip[2]);
+      w->getBase(tip);
+      _wingsN->getNode("base-x", true)->setFloatValue(tip[0]);
+      _wingsN->getNode("base-y", true)->setFloatValue(tip[1]);
+      _wingsN->getNode("base-z", true)->setFloatValue(tip[2]);
+      _wingsN->getNode("wing-span", true)->setFloatValue(w->getSpan());
+      _wingsN->getNode("wing-area", true)->setFloatValue(w->getArea());
+      _wingsN->getNode("aspect-ratio", true)->setFloatValue(w->getAspectRatio());
+      _wingsN->getNode("standard-mean-chord", true)->setFloatValue(w->getSMC());
+    }
 
-	float td = s->getTotalDrag();
-	s->setTotalDrag(td);
+    float wgt = 0;
+    float dragSum = 0;
+    for(int i=0; i<w->numSurfaces(); i++) {
+        Surface* s = (Surface*)w->getSurface(i);
+        float td = s->getTotalDrag();
+        int sid = s->getID();
 
         _model.addSurface(s);
 
@@ -495,8 +386,18 @@ float Airplane::compileWing(Wing* w)
         mass = mass * Math::sqrt(mass);
         float pos[3];
         s->getPosition(pos);
-        _model.getBody()->addMass(mass, pos);
+        int mid = _model.getBody()->addMass(mass, pos, true);
+        if (_wingsN != 0) {
+          SGPropertyNode_ptr n = _wingsN->getNode("surfaces", true)->getChild("surface", sid, true);
+          n->getNode("drag", true)->setFloatValue(td);
+          n->getNode("mass-id", true)->setIntValue(mid);
+        }
         wgt += mass;
+        dragSum += td;
+    }
+    if (_wingsN != 0)  {
+      _wingsN->getNode("weight", true)->setFloatValue(wgt);	
+      _wingsN->getNode("drag", true)->setFloatValue(dragSum);	
     }
     return wgt;
 }
@@ -547,7 +448,7 @@ float Airplane::compileFuselage(Fuselage* f)
 
         // _Mass_ weighting goes as surface area^(3/2)
         float mass = scale*segWgt * Math::sqrt(scale*segWgt);
-        _model.getBody()->addMass(mass, pos);
+        _model.getBody()->addMass(mass, pos, true);
         wgt += mass;
 
         // Make a Surface too
@@ -635,6 +536,13 @@ void Airplane::compileGear(GearRec* gr)
     _surfs.add(s);
 }
 
+/**
+ * @brief add "fake gear" per contact point
+ * 
+ * 
+ * @return void
+ */
+
 void Airplane::compileContactPoints()
 {
     // Figure it will compress by 20cm
@@ -648,8 +556,7 @@ void Airplane::compileContactPoints()
     float spring = (1/DIST) * 9.8f * 10.0f * mass;
     float damp = 2 * Math::sqrt(spring * mass);
 
-    int i;
-    for(i=0; i<_contacts.size(); i++) {
+    for(int i=0; i<_contacts.size(); i++) {
         ContactRec* c = (ContactRec*)_contacts.get(i);
 
         Gear* g = new Gear();
@@ -674,6 +581,8 @@ void Airplane::compile()
 {
     RigidBody* body = _model.getBody();
     int firstMass = body->numMasses();
+    SGPropertyNode_ptr baseN = fgGetNode("/fdm/yasim/model/wings", true);
+    SGPropertyNode_ptr n;
 
     // Generate the point masses for the plane.  Just use unitless
     // numbers for a first pass, then go back through and rescale to
@@ -682,13 +591,21 @@ void Airplane::compile()
 
     // The Wing objects
     if (_wing)
+    {
+      if (baseN != 0) _wingsN = baseN->getChild("wing", 0, true);
       aeroWgt += compileWing(_wing);
+    }
     if (_tail)
+    {
+      if (baseN != 0) _wingsN = baseN->getChild("tail", 0, true);
       aeroWgt += compileWing(_tail);
+    }
     int i;
     for(i=0; i<_vstabs.size(); i++)
-        aeroWgt += compileWing((Wing*)_vstabs.get(i)); 
-
+    {
+      if (baseN != 0) _wingsN = baseN->getChild("stab", i, true);
+      aeroWgt += compileWing((Wing*)_vstabs.get(i));
+    }
 
     // The fuselage(s)
     for(i=0; i<_fuselages.size(); i++)
@@ -707,7 +624,7 @@ void Airplane::compile()
     // Add the thruster masses
     for(i=0; i<_thrusters.size(); i++) {
         ThrustRec* t = (ThrustRec*)_thrusters.get(i);
-        body->addMass(t->mass, t->cg);
+        body->addMass(t->mass, t->cg, true);
     }
 
     // Add the tanks, empty for now.
@@ -733,10 +650,23 @@ void Airplane::compile()
     }
 
     // Ground effect
+    // If a double tapered wing is modelled with wing and mstab, wing must 
+    // be outboard to get correct wingspan.
     if(_wing) {
         float gepos[3];
         float gespan = 0;
-        gespan = _wing->getGroundEffect(gepos);
+        gespan = _wing->getSpan();
+        _wing->getBase(gepos);
+        if(!isVersionOrNewer( Version::YASIM_VERSION_2017_2 )) {
+          //old code
+          //float span = _length * Math::cos(_sweep) * Math::cos(_dihedral);
+          //span = 2*(span + Math::abs(_base[2]));
+          gespan -= 2*gepos[1]; // cut away base (y-distance)
+          gespan += 2*Math::abs(gepos[2]); // add (wrong) z-distance
+        }
+        if (baseN != 0)
+          baseN->getChild("wing", 0)->getNode("gnd-eff-span", true)->setFloatValue(gespan);
+        // where does the hard coded factor 0.15 come from?
         _model.setGroundEffect(gepos, gespan, 0.15f);
     }
 
@@ -822,11 +752,11 @@ void Airplane::initEngines()
 
 void Airplane::stabilizeThrust()
 {
-    int i;
-    for(i=0; i<_thrusters.size(); i++)
+    for(int i=0; i<_thrusters.size(); i++)
 	_model.getThruster(i)->stabilize();
 }
 
+/// Setup weights for cruise or approach during solve.
 void Airplane::setupWeights(bool isApproach)
 {
     int i;
@@ -839,6 +769,18 @@ void Airplane::setupWeights(bool isApproach)
     }
 }
 
+/// load values for controls as defined in cruise configuration
+void Airplane::loadCruiseControls()
+{
+  _controls.reset();
+  for(int i=0; i<_cruiseControls.size(); i++) {
+    Control* c = (Control*)_cruiseControls.get(i);
+    _controls.setInput(c->control, c->val);
+  }
+  _controls.applyControls(); 
+}
+
+/// Helper for solve()
 void Airplane::runCruise()
 {
     setupState(_cruiseAoA, _cruiseSpeed,_cruiseGlideAngle, &_cruiseState);
@@ -847,14 +789,8 @@ void Airplane::runCruise()
                   Atmosphere::calcStdDensity(_cruiseP, _cruiseT));
 
     // The control configuration
-    _controls.reset();
-    int i;
-    for(i=0; i<_cruiseControls.size(); i++) {
-	Control* c = (Control*)_cruiseControls.get(i);
-	_controls.setInput(c->control, c->val);
-    }
-    _controls.applyControls(1000000); // Huge dt value
-
+    loadCruiseControls();
+    
     // The local wind
     float wind[3];
     Math::mul3(-1, _cruiseState.v, wind);
@@ -865,7 +801,7 @@ void Airplane::runCruise()
    
     // Set up the thruster parameters and iterate until the thrust
     // stabilizes.
-    for(i=0; i<_thrusters.size(); i++) {
+    for(int i=0; i<_thrusters.size(); i++) {
 	Thruster* t = ((ThrustRec*)_thrusters.get(i))->thruster;
 	t->setWind(wind);
 	t->setAir(_cruiseP, _cruiseT,
@@ -882,6 +818,18 @@ void Airplane::runCruise()
     _model.calcForces(&_cruiseState);
 }
 
+/// load values for controls as defined in approach configuration
+void Airplane::loadApproachControls()
+{
+  _controls.reset();
+  for(int i=0; i<_approachControls.size(); i++) {
+    Control* c = (Control*)_approachControls.get(i);
+    _controls.setInput(c->control, c->val);
+  }
+  _controls.applyControls();
+}
+
+/// Helper for solve()
 void Airplane::runApproach()
 {
     setupState(_approachAoA, _approachSpeed,_approachGlideAngle, &_approachState);
@@ -890,14 +838,8 @@ void Airplane::runApproach()
                   Atmosphere::calcStdDensity(_approachP, _approachT));
 
     // The control configuration
-    _controls.reset();
-    int i;
-    for(i=0; i<_approachControls.size(); i++) {
-	Control* c = (Control*)_approachControls.get(i);
-	_controls.setInput(c->control, c->val);
-    }
-    _controls.applyControls(1000000);
-
+    loadApproachControls();
+    
     // The local wind
     float wind[3];
     Math::mul3(-1, _approachState.v, wind);
@@ -909,7 +851,7 @@ void Airplane::runApproach()
 
     // Run the thrusters until they get to a stable setting.  FIXME:
     // this is lots of wasted work.
-    for(i=0; i<_thrusters.size(); i++) {
+    for(int i=0; i<_thrusters.size(); i++) {
 	Thruster* t = ((ThrustRec*)_thrusters.get(i))->thruster;
 	t->setWind(wind);
 	t->setAir(_approachP, _approachT,
@@ -926,6 +868,7 @@ void Airplane::runApproach()
     _model.calcForces(&_approachState);
 }
 
+/// Used only in Airplane::solve() and solveHelicopter(), not at runtime
 void Airplane::applyDragFactor(float factor)
 {
     float applied = Math::pow(factor, SOLVE_TWEAK);
@@ -971,6 +914,7 @@ void Airplane::applyDragFactor(float factor)
     }
 }
 
+/// Used only in Airplane::solve() and solveHelicopter(), not at runtime
 void Airplane::applyLiftRatio(float factor)
 {
     float applied = Math::pow(factor, SOLVE_TWEAK);
@@ -986,13 +930,7 @@ void Airplane::applyLiftRatio(float factor)
     }
 }
 
-float Airplane::clamp(float val, float min, float max)
-{
-    if(val < min) return min;
-    if(val > max) return max;
-    return val;
-}
-
+/// Helper for solve()
 float Airplane::normFactor(float f)
 {
     if(f < 0) f = -f;
@@ -1102,8 +1040,8 @@ void Airplane::solve()
 	_cruiseAoA += SOLVE_TWEAK*aoaDelta;
 	_tailIncidence += SOLVE_TWEAK*tailDelta;
 	
-	_cruiseAoA = clamp(_cruiseAoA, -0.175f, 0.175f);
-	_tailIncidence = clamp(_tailIncidence, -0.175f, 0.175f);
+	_cruiseAoA = Math::clamp(_cruiseAoA, -0.175f, 0.175f);
+	_tailIncidence = Math::clamp(_tailIncidence, -0.175f, 0.175f);
 
         if(abs(xforce/_cruiseWeight) < STHRESH*0.0001 &&
            abs(alift/_approachWeight) < STHRESH*0.0001 &&
diff --git a/src/FDM/YASim/Airplane.hpp b/src/FDM/YASim/Airplane.hpp
index a280efa8a..43e570778 100644
--- a/src/FDM/YASim/Airplane.hpp
+++ b/src/FDM/YASim/Airplane.hpp
@@ -7,6 +7,7 @@
 #include "Rotor.hpp"
 #include "Vector.hpp"
 #include "Version.hpp"
+#include <simgear/props/props.hxx>
 
 namespace yasim {
 
@@ -16,7 +17,9 @@ class Launchbar;
 class Thruster;
 class Hitch;
 
+/// The Airplane class ties together the different components
 class Airplane : public Version {
+    SGPropertyNode_ptr _wingsN;
 public:
     Airplane();
     ~Airplane();
@@ -24,30 +27,30 @@ public:
     void iterate(float dt);
     void calcFuelWeights();
 
-    ControlMap* getControlMap();
-    Model* getModel();
+    ControlMap* getControlMap() { return &_controls; }
+    Model* getModel() { return &_model; }
 
-    void setPilotPos(float* pos);
-    void getPilotPos(float* out);
+    void setPilotPos(float* pos) { Math::set3(pos, _pilotPos); }
+    void getPilotPos(float* out) { Math::set3(_pilotPos, out); }
 
     void getPilotAccel(float* out);
 
-    void setWeight(float weight);
+    void setEmptyWeight(float weight) {  _emptyWeight = weight; }
 
-    void setWing(Wing* wing);
-    void setTail(Wing* tail);
-    void addVStab(Wing* vstab);
+    void setWing(Wing* wing) { _wing = wing; }
+    void setTail(Wing* tail) { _tail = tail; }
+    void addVStab(Wing* vstab) { _vstabs.add(vstab); }
 
     void addFuselage(float* front, float* back, float width,
                      float taper=1, float mid=0.5,
                      float cx=1, float cy=1, float cz=1, float idrag=1);
     int addTank(float* pos, float cap, float fuelDensity);
     void addGear(Gear* g);
-    void addHook(Hook* h);
-    void addLaunchbar(Launchbar* l);
+    void addHook(Hook* h) { _model.addHook(h); }
+    void addLaunchbar(Launchbar* l) { _model.addLaunchbar(l); }
     void addThruster(Thruster* t, float mass, float* cg);
     void addBallast(float* pos, float mass);
-    void addHitch(Hitch* h);
+    void addHitch(Hitch* h) { _model.addHitch(h); }
 
     int addWeight(float* pos, float size);
     void setWeight(int handle, float mass);
@@ -61,40 +64,48 @@ public:
 
     void addSolutionWeight(bool approach, int idx, float wgt);
 
-    int numGear();
-    Gear* getGear(int g);
-    Hook* getHook();
+    int numGear() { return _gears.size(); }
+    Gear* getGear(int g) { return ((GearRec*)_gears.get(g))->gear; }
+    Hook* getHook() { return _model.getHook(); }
     int numHitches() { return _hitches.size(); }
     Hitch* getHitch(int h);
-    Rotorgear* getRotorgear();
-    Launchbar* getLaunchbar();
+    Rotorgear* getRotorgear() { return _model.getRotorgear(); }
+    Launchbar* getLaunchbar() { return _model.getLaunchbar(); }
 
     int numThrusters() { return _thrusters.size(); }
     Thruster* getThruster(int n) {
         return ((ThrustRec*)_thrusters.get(n))->thruster; }
     
-    int numTanks();
+    int numTanks() { return _tanks.size(); }
     void setFuelFraction(float frac); // 0-1, total amount of fuel
-    float getFuel(int tank); // in kg!
-    float setFuel(int tank, float fuel); // in kg!
-    float getFuelDensity(int tank); // kg/m^3
-    float getTankCapacity(int tank);
+    // get fuel in kg
+    float getFuel(int tank) { return ((Tank*)_tanks.get(tank))->fill; }
+    // set fuel in kg
+    float setFuel(int tank, float fuel) { return ((Tank*)_tanks.get(tank))->fill = fuel; }
+    // get fuel density in kg/m^3
+    float getFuelDensity(int tank) { return ((Tank*)_tanks.get(tank))->density; }
+    float getTankCapacity(int tank) { return ((Tank*)_tanks.get(tank))->cap; }
 
     void compile(); // generate point masses & such, then solve
     void initEngines();
     void stabilizeThrust();
 
     // Solution output values
-    int getSolutionIterations();
-    float getDragCoefficient();
-    float getLiftRatio();
-    float getCruiseAoA();
-    float getTailIncidence();
+    int getSolutionIterations() { return _solutionIterations; }
+    float getDragCoefficient() { return _dragFactor; }
+    float getLiftRatio() { return _liftRatio; }
+    float getCruiseAoA() { return _cruiseAoA; }
+    float getTailIncidence() { return _tailIncidence; }
     float getApproachElevator() { return _approachElevator.val; }
-    const char* getFailureMsg();
-
-    static void setupState(float aoa, float speed, float gla, State* s); // utility
+    const char* getFailureMsg() { return _failureMsg; }
 
+    static void setupState(const float aoa, const float speed, const float gla, yasim::State* s); // utility
+    void loadApproachControls();
+    void loadCruiseControls();
+    
+    float getCGMinX() { return _cgMinX; }
+    float getCGMaxX() { return _cgMaxX; }
+    
 private:
     struct Tank { float pos[3]; float cap; float fill;
 	          float density; int handle; };
@@ -119,7 +130,6 @@ private:
     void compileGear(GearRec* gr);
     void applyDragFactor(float factor);
     void applyLiftRatio(float factor);
-    float clamp(float val, float min, float max);
     void addContactPoint(float* pos);
     void compileContactPoints();
     float normFactor(float f);
@@ -175,6 +185,10 @@ private:
     float _tailIncidence;
     Control _approachElevator;
     const char* _failureMsg;
+
+    // hard limits for cg from gear positions
+    float _cgMaxX;
+    float _cgMinX;
 };
 
 }; // namespace yasim
diff --git a/src/FDM/YASim/ControlMap.cpp b/src/FDM/YASim/ControlMap.cpp
index c300c4c75..bf56615f9 100644
--- a/src/FDM/YASim/ControlMap.cpp
+++ b/src/FDM/YASim/ControlMap.cpp
@@ -21,25 +21,29 @@ namespace yasim {
 
 ControlMap::~ControlMap()
 {
-    int i;
-    for(i=0; i<_inputs.size(); i++) {
-	Vector* v = (Vector*)_inputs.get(i);
-	int j;
-	for(j=0; j<v->size(); j++)
+  int i;
+  for(i=0; i<_inputs.size(); i++) {
+    Vector* v = (Vector*)_inputs.get(i);
+    int j;
+    for(j=0; j<v->size(); j++)
 	    delete (MapRec*)v->get(j);
-	delete v;
-    }
+    delete v;
+  }
 
-    for(i=0; i<_outputs.size(); i++)
-	delete (OutRec*)_outputs.get(i);
-}
-
-int ControlMap::newInput()
-{
-    Vector* v = new Vector();
-    return _inputs.add(v);
+  for(i=0; i<_outputs.size(); i++)
+    delete (OutRec*)_outputs.get(i);
+   
+	for(i=0; i<_properties.size(); i++) {
+    PropHandle* p = (PropHandle*)_properties.get(i);
+    delete[] p->name;
+    delete p;
+  }  
 }
 
+/**
+	input : index to _inputs
+	type: identifier (see enum OutputType)
+*/
 void ControlMap::addMapping(int input, int type, void* object, int options,
 			    float src0, float src1, float dst0, float dst1)
 {
@@ -55,6 +59,10 @@ void ControlMap::addMapping(int input, int type, void* object, int options,
     m->dst1 = dst1;
 }
 
+/**
+	input : index to _inputs
+	type: identifier (see enum OutputType)
+*/
 void ControlMap::addMapping(int input, int type, void* object, int options)
 {
     // See if the output object already exists
@@ -210,12 +218,12 @@ void ControlMap::applyControls(float dt)
 	case LEXTEND:  ((Launchbar*)obj)->setExtension(lval);      break;
     case LACCEL:   ((Launchbar*)obj)->setAcceleration(lval);   break;
 	case CASTERING:((Gear*)obj)->setCastering(lval != 0);      break;
-	case SLAT:     ((Wing*)obj)->setSlat(lval);                break;
-	case FLAP0:    ((Wing*)obj)->setFlap0(lval, rval);         break;
+	case SLAT:     ((Wing*)obj)->setSlatPos(lval);                break;
+	case FLAP0:    ((Wing*)obj)->setFlap0Pos(lval, rval);         break;
 	case FLAP0EFFECTIVENESS: ((Wing*)obj)->setFlap0Effectiveness(lval); break;
-	case FLAP1:    ((Wing*)obj)->setFlap1(lval, rval);         break;
+	case FLAP1:    ((Wing*)obj)->setFlap1Pos(lval, rval);         break;
 	case FLAP1EFFECTIVENESS: ((Wing*)obj)->setFlap1Effectiveness(lval); break;
-	case SPOILER:  ((Wing*)obj)->setSpoiler(lval, rval);       break;
+	case SPOILER:  ((Wing*)obj)->setSpoilerPos(lval, rval);       break;
         case COLLECTIVE:   ((Rotor*)obj)->setCollective(lval);     break;
         case CYCLICAIL:    ((Rotor*)obj)->setCyclicail(lval,rval); break;
         case CYCLICELE:    ((Rotor*)obj)->setCyclicele(lval,rval); break;
@@ -278,4 +286,45 @@ float ControlMap::rangeMax(int type)
     }
 }
 
+/// duplicate null-terminated string
+char* ControlMap::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;
+}
+
+/// compare null-terminated strings
+bool ControlMap::eq(const char* a, const char* b)
+{
+	while(*a && *b && *a == *b) { a++; b++; }
+	// equal if both a and b points to null chars
+	return !(*a || *b);
+}
+
+/// register property name, return ID (int)
+int ControlMap::propertyHandle(const char* name)
+{
+	for(int i=0; i < _properties.size(); i++) {
+		PropHandle* p = (PropHandle*)_properties.get(i);
+		if(eq(p->name, name))
+			return p->handle;
+	}
+
+	// create new
+	PropHandle* p = new PropHandle();
+	p->name = dup(name);
+	
+	fgGetNode(p->name, true); 
+
+	Vector* v = new Vector();
+	p->handle = _inputs.add(v);
+	_properties.add(p);
+	return p->handle;
+}
+
 } // namespace yasim
diff --git a/src/FDM/YASim/ControlMap.hpp b/src/FDM/YASim/ControlMap.hpp
index 839ae2c4f..dc7f75f79 100644
--- a/src/FDM/YASim/ControlMap.hpp
+++ b/src/FDM/YASim/ControlMap.hpp
@@ -1,6 +1,7 @@
 #ifndef _CONTROL_MAP_HPP
 #define _CONTROL_MAP_HPP
 
+#include <simgear/props/props.hxx>
 #include "Vector.hpp"
 
 namespace yasim {
@@ -26,9 +27,7 @@ public:
            OPT_INVERT = 0x02,
            OPT_SQUARE = 0x04 };
 
-    // Returns a new, not-yet-used "input handle" for addMapping and
-    // setInput.  This typically corresponds to one user axis.
-    int newInput();
+    struct PropHandle { char* name; int handle; };
 
     // Adds a mapping to between input handle and a particular setting
     // on an output object.  The value of output MUST match the type
@@ -45,12 +44,13 @@ public:
     // setInput() invokations.
     void reset();
 
-    // Sets the specified input (as returned by newInput) to the
+    // Sets the specified input (as returned by propertyHandle) to the
     // specified value.
-    void setInput(int input, float value);
+    void setInput(int propHandle, float value);
 
-    // Calculates and applies the settings received since the last reset().
-    void applyControls(float dt);
+    /// Calculates and applies the settings received since the last reset(). 
+    /// dt defaults to a large value used at solve time.
+    void applyControls(float dt=1e6);
 
     // Returns the input/output range appropriate for the given
     // control.  Ailerons go from -1 to 1, while throttles are never
@@ -72,6 +72,15 @@ public:
     float getOutput(int handle);
     float getOutputR(int handle);
 
+    // register property name, return handle
+    int propertyHandle(const char* name);
+    int numProperties() { return _properties.size(); }
+    PropHandle* getProperty(const int i) { return ((PropHandle*)_properties.get(i)); }
+
+    // helper
+    char* dup(const char* s);
+    bool eq(const char* a, const char* b);
+
 private:
     struct OutRec { int type; void* object; Vector maps;
                     float oldL, oldR, time; };
@@ -84,6 +93,8 @@ private:
 
     // An unordered list of output settings.
     Vector _outputs;
+    // control properties
+    Vector _properties;
 };
 
 }; // namespace yasim
diff --git a/src/FDM/YASim/FGFDM.cpp b/src/FDM/YASim/FGFDM.cpp
index 88013299a..40bf6efa2 100644
--- a/src/FDM/YASim/FGFDM.cpp
+++ b/src/FDM/YASim/FGFDM.cpp
@@ -21,6 +21,7 @@
 #include "Rotor.hpp"
 #include "Rotorpart.hpp"
 #include "Hitch.hpp"
+#include "Surface.hpp"
 
 #include "FGFDM.hpp"
 
@@ -28,6 +29,7 @@ namespace yasim {
 
 // Some conversion factors
 static const float KTS2MPS = 0.514444444444;
+static const float KMH2MPS = 1/3.6;
 static const float FT2M = 0.3048;
 static const float DEG2RAD = 0.0174532925199;
 static const float RPM2RAD = 0.10471975512;
@@ -59,7 +61,7 @@ FGFDM::FGFDM()
     // Map /controls/flight/elevator to the approach elevator control.  This
     // should probably be settable, but there are very few aircraft
     // who trim their approaches using things other than elevator.
-    _airplane.setElevatorControl(parseAxis("/controls/flight/elevator-trim"));
+    _airplane.setElevatorControl(_airplane.getControlMap()->propertyHandle("/controls/flight/elevator-trim"));
 
     // FIXME: read seed from somewhere?
     int seed = 0;
@@ -68,12 +70,6 @@ FGFDM::FGFDM()
 
 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<_thrusters.size(); i++) {
         EngRec* er = (EngRec*)_thrusters.get(i);
         delete[] er->prefix;
@@ -113,7 +109,7 @@ void FGFDM::iterate(float dt)
         _airplane.setFuel(i, LBS2KG * _tank_level_lbs[i]->getFloatValue());
     }
     _airplane.calcFuelWeights();
-
+    
     setOutputProperties(dt);
 }
 
@@ -124,19 +120,39 @@ Airplane* FGFDM::getAirplane()
 
 void FGFDM::init()
 {
+    //reset id generator, needed on simulator reset/re-init
+    Surface::resetIDgen();
     _turb_magnitude_norm = fgGetNode("/environment/turbulence/magnitude-norm", true);
     _turb_rate_hz        = fgGetNode("/environment/turbulence/rate-hz", true);
 
-    SGPropertyNode_ptr yasimNode = fgGetNode("/fdm/yasim", true);
-    _gross_weight_lbs = yasimNode->getNode("gross-weight-lbs", true);
+    _yasimN = fgGetNode("/fdm/yasim", true);
+    _gross_weight_lbs = _yasimN->getNode("gross-weight-lbs", true);
 
     // alias to older name
     fgGetNode("/yasim/gross-weight-lbs", true)->alias(_gross_weight_lbs);
 
-    _cg_x = yasimNode->getNode("cg-x-m", true);
-    _cg_y = yasimNode->getNode("cg-y-m", true);
-    _cg_z = yasimNode->getNode("cg-z-m", true);
-
+    // write some compile time information to property tree
+    _yasimN->getNode("config-version",true)->setIntValue(_airplane.getVersion());
+    _yasimN->getNode("model/cg-x-min",true)->setFloatValue(_airplane.getCGMinX());
+    _yasimN->getNode("model/cg-x-max",true)->setFloatValue(_airplane.getCGMaxX());
+    
+    // prepare nodes for write at runtime
+    _cg_x = _yasimN->getNode("cg-x-m", true);
+    _cg_y = _yasimN->getNode("cg-y-m", true);
+    _cg_z = _yasimN->getNode("cg-z-m", true);
+    _vxN = _yasimN->getNode("velocities/v-x", true);
+    _vyN = _yasimN->getNode("velocities/v-y", true);
+    _vzN = _yasimN->getNode("velocities/v-z", true);
+    _vrxN = _yasimN->getNode("velocities/vrot-x", true);
+    _vryN = _yasimN->getNode("velocities/vrot-y", true);
+    _vrzN = _yasimN->getNode("velocities/vrot-z", true);
+    _axN = _yasimN->getNode("accelerations/a-x", true);
+    _ayN = _yasimN->getNode("accelerations/a-y", true);
+    _azN = _yasimN->getNode("accelerations/a-z", true);
+    _arxN = _yasimN->getNode("accelerations/arot-x", true);
+    _aryN = _yasimN->getNode("accelerations/arot-y", true);
+    _arzN = _yasimN->getNode("accelerations/arot-z", true);
+    
     // Allows the user to start with something other than full fuel
     _airplane.setFuelFraction(fgGetFloat("/sim/fuel-fraction", 1));
 
@@ -225,32 +241,64 @@ void FGFDM::startElement(const char* name, const XMLAttributes &atts)
     XMLAttributes* a = (XMLAttributes*)&atts;
     float v[3];
     char buf[64];
-
+    float f = 0;
+    
     if(eq(name, "airplane")) {
-	_airplane.setWeight(attrf(a, "mass") * LBS2KG);
-        if(a->hasAttribute("version")) {
-          _airplane.setVersion( a->getValue("version") );
-        }
-        if( !_airplane.isVersionOrNewer( Version::YASIM_VERSION_CURRENT ) ) {
-          SG_LOG(SG_FLIGHT, SG_DEV_ALERT, "This aircraft does not use the latest yasim configuration version.");
-        }
+      if(a->hasAttribute("mass")) { f = attrf(a, "mass") * LBS2KG; } 
+      else if (a->hasAttribute("mass-lbs")) { f = attrf(a, "mass-lbs") * LBS2KG; } 
+      else if (a->hasAttribute("mass-kg")) { f = attrf(a, "mass-kg"); }
+      else {
+        SG_LOG(SG_FLIGHT,SG_ALERT,"YASim fatal: missing attribute, airplane needs one of {mass-lbs, mass-kg}");
+        exit(1);       
+      }
+      _airplane.setEmptyWeight(f);
+      if(a->hasAttribute("version")) {
+        _airplane.setVersion( a->getValue("version") );
+      }
+      if( !_airplane.isVersionOrNewer( Version::YASIM_VERSION_CURRENT ) ) {
+        SG_LOG(SG_FLIGHT, SG_DEV_ALERT, "This aircraft does not use the latest yasim configuration version.");
+      }
     } 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;
-        float gla = attrf(a, "glide-angle", 0) * DEG2RAD;
-	_airplane.setApproach(spd, alt, aoa, attrf(a, "fuel", 0.2),gla);
-	_cruiseCurr = false;
+      float spd, alt = 0;
+      if (a->hasAttribute("speed")) { spd = attrf(a, "speed") * KTS2MPS; }
+      else if (a->hasAttribute("speed-kt")) { spd = attrf(a, "speed-kt") * KTS2MPS; }
+      else if (a->hasAttribute("speed-kmh")) { spd = attrf(a, "speed-kmh") * KMH2MPS; }
+      else {
+        SG_LOG(SG_FLIGHT,SG_ALERT,"YASim fatal: missing attribute, approach needs one of {speed-kt, speed-kmh}");
+        exit(1);       
+      }
+      if (a->hasAttribute("alt")) { alt = attrf(a, "alt") * FT2M; }
+      else if (a->hasAttribute("alt-ft")) { alt = attrf(a, "alt-ft") * FT2M; }
+      else if (a->hasAttribute("alt-m")) { alt = attrf(a, "alt-m"); }
+      float aoa = attrf(a, "aoa", 0) * DEG2RAD;
+      float gla = attrf(a, "glide-angle", 0) * DEG2RAD;
+      _airplane.setApproach(spd, alt, aoa, attrf(a, "fuel", 0.2), gla);
+      _cruiseCurr = false;
     } else if(eq(name, "cruise")) {
-	float spd = attrf(a, "speed") * KTS2MPS;
-	float alt = attrf(a, "alt") * FT2M;
-        float gla = attrf(a, "glide-angle", 0) * DEG2RAD;
-	_airplane.setCruise(spd, alt, attrf(a, "fuel", 0.5),gla);
-	_cruiseCurr = true;
+      float spd, alt = 0;
+      if (a->hasAttribute("speed")) { spd = attrf(a, "speed") * KTS2MPS; }
+      else if (a->hasAttribute("speed-kt")) { spd = attrf(a, "speed-kt") * KTS2MPS; }
+      else if (a->hasAttribute("speed-kmh")) { spd = attrf(a, "speed-kmh") * KMH2MPS; }
+      else {
+        SG_LOG(SG_FLIGHT,SG_ALERT,"YASim fatal: missing attribute, approach needs one of {speed-kt, speed-kmh}");
+        exit(1);       
+      }
+      if (a->hasAttribute("alt")) { alt = attrf(a, "alt") * FT2M; }
+      else if (a->hasAttribute("alt-ft")) { alt = attrf(a, "alt-ft") * FT2M; }
+      else if (a->hasAttribute("alt-m")) { alt = attrf(a, "alt-m"); }
+      float gla = attrf(a, "glide-angle", 0) * DEG2RAD;
+      _airplane.setCruise(spd, alt, attrf(a, "fuel", 0.5),gla);
+      _cruiseCurr = true;
     } else if(eq(name, "solve-weight")) {
         int idx = attri(a, "idx");
-        float wgt = attrf(a, "weight") * LBS2KG;
-        _airplane.addSolutionWeight(!_cruiseCurr, idx, wgt);
+        if(a->hasAttribute("weight")) { f = attrf(a, "weight") * LBS2KG; }
+        else if(a->hasAttribute("weight-lbs")) { f = attrf(a, "weight-lbs") * LBS2KG; }
+        else if(a->hasAttribute("weight-kg")) { f = attrf(a, "weight-kg"); }
+        else {
+          SG_LOG(SG_FLIGHT,SG_ALERT,"YASim fatal: missing attribute, solve-weight needs one of {weight-lbs, weight-kg}");
+          exit(1);       
+        }
+        _airplane.addSolutionWeight(!_cruiseCurr, idx, f);
     } else if(eq(name, "cockpit")) {
 	v[0] = attrf(a, "x");
 	v[1] = attrf(a, "y");
@@ -300,8 +348,15 @@ void FGFDM::startElement(const char* name, const XMLAttributes &atts)
 	v[0] = attrf(a, "x");
 	v[1] = attrf(a, "y");
 	v[2] = attrf(a, "z");
-	float mass = attrf(a, "mass") * LBS2KG;
-	j->setMaxThrust(attrf(a, "thrust") * LBS2N,
+	float mass;
+        if(a->hasAttribute("mass")) { mass = attrf(a, "mass") * LBS2KG; }
+        else if(a->hasAttribute("mass-lbs")) { mass = attrf(a, "mass-lbs") * LBS2KG; }
+        else if(a->hasAttribute("mass-kg")) { mass = attrf(a, "mass-kg"); }
+        else {
+          SG_LOG(SG_FLIGHT,SG_ALERT,"YASim fatal: missing attribute, jet needs one of {mass-lbs, mass-kg}");
+          exit(1);       
+        }       
+        j->setMaxThrust(attrf(a, "thrust") * LBS2N,
 			attrf(a, "afterburner", 0) * LBS2N);
 	j->setVectorAngle(attrf(a, "rotate", 0) * DEG2RAD);
         j->setReverseThrust(attrf(a, "reverse", 0.2));
@@ -320,7 +375,7 @@ void FGFDM::startElement(const char* name, const XMLAttributes &atts)
 	    j->setVMax(attrf(a, "exhaust-speed") * KTS2MPS);
 	if(a->hasAttribute("spool-time"))
 	    j->setSpooling(attrf(a, "spool-time"));
-
+	
 	j->setPosition(v);
 	_airplane.addThruster(j, mass, v);
 	sprintf(buf, "/engines/engine[%d]", _nextEngine++);
@@ -459,21 +514,36 @@ void FGFDM::startElement(const char* name, const XMLAttributes &atts)
         float cy = attrf(a, "cy", 1);
         float cz = attrf(a, "cz", 1);
 	float idrag = attrf(a, "idrag", 1);
-	_airplane.addFuselage(v, b, attrf(a, "width"), taper, mid,
+	_airplane.addFuselage(v, b, attrf(a, "width"), taper, mid, 
             cx, cy, cz, idrag);
     } 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;
+	if(a->hasAttribute("jet")) density = 6.72; 
 	density *= LBS2KG*CM2GALS;
-	_airplane.addTank(v, attrf(a, "capacity") * LBS2KG, density);
+        float capacity = 0;
+        if(a->hasAttribute("capacity")) { capacity = attrf(a, "capacity") * LBS2KG; }
+        else if(a->hasAttribute("capacity-lbs")) { capacity = attrf(a, "capacity-lbs") * LBS2KG; }
+        else if(a->hasAttribute("capacity-kg")) { capacity = attrf(a, "capacity-kg"); }
+        else {
+          SG_LOG(SG_FLIGHT,SG_ALERT,"YASim fatal: missing attribute, tank needs one of {capacity-lbs, capacity-kg}");
+          exit(1);                 
+        }
+        _airplane.addTank(v, capacity, 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);
+        if(a->hasAttribute("mass")) { f = attrf(a, "mass") * LBS2KG; } 
+        else if (a->hasAttribute("mass-lbs")) { f = attrf(a, "mass-lbs") * LBS2KG; } 
+        else if (a->hasAttribute("mass-kg")) { f = attrf(a, "mass-kg"); }
+        else {
+          SG_LOG(SG_FLIGHT,SG_ALERT,"YASim fatal: missing attribute, airplane needs one of {mass-lbs, mass-kg}");
+          exit(1);       
+        }
+        _airplane.addBallast(v, f);
     } else if(eq(name, "weight")) {
 	parseWeight(a);
     } else if(eq(name, "stall")) {
@@ -482,22 +552,22 @@ void FGFDM::startElement(const char* name, const XMLAttributes &atts)
 	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"),
+	((Wing*)_currObj)->setFlap0Params(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"),
+	((Wing*)_currObj)->setFlap1Params(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"),
+	((Wing*)_currObj)->setSlatParams(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"),
+	((Wing*)_currObj)->setSpoilerParams(attrf(a, "start"), attrf(a, "end"),
 				      attrf(a, "lift"), attrf(a, "drag"));
     /* } else if(eq(name, "collective")) {
         ((Rotor*)_currObj)->setcollective(attrf(a, "min"), attrf(a, "max"));
     } else if(eq(name, "cyclic")) {
         ((Rotor*)_currObj)->setcyclic(attrf(a, "ail"), attrf(a, "ele"));
-    */
+    */                               
     } else if(eq(name, "actionpt")) {
  	v[0] = attrf(a, "x");
 	v[1] = attrf(a, "y");
@@ -508,33 +578,32 @@ void FGFDM::startElement(const char* name, const XMLAttributes &atts)
 	v[1] = attrf(a, "y");
 	v[2] = attrf(a, "z");
 	((Thruster*)_currObj)->setDirection(v);
-    } else if(eq(name, "control-setting")) {
-	// A cruise or approach control setting
-	const char* axis = a->getValue("axis");
-	float value = attrf(a, "value", 0);
-	if(_cruiseCurr)
-	    _airplane.addCruiseControl(parseAxis(axis), value);
-	else
-	    _airplane.addApproachControl(parseAxis(axis), value);
-    } else if(eq(name, "control-input")) {
-
-	// A mapping of input property to a control
-        int axis = parseAxis(a->getValue("axis"));
-	int control = parseOutput(a->getValue("control"));
-	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;
-
-	ControlMap* cm = _airplane.getControlMap();
-	if(a->hasAttribute("src0")) {
-                           cm->addMapping(axis, control, _currObj, opt,
-			   attrf(a, "src0"), attrf(a, "src1"),
+  } else if(eq(name, "control-setting")) {
+    // A cruise or approach control setting
+    const char* axis = a->getValue("axis");
+    float value = attrf(a, "value", 0);
+    ControlMap* cm = _airplane.getControlMap();
+    if(_cruiseCurr)
+        _airplane.addCruiseControl(cm->propertyHandle(axis), value);
+    else
+	    _airplane.addApproachControl(cm->propertyHandle(axis), value);
+  } else if(eq(name, "control-input")) {
+    ControlMap* cm = _airplane.getControlMap();
+    // A mapping of input property to a control
+    int axis = cm->propertyHandle(a->getValue("axis"));
+    int control = parseOutput(a->getValue("control"));
+    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;
+    if(a->hasAttribute("src0")) {
+       cm->addMapping(axis, control, _currObj, opt,
+			   attrf(a, "src0"), attrf(a, "src1"), 
 			   attrf(a, "dst0"), attrf(a, "dst1"));
-	} else {
-            cm->addMapping(axis, control, _currObj, opt);
-	}
-    } else if(eq(name, "control-output")) {
+    } else {
+      cm->addMapping(axis, control, _currObj, opt);
+    }
+  } else if(eq(name, "control-output")) {
         // A property output for a control on the current object
         ControlMap* cm = _airplane.getControlMap();
         int type = parseOutput(a->getValue("control"));
@@ -555,7 +624,7 @@ void FGFDM::startElement(const char* name, const XMLAttributes &atts)
         int type = parseOutput(a->getValue("control"));
         int handle = cm->getOutputHandle(_currObj, type);
         float time = attrf(a, "transition-time", 0);
-
+        
         cm->setTransitionTime(handle, time);
     } else {
         SG_LOG(SG_FLIGHT,SG_ALERT,"Unexpected tag '"
@@ -575,10 +644,10 @@ void FGFDM::getExternalInput(float dt)
     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);
+    for(int i=0; i < cm->numProperties(); i++) {
+        ControlMap::PropHandle *p = cm->getProperty(i);
+        float val = fgGetFloat(p->name, 0);
+        cm->setInput(p->handle, val);
     }
     cm->applyControls(dt);
 
@@ -623,6 +692,26 @@ void FGFDM::setOutputProperties(float dt)
     _cg_y->setFloatValue(cg[1]);
     _cg_z->setFloatValue(cg[2]);
 
+    State* s = _airplane.getModel()->getState();
+    float v[3], acc[3], rot[3], racc[3];
+    Math::vmul33(s->orient, s->v, v);
+    Math::vmul33(s->orient, s->acc, acc);
+    Math::vmul33(s->orient, s->rot, rot);
+    Math::vmul33(s->orient, s->racc, racc);
+
+    _vxN->setFloatValue(v[0]);
+    _vyN->setFloatValue(v[1]);
+    _vzN->setFloatValue(v[2]);
+    _vrxN->setFloatValue(rot[0]);
+    _vryN->setFloatValue(rot[1]);
+    _vrzN->setFloatValue(rot[2]);
+    _axN->setFloatValue(acc[0]);
+    _ayN->setFloatValue(acc[1]);
+    _azN->setFloatValue(acc[2]);
+    _arxN->setFloatValue(racc[0]);
+    _aryN->setFloatValue(racc[1]);
+    _arzN->setFloatValue(racc[2]);
+
     ControlMap* cm = _airplane.getControlMap();
     for(int i=0; i<_controlProps.size(); i++) {
         PropOut* p = (PropOut*)_controlProps.get(i);
@@ -716,7 +805,7 @@ void FGFDM::setOutputProperties(float dt)
             // cockpit code can scale them to the right values.
             float pnorm = j->getPerfNorm();
             moveprop(node, "oilp-norm", pnorm, dt/3); // 3s seek time
-            moveprop(node, "oilt-norm", pnorm, dt/30); // 30s
+            moveprop(node, "oilt-norm", pnorm, dt/30); // 30s 
             moveprop(node, "itt-norm", pnorm, dt/1); // 1s
         }
     }
@@ -824,9 +913,9 @@ Rotor* FGFDM::parseRotor(XMLAttributes* a, const char* type)
     w->setTiltCenterZ(attrf(a,"tiltcenterz",0.0));
     w->setDownwashFactor(attrf(a, "downwashfactor", 1));
     if(attrb(a,"ccw"))
-       w->setCcw(1);
+       w->setCcw(1); 
     if(attrb(a,"sharedflaphinge"))
-       w->setSharedFlapHinge(true);
+       w->setSharedFlapHinge(true); 
 
     if(a->hasAttribute("name"))
        w->setName(a->getValue("name") );
@@ -845,7 +934,7 @@ Rotor* FGFDM::parseRotor(XMLAttributes* a, const char* type)
     w->setPowerAtPitch0(attrf(a, "poweratpitch-0", 300));
     w->setPowerAtPitchB(attrf(a, "poweratpitch-b", 3000));
     if(attrb(a,"notorque"))
-       w->setNotorque(1);
+       w->setNotorque(1); 
 
 #define p(x) if (a->hasAttribute(#x)) w->setParameter((char *)#x,attrf(a,#x) );
 #define p2(x,y) if (a->hasAttribute(y)) w->setParameter((char *)#x,attrf(a,y) );
@@ -893,7 +982,7 @@ void FGFDM::parsePistonEngine(XMLAttributes* a)
         eng->setDisplacement(attrf(a, "displacement") * CIN2CM);
 
     if(a->hasAttribute("compression"))
-        eng->setCompression(attrf(a, "compression"));
+        eng->setCompression(attrf(a, "compression"));        
 
     if(a->hasAttribute("min-throttle"))
         eng->setMinThrottle(attrf(a, "min-throttle"));
@@ -944,7 +1033,7 @@ void FGFDM::parsePropeller(XMLAttributes* a)
         if(a->hasAttribute("displacement"))
             eng->setDisplacement(attrf(a, "displacement") * CIN2CM);
         if(a->hasAttribute("compression"))
-            eng->setCompression(attrf(a, "compression"));
+            eng->setCompression(attrf(a, "compression"));        
         if(a->hasAttribute("turbo-mul")) {
             float mul = attrf(a, "turbo-mul");
             float mp = attrf(a, "wastegate-mp", 1e6) * INHG2PA;
@@ -1005,26 +1094,7 @@ void FGFDM::parsePropeller(XMLAttributes* a)
     _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);
-    fgGetNode( a->name, true ); // make sure the property name exists
-    a->handle = _airplane.getControlMap()->newInput();
-    _axes.add(a);
-    return a->handle;
-}
-
+/// map identifier (string) to int (enum in ControlMap)
 int FGFDM::parseOutput(const char* name)
 {
     if(eq(name, "THROTTLE"))  return ControlMap::THROTTLE;
@@ -1061,7 +1131,7 @@ int FGFDM::parseOutput(const char* name)
     if(eq(name, "TILTYAW")) return ControlMap::TILTYAW;
     if(eq(name, "ROTORGEARENGINEON")) return ControlMap::ROTORENGINEON;
     if(eq(name, "ROTORBRAKE")) return ControlMap::ROTORBRAKE;
-    if(eq(name, "ROTORENGINEMAXRELTORQUE"))
+    if(eq(name, "ROTORENGINEMAXRELTORQUE")) 
         return ControlMap::ROTORENGINEMAXRELTORQUE;
     if(eq(name, "ROTORRELTARGET")) return ControlMap::ROTORRELTARGET;
     if(eq(name, "ROTORBALANCE")) return ControlMap::ROTORBALANCE;
@@ -1143,7 +1213,7 @@ float FGFDM::attrf(XMLAttributes* atts, const char* attr, float def)
 {
     const char* val = atts->getValue(attr);
     if(val == 0) return def;
-    else         return (float)atof(val);
+    else         return (float)atof(val);    
 }
 
 double FGFDM::attrd(XMLAttributes* atts, const char* attr)
diff --git a/src/FDM/YASim/FGFDM.hpp b/src/FDM/YASim/FGFDM.hpp
index b8fb8ca18..066862cfd 100644
--- a/src/FDM/YASim/FGFDM.hpp
+++ b/src/FDM/YASim/FGFDM.hpp
@@ -31,7 +31,6 @@ public:
     float getVehicleRadius(void) const { return _vehicle_radius; }
 
 private:
-    struct AxisRec { char* name; int handle; };
     struct EngRec { char* prefix; Thruster* eng; };
     struct WeightRec { char* prop; float size; int handle; };
     struct PropOut { SGPropertyNode* prop; int handle, type; bool left;
@@ -41,7 +40,6 @@ private:
 
     Rotor* parseRotor(XMLAttributes* a, const char* name);
     Wing* parseWing(XMLAttributes* a, const char* name, Version * version);
-    int parseAxis(const char* name);
     int parseOutput(const char* name);
     void parseWeight(XMLAttributes* a);
     void parseTurbineEngine(XMLAttributes* a);
@@ -64,10 +62,6 @@ private:
     // Aerodynamic turbulence model
     Turbulence* _turb;
 
-    // The list of "axes" that we expect to find as input.  These are
-    // typically property names.
-    Vector _axes;
-
     // Settable weights
     Vector _weights;
 
@@ -107,9 +101,23 @@ private:
     SGPropertyNode_ptr _cg_x;
     SGPropertyNode_ptr _cg_y;
     SGPropertyNode_ptr _cg_z;
+    SGPropertyNode_ptr _yasimN;
+
     std::vector<SGPropertyNode_ptr> _tank_level_lbs;
     std::vector<ThrusterProps> _thrust_props;
     std::vector<FuelProps> _fuel_props;
+    SGPropertyNode_ptr _vxN;
+    SGPropertyNode_ptr _vyN;
+    SGPropertyNode_ptr _vzN;
+    SGPropertyNode_ptr _vrxN;
+    SGPropertyNode_ptr _vryN;
+    SGPropertyNode_ptr _vrzN;
+    SGPropertyNode_ptr _axN;
+    SGPropertyNode_ptr _ayN;
+    SGPropertyNode_ptr _azN;
+    SGPropertyNode_ptr _arxN;
+    SGPropertyNode_ptr _aryN;
+    SGPropertyNode_ptr _arzN;
 };
 
 }; // namespace yasim
diff --git a/src/FDM/YASim/Gear.cpp b/src/FDM/YASim/Gear.cpp
index 2ef322899..74c2a9a04 100644
--- a/src/FDM/YASim/Gear.cpp
+++ b/src/FDM/YASim/Gear.cpp
@@ -53,98 +53,6 @@ Gear::Gear()
     _global_ground[3] = -1e3;
 }
 
-void Gear::setPosition(float* position)
-{
-    int i;
-    for(i=0; i<3; i++) _pos[i] = position[i];
-}
-
-void Gear::setCompression(float* compression)
-{
-    int i;
-    for(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 = Math::clamp(brake, 0, 1);
-}
-
-void Gear::setRotation(float rotation)
-{
-    _rot = rotation;
-}
-
-void Gear::setExtension(float extension)
-{
-    _extension = Math::clamp(extension, 0, 1);
-}
-
-void Gear::setCastering(bool c)
-{
-    _castering = c;
-}
-
-void Gear::setContactPoint(bool c)
-{
-    _isContactPoint=c;
-}
-
-void Gear::setOnWater(bool c)
-{
-    _onWater = c;
-}
-
-void Gear::setOnSolid(bool c)
-{
-    _onSolid = c;
-}
-
-void Gear::setIgnoreWhileSolving(bool c)
-{
-    _ignoreWhileSolving = c;
-}
-
-void Gear::setSpringFactorNotPlaning(float f)
-{
-    _spring_factor_not_planing = f;
-}
-
-void Gear::setSpeedPlaning(float s)
-{
-    _speed_planing = s;
-}
-
-void Gear::setReduceFrictionByExtension(float s)
-{
-    _reduceFrictionByExtension = s;
-}
-
-void Gear::setInitialLoad(float l)
-{
-    _initialLoad = l;
-}
-
 void Gear::setGlobalGround(double *global_ground, float* global_vel,
                            double globalX, double globalY,
                            const simgear::BVHMaterial *material)
@@ -183,57 +91,9 @@ void Gear::setGlobalGround(double *global_ground, float* global_vel,
 
 }
 
-void Gear::getPosition(float* out)
-{
-    int i;
-    for(i=0; i<3; i++) out[i] = _pos[i];
-}
-
-void Gear::getCompression(float* out)
-{
-    int i;
-    for(i=0; i<3; i++) out[i] = _cmpr[i];
-}
-
 void Gear::getGlobalGround(double* global_ground)
 {
-    int i;
-    for(i=0; i<4; i++) global_ground[i] = _global_ground[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;
+    for(int i=0; i<4; i++) global_ground[i] = _global_ground[i];
 }
 
 void Gear::getForce(float* force, float* contact)
@@ -242,25 +102,6 @@ void Gear::getForce(float* force, float* contact)
     Math::set3(_contact, contact);
 }
 
-float Gear::getWoW()
-{
-    return _wow;
-}
-
-float Gear::getCompressFraction()
-{
-    return _frac;
-}
-
-bool Gear::getCastering()
-{
-    return _castering;
-}
-
-bool Gear::getGroundIsSolid()
-{
-    return _ground_isSolid;
-}
 
 float Gear::getBumpAltitude()
 {
diff --git a/src/FDM/YASim/Gear.hpp b/src/FDM/YASim/Gear.hpp
index f09e8a532..62f26ed30 100644
--- a/src/FDM/YASim/Gear.hpp
+++ b/src/FDM/YASim/Gear.hpp
@@ -1,5 +1,6 @@
 #ifndef _GEAR_HPP
 #define _GEAR_HPP
+#include "Math.hpp"
 
 namespace simgear {
 class BVHMaterial;
@@ -33,42 +34,43 @@ 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 setCastering(bool castering);
-    void setOnWater(bool c);
-    void setOnSolid(bool c);
-    void setSpringFactorNotPlaning(float f);
-    void setSpeedPlaning(float s);
-    void setReduceFrictionByExtension(float s);
-    void setInitialLoad(float l);
-    void setIgnoreWhileSolving(bool c);
+    void setPosition(float* position) { Math::set3(position, _pos); }
+    void getPosition(float* out) { Math::set3(_pos, out); }
+    void setCompression(float* compression) { Math::set3(compression, _cmpr); }
+    void getCompression(float* out) { Math::set3(_cmpr, out); }
+    void setSpring(float spring) { _spring = spring; }
+    float getSpring() { return _spring; }
+    void setDamping(float damping) { _damp = damping; }
+    float getDamping() {return _damp; }
+    void setStaticFriction(float sfric) { _sfric = sfric; }
+    float getStaticFriction() { return _sfric; }
+    void setDynamicFriction(float dfric) { _dfric = dfric; }
+    float getDynamicFriction() { return _dfric; }
+    void setBrake(float brake) { _brake = Math::clamp(brake, 0, 1); }
+    float getBrake() { return _brake; }
+    void setRotation(float rotation) { _rot = rotation; }
+    float getRotation() { return _rot; }
+    void setExtension(float extension) { _extension = Math::clamp(extension, 0, 1); }  
+    float getExtension() { return _extension; }
+    void setCastering(bool c) { _castering = c; }
+    bool getCastering() { return _castering; }
+    void setOnWater(bool c) { _onWater = c; }
+    void setOnSolid(bool c) { _onSolid = c; }
+    void setSpringFactorNotPlaning(float f) { _spring_factor_not_planing = f; }
+    void setSpeedPlaning(float s) { _speed_planing = s; }
+    void setReduceFrictionByExtension(float s) { _reduceFrictionByExtension = s; }
+    void setInitialLoad(float l) { _initialLoad = l; }
+    float getInitialLoad() {return _initialLoad; }
+    void setIgnoreWhileSolving(bool c) { _ignoreWhileSolving = c; }
+
     void setGlobalGround(double* global_ground, float* global_vel,
         double globalX, double globalY,
-                         const simgear::BVHMaterial *material);
-    void getPosition(float* out);
-    void getCompression(float* out);
+        const simgear::BVHMaterial *material);
     void getGlobalGround(double* global_ground);
-    float getSpring();
-    float getDamping();
-    float getStaticFriction();
-    float getDynamicFriction();
-    float getBrake();
-    float getRotation();
-    float getExtension();
-    float getInitialLoad() {return _initialLoad; }
-    bool getCastering();
     float getCasterAngle() { return _casterAngle; }
     float getRollSpeed() { return _rollSpeed; }
     float getBumpAltitude();
-    bool getGroundIsSolid();
+    bool getGroundIsSolid() { return _ground_isSolid; }
     float getGroundFrictionFactor() { return (float)_ground_frictionFactor; }
     void integrate(float dt);
 
@@ -81,12 +83,12 @@ public:
     // Computed values: total force, weight-on-wheels (force normal to
     // ground) and compression fraction.
     void getForce(float* force, float* contact);
-    float getWoW();
-    float getCompressFraction();
+    float getWoW() { return _wow; }
+    float getCompressFraction() { return _frac; }
     float getCompressDist() { return _compressDist; }
     bool getSubmergable() {return (!_ground_isSolid)&&(!_isContactPoint); }
     bool getIgnoreWhileSolving() {return _ignoreWhileSolving; }
-    void setContactPoint(bool c);
+    void setContactPoint(bool c) { _isContactPoint=c; }
 
 private:
     float calcFriction(float wgt, float v);
diff --git a/src/FDM/YASim/Math.hpp b/src/FDM/YASim/Math.hpp
index 82bad4c5c..1aca16478 100644
--- a/src/FDM/YASim/Math.hpp
+++ b/src/FDM/YASim/Math.hpp
@@ -44,17 +44,17 @@ public:
 
     // Some 3D vector stuff.  In all cases, it is permissible for the
     // "out" vector to be the same as one of the inputs.
-    static inline void  set3(float* v, float* out) {
+    static inline void  set3(const float* v, float* out) {
         out[0] = v[0];
         out[1] = v[1];
         out[2] = v[2];
     }
 
-    static inline float dot3(float* a, float* b) {
+    static inline float dot3(const float* a, const float* b) {
         return a[0]*b[0] + a[1]*b[1] + a[2]*b[2];
     }
 
-    static inline void  cross3(float* a, float* b, float* out) {
+    static inline void  cross3(const float* a, const 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;
@@ -62,30 +62,30 @@ public:
         out[2] = ax*by - bx*ay;
     }
 
-    static inline void  mul3(float scalar, float* v, float* out)
+    static inline void  mul3(const float scalar, const float* v, float* out)
     {
         out[0] = scalar * v[0];
         out[1] = scalar * v[1];
         out[2] = scalar * v[2];
     }
 
-    static inline void  add3(float* a, float* b, float* out){
+    static inline void  add3(const float* a, const float* b, float* out){
         out[0] = a[0] + b[0];
         out[1] = a[1] + b[1];
         out[2] = a[2] + b[2];
     }
 
-    static inline void  sub3(float* a, float* b, float* out) {
+    static inline void  sub3(const float* a, const float* b, float* out) {
         out[0] = a[0] - b[0];
         out[1] = a[1] - b[1];
         out[2] = a[2] - b[2];
     }
 
-    static inline float mag3(float* v) {
+    static inline float mag3(const float* v) {
         return sqrt(dot3(v, v));
     }
 
-    static inline void  unit3(float* v, float* out) {
+    static inline void  unit3(const float* v, float* out) {
         float imag = 1/mag3(v);
         mul3(imag, v, out);
     }
@@ -95,7 +95,7 @@ public:
     //                          6 7 8
 
     // Multiply two matrices
-    static void mmul33(float* a, float* b, float* out) {
+    static void mmul33(const float* a, const 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];
@@ -114,7 +114,7 @@ public:
     }
 
     // Multiply by vector
-    static inline void vmul33(float* m, float* v, float* out) {
+    static inline void vmul33(const float* m, const 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];
@@ -123,43 +123,41 @@ public:
 
     // Multiply the vector by the matrix transpose.  Or pre-multiply the
     // matrix by v as a row vector.  Same thing.
-    static inline void tmul33(float* m, float* v, float* out) {
+    static inline void tmul33(const float* m, const 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];
     }
 
-    // Invert matrix
-    static void invert33(float* m, float* out) {
+    /// Invert symmetric matrix; ~1/3 less calculations due to symmetry
+    static void invert33_sym(const 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
+        // symetric: d=b, g=c, h=f
         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         e=m[4], f=m[5];
+        float                 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 A =  (e*i - f*f);
+        float B = -(b*i - c*f);
+        float C =  (b*f - c*e);
+        float E =  (a*i - c*c);
+        float F = -(a*f - c*b);
+        float I =  (a*e - b*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;
+        out[0] = id*A;   out[1] = id*B;   out[2] = id*C;
+        out[3] = out[1]; out[4] = id*E;   out[5] = id*F;
+        out[6] = out[2]; out[7] = out[5]; out[8] = id*I;
     }
 
     // Transpose matrix (for an orthonormal orientation matrix, this
     // is the same as the inverse).
-    static inline void trans33(float* m, float* out) {
+    static inline void trans33(const 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
@@ -184,7 +182,7 @@ public:
     //   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,
+    static void ortho33(const float* x, const float* y,
                         float* xOut, float* yOut, float* zOut) {
         float x0[3], y0[3];
         set3(x, x0);
diff --git a/src/FDM/YASim/Model.cpp b/src/FDM/YASim/Model.cpp
index 25b28a1e2..4ffaa063e 100644
--- a/src/FDM/YASim/Model.cpp
+++ b/src/FDM/YASim/Model.cpp
@@ -66,13 +66,20 @@ Model::Model()
     _hook = 0;
     _launchbar = 0;
 
-    _groundEffectSpan = 0;
+    _wingSpan = 0;
     _groundEffect = 0;
-    for(i=0; i<3; i++) _wingCenter[i] = 0;
+    for(i=0; i<3; i++) _geRefPoint[i] = 0;
 
     _global_ground[0] = 0; _global_ground[1] = 0; _global_ground[2] = 1;
     _global_ground[3] = -100000;
-
+    _modelN = fgGetNode("/fdm/yasim/forces", true);
+    _f0xN = _modelN->getNode("f0-aero-x-drag", true);
+    _f0yN = _modelN->getNode("f0-aero-y-side", true);
+    _f0zN = _modelN->getNode("f0-aero-z-lift", true);
+    _gefxN = _modelN->getNode("gndeff-f-x", true);
+    _gefyN = _modelN->getNode("gndeff-f-y", true);
+    _gefzN = _modelN->getNode("gndeff-f-z", true);
+    _wgdistN = _modelN->getNode("wing-gnd-dist", true);    
 }
 
 Model::~Model()
@@ -175,106 +182,12 @@ void Model::iterate()
     _integrator.calcNewInterval();
 }
 
-bool Model::isCrashed()
-{
-    return _crashed;
-}
-
-void Model::setCrashed(bool crashed)
-{
-    _crashed = crashed;
-}
-
-float Model::getAGL()
-{
-    return _agl;
-}
-
-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);
-}
-
-Rotorgear* Model::getRotorgear(void)
-{
-    return &_rotorgear;
-}
-
-int Model::addThruster(Thruster* t)
-{
-    return _thrusters.add(t);
-}
-
-Hook* Model::getHook(void)
-{
-    return _hook;
-}
-
-Launchbar* Model::getLaunchbar(void)
-{
-    return _launchbar;
-}
-
-int Model::numThrusters()
-{
-    return _thrusters.size();
-}
-
-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);
-}
-
-void Model::addHook(Hook* hook)
-{
-    _hook = hook;
-}
-
-void Model::addLaunchbar(Launchbar* launchbar)
-{
-    _launchbar = launchbar;
-}
-
-int Model::addHitch(Hitch* hitch)
-{
-    return _hitches.add(hitch);
-}
 
 void Model::setGroundCallback(Ground* ground_cb)
 {
@@ -282,30 +195,20 @@ void Model::setGroundCallback(Ground* ground_cb)
     _ground_cb = ground_cb;
 }
 
-Ground* Model::getGroundCallback(void)
+void Model::setGroundEffect(const float* pos, const float span, const float mul)
 {
-    return _ground_cb;
-}
-
-void Model::setGroundEffect(float* pos, float span, float mul)
-{
-    Math::set3(pos, _wingCenter);
-    _groundEffectSpan = span;
+    Math::set3(pos, _geRefPoint);
+    _wingSpan = span;
     _groundEffect = mul;
 }
 
-void Model::setAir(float pressure, float temp, float density)
+void Model::setAir(const float pressure, const float temp, const float density)
 {
     _pressure = pressure;
     _temp = temp;
     _rho = density;
 }
 
-void Model::setWind(float* wind)
-{
-    Math::set3(wind, _wind);
-}
-
 void Model::updateGround(State* s)
 {
     float dummy[3];
@@ -390,11 +293,11 @@ void Model::calcForces(State* s)
     _body.addTorque(_torque);
     int i,j;
     for(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);
+      Thruster* t = (Thruster*)_thrusters.get(i);
+      float thrust[3], pos[3];
+      t->getThrust(thrust);
+      t->getPosition(pos);
+      _body.addForce(pos, thrust);
     }
 
     // Get a ground plane in local coordinates.  The first three
@@ -417,20 +320,26 @@ void Model::calcForces(State* s)
     float faero[3];
     faero[0] = faero[1] = faero[2] = 0;
     for(i=0; i<_surfaces.size(); i++) {
-	Surface* sf = (Surface*)_surfaces.get(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, alt);
+      // Vsurf = wind - velocity + (rot cross (cg - pos))
+      float vs[3], pos[3];
+      sf->getPosition(pos);
+      localWind(pos, s, vs, alt);
 
-	float force[3], torque[3];
-	sf->calcForce(vs, _rho, force, torque);
-	Math::add3(faero, force, faero);
+      float force[3], torque[3];
+      sf->calcForce(vs, _rho, force, torque);
+      Math::add3(faero, force, faero);
 
-	_body.addForce(pos, force);
-	_body.addTorque(torque);
+      _body.addForce(pos, force);
+      _body.addTorque(torque);
     }
+    if (_modelN != 0) {
+      _f0xN->setFloatValue(faero[0]);
+      _f0yN->setFloatValue(faero[1]);
+      _f0zN->setFloatValue(faero[2]);
+    }
+
     for (j=0; j<_rotorgear.getRotors()->size();j++)
     {
         Rotor* r = (Rotor *)_rotorgear.getRotors()->get(j);
@@ -470,18 +379,30 @@ void Model::calcForces(State* s)
     // Account for ground effect by multiplying the vertical force
     // component by an amount linear with the fraction of the wingspan
     // above the ground.
-    if ((_groundEffectSpan != 0) && (_groundEffect != 0 ))
+    if ((_wingSpan != 0) && (_groundEffect != 0 ))
     {
-        float dist = ground[3] - Math::dot3(ground, _wingCenter);
-        if(dist > 0 && dist < _groundEffectSpan) {
-        float fz = Math::dot3(faero, ground);
-            fz *= (_groundEffectSpan - dist) / _groundEffectSpan;
-            fz *= _groundEffect;
-        Math::mul3(fz, ground, faero);
-        _body.addForce(faero);
-        }
+      // distance between ground and wing ref. point
+      float dist = ground[3] - Math::dot3(ground, _geRefPoint); 
+      float fz = 0;
+      float geForce[3];
+      if(dist > 0 && dist < _wingSpan) {
+        fz = Math::dot3(faero, ground);
+        fz *= (_wingSpan - dist) / _wingSpan;
+        fz *= _groundEffect;
+        Math::mul3(fz, ground, geForce);
+        _body.addForce(geForce);
+      }
+      if (_modelN != 0) {
+        _gefxN->setFloatValue(geForce[0]);
+        _gefyN->setFloatValue(geForce[1]);
+        _gefzN->setFloatValue(geForce[2]);
+        _wgdistN->setFloatValue(dist);
+        //float ld0 = faero[2]/faero[0];
+        //float ld = (geForce[2]+faero[2])/(geForce[0]+faero[0]);
+        //n->getNode("gndeff-ld-ld0", true)->setFloatValue(ld/ld0);
+        
+      }
     }
-    
     // Convert the velocity and rotation vectors to local coordinates
     float lrot[3], lv[3];
     Math::vmul33(s->orient, s->rot, lrot);
@@ -561,7 +482,7 @@ void Model::newState(State* s)
 
 // Calculates the airflow direction at the given point and for the
 // specified aircraft velocity.
-void Model::localWind(float* pos, State* s, float* out, float alt, bool is_rotor)
+void Model::localWind(const float* pos, yasim::State* s, float* out, float alt, bool is_rotor)
 {
     float tmp[3], lwind[3], lrot[3], lv[3];
 
diff --git a/src/FDM/YASim/Model.hpp b/src/FDM/YASim/Model.hpp
index 7c31b8ac8..6fa92c817 100644
--- a/src/FDM/YASim/Model.hpp
+++ b/src/FDM/YASim/Model.hpp
@@ -7,6 +7,7 @@
 #include "Vector.hpp"
 #include "Turbulence.hpp"
 #include "Rotor.hpp"
+#include <simgear/props/props.hxx>
 
 namespace yasim {
 
@@ -26,49 +27,49 @@ public:
     Model();
     virtual ~Model();
 
-    RigidBody* getBody();
-    Integrator* getIntegrator();
+    RigidBody* getBody() { return &_body; }
+    Integrator* getIntegrator() { return &_integrator; }
 
     void setTurbulence(Turbulence* turb) { _turb = turb; }
 
-    State* getState();
+    State* getState() { return _s; }
     void setState(State* s);
 
-    bool isCrashed();
-    void setCrashed(bool crashed);
-    float getAGL();
+    bool isCrashed() { return _crashed; } 
+    void setCrashed(bool crashed) { _crashed = crashed; }
+    float getAGL() { return _agl; }
 
     void iterate();
 
     // Externally-managed subcomponents
-    int addThruster(Thruster* t);
-    int addSurface(Surface* surf);
-    int addGear(Gear* gear);
-    void addHook(Hook* hook);
-    void addLaunchbar(Launchbar* launchbar);
-    Surface* getSurface(int handle);
-    Rotorgear* getRotorgear(void);
+    int addThruster(Thruster* t) { return _thrusters.add(t); }
+    int addSurface(Surface* surf) { return _surfaces.add(surf); }
+    int addGear(Gear* gear) { return _gears.add(gear); }
+    void addHook(Hook* hook) { _hook = hook; }
+    void addLaunchbar(Launchbar* launchbar) { _launchbar = launchbar; }
+    Surface* getSurface(int handle) { return (Surface*)_surfaces.get(handle); }
+    Rotorgear* getRotorgear(void) { return &_rotorgear; }
     Gear* getGear(int handle);
-    Hook* getHook(void);
-    int addHitch(Hitch* hitch);
-    Launchbar* getLaunchbar(void);
+    Hook* getHook(void) { return _hook; }
+    int addHitch(Hitch* hitch) { return _hitches.add(hitch); }
+    Launchbar* getLaunchbar(void) { return _launchbar; }
 
     // Semi-private methods for use by the Airplane solver.
-    int numThrusters();
-    Thruster* getThruster(int handle);
-    void setThruster(int handle, Thruster* t);
+    int numThrusters() { return _thrusters.size(); }
+    Thruster* getThruster(int handle) { return (Thruster*)_thrusters.get(handle); }
+    void setThruster(int handle, Thruster* t) { _thrusters.set(handle, t); }
     void initIteration();
     void getThrust(float* out);
 
     void setGroundCallback(Ground* ground_cb);
-    Ground* getGroundCallback(void);
+    Ground* getGroundCallback(void) { return _ground_cb; }
 
     //
     // Per-iteration settables
     //
-    void setGroundEffect(float* pos, float span, float mul);
-    void setWind(float* wind);
-    void setAir(float pressure, float temp, float density);
+    void setGroundEffect(const float* pos, const float span, const float mul);
+    void setWind(float* wind) { Math::set3(wind, _wind); }
+    void setAir(const float pressure, const float temp, const float density);
 
     void updateGround(State* s);
 
@@ -80,8 +81,7 @@ private:
     void initRotorIteration();
     void calcGearForce(Gear* g, float* v, float* rot, float* ground);
     float gearFriction(float wgt, float v, Gear* g);
-    void localWind(float* pos, State* s, float* out, float alt,
-        bool is_rotor = false);
+    void localWind(const float* pos, State* s, float* out, float alt, bool is_rotor = false);
 
     Integrator _integrator;
     RigidBody _body;
@@ -96,9 +96,9 @@ private:
     Launchbar* _launchbar;
     Vector _hitches;
 
-    float _groundEffectSpan;
+    float _wingSpan;
     float _groundEffect;
-    float _wingCenter[3];
+    float _geRefPoint[3];
 
     Ground* _ground_cb;
     double _global_ground[4];
@@ -114,6 +114,14 @@ private:
     State* _s;
     bool _crashed;
     float _agl;
+    SGPropertyNode_ptr _modelN;  
+    SGPropertyNode_ptr _f0xN;
+    SGPropertyNode_ptr _f0yN;
+    SGPropertyNode_ptr _f0zN;
+    SGPropertyNode_ptr _gefxN;
+    SGPropertyNode_ptr _gefyN;
+    SGPropertyNode_ptr _gefzN;
+    SGPropertyNode_ptr _wgdistN;
 };
 
 }; // namespace yasim
diff --git a/src/FDM/YASim/RigidBody.cpp b/src/FDM/YASim/RigidBody.cpp
index 7f530305b..e1fb94b70 100644
--- a/src/FDM/YASim/RigidBody.cpp
+++ b/src/FDM/YASim/RigidBody.cpp
@@ -1,5 +1,6 @@
-#include "Math.hpp"
+#include <Main/fg_props.hxx>
 #include "RigidBody.hpp"
+
 namespace yasim {
 
 RigidBody::RigidBody()
@@ -11,6 +12,7 @@ RigidBody::RigidBody()
     _masses = new Mass[_massesAlloced];
     _gyro[0] = _gyro[1] = _gyro[2] = 0;
     _spin[0] = _spin[1] = _spin[2] = 0;
+    _bodyN = fgGetNode("/fdm/yasim/model/masses", true);
 }
 
 RigidBody::~RigidBody()
@@ -18,7 +20,9 @@ RigidBody::~RigidBody()
     delete[] _masses;
 }
 
-int RigidBody::addMass(float mass, float* pos)
+/// add new point mass to body
+/// isStatic: set to true for masses that do not change per iteration (everything but fuel?)
+int RigidBody::addMass(float mass, float* pos, bool isStatic)
 {
     // If out of space, reallocate twice as much
     if(_nMasses == _massesAlloced) {
@@ -30,31 +34,38 @@ int RigidBody::addMass(float mass, float* pos)
         delete[] _masses;
         _masses = m2;
     }
-
-    _masses[_nMasses].m = mass;
-    Math::set3(pos, _masses[_nMasses].p);
+    setMass(_nMasses, mass, pos, isStatic);
     return _nMasses++;
 }
 
+/// change mass
+/// handle: returned by addMass
 void RigidBody::setMass(int handle, float mass)
 {
+    if (_masses[handle].m  == mass) 
+      return;
     _masses[handle].m = mass;
+    // if static mass is changed, reset pre-calculated mass
+    // may apply to weights like cargo, pax, that usually do not change with FDM rate 
+    if (_masses[handle].isStatic)
+      _staticMass.m = 0;
+    if (_bodyN != 0)
+      _bodyN->getChild("mass", handle, true)->getNode("mass", true)->setFloatValue(mass);
 }
 
-void RigidBody::setMass(int handle, float mass, float* pos)
+void RigidBody::setMass(int handle, float mass, const float* pos, bool isStatic)
 {
     _masses[handle].m = mass;
+    _masses[handle].isStatic = isStatic;
     Math::set3(pos, _masses[handle].p);
-}
-
-int RigidBody::numMasses()
-{
-    return _nMasses;
-}
-
-float RigidBody::getMass(int handle)
-{
-    return _masses[handle].m;
+    if (_bodyN != 0) {
+      SGPropertyNode_ptr n = _bodyN->getChild("mass", handle, true);
+      n->getNode("isStatic", true)->setValue(isStatic);
+      n->getNode("mass", true)->setFloatValue(mass);
+      n->getNode("pos-x", true)->setFloatValue(pos[0]);
+      n->getNode("pos-y", true)->setFloatValue(pos[1]);
+      n->getNode("pos-z", true)->setFloatValue(pos[2]);	
+    }
 }
 
 void RigidBody::getMassPosition(int handle, float* out)
@@ -64,60 +75,129 @@ void RigidBody::getMassPosition(int handle, float* out)
     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)
+void RigidBody::pointVelocity(const float* pos, const 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)
+void RigidBody::_recalcStatic()
 {
-    Math::set3(angularMomentum, _gyro);
+	// aggregate all masses that do not change (e.g. fuselage, wings) into one point mass
+	_staticMass.m = 0;
+	_staticMass.p[0] = 0;
+	_staticMass.p[1] = 0;
+	_staticMass.p[2] = 0;
+	int i;
+	int s = 0;
+	for(i=0; i<_nMasses; i++) {
+		if (_masses[i].isStatic) {
+			s++;
+			float m = _masses[i].m;
+			_staticMass.m += m;
+			_staticMass.p[0] += m * _masses[i].p[0];
+			_staticMass.p[1] += m * _masses[i].p[1];
+			_staticMass.p[2] += m * _masses[i].p[2];
+		}
+	}
+	Math::mul3(1/_staticMass.m, _staticMass.p, _staticMass.p);
+  if (_bodyN != 0) {
+    _bodyN->getNode("aggregated-mass", true)->setFloatValue(_staticMass.m);
+    _bodyN->getNode("aggregated-count", true)->setIntValue(s);
+    _bodyN->getNode("aggregated-pos-x", true)->setFloatValue(_staticMass.p[0]);
+    _bodyN->getNode("aggregated-pos-y", true)->setFloatValue(_staticMass.p[1]);
+    _bodyN->getNode("aggregated-pos-z", true)->setFloatValue(_staticMass.p[2]);	
+  }
+	// Now the inertia tensor:
+	for(i=0; i<9; i++)
+		_tI_static[i] = 0;
+
+	for(i=0; i<_nMasses; i++) {
+		if (_masses[i].isStatic) {
+			float m = _masses[i].m;
+
+			float x = _masses[i].p[0] - _staticMass.p[0];
+			float y = _masses[i].p[1] - _staticMass.p[1];
+			float z = _masses[i].p[2] - _staticMass.p[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;
+
+			// tensor is symmetric, so we can save some calculations in the loop
+			_tI_static[0] += y2+z2;  _tI_static[1] -=    xy;  _tI_static[2] -=    zx;
+			                         _tI_static[4] += x2+z2;  _tI_static[5] -=    yz;
+			                                                  _tI_static[8] += x2+y2;
+		}
+	}
+	// copy symmetric elements
+	_tI_static[3] = _tI_static[1];
+	_tI_static[6] = _tI_static[2];
+	_tI_static[7] = _tI_static[5];
 }
 
+/// calculate the total mass, centre of gravity and inertia tensor
+/**
+  recalc is used when compiling the model but more important it is called in
+  Model::iterate() e.g. at FDM rate (120 Hz)
+  We can save some CPU due to the symmetry of the tensor and by aggregating
+  masses that do not change during flight.
+ */
 void RigidBody::recalc()
 {
-    // Calculate the c.g and total mass:
-    _totalMass = 0;
-    _cg[0] = _cg[1] = _cg[2] = 0;
+    //aggregate static masses into one mass
+    if (_staticMass.m == 0) _recalcStatic();
+
+    // Calculate the c.g and total mass
+    // init with pre-calculated static mass
+    _totalMass = _staticMass.m;
+    _cg[0] = _staticMass.m * _staticMass.p[0];
+    _cg[1] = _staticMass.m * _staticMass.p[1];
+    _cg[2] = _staticMass.m * _staticMass.p[2];
     int i;
     for(i=0; i<_nMasses; i++) {
+      // only masses we did not aggregate
+      if (!_masses[i].isStatic) { 
         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(i=0; i<9; i++)
-	_tI[i] = 0;
+      _tI[i] = _tI_static[i];
 
     for(i=0; i<_nMasses; i++) {
-	float m = _masses[i].m;
+      if (!_masses[i].isStatic) {
+        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 x = _masses[i].p[0] - _cg[0];
+        float y = _masses[i].p[1] - _cg[1];
+        float z = _masses[i].p[2] - _cg[2];
+        float mx = m*x;
+        float my = m*y;
+        float mz = m*z;
+        
+        float xy = mx*y; float yz = my*z; float zx = mz*x;
+        float x2 = mx*x; float y2 = my*y; float z2 = mz*z;
 
-	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;
-
-	_tI[0] += y2+z2;  _tI[1] -=    xy;  _tI[2] -=    zx;
-	_tI[3] -=    xy;  _tI[4] += x2+z2;  _tI[5] -=    yz;
-	_tI[6] -=    zx;   _tI[7] -=   yz;  _tI[8] += x2+y2;
+        _tI[0] += y2+z2;  _tI[1] -=    xy;  _tI[2] -=    zx;
+                          _tI[4] += x2+z2;  _tI[5] -=    yz;
+                                            _tI[8] += x2+y2;
+      }
     }
+    // copy symmetric elements 
+    _tI[3] = _tI[1];
+    _tI[6] = _tI[2];
+    _tI[7] = _tI[5];
 
-    // And its inverse
-    Math::invert33(_tI, _invI);
+    //calculate inverse
+    Math::invert33_sym(_tI, _invI);
 }
 
 void RigidBody::reset()
@@ -126,17 +206,7 @@ void RigidBody::reset()
     _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)
+void RigidBody::addForce(const float* pos, const float* force)
 {
     addForce(force);
     
@@ -148,21 +218,6 @@ void RigidBody::addForce(float* pos, float* force)
     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);
diff --git a/src/FDM/YASim/RigidBody.hpp b/src/FDM/YASim/RigidBody.hpp
index 6e4a4ad30..d7ca39939 100644
--- a/src/FDM/YASim/RigidBody.hpp
+++ b/src/FDM/YASim/RigidBody.hpp
@@ -1,5 +1,8 @@
 #ifndef _RIGIDBODY_HPP
 #define _RIGIDBODY_HPP
+#include <simgear/props/props.hxx>
+#include "Vector.hpp"
+#include "Math.hpp"
 
 namespace yasim {
 
@@ -20,27 +23,28 @@ namespace yasim {
 //
 class RigidBody
 {
+    SGPropertyNode_ptr _bodyN;
 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);
+    int addMass(float mass, float* pos, bool isStatic = false);
 
     // 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);
+    void setMass(int handle, float mass, const float* pos, bool isStatic = false);
 
-    int numMasses();
-    float getMass(int handle);
+    int numMasses() { return _nMasses; }
+    float getMass(int handle) { return _masses[handle].m; }
     void getMassPosition(int handle, float* out);
-    float getTotalMass();
+    float getTotalMass() { return _totalMass; }
 
     // 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);
+    void pointVelocity(const float* pos, const float* rot, float* out);
 
     // Sets the "gyroscope" for the body.  This is the total
     // "intrinsic" angular momentum of the body; that is, rotations of
@@ -48,27 +52,26 @@ public:
     // 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);
+    void setGyro(const float* angularMomentum) { Math::set3(angularMomentum, _gyro); }
 
 
     // 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.
+    // multiple changes. see also _recalcStatic() 
     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);
-
+    void addForce(const float* force) { Math::add3(_force, force, _force); }
+    
+    // Applies a force at the specified position.
+    void addForce(const float* pos, const float* force); 
+    
     // Adds a torque with the specified axis and magnitude
-    void addTorque(float* torque);
+    void addTorque(const float* torque) { Math::add3(_torque, torque, _torque); }
 
     // Sets the rotation rate of the body (about its c.g.) within the
     // surrounding environment.  This is needed to compute torque on
@@ -76,17 +79,15 @@ public:
     // rotation.  NOTE: the rotation vector, like all other
     // coordinates used here, is specified IN THE LOCAL COORDINATE
     // SYSTEM.
-    void setBodySpin(float* rotation);
-
-
+    void setBodySpin(const float* rotation) { Math::set3(rotation, _spin); }
 
     // Returns the center of gravity of the masses, in the body
     // coordinate system.
-    void getCG(float* cgOut);
+    void getCG(float* cgOut) { Math::set3(_cg, 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);
+    void getAccel(float* accelOut) { Math::mul3(1/_totalMass, _force, accelOut); }
 
     // Returns the acceleration of a specific location in local
     // coordinates.  If the body is rotating, this will be different
@@ -102,17 +103,24 @@ public:
     void getInertiaMatrix(float* inertiaOut);
 
 private:
-    struct Mass { float m; float p[3]; };
+    /** 
+    Most of the mass points do not change after compilation of the aircraft so
+    they can be replaced by one aggregated mass at the c.g. of the static masses.
+    The isStatic flag is used to mark those masses.
+    */
+    struct Mass { float m; float p[3]; bool isStatic; };
+    void _recalcStatic(); /// aggregate static masses
+    Mass  _staticMass;		/// aggregated static masses, calculated once
+    Mass* _masses;        /// mass elements
+    int   _nMasses;       /// number of masses
+    int   _massesAlloced; /// counter for memory allocation
 
-    // 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 _tI_static[9];
     float _tI[9];
     float _invI[9];
 
diff --git a/src/FDM/YASim/Rotor.cpp b/src/FDM/YASim/Rotor.cpp
index c22235814..b9788174f 100644
--- a/src/FDM/YASim/Rotor.cpp
+++ b/src/FDM/YASim/Rotor.cpp
@@ -7,7 +7,7 @@
 
 #include "Math.hpp"
 #include <Main/fg_props.hxx>
-#include "Surface.hpp"
+//#include "Surface.hpp"
 #include "Rotorpart.hpp"
 #include "Glue.hpp"
 #include "Ground.hpp"
@@ -962,7 +962,7 @@ float Rotor::findGroundEffectAltitude(Ground * ground_cb,State *s,
         );
 }
 
-void Rotor::getDownWash(float *pos, float *v_heli, float *downwash)
+void Rotor::getDownWash(const float *pos, const float *v_heli, float *downwash)
 {
     float pos2rotor[3],tmp[3];
     Math::sub3(_base,pos,pos2rotor);
@@ -1632,7 +1632,7 @@ void Rotorgear::compile()
     }
 }
 
-void Rotorgear::getDownWash(float *pos, float * v_heli, float *downwash)
+void Rotorgear::getDownWash(const float *pos, const float * v_heli, float *downwash)
 {
     float tmp[3];
     downwash[0]=downwash[1]=downwash[2]=0;
diff --git a/src/FDM/YASim/Rotor.hpp b/src/FDM/YASim/Rotor.hpp
index 43ea60e04..7abd764ea 100644
--- a/src/FDM/YASim/Rotor.hpp
+++ b/src/FDM/YASim/Rotor.hpp
@@ -9,7 +9,7 @@
 
 namespace yasim {
 
-class Surface;
+//class Surface;
 class Rotorpart;
 class Ground;
 const float rho_null=1.184f; //25DegC, 101325Pa
@@ -107,7 +107,7 @@ public:
     void updateDirectionsAndPositions(float *rot);
     void getTip(float* tip);
     void calcLiftFactor(float* v, float rho, State *s);
-    void getDownWash(float *pos, float * v_heli, float *downwash);
+    void getDownWash(const float* pos, const float* v_heli, float* downwash);
     int getNumberOfBlades(){return _number_of_blades;}
     void setDownwashFactor(float value);
 
@@ -287,7 +287,7 @@ public:
     float getEnginePropFactor() {return _engine_prop_factor;}
     Vector* getRotors() { return &_rotors;}
     void initRotorIteration(float *lrot,float dt);
-    void getDownWash(float *pos, float * v_heli, float *downwash);
+    void getDownWash(const float* pos, const float* v_heli, float* downwash);
     int getValueforFGSet(int j,char *b,float *f);
 };
 
diff --git a/src/FDM/YASim/Surface.cpp b/src/FDM/YASim/Surface.cpp
index 88e4a07a8..0b844d1f7 100644
--- a/src/FDM/YASim/Surface.cpp
+++ b/src/FDM/YASim/Surface.cpp
@@ -1,10 +1,14 @@
-#include "Math.hpp"
+#include <Main/fg_props.hxx>
 #include "Surface.hpp"
+
 namespace yasim {
+int Surface::s_idGenerator = 0;
 
 Surface::Surface( Version * version ) :
     _version(version)
 {
+    // create id for surface
+    _id = s_idGenerator++;
     // Start in a "sane" mode, so unset stuff doesn't freak us out
     _c0 = 1;
     _cx = _cy = _cz = 1;
@@ -30,91 +34,40 @@ Surface::Surface( Version * version ) :
     _slatAlpha = 0;
     _spoilerLift = 1;
     _inducedDrag = 1;
+    _stallAlpha = 0;
+    _alpha = 0;
+    _surfN = fgGetNode("/fdm/yasim/surfaces", true);
+    if (_surfN != 0) {
+      _surfN = _surfN->getChild("surface", _id, true);
+      _fxN = _surfN->getNode("f-x", true);
+      _fyN = _surfN->getNode("f-y", true);
+      _fzN = _surfN->getNode("f-z", true);
+      _fabsN = _surfN->getNode("f-abs", true);
+      _alphaN = _surfN->getNode("alpha", true);
+      _stallAlphaN = _surfN->getNode("stall-alpha", true);
+      _flapN = _surfN->getNode("flap-pos", true);
+      _slatN = _surfN->getNode("slat-pos", true);
+      _spoilerN = _surfN->getNode("spoiler-pos", true);
+    }
 }
 
-void Surface::setPosition(float* p)
+
+void Surface::setPosition(const float* p)
 {
     int i;
     for(i=0; i<3; i++) _pos[i] = p[i];
+    if (_surfN != 0) {
+      _surfN->getNode("pos-x", true)->setFloatValue(p[0]);
+      _surfN->getNode("pos-y", true)->setFloatValue(p[1]);
+      _surfN->getNode("pos-z", true)->setFloatValue(p[2]);
+    }
 }
 
-void Surface::getPosition(float* out)
+void Surface::setOrientation(const float* o)
 {
-    int i;
-    for(i=0; i<3; i++) out[i] = _pos[i];
+  for(int i=0; i<9; i++) _orient[i] = o[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;
-}
-
-float Surface::getXDrag()
-{
-    return _cx;
-}
-
-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)
-{
-    int i;
-    for(i=0; i<9; i++)
-        _orient[i] = o[i];
-}
-
-void Surface::setIncidence(float angle)
-{
-    _incidence = angle;
-}
-
-void Surface::setTwist(float angle)
-{
-    _twist = angle;
-}
 
 void Surface::setSlatParams(float stallDelta, float dragPenalty)
 {
@@ -134,42 +87,39 @@ void Surface::setSpoilerParams(float liftPenalty, float dragPenalty)
     _spoilerDrag = dragPenalty;
 }
 
-void Surface::setFlap(float pos)
+void Surface::setFlapPos(float pos)
 {
-    _flapPos = pos;
+  if (_flapPos != pos) {
+      _flapPos = pos;
+    if (_surfN != 0) _flapN->setFloatValue(pos);
+  }
 }
 
-void Surface::setFlapEffectiveness(float effectiveness)
-{
-    _flapEffectiveness = effectiveness;
-}
-
-double Surface::getFlapEffectiveness()
-{
-    return _flapEffectiveness;
-}
-
-
-void Surface::setSlat(float pos)
+void Surface::setSlatPos(float pos)
 {
+  if (_slatPos != pos) {
     _slatPos = pos;
+    if (_surfN != 0) _slatN->setFloatValue(pos);
+  }
 }
 
-void Surface::setSpoiler(float pos)
+void Surface::setSpoilerPos(float pos)
 {
+  if (_spoilerPos != pos) {
     _spoilerPos = pos;
+    if (_surfN != 0) _spoilerN->setFloatValue(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)
+void Surface::calcForce(const float* v, const 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.
+    // Zero velocity means zero force by definition (also prevents div0).
     if(vel == 0) {
         int i;
 	for(i=0; i<3; i++) out[i] = torque[i] = 0;
@@ -182,10 +132,9 @@ void Surface::calcForce(float* v, float rho, float* out, float* torque)
         for(int i=0; i<3; i++) out[i] = torque[i] = 0.;
         return;
     }
-
+    
+    // Normalize wind and convert to the surface's coordinates
     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
@@ -247,6 +196,15 @@ void Surface::calcForce(float* v, float rho, float* out, float* torque)
     float scale = 0.5f*rho*vel*vel*_c0;
     Math::mul3(scale, out, out);
     Math::mul3(scale, torque, torque);
+    // if we have a property tree, export info
+    if (_surfN != 0) {
+      _fabsN->setFloatValue(Math::mag3(out));
+      _fxN->setFloatValue(out[0]);
+      _fyN->setFloatValue(out[1]);
+      _fzN->setFloatValue(out[2]);
+      _alphaN->setFloatValue(_alpha);
+      _stallAlphaN->setFloatValue(_stallAlpha);      
+    }
 }
 
 #if 0
@@ -257,9 +215,9 @@ void Surface::test()
     float rho = Atmosphere::getStdDensity(0);
     float spd = 30;
 
-    setFlap(0);
-    setSlat(0);
-    setSpoiler(0);
+    setFlapPos(0);
+    setSlatPos(0);
+    setSpoilerPos(0);
 
     for(float angle = -90; angle<90; angle += 0.01) {
         float rad = angle * DEG2RAD;
@@ -281,39 +239,40 @@ 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]);
+    _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 airflow is toward -z
     int i = (fwdBak<<1) | posNeg;
 
-    float stallAlpha = _stalls[i];
-    if(stallAlpha == 0)
+    _stallAlpha = _stalls[i];
+    if(_stallAlpha == 0)
         return 1;
 
+    // consider slat position, moves the stall aoa some degrees
     if(i == 0) {
 	if( _version->isVersionOrNewer( Version::YASIM_VERSION_32 )) {
-	    stallAlpha += _slatPos * _slatAlpha;
+	    _stallAlpha += _slatPos * _slatAlpha;
 	} else {
-	    stallAlpha += _slatAlpha;
+	    _stallAlpha += _slatAlpha;
 	}
     }
 
     // Beyond the stall
-    if(alpha > stallAlpha+_widths[i])
+    if(_alpha > _stallAlpha+_widths[i])
 	return 1;
 
     // (note mask: we want to use the "positive" stall angle here)
     float scale = 0.5f*_peaks[fwdBak]/_stalls[i&2];
 
     // Before the stall
-    if(alpha <= stallAlpha)
+    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];
+    float frac = (_alpha - _stallAlpha) / _widths[i];
     frac = frac*frac*(3-2*frac);
 
     return scale*(1-frac) + frac;
diff --git a/src/FDM/YASim/Surface.hpp b/src/FDM/YASim/Surface.hpp
index 3679ab4ce..861f2c5f8 100644
--- a/src/FDM/YASim/Surface.hpp
+++ b/src/FDM/YASim/Surface.hpp
@@ -1,7 +1,9 @@
 #ifndef _SURFACE_HPP
 #define _SURFACE_HPP
 
+#include <simgear/props/props.hxx>
 #include "Version.hpp"
+#include "Math.hpp"
 
 namespace yasim {
 
@@ -10,15 +12,21 @@ namespace yasim {
 // front, and flaps act (in both lift and drag) toward the back.
 class Surface
 {
+    static int s_idGenerator;
+    int _id;        //index for property tree
+
 public:
     Surface( Version * version );
 
+    int getID() { return _id; };
+    static void resetIDgen() { s_idGenerator = 0; };
+
     // Position of this surface in local coords
-    void setPosition(float* p);
-    void getPosition(float* out);
+    void setPosition(const float* p);
+    void getPosition(float* out) { Math::set3(_pos, out); }
 
     // Distance scale along the X axis
-    void setChord(float chord);
+    void setChord(float chord) { _chord = chord; }
 
     // Slats act to move the stall peak by the specified angle, and
     // increase drag by the multiplier specified.
@@ -32,49 +40,54 @@ public:
 
     // 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);
+    void setFlapPos(float pos);
+    void setSlatPos(float pos);
+    void setSpoilerPos(float pos);
 
     // Modifier for flap lift coefficient, useful for simulating flap blowing etc.
-    void setFlapEffectiveness(float effectiveness);
-	double getFlapEffectiveness();
+    void setFlapEffectiveness(float effectiveness) { _flapEffectiveness = effectiveness; }
+    double getFlapEffectiveness() { return _flapEffectiveness; }
 
     // local -> Surface coords
-    void setOrientation(float* o);
+    void setOrientation(const 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 setIncidence(float angle) { _incidence = angle; }
 
     // The offset from base incidence for this surface.
-    void setTwist(float angle);
+    void setTwist(float angle) { _twist = angle; }
 
-    void setTotalDrag(float c0);
-    float getTotalDrag();
-
-    void setXDrag(float cx);
-    void setYDrag(float cy);
-    void setZDrag(float cz);
-    float getXDrag();
+    void setTotalDrag(float c0) { _c0 = c0; }
+    float getTotalDrag() { return _c0; }
+    
+    void setXDrag(float cx) { _cx = cx; }
+    void setYDrag(float cy) { _cy = cy; }
+    void setZDrag(float cz) { _cz = cz; }
+    float getXDrag() { return _cx; }
 
     // zero-alpha Z drag ("camber") specified as a fraction of cz
-    void setBaseZDrag(float cz0);
+    void setBaseZDrag(float cz0) { _cz0 = cz0; }
 
     // i: 0 == forward, 1 == backwards
-    void setStallPeak(int i, float peak);
+    void setStallPeak(int i, float peak) { _peaks[i] = 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 setStall(int i, float alpha) { _stalls[i] = alpha; }
+    void setStallWidth(int i, float width) { _widths[i] = width; }
 
     // Induced drag multiplier
     void setInducedDrag(float mul) { _inducedDrag = mul; }
 
-    void calcForce(float* v, float rho, float* forceOut, float* torqueOut);
+    void calcForce(const float* v, const float rho, float* out, float* torque);
 
+    float getAlpha() { return _alpha; };
+    float getStallAlpha() { return _stallAlpha; };
+    
 private:
+    SGPropertyNode_ptr _surfN;
+    
     float stallFunc(float* v);
     float flapLift(float alpha);
     float controlDrag(float lift, float drag);
@@ -105,8 +118,21 @@ private:
     float _incidence;
     float _twist;
     float _inducedDrag;
+    
+    // used during calculations
+    float _stallAlpha;
+    float _alpha;
 
     Version * _version;
+    SGPropertyNode* _fxN;
+    SGPropertyNode* _fyN;
+    SGPropertyNode* _fzN;
+    SGPropertyNode* _stallAlphaN;
+    SGPropertyNode* _alphaN;
+    SGPropertyNode* _flapN;
+    SGPropertyNode* _slatN;
+    SGPropertyNode* _spoilerN;
+    SGPropertyNode* _fabsN;
 };
 
 }; // namespace yasim
diff --git a/src/FDM/YASim/Version.cpp b/src/FDM/YASim/Version.cpp
index 5c70211fc..c449db034 100644
--- a/src/FDM/YASim/Version.cpp
+++ b/src/FDM/YASim/Version.cpp
@@ -5,6 +5,7 @@
 #include "Version.hpp"
 #include <simgear/debug/logstream.hxx>
 #include <string>
+#include <iostream>
 
 namespace yasim {
 void Version::setVersion( const char * version )
@@ -15,11 +16,15 @@ void Version::setVersion( const char * version )
     _version = YASIM_VERSION_ORIGINAL;
   } else if( v == "YASIM_VERSION_32" ) {
     _version = YASIM_VERSION_32;
+  } else if( v == "2017.2" ) {
+    _version = YASIM_VERSION_2017_2;
   } else if( v == "YASIM_VERSION_CURRENT" ) {
     _version = YASIM_VERSION_CURRENT;
   } else {
     SG_LOG(SG_FLIGHT,SG_ALERT,"unknown yasim version '" << version << "' ignored, using YASIM_VERSION_ORIGINAL");
+    return;
   }
+  std::cout << "This aircraft uses yasim version '" << v << "'\n";
 }
 
 } // namespace yasim
diff --git a/src/FDM/YASim/Version.hpp b/src/FDM/YASim/Version.hpp
index 3df5ef4ae..0c56f06a6 100644
--- a/src/FDM/YASim/Version.hpp
+++ b/src/FDM/YASim/Version.hpp
@@ -11,10 +11,12 @@ public:
   typedef enum {
     YASIM_VERSION_ORIGINAL = 0,
     YASIM_VERSION_32,
-    YASIM_VERSION_CURRENT = YASIM_VERSION_32
+    YASIM_VERSION_2017_2,
+    YASIM_VERSION_CURRENT = YASIM_VERSION_2017_2
   } YASIM_VERSION;
 
   void setVersion( const char * version );
+  int getVersion() { return _version; }
   bool isVersion( YASIM_VERSION version );
   bool isVersionOrNewer( YASIM_VERSION version );
 
diff --git a/src/FDM/YASim/Wing.cpp b/src/FDM/YASim/Wing.cpp
index a25d558f0..12a20728c 100644
--- a/src/FDM/YASim/Wing.cpp
+++ b/src/FDM/YASim/Wing.cpp
@@ -1,9 +1,9 @@
-#include "Math.hpp"
 #include "Surface.hpp"
 #include "Wing.hpp"
 
 namespace yasim {
-
+static const float RAD2DEG = 57.2957795131;
+  
 Wing::Wing( Version * version ) :
   _version(version)
 {
@@ -39,6 +39,9 @@ Wing::Wing( Version * version ) :
     _slatEnd = 0;
     _slatAoA = 0;
     _slatDrag = 0;
+    _meanChord = 0;
+    _wingspan = 0;
+    _aspectRatio = 1;
 }
 
 Wing::~Wing()
@@ -51,82 +54,6 @@ Wing::~Wing()
     }
 }
 
-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)
-{
-    int i;
-    for(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::setTwist(float angle)
-{
-    _twist = angle;
-}
-
-void Wing::setCamber(float camber)
-{
-    _camber = camber;
-}
-
 void Wing::setIncidence(float incidence)
 {
     _incidence = incidence;
@@ -135,7 +62,7 @@ void Wing::setIncidence(float incidence)
         ((SurfRec*)_surfs.get(i))->surface->setIncidence(incidence);
 }
 
-void Wing::setFlap0(float start, float end, float lift, float drag)
+void Wing::setFlap0Params(float start, float end, float lift, float drag)
 {
     _flap0Start = start;
     _flap0End = end;
@@ -143,7 +70,7 @@ void Wing::setFlap0(float start, float end, float lift, float drag)
     _flap0Drag = drag;
 }
 
-void Wing::setFlap1(float start, float end, float lift, float drag)
+void Wing::setFlap1Params(float start, float end, float lift, float drag)
 {
     _flap1Start = start;
     _flap1End = end;
@@ -151,7 +78,7 @@ void Wing::setFlap1(float start, float end, float lift, float drag)
     _flap1Drag = drag;
 }
 
-void Wing::setSlat(float start, float end, float aoa, float drag)
+void Wing::setSlatParams(float start, float end, float aoa, float drag)
 {
     _slatStart = start;
     _slatEnd = end;
@@ -159,7 +86,7 @@ void Wing::setSlat(float start, float end, float aoa, float drag)
     _slatDrag = drag;
 }
 
-void Wing::setSpoiler(float start, float end, float lift, float drag)
+void Wing::setSpoilerParams(float start, float end, float lift, float drag)
 {
     _spoilerStart = start;
     _spoilerEnd = end;
@@ -167,14 +94,14 @@ void Wing::setSpoiler(float start, float end, float lift, float drag)
     _spoilerDrag = drag;
 }
 
-void Wing::setFlap0(float lval, float rval)
+void Wing::setFlap0Pos(float lval, float rval)
 {
     lval = Math::clamp(lval, -1, 1);
     rval = Math::clamp(rval, -1, 1);
     int i;
     for(i=0; i<_flap0Surfs.size(); i++) {
-	((Surface*)_flap0Surfs.get(i))->setFlap(lval);
-	if(_mirror) ((Surface*)_flap0Surfs.get(++i))->setFlap(rval);
+      ((Surface*)_flap0Surfs.get(i))->setFlapPos(lval);
+      if(_mirror) ((Surface*)_flap0Surfs.get(++i))->setFlapPos(rval);
     }
 }
 
@@ -184,18 +111,17 @@ void Wing::setFlap0Effectiveness(float lval)
     int i;
     for(i=0; i<_flap0Surfs.size(); i++) {
         ((Surface*)_flap0Surfs.get(i))->setFlapEffectiveness(lval);
-//	if(_mirror) ((Surface*)_flap0Surfs.get(++i))->setFlapEffectiveness(rval);
     }
 }
 
-void Wing::setFlap1(float lval, float rval)
+void Wing::setFlap1Pos(float lval, float rval)
 {
     lval = Math::clamp(lval, -1, 1);
     rval = Math::clamp(rval, -1, 1);
     int i;
     for(i=0; i<_flap1Surfs.size(); i++) {
-	((Surface*)_flap1Surfs.get(i))->setFlap(lval);
-	if(_mirror) ((Surface*)_flap1Surfs.get(++i))->setFlap(rval);
+      ((Surface*)_flap1Surfs.get(i))->setFlapPos(lval);
+      if(_mirror) ((Surface*)_flap1Surfs.get(++i))->setFlapPos(rval);
     }
 }
 
@@ -205,51 +131,26 @@ void Wing::setFlap1Effectiveness(float lval)
     int i;
     for(i=0; i<_flap1Surfs.size(); i++) {
         ((Surface*)_flap1Surfs.get(i))->setFlapEffectiveness(lval);
-//	if(_mirror) ((Surface*)_flap1Surfs.get(++i))->setFlap(rval);
     }
 }
 
-void Wing::setSpoiler(float lval, float rval)
+void Wing::setSpoilerPos(float lval, float rval)
 {
     lval = Math::clamp(lval, 0, 1);
     rval = Math::clamp(rval, 0, 1);
     int i;
     for(i=0; i<_spoilerSurfs.size(); i++) {
-	((Surface*)_spoilerSurfs.get(i))->setSpoiler(lval);
-	if(_mirror) ((Surface*)_spoilerSurfs.get(++i))->setSpoiler(rval);
+      ((Surface*)_spoilerSurfs.get(i))->setSpoilerPos(lval);
+      if(_mirror) ((Surface*)_spoilerSurfs.get(++i))->setSpoilerPos(rval);
     }
 }
 
-void Wing::setSlat(float val)
+void Wing::setSlatPos(float val)
 {
     val = Math::clamp(val, 0, 1);
     int i;
     for(i=0; i<_slatSurfs.size(); i++)
-	((Surface*)_slatSurfs.get(i))->setSlat(val);
-}
-
-float Wing::getGroundEffect(float* posOut)
-{
-    int i;
-    for(i=0; i<3; i++) posOut[i] = _base[i];
-    float span = _length * Math::cos(_sweep) * Math::cos(_dihedral);
-    span = 2*(span + Math::abs(_base[2]));
-    return span;
-}
-
-void Wing::getTip(float* tip)
-{
-    tip[0] = -Math::tan(_sweep);
-    tip[1] = Math::cos(_dihedral);
-    tip[2] = Math::sin(_dihedral);
-    Math::unit3(tip, tip);
-    Math::mul3(_length, tip, tip);
-    Math::add3(_base, tip, tip);
-}
-
-bool Wing::isMirrored()
-{
-    return _mirror;
+      ((Surface*)_slatSurfs.get(i))->setSlatPos(val);
 }
 
 void Wing::compile()
@@ -272,17 +173,17 @@ void Wing::compile()
     // Sort in increasing order
     int i;
     for(i=0; i<10; i++) {
-        int minIdx = i;
-	float minVal = bounds[i];
-        int j;
-        for(j=i+1; j<10; j++) {
-            if(bounds[j] < minVal) {
-                minIdx = j;
-		minVal = bounds[j];
-	    }
-	}
-        float tmp = bounds[i];
-        bounds[i] = minVal; bounds[minIdx] = tmp;
+      int minIdx = i;
+      float minVal = bounds[i];
+      int j;
+      for(j=i+1; j<10; j++) {
+        if(bounds[j] < minVal) {
+          minIdx = j;
+          minVal = bounds[j];
+        }
+      }
+      float tmp = bounds[i];
+      bounds[i] = minVal; bounds[minIdx] = tmp;
     }
 
     // Uniqify
@@ -294,10 +195,9 @@ void Wing::compile()
         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.5f*(_taper+1)) / _length;
-
+    // prepare wing coordinate system, ignoring incidence and twist for now
+    // (tail incidence is varied by the solver)
+    
     // Generating a unit vector pointing out the left wing.
     float left[3];
     left[0] = -Math::tan(_sweep);
@@ -306,12 +206,13 @@ void Wing::compile()
     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);
-
+    Math::mul3(_length, left, _tip);
+    Math::add3(_base, _tip, _tip);
+    _meanChord = _chord*(_taper+1)*0.5f;
+    // wingspan in y-direction (not for vstab)
+    _wingspan = Math::abs(2*_tip[1]);
+    _aspectRatio = _wingspan / _meanChord;
+    
     // 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
@@ -326,19 +227,23 @@ void Wing::compile()
     Math::cross3(y, z, x);
 
     if(_mirror) {
-	// Derive the right side orientation matrix from this one.
-        int i;
-        for(i=0; i<9; i++)  rightOrient[i] = orient[i];
+      // Derive the right side orientation matrix from this one.
+      int i;
+      for(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(i=1; i<9; i+=3) rightOrient[i] = -rightOrient[i];
+      // Negate all Y coordinates, this gets us a valid basis, but
+      // it's left handed!  So...
+      for(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(i=3; i<6; i++)  rightOrient[i] = -rightOrient[i];
+      // Change the direction of the Y axis to get back to a
+      // right-handed system.
+      for(i=3; i<6; i++)  rightOrient[i] = -rightOrient[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 = _meanChord / _length;
+
     // Now go through each boundary and make segments
     for(i=0; i<(nbounds-1); i++) {
         float start = bounds[i];
@@ -363,7 +268,7 @@ void Wing::compile()
         for(j=0; j<nSegs; j++) {
             float frac = start + (j+0.5f) * (end-start)/nSegs;
             float pos[3];
-            interp(root, tip, frac, pos);
+            interp(_base, _tip, frac, pos);
 
             float chord = _chord * (1 - (1-_taper)*frac);
 
@@ -378,7 +283,7 @@ void Wing::compile()
             _surfs.add(sr);
 
             if(_mirror) {
-		pos[1] = -pos[1];
+                pos[1] = -pos[1];
                 s = newSurface(pos, rightOrient, chord,
                                flap0, flap1, slat, spoiler);
                 sr = new SurfRec();
@@ -390,22 +295,16 @@ void Wing::compile()
             }
         }
     }
-
     // Last of all, re-set the incidence in case setIncidence() was
     // called before we were compiled.
     setIncidence(_incidence);
 }
 
-float Wing::getDragScale()
-{
-    return _dragScale;
-}
 
 void Wing::setDragScale(float scale)
 {
     _dragScale = scale;
-    int i;
-    for(i=0; i<_surfs.size(); i++) {
+    for(int i=0; i<_surfs.size(); i++) {
         SurfRec* s = (SurfRec*)_surfs.get(i);
         s->surface->setTotalDrag(scale * s->weight);
     }
@@ -414,16 +313,10 @@ void Wing::setDragScale(float scale)
 void Wing::setLiftRatio(float ratio)
 {
     _liftRatio = ratio;
-    int i;
-    for(i=0; i<_surfs.size(); i++)
+    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)
 {
@@ -442,14 +335,20 @@ Surface* Wing::newSurface(float* pos, float* orient, float chord,
     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".
+    // The negative AoA stall is the same if we're using an symmetric
+    // airfoil, otherwise a "little worse".
     if(_camber > 0) {
         s->setStall(1, stallAoA * 0.8f);
         s->setStallWidth(1, _stallWidth * 0.5f);
     } else {
-        s->setStall(1, stallAoA);
+      s->setStall(1, stallAoA);
+      if( _version->isVersionOrNewer( Version::YASIM_VERSION_2017_2 )) {
+        // what was presumably meant
+        s->setStallWidth(1, _stallWidth);
+      } else {
+        // old code; presumably a copy&paste error
         s->setStall(1, _stallWidth);
+      }
     }
 
     // The "reverse" stalls are unmeasurable junk.  Just use 13deg and
@@ -476,7 +375,7 @@ Surface* Wing::newSurface(float* pos, float* orient, float chord,
     return s;
 }
 
-void Wing::interp(float* v1, float* v2, float frac, float* out)
+void Wing::interp(const float* v1, const float* v2, const float frac, float* out)
 {
     out[0] = v1[0] + frac*(v2[0]-v1[0]);
     out[1] = v1[1] + frac*(v2[1]-v1[1]);
diff --git a/src/FDM/YASim/Wing.hpp b/src/FDM/YASim/Wing.hpp
index addcd4b6c..d757f8236 100644
--- a/src/FDM/YASim/Wing.hpp
+++ b/src/FDM/YASim/Wing.hpp
@@ -3,6 +3,7 @@
 
 #include "Vector.hpp"
 #include "Version.hpp"
+#include "Math.hpp"
 
 namespace yasim {
 
@@ -15,64 +16,81 @@ public:
     ~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 setTwist(float angle);
-    void setCamber(float camber);
+    void setMirror(bool mirror) { _mirror = mirror; }
+    bool isMirrored() { return _mirror; };
+    
+    // Wing geometry in local coordinates:
+    
+    // base point of wing
+    void setBase(const float* base) { Math::set3(base, _base); }
+    void getBase(float* base) { Math::set3(_base, base); };
+    // dist. ALONG wing (not span!)     
+    void setLength(float length) { _length = length; }
+    float getLength() { return _length; };
+    // at base, measured along X axis
+    void setChord(float chord) { _chord = chord; }
+    float getChord() { return _chord; };
+    // fraction of chord at wing tip, 0..1
+    void setTaper(float taper) { _taper = taper; }
+    float getTaper() { return _taper; };
+    // radians
+    void setSweep(float sweep) { _sweep = sweep; }
+    float getSweep() { return _sweep; };
+    // radians, positive is "up"
+    void setDihedral(float dihedral) { _dihedral = dihedral; }
+    float getDihedral() { return _dihedral; };
+    
     void setIncidence(float incidence);
+    void setTwist(float angle) { _twist = angle; }
+    
+    
+    // parameters for stall curve
+    void setStall(float aoa) { _stall = aoa; }
+    void setStallWidth(float angle) { _stallWidth = angle; }
+    void setStallPeak(float fraction) { _stallPeak = fraction; }
+    void setCamber(float camber) { _camber = camber; }
     void setInducedDrag(float drag) { _inducedDrag = drag; }
     
-    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);
+    
+    void setFlap0Params(float start, float end, float lift, float drag);
+    void setFlap1Params(float start, float end, float lift, float drag);
+    void setSpoilerParams(float start, float end, float lift, float drag);
+    void setSlatParams(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);
+    void setFlap0Pos(float lval, float rval);
+    void setFlap1Pos(float lval, float rval);
+    void setSpoilerPos(float lval, float rval);
+    void setSlatPos(float val);
     void setFlap0Effectiveness(float lval);
     void setFlap1Effectiveness(float lval);
 
     // Compile the thing into a bunch of Surface objects
     void compile();
-
-    void getTip(float* tip);
-
-    bool isMirrored();
-
-    // Ground effect information
-    float getGroundEffect(float* posOut);
+    void getTip(float* tip) { Math::set3(_tip, tip);};
     
-    // Query the list of Surface objects
-    int numSurfaces();
-    Surface* getSurface(int n);
-    float getSurfaceWeight(int n);
+    // valid only after Wing::compile() was called
+    float getSpan() { return _wingspan; };
+    float getArea() { return _wingspan*_meanChord; };
+    float getAspectRatio() { return _aspectRatio; };
+    float getSMC() { return _meanChord; };
+    
+    int numSurfaces() { return _surfs.size(); }
+    Surface* getSurface(int n) { return ((SurfRec*)_surfs.get(n))->surface; }
+    float getSurfaceWeight(int n) { return ((SurfRec*)_surfs.get(n))->weight; }
 
     // The overall drag coefficient for the wing as a whole.  Units are
     // arbitrary.
     void setDragScale(float scale);
-    float getDragScale();
+    float getDragScale() { return _dragScale; }
 
     // 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();
+    float getLiftRatio() { return _liftRatio; }
 
 private:
-    void interp(float* v1, float* v2, float frac, float* out);
+    void interp(const float* v1, const float* v2, const float frac, float* out);
     Surface* newSurface(float* pos, float* orient, float chord,
                         bool flap0, bool flap1, bool slat, bool spoiler);
 
@@ -92,6 +110,12 @@ private:
     float _taper;
     float _sweep;
     float _dihedral;
+    
+    // calculated from above
+    float _tip[3];
+    float _meanChord; // std. mean chord
+    float _wingspan;
+    float _aspectRatio;
 
     float _stall;
     float _stallWidth;
diff --git a/src/FDM/YASim/yasim-test.cpp b/src/FDM/YASim/yasim-test.cpp
index 61ed2a3dd..71c4ac8ef 100644
--- a/src/FDM/YASim/yasim-test.cpp
+++ b/src/FDM/YASim/yasim-test.cpp
@@ -9,6 +9,7 @@
 
 #include "FGFDM.hpp"
 #include "Atmosphere.hpp"
+#include "RigidBody.hpp"
 #include "Airplane.hpp"
 
 using namespace yasim;
@@ -27,9 +28,17 @@ bool fgSetDouble (const char * name, double defaultValue = 0.0) { return 0; }
 
 static const float RAD2DEG = 57.2957795131;
 static const float DEG2RAD = 0.0174532925199;
+/// knots 2 meters per second
 static const float KTS2MPS = 0.514444444444;
 
 
+enum Config 
+{
+  CONFIG_NONE,
+  CONFIG_APPROACH,
+  CONFIG_CRUISE,
+};
+
 // Generate a graph of lift, drag and L/D against AoA at the specified
 // speed and altitude.  The result is a space-separated file of
 // numbers: "aoa lift drag LD" (aoa in degrees, lift and drag in
@@ -40,92 +49,218 @@ static const float KTS2MPS = 0.514444444444;
       "dat" using 1:3 with lines title 'drag', \ 
       "dat" using 1:4 with lines title 'LD'
 */
-void yasim_graph(Airplane* a, float alt, float kts)
+void yasim_graph(Airplane* a, const float alt, const float kts, int cfg = CONFIG_NONE)
 {
-    Model* m = a->getModel();
-    State s;
+  Model* m = a->getModel();
+  State s;
 
-    m->setAir(Atmosphere::getStdPressure(alt),
-              Atmosphere::getStdTemperature(alt),
-              Atmosphere::getStdDensity(alt));
-    m->getBody()->recalc();
+  m->setAir(Atmosphere::getStdPressure(alt),
+  Atmosphere::getStdTemperature(alt),
+  Atmosphere::getStdDensity(alt));
 
-    for(int deg=-179; deg<=179; deg++) {
-        float aoa = deg * DEG2RAD;
-        Airplane::setupState(aoa, kts * KTS2MPS, 0 ,&s);
-        m->getBody()->reset();
-        m->initIteration();
-        m->calcForces(&s);
+  switch (cfg) {
+    case CONFIG_APPROACH:
+      a->loadApproachControls();
+      break;
+    case CONFIG_CRUISE:
+      a->loadCruiseControls();
+      break;
+    case CONFIG_NONE:
+      break;
+  }
+  //if we fake the properties we could also use FGFDM::getExternalInput()
 
-        float acc[3];
-	m->getBody()->getAccel(acc);
-        Math::tmul33(s.orient, acc, acc);
+  m->getBody()->recalc();
+  float cl_max = 0, cd_min = 1e6, ld_max = 0;
+  int cl_max_deg = 0, cd_min_deg = 0, ld_max_deg = 0;
 
-        float drag = acc[0] * (-1/9.8);
-        float lift = 1 + acc[2] * (1/9.8);
+  for(int deg=-15; deg<=90; deg++) {
+    float aoa = deg * DEG2RAD;
+    Airplane::setupState(aoa, kts * KTS2MPS, 0 ,&s);
+    m->getBody()->reset();
+    m->initIteration();
+    m->calcForces(&s);
 
-        printf("%d %g %g %g\n", deg, lift, drag, lift/drag);
+    float acc[3];
+    m->getBody()->getAccel(acc);
+    Math::tmul33(s.orient, acc, acc);
+
+    float drag = acc[0] * (-1/9.8);
+    float lift = 1 + acc[2] * (1/9.8);
+    float ld = lift/drag;
+    
+    if (cd_min > drag) {
+      cd_min = drag;
+      cd_min_deg = deg;
     }
+    if (cl_max < lift) {
+      cl_max = lift;
+      cl_max_deg = deg;
+    }
+    if (ld_max < ld) {
+      ld_max= ld;
+      ld_max_deg = deg;
+    }    
+    printf("%d %g %g %g\n", deg, lift, drag, ld);
+  }
+  printf("# cl_max %g at %d deg\n", cl_max, cl_max_deg);
+  printf("# cd_min %g at %d deg\n", cd_min, cd_min_deg);
+  printf("# ld_max %g at %d deg\n", ld_max, ld_max_deg);  
+}
+
+void yasim_masses(Airplane* a)
+{
+  RigidBody* body = a->getModel()->getBody();
+  int i, N = body->numMasses();
+  float pos[3];
+  float m, mass = 0;
+  printf("id posx posy posz mass\n");
+  for (i = 0; i < N; i++)
+  {
+    body->getMassPosition(i, pos);
+    m = body->getMass(i);
+    printf("%d %.3f %.3f %.3f %.3f\n", i, pos[0], pos[1], pos[2], m);
+    mass += m;
+  }
+  printf("Total mass: %g", mass); 
+}
+
+void yasim_drag(Airplane* a, const float aoa, const float alt, int cfg = CONFIG_NONE)
+{
+  fprintf(stderr,"yasim_drag");
+  Model* m = a->getModel();
+  State s;
+  
+  m->setAir(Atmosphere::getStdPressure(alt),
+	    Atmosphere::getStdTemperature(alt),
+	    Atmosphere::getStdDensity(alt));
+  
+  switch (cfg) {
+    case CONFIG_APPROACH:
+      a->loadApproachControls();
+      break;
+    case CONFIG_CRUISE:
+      a->loadCruiseControls();
+      break;
+    case CONFIG_NONE:
+      break;
+  }
+  
+  m->getBody()->recalc();
+  float cd_min = 1e6;
+  int cd_min_kts = 0;
+  printf("#kts, drag\n");
+  
+  for(int kts=15; kts<=150; kts++) {
+    Airplane::setupState(aoa, kts * KTS2MPS, 0 ,&s);
+    m->getBody()->reset();
+    m->initIteration();
+    m->calcForces(&s);
+    
+    float acc[3];
+    m->getBody()->getAccel(acc);
+    Math::tmul33(s.orient, acc, acc);
+    
+    float drag = acc[0] * (-1/9.8);
+    
+    if (cd_min > drag) {
+      cd_min = drag;
+      cd_min_kts = kts;
+    }
+    printf("%d %g\n", kts, drag);
+  }
+  printf("# cd_min %g at %d kts\n", cd_min, cd_min_kts);
 }
 
 int usage()
 {
-    fprintf(stderr, "Usage: yasim <ac.xml> [-g [-a alt] [-s kts]]\n");
-    return 1;
+  fprintf(stderr, "Usage: yasim <ac.xml> [-g [-a alt] [-s kts] [-approach | -cruise] ]\n");
+  fprintf(stderr, "       yasim <ac.xml> [-d [-a alt] [-approach | -cruise] ]\n");
+  fprintf(stderr, "       yasim <ac.xml> [-m]\n");
+  fprintf(stderr, "       -g print lift/drag table: aoa, lift, drag, lift/drag \n");
+  fprintf(stderr, "       -d print drag over TAS: kts, drag\n");
+  fprintf(stderr, "       -m print mass distribution table: id, x, y, z, mass \n");
+  return 1;
 }
 
+
 int main(int argc, char** argv)
 {
-    FGFDM* fdm = new FGFDM();
-    Airplane* a = fdm->getAirplane();
+  FGFDM* fdm = new FGFDM();
+  Airplane* a = fdm->getAirplane();
 
-    if(argc < 2) return usage();
+  if(argc < 2) return usage();
+  // Read
+  try {
+    string file = argv[1];
+    readXML(SGPath(file), *fdm);
+  } 
+  catch (const sg_exception &e) {
+    printf("XML parse error: %s (%s)\n", e.getFormattedMessage().c_str(), e.getOrigin());
+  }
 
-    // Read
-    try {
-        string file = argv[1];
-        readXML(SGPath(file), *fdm);
-    } catch (const sg_exception &e) {
-        printf("XML parse error: %s (%s)\n",
-               e.getFormattedMessage().c_str(), e.getOrigin());
-    }
-
-    // ... and run
-    a->compile();
-    if(a->getFailureMsg())
-        printf("SOLUTION FAILURE: %s\n", a->getFailureMsg());
-
-    if(!a->getFailureMsg() && argc > 2 && strcmp(argv[2], "-g") == 0) {
-        float alt = 5000, kts = 100;
-        for(int i=3; i<argc; i++) {
-            if     (std::strcmp(argv[i], "-a") == 0) alt = std::atof(argv[++i]);
-            else if(std::strcmp(argv[i], "-s") == 0) kts = std::atof(argv[++i]);
-            else return usage();
+  // ... and run
+  a->compile();
+  if(a->getFailureMsg())
+      printf("SOLUTION FAILURE: %s\n", a->getFailureMsg());
+  if(!a->getFailureMsg() && argc > 2 ) {
+    if(strcmp(argv[2], "-g") == 0) {
+      float alt = 5000, kts = 100;
+      int cfg = CONFIG_NONE;
+      for(int i=3; i<argc; i++) {
+        if (std::strcmp(argv[i], "-a") == 0) {
+          if (i+1 < argc) alt = std::atof(argv[++i]);
         }
-        yasim_graph(a, alt, kts);
-    } else {
-        float aoa = a->getCruiseAoA() * RAD2DEG;
-        float tail = -1 * a->getTailIncidence() * RAD2DEG;
-        float drag = 1000 * a->getDragCoefficient();
-        float cg[3];
-        a->getModel()->getBody()->getCG(cg);
-        a->getModel()->getBody()->recalc();
-
-        float SI_inertia[9];
-        a->getModel()->getBody()->getInertiaMatrix(SI_inertia);
-        
-        printf("Solution results:");
-        printf("       Iterations: %d\n", a->getSolutionIterations());
-        printf(" Drag Coefficient: %f\n", drag);
-        printf("       Lift Ratio: %f\n", a->getLiftRatio());
-        printf("       Cruise AoA: %f\n", aoa);
-        printf("   Tail Incidence: %f\n", tail);
-        printf("Approach Elevator: %f\n", a->getApproachElevator());
-        printf("               CG: x:%.3f, y:%.3f, z:%.3f\n\n", cg[0], cg[1], cg[2]);
-        printf("  Inertia tensor : %.3f, %.3f, %.3f\n", SI_inertia[0], SI_inertia[1], SI_inertia[2]);
-        printf("        [kg*m^2]   %.3f, %.3f, %.3f\n", SI_inertia[3], SI_inertia[4], SI_inertia[5]);
-        printf("     Origo at CG   %.3f, %.3f, %.3f\n", SI_inertia[6], SI_inertia[7], SI_inertia[8]);
+        else if(std::strcmp(argv[i], "-s") == 0) {
+          if(i+1 < argc) kts = std::atof(argv[++i]);
+        }
+        else if(std::strcmp(argv[i], "-approach") == 0) cfg = CONFIG_APPROACH;
+        else if(std::strcmp(argv[i], "-cruise") == 0) cfg = CONFIG_CRUISE;
+        else return usage();
+      }
+      yasim_graph(a, alt, kts, cfg);
+    } 
+    else if(strcmp(argv[2], "-d") == 0) {
+      float alt = 2000, aoa = a->getCruiseAoA();
+      int cfg = CONFIG_NONE;
+      for(int i=3; i<argc; i++) {
+	if (std::strcmp(argv[i], "-a") == 0) {
+	  if (i+1 < argc) alt = std::atof(argv[++i]);
+	}
+	else if(std::strcmp(argv[i], "-approach") == 0) cfg = CONFIG_APPROACH;
+	else if(std::strcmp(argv[i], "-cruise") == 0) cfg = CONFIG_CRUISE;
+	else return usage();
+      }
+      yasim_drag(a, aoa, alt, cfg);
+    } 
+    else if(strcmp(argv[2], "-m") == 0) {
+      yasim_masses(a);
     }
+  }
+  else {
+    printf("Solution results:");
+    float aoa = a->getCruiseAoA() * RAD2DEG;
+    float tail = -1 * a->getTailIncidence() * RAD2DEG;
+    float drag = 1000 * a->getDragCoefficient();
+    float cg[3];
+    a->getModel()->getBody()->getCG(cg);
+    a->getModel()->getBody()->recalc();
+
+    float SI_inertia[9];
+    a->getModel()->getBody()->getInertiaMatrix(SI_inertia);
+    
+    printf("       Iterations: %d\n", a->getSolutionIterations());
+    printf(" Drag Coefficient: %f\n", drag);
+    printf("       Lift Ratio: %f\n", a->getLiftRatio());
+    printf("       Cruise AoA: %f\n", aoa);
+    printf("   Tail Incidence: %f\n", tail);
+    printf("Approach Elevator: %f\n", a->getApproachElevator());
+    printf("               CG: x:%.3f, y:%.3f, z:%.3f\n\n", cg[0], cg[1], cg[2]);
+    printf("Inertia tensor [kg*m^2], origo at CG:\n");
+    printf("  %7.3f, %7.3f, %7.3f\n", SI_inertia[0], SI_inertia[1], SI_inertia[2]);
+    printf("  %7.3f, %7.3f, %7.3f\n", SI_inertia[3], SI_inertia[4], SI_inertia[5]);
+    printf("  %7.3f, %7.3f, %7.3f\n", SI_inertia[6], SI_inertia[7], SI_inertia[8]);
+  }
     delete fdm;
     return 0;
 }