diff --git a/src/FDM/YASim/Airplane.cpp b/src/FDM/YASim/Airplane.cpp
index d5144d1e9..a01a9d10a 100644
--- a/src/FDM/YASim/Airplane.cpp
+++ b/src/FDM/YASim/Airplane.cpp
@@ -13,18 +13,17 @@
 #include "Thruster.hpp"
 #include "Hitch.hpp"
 #include "Airplane.hpp"
+#include "yasim-common.hpp"
 
 namespace yasim {
 
 //default prop names
-static const char* DEF_PROP_ELEVATOR_TRIM = "/controls/flight/elevator-trim";
 
 // gadgets
 inline float abs(float f) { return f<0 ? -f : f; }
 
 Airplane::Airplane()
 {
-    _approachConfig.isApproach = true;
 }
 
 Airplane::~Airplane()
@@ -50,12 +49,12 @@ Airplane::~Airplane()
     }
     for(i=0; i<_solveWeights.size(); i++)
         delete (SolveWeight*)_solveWeights.get(i);
-    for(i=0; i<_cruiseConfig.controls.size(); i++) {
-        ControlSetting* c = (ControlSetting*)_cruiseConfig.controls.get(i);
+    for(i=0; i<_config[CRUISE].controls.size(); i++) {
+        ControlSetting* c = (ControlSetting*)_config[CRUISE].controls.get(i);
         delete c;        
     }
-    for(i=0; i<_approachConfig.controls.size(); i++) {
-        ControlSetting* c = (ControlSetting*)_approachConfig.controls.get(i);
+    for(i=0; i<_config[APPROACH].controls.size(); i++) {
+        ControlSetting* c = (ControlSetting*)_config[APPROACH].controls.get(i);
         delete c;
     }
     delete _wing;
@@ -130,23 +129,18 @@ void Airplane::updateGearState()
     }
 }
 
-void Airplane::setApproach(float speed, float altitude, float aoa, float fuel, float gla)
+/// setup a config record
+void Airplane::setConfig(Configuration cfg, float speed, float altitude, float fuel, float gla, float aoa)
 {
-    _approachConfig.speed = speed;
-    _approachConfig.altitude = altitude;
-    // solver assumes fixed (given) AoA for approach, so setup once
-    _approachConfig.state.setupOrientationFromAoa(aoa); 
-    _approachConfig.aoa = aoa; // not strictly needed, see runConfig()
-    _approachConfig.fuel = fuel;
-    _approachConfig.glideAngle = gla;
-}
- 
-void Airplane::setCruise(float speed, float altitude, float fuel, float gla)
-{
-    _cruiseConfig.speed = speed;
-    _cruiseConfig.altitude = altitude;
-    _cruiseConfig.fuel = fuel;
-    _cruiseConfig.glideAngle = gla;
+    _config[cfg].id = cfg;
+    _config[cfg].speed = speed;
+    _config[cfg].altitude = altitude;
+    // solver assumes fixed (given) AoA for approach, so setup once; 
+    // solver will change this for cruise config
+    _config[cfg].state.setupOrientationFromAoa(aoa); 
+    _config[cfg].aoa = aoa; // not strictly needed, see runConfig()
+    _config[cfg].fuel = fuel;
+    _config[cfg].glideAngle = gla;
 }
 
 /// set property name for elevator  
@@ -174,14 +168,7 @@ Airplane::ControlSetting* Airplane::_addControlSetting(Configuration cfg, const
     ControlSetting* c = new ControlSetting();
     c->propHandle = _controlMap.getInputPropertyHandle(prop);
     c->val = val;
-    switch (cfg) {
-        case APPROACH:
-            _approachConfig.controls.add(c);
-            break;
-        case CRUISE:
-            _cruiseConfig.controls.add(c);
-            break;
-    }
+    _config[cfg].controls.add(c);
     return c;
 }
 
@@ -202,7 +189,7 @@ void Airplane::addControlInput(const char* propName, ControlMap::ControlType typ
 void Airplane::addSolutionWeight(Configuration cfg, int idx, float wgt)
 {
     SolveWeight* w = new SolveWeight();
-    w->approach = (cfg == APPROACH);
+    w->cfg = cfg;
     w->id = idx;
     w->wgt = wgt;
     _solveWeights.add(w);
@@ -594,8 +581,8 @@ void Airplane::compile(bool verbose)
         t->handle = body->addMass(0, t->pos);
         totalFuel += t->cap;
     }
-    _cruiseConfig.weight = _emptyWeight + totalFuel*_cruiseConfig.fuel;
-    _approachConfig.weight = _emptyWeight + totalFuel*_approachConfig.fuel;
+    _config[CRUISE].weight = _emptyWeight + totalFuel*_config[CRUISE].fuel;
+    _config[APPROACH].weight = _emptyWeight + totalFuel*_config[APPROACH].fuel;
 
     
     body->recalc();
@@ -680,12 +667,12 @@ void Airplane::solveGear()
     // The force at max compression should be sufficient to stop a
     // plane moving downwards at 2x the approach descent rate.  Assume
     // a 3 degree approach.
-    float descentRate = 2.0f*_approachConfig.speed/19.1f;
+    float descentRate = 2.0f*_config[APPROACH].speed/19.1f;
 
     // Spread the kinetic energy according to the gear weights.  This
     // will results in an equal compression fraction (not distance) of
     // each gear.
-    float energy = 0.5f*_approachConfig.weight*descentRate*descentRate;
+    float energy = 0.5f*_config[APPROACH].weight*descentRate*descentRate;
 
     for(i=0; i<_gears.size(); i++) {
         GearRec* gr = (GearRec*)_gears.get(i);
@@ -700,7 +687,7 @@ void Airplane::solveGear()
         gr->gear->setSpring(k * gr->gear->getSpring());
 
         // Critically damped (too damped, too!)
-        gr->gear->setDamping(2*Math::sqrt(k*_approachConfig.weight*gr->wgt)
+        gr->gear->setDamping(2*Math::sqrt(k*_config[APPROACH].weight*gr->wgt)
                              * gr->gear->getDamping());
     }
 }
@@ -733,14 +720,14 @@ void Airplane::stabilizeThrust()
 }
 
 /// Setup weights for cruise or approach during solve.
-void Airplane::setupWeights(bool isApproach)
+void Airplane::setupWeights(Configuration cfg)
 {
     int i;
     for(i=0; i<_weights.size(); i++)
         setWeight(i, 0);
     for(i=0; i<_solveWeights.size(); i++) {
         SolveWeight* w = (SolveWeight*)_solveWeights.get(i);
-        if(w->approach == isApproach)
+        if(w->cfg == cfg)
             setWeight(w->id, w->wgt);
     }
 }
@@ -760,8 +747,8 @@ void Airplane::setControlValues(const Vector& controls)
 void Airplane::runConfig(Config &cfg)
 {
     // aoa is consider to be given for approach so we calculate orientation 
-    // for approach only once in setApproach() but everytime for cruise here.
-    if (!cfg.isApproach) {
+    // for approach only once in setConfig() but everytime for cruise here.
+    if (!(cfg.id == APPROACH)) {
         cfg.state.setupOrientationFromAoa(cfg.aoa);
     }
     cfg.state.setupSpeedAndPosition(cfg.speed, cfg.glideAngle);
@@ -775,7 +762,7 @@ void Airplane::runConfig(Config &cfg)
     cfg.state.globalToLocal(wind, wind);
     
     setFuelFraction(cfg.fuel);
-    setupWeights(cfg.isApproach);
+    setupWeights(cfg.id);
     
     // Set up the thruster parameters and iterate until the thrust
     // stabilizes.
@@ -875,7 +862,7 @@ float Airplane::_getPitch(Config &cfg)
 }
 
 ///helper for solveAirplane()
-float Airplane::_getLift(Config &cfg)
+float Airplane::_getLiftForce(Config &cfg)
 {
     float tmp[3];
     _model.getBody()->getAccel(tmp);
@@ -883,6 +870,15 @@ float Airplane::_getLift(Config &cfg)
     return cfg.weight * tmp[2];
 }
 
+///helper for solveAirplane()
+float Airplane::_getDragForce(Config &cfg)
+{
+    float tmp[3];
+    _model.getBody()->getAccel(tmp);
+    cfg.state.localToGlobal(tmp, tmp);
+    return cfg.weight * tmp[0];
+}
+
 void Airplane::solveAirplane(bool verbose)
 {
     static const float ARCMIN = 0.0002909f;
@@ -911,28 +907,26 @@ void Airplane::solveAirplane(bool verbose)
             return;
         }
         // Run an iteration at cruise, and extract the needed numbers:
-        runConfig(_cruiseConfig);
+        runConfig(_config[CRUISE]);
         
         _model.getThrust(tmp);
-        float thrust = tmp[0] + _cruiseConfig.weight * Math::sin(_cruiseConfig.glideAngle) * 9.81;
-
-        _model.getBody()->getAccel(tmp);
-        _cruiseConfig.state.localToGlobal(tmp, tmp);
-        float xforce = _cruiseConfig.weight * tmp[0];
-        float clift0 = _getLift(_cruiseConfig);
-        float cpitch0 = _getPitch(_cruiseConfig);
+        float thrust = tmp[0] + _config[CRUISE].weight * Math::sin(_config[CRUISE].glideAngle) * 9.81;
+        
+        float cDragForce = _getDragForce(_config[CRUISE]);
+        float clift0 = _getLiftForce(_config[CRUISE]);
+        float cpitch0 = _getPitch(_config[CRUISE]);
 
         // Run an approach iteration, and do likewise
-        runConfig(_approachConfig);
-        double apitch0 = _getPitch(_approachConfig);
-        float alift = _getLift(_approachConfig);
+        runConfig(_config[APPROACH]);
+        double apitch0 = _getPitch(_config[APPROACH]);
+        float alift = _getLiftForce(_config[APPROACH]);
 
         // Modify the cruise AoA a bit to get a derivative
-        _cruiseConfig.aoa += ARCMIN;
-        runConfig(_cruiseConfig);
-        _cruiseConfig.aoa -= ARCMIN;
+        _config[CRUISE].aoa += ARCMIN;
+        runConfig(_config[CRUISE]);
+        _config[CRUISE].aoa -= ARCMIN;
             
-        float clift1 = _getLift(_cruiseConfig);
+        float clift1 = _getLiftForce(_config[CRUISE]);
 
         // Do the same with the tail incidence
         float savedIncidence = _tailIncidence->val;
@@ -942,16 +936,16 @@ void Airplane::solveAirplane(bool verbose)
             _failureMsg = "Tail incidence out of bounds.";
             return;
         };
-        runConfig(_cruiseConfig);
+        runConfig(_config[CRUISE]);
         _tailIncidenceCopy->val = _tailIncidence->val = savedIncidence;
         _tail->setIncidence(_tailIncidence->val);
 
-        float cpitch1 = _getPitch(_cruiseConfig);
+        float cpitch1 = _getPitch(_config[CRUISE]);
 
         // Now calculate:
-        float awgt = 9.8f * _approachConfig.weight;
+        float awgt = 9.8f * _config[APPROACH].weight;
 
-        float dragFactor = thrust / (thrust-xforce);
+        float dragFactor = thrust / (thrust-cDragForce);
         float liftFactor = awgt / (awgt+alift);
 
         // Sanity:
@@ -964,10 +958,10 @@ void Airplane::solveAirplane(bool verbose)
         // variable).
         const float ELEVDIDDLE = 0.001f;
         _approachElevator->val += ELEVDIDDLE;
-        runConfig(_approachConfig);
+        runConfig(_config[APPROACH]);
         _approachElevator->val -= ELEVDIDDLE;
 
-        double apitch1 = _getPitch(_approachConfig);
+        double apitch1 = _getPitch(_config[APPROACH]);
 
         // Now apply the values we just computed.  Note that the
         // "minor" variables are deferred until we get the lift/drag
@@ -990,14 +984,14 @@ void Airplane::solveAirplane(bool verbose)
         if (verbose) {
             fprintf(stdout,"%4d\t%f\t%f\t%f\t%f\n", _solutionIterations, aoaDelta, tailDelta, clift0, cpitch1);
         }
-        _cruiseConfig.aoa += _solverDelta*aoaDelta;
+        _config[CRUISE].aoa += _solverDelta*aoaDelta;
         _tailIncidence->val += _solverDelta*tailDelta;
         
-        _cruiseConfig.aoa = Math::clamp(_cruiseConfig.aoa, -0.175f, 0.175f);
+        _config[CRUISE].aoa = Math::clamp(_config[CRUISE].aoa, -0.175f, 0.175f);
         _tailIncidence->val = Math::clamp(_tailIncidence->val, -0.175f, 0.175f);
 
-        if(abs(xforce/_cruiseConfig.weight) < _solverThreshold*0.0001 &&
-          abs(alift/_approachConfig.weight) < _solverThreshold*0.0001 &&
+        if(abs(cDragForce/_config[CRUISE].weight) < _solverThreshold*0.0001 &&
+          abs(alift/_config[APPROACH].weight) < _solverThreshold*0.0001 &&
           abs(aoaDelta) < _solverThreshold*.000017 &&
           abs(tailDelta) < _solverThreshold*.000017)
         {
@@ -1024,7 +1018,7 @@ void Airplane::solveAirplane(bool verbose)
     } else if(_liftRatio < 1e-04 || _liftRatio > 1e4) {
         _failureMsg = "Lift ratio beyond reasonable bounds.";
         return;
-    } else if(Math::abs(_cruiseConfig.aoa) >= .17453293) {
+    } else if(Math::abs(_config[CRUISE].aoa) >= .17453293) {
         _failureMsg = "Cruise AoA > 10 degrees";
         return;
     } else if(Math::abs(_tailIncidence->val) >= .17453293) {
@@ -1058,12 +1052,12 @@ void Airplane::solveHelicopter(bool verbose)
         applyDragFactor(Math::pow(15.7/1000, 1/_solverDelta));
         applyLiftRatio(Math::pow(104, 1/_solverDelta));
     }
-    _cruiseConfig.state.setupState(0,0,0);
-    _model.setState(&_cruiseConfig.state);
-    setupWeights(true);
+    _config[CRUISE].state.setupState(0,0,0);
+    _model.setState(&_config[CRUISE].state);
+    setupWeights(APPROACH);
     _controlMap.reset();
     _model.getBody()->reset();
-    _model.setStandardAtmosphere(_cruiseConfig.altitude);    
+    _model.setStandardAtmosphere(_config[CRUISE].altitude);    
 }
 
 float Airplane::getCGMAC()
diff --git a/src/FDM/YASim/Airplane.hpp b/src/FDM/YASim/Airplane.hpp
index ca6a83899..5a4fc806e 100644
--- a/src/FDM/YASim/Airplane.hpp
+++ b/src/FDM/YASim/Airplane.hpp
@@ -25,8 +25,11 @@ public:
     ~Airplane();
 
     enum Configuration {
+        NONE,
         APPROACH,
         CRUISE,
+        TAKEOFF,  // for testing
+        TEST,     // for testing
     };
     
     void iterate(float dt);
@@ -61,8 +64,8 @@ public:
     int  addWeight(const float* pos, float size);
     void setWeight(int handle, float mass);
 
-    void setApproach(float speed, float altitude, float aoa, float fuel, float gla);
-    void setCruise(float speed, float altitude, float fuel, float gla);
+    void setConfig(Configuration cfg, float speed, float altitude, float fuel, 
+                   float gla = 0, float aoa = 0);
     
     /// add (fixed) control setting to approach/cruise config (for solver)
     void addControlSetting(Configuration cfg, const char* prop, float val);
@@ -100,14 +103,15 @@ public:
     int getSolutionIterations() const { return _solutionIterations; }
     float getDragCoefficient() const { return _dragFactor; }
     float getLiftRatio() const { return _liftRatio; }
-    float getCruiseAoA() const { return _cruiseConfig.aoa; }
+    float getCruiseAoA() const { return _config[CRUISE].aoa; }
     float getTailIncidence()const;
     float getApproachElevator() const;
     const char* getFailureMsg() const { return _failureMsg; }
-
+    float getMass() const { return _model.getMass(); };
+    
     // next two are used only in yasim CLI tool
-    void setApproachControls() { setControlValues(_approachConfig.controls); }
-    void setCruiseControls() { setControlValues(_cruiseConfig.controls); }
+    void setApproachControls() { setControlValues(_config[APPROACH].controls); }
+    void setCruiseControls() { setControlValues(_config[CRUISE].controls); }
     
     float getCGHardLimitXMin() const { return _cgMin; } // get min x-coordinate for c.g (from main gear)
     float getCGHardLimitXMax() const { return _cgMax; } // get max x-coordinate for c.g (from nose gear)
@@ -166,7 +170,7 @@ private:
     };
     struct SolveWeight { 
       int id {-1};
-      bool approach {false};
+      Configuration cfg {APPROACH};
       float wgt {0};
     };
     struct ContactRec {
@@ -174,18 +178,17 @@ private:
       float p[3] {0,0,0};
     };
     struct Config {
-      bool isApproach {false};
-      float speed {0};
-      float fuel {0};
-      float glideAngle {0};      
-      float aoa {0};
-      float altitude {0};
-      float weight {0};
-      State state;
-      Vector controls;
+        Configuration id;
+        float speed {0};
+        float fuel {0};
+        float glideAngle {0};
+        float aoa {0};
+        float altitude {0};
+        float weight {0};
+        State state;
+        Vector controls;
     };
-    Config _cruiseConfig;
-    Config _approachConfig;
+    Config _config[Configuration::TEST];
 
     /// load values for controls as defined in cruise/approach configuration
     void setControlValues(const Vector& controls);
@@ -193,7 +196,8 @@ private:
     void runConfig(Config &cfg);
     void solveGear();
     float _getPitch(Config &cfg);
-    float _getLift(Config &cfg);
+    float _getLiftForce(Config &cfg);
+    float _getDragForce(Config &cfg);
     void solveAirplane(bool verbose = false);
     void solveHelicopter(bool verbose = false);
     float compileWing(Wing* w);
@@ -206,7 +210,7 @@ private:
     void compileContactPoints();
     float normFactor(float f);
     void updateGearState();
-    void setupWeights(bool isApproach);
+    void setupWeights(Configuration cfg);
     void calculateCGHardLimits();
     ///calculate mass divided by area of main wing
     float _getWingLoad(float mass) const;
diff --git a/src/FDM/YASim/FGFDM.cpp b/src/FDM/YASim/FGFDM.cpp
index a5ab68121..7fa94a85c 100644
--- a/src/FDM/YASim/FGFDM.cpp
+++ b/src/FDM/YASim/FGFDM.cpp
@@ -302,12 +302,12 @@ void FGFDM::parseApproachCruise(const XMLAttributes* a, const char* name)
     float gla = attrf(a, "glide-angle", 0) * DEG2RAD;
     if (!strcmp(name, "approach")) {
         float aoa = attrf(a, "aoa", 0) * DEG2RAD;
-        _airplane.setApproach(spd, alt, aoa, attrf(a, "fuel", 0.2), gla);
         _airplaneCfg = Airplane::Configuration::APPROACH;
+        _airplane.setConfig(_airplaneCfg, spd, alt, attrf(a, "fuel", 0.2), gla, aoa);
     }
     else {
-        _airplane.setCruise(spd, alt, attrf(a, "fuel", 0.5),gla);
         _airplaneCfg = Airplane::Configuration::CRUISE;
+        _airplane.setConfig(_airplaneCfg, spd, alt, attrf(a, "fuel", 0.5), gla);
     }
 }
 
diff --git a/src/FDM/YASim/Model.cpp b/src/FDM/YASim/Model.cpp
index 5c4e50f9f..45dada3bd 100644
--- a/src/FDM/YASim/Model.cpp
+++ b/src/FDM/YASim/Model.cpp
@@ -99,32 +99,30 @@ void Model::getThrust(float* out) const
 void Model::initIteration()
 {
     // Precompute torque and angular momentum for the thrusters
-    int i;
-    for(i=0; i<3; i++)
-	_gyro[i] = _torque[i] = 0;
+    Math::zero3(_torque);
+    Math::zero3(_gyro);
 
     // Need a local altitude for the wind calculation
     float lground[4];
     _s->planeGlobalToLocal(_global_ground, lground);
     float alt = Math::abs(lground[3]);
 
-    for(i=0; i<_thrusters.size(); i++) {
-	Thruster* t = (Thruster*)_thrusters.get(i);
-	
+    for(int i=0; i<_thrusters.size(); i++) {
+        Thruster* t = (Thruster*)_thrusters.get(i);
         // Get the wind velocity at the thruster location
         float pos[3], v[3];
-	t->getPosition(pos);
+        t->getPosition(pos);
         localWind(pos, _s, v, alt);
 
-	t->setWind(v);
-	t->setAtmosphere(_atmo);
-	t->integrate(_integrator.getInterval());
+        t->setWind(v);
+        t->setAtmosphere(_atmo);
+        t->integrate(_integrator.getInterval());
 
-	t->getTorque(v);
-	Math::add3(v, _torque, _torque);
+        t->getTorque(v);
+        Math::add3(v, _torque, _torque);
 
-	t->getGyro(v);
-	Math::add3(v, _gyro, _gyro);
+        t->getGyro(v);
+        Math::add3(v, _gyro, _gyro);
     }
 
     // Displace the turbulence coordinates according to the local wind.
@@ -134,12 +132,12 @@ void Model::initIteration()
         _turb->offset(toff);
     }
 
-    for(i=0; i<_gears.size(); i++) {
+    for(int i=0; i<_gears.size(); i++) {
         Gear* g = (Gear*)_gears.get(i);
         g->integrate(_integrator.getInterval());
     }
 
-    for(i=0; i<_hitches.size(); i++) {
+    for(int i=0; i<_hitches.size(); i++) {
         Hitch* h = (Hitch*)_hitches.get(i);
         h->integrate(_integrator.getInterval());
     }
@@ -272,7 +270,7 @@ void Model::calcForces(State* s)
     // calcForces.  They get computed before we begin the integration
     // step.
     _body.setGyro(_gyro);
-    _body.addTorque(_torque);
+    _body.setTorque(_torque);
     int i,j;
     for(i=0; i<_thrusters.size(); i++) {
       Thruster* t = (Thruster*)_thrusters.get(i);
diff --git a/src/FDM/YASim/Model.hpp b/src/FDM/YASim/Model.hpp
index 6786ffc92..3894f7480 100644
--- a/src/FDM/YASim/Model.hpp
+++ b/src/FDM/YASim/Model.hpp
@@ -30,6 +30,7 @@ public:
 
     RigidBody* getBody() { return &_body; }
     void getCG(float* cg) const { return _body.getCG(cg); }
+    float getMass() const {return _body.getTotalMass(); }
     Integrator* getIntegrator() { return &_integrator; }
 
     void setTurbulence(Turbulence* turb) { _turb = turb; }
diff --git a/src/FDM/YASim/RigidBody.hpp b/src/FDM/YASim/RigidBody.hpp
index 5a61c875a..dd9005217 100644
--- a/src/FDM/YASim/RigidBody.hpp
+++ b/src/FDM/YASim/RigidBody.hpp
@@ -73,6 +73,7 @@ public:
     
     /// Adds a torque with the specified axis and magnitude
     void addTorque(const float* torque) { Math::add3(_torque, torque, _torque); }
+    void setTorque(const float* torque) { Math::set3(torque, _torque); }
 
     // Sets the rotation rate of the body (about its c.g.) within the
     // surrounding environment.  This is needed to compute torque on
diff --git a/src/FDM/YASim/yasim-common.hpp b/src/FDM/YASim/yasim-common.hpp
index c591d19ba..7a5925365 100644
--- a/src/FDM/YASim/yasim-common.hpp
+++ b/src/FDM/YASim/yasim-common.hpp
@@ -38,6 +38,7 @@ namespace yasim {
 
     static const float INCIDENCE_MIN = -20*DEG2RAD;
     static const float INCIDENCE_MAX = 20*DEG2RAD;
+    static const char* DEF_PROP_ELEVATOR_TRIM = "/controls/flight/elevator-trim";
 }; //namespace yasim
 
 #endif // ifndef _YASIM_COMMON_HPP
diff --git a/src/FDM/YASim/yasim-test.cpp b/src/FDM/YASim/yasim-test.cpp
index 1f2a0ae4d..5156a4826 100644
--- a/src/FDM/YASim/yasim-test.cpp
+++ b/src/FDM/YASim/yasim-test.cpp
@@ -28,11 +28,35 @@ double fgGetDouble (const char * name, double defaultValue = 0.0) { return 0; }
 bool fgSetDouble (const char * name, double defaultValue = 0.0) { return 0; }
 
 
-enum Config 
+void _setup(Airplane* a, Airplane::Configuration cfgID, float altitude)
 {
-  CONFIG_NONE,
-  CONFIG_APPROACH,
-  CONFIG_CRUISE,
+    Model* m = a->getModel();
+    m->setStandardAtmosphere(altitude);
+    switch (cfgID) {
+        case Airplane::APPROACH:
+            fprintf(stderr,"Setting approach controls.\n");
+            a->setApproachControls();
+            break;
+        case Airplane::CRUISE:
+            fprintf(stderr,"Setting cruise controls.\n");
+            a->setCruiseControls();
+            break;
+        default:
+            break;
+    }
+    m->getBody()->recalc();    
+};
+
+void _calculateAcceleration(Airplane* a, float aoa_rad, float speed_mps, float* output)
+{
+    Model* m = a->getModel();
+    State s;
+    s.setupState(aoa_rad, speed_mps, 0);
+    m->getBody()->reset();
+    m->initIteration();
+    m->calcForces(&s);    
+    m->getBody()->getAccel(output);
+    s.localToGlobal(output, output);    
 };
 
 // Generate a graph of lift, drag and L/D against AoA at the specified
@@ -45,123 +69,97 @@ enum Config
       "dat" using 1:3 with lines title 'drag', \ 
       "dat" using 1:4 with lines title 'LD'
 */
-void yasim_graph(Airplane* a, const float alt, const float kts, int cfg = CONFIG_NONE)
+void yasim_graph(Airplane* a, const float alt, const float kts, Airplane::Configuration cfgID)
 {
-  Model* m = a->getModel();
-  State s;
-
-  m->setStandardAtmosphere(alt);
-
-  switch (cfg) {
-    case CONFIG_APPROACH:
-      a->setApproachControls();
-      break;
-    case CONFIG_CRUISE:
-      a->setCruiseControls();
-      break;
-    case CONFIG_NONE:
-      break;
-  }
-  //if we fake the properties we could also use FGFDM::getExternalInput()
-
-  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;
-
-  for(int deg=-15; deg<=90; deg++) {
-    float aoa = deg * DEG2RAD;
-    s.setupState(aoa, kts * KTS2MPS, 0);
-    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);
-    float lift = 1 + acc[2] * (1/9.8);
-    float ld = lift/drag;
+    _setup(a, cfgID, alt);
+    float speed = kts * KTS2MPS;
+    float acc[3] {0,0,0};
+    float cl_max = 0, cd_min = 1e6, ld_max = 0;
+    int   cl_max_deg = 0, cd_min_deg = 0, ld_max_deg = 0;
     
-    if (cd_min > drag) {
-      cd_min = drag;
-      cd_min_deg = deg;
+    printf("aoa\tlift\tdrag\n");
+    for(int deg=-15; deg<=90; deg++) {
+        float aoa = deg * DEG2RAD;
+        _calculateAcceleration(a, aoa, speed, 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("%2d\t%.4f\t%.4f\n", deg, lift, drag);
     }
-    if (cl_max < lift) {
-      cl_max = lift;
-      cl_max_deg = deg;
-    }
-    if (ld_max < ld) {
-      ld_max= ld;
-      ld_max_deg = deg;
-    }    
-    printf("%2d\t%.4f\t%.4f\t%.4f\n", deg, lift, drag, ld);
-  }
-  printf("# cl_max %.4f at %d deg\n", cl_max, cl_max_deg);
-  printf("# cd_min %.4f at %d deg\n", cd_min, cd_min_deg);
-  printf("# ld_max %.4f at %d deg\n", ld_max, ld_max_deg);  
+    printf("# cl_max %.4f at %d deg\n", cl_max, cl_max_deg);
+    printf("# cd_min %.4f at %d deg\n", cd_min, cd_min_deg);
+    printf("# ld_max %.4f 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); 
+    RigidBody* body = a->getModel()->getBody();
+    int N = body->numMasses();
+    float pos[3];
+    float m, mass = 0;
+    printf("id\tposx\tposy\tposz\tmass\n");
+    for (int i = 0; i < N; i++)
+    {
+        body->getMassPosition(i, pos);
+        m = body->getMass(i);
+        printf("%d\t%.3f\t%.3f\t%.3f\t%.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)
+void yasim_drag(Airplane* a, const float aoa, const float alt, Airplane::Configuration cfgID)
 {
-  fprintf(stderr,"yasim_drag");
-  Model* m = a->getModel();
-  State s;
-  
-  m->setStandardAtmosphere(alt);
-  
-  switch (cfg) {
-    case CONFIG_APPROACH:
-      a->setApproachControls();
-      break;
-    case CONFIG_CRUISE:
-      a->setCruiseControls();
-      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++) {
-    s.setupState(aoa, kts * KTS2MPS, 0);
-    m->getBody()->reset();
-    m->initIteration();
-    m->calcForces(&s);
+    _setup(a, cfgID, alt);
     
-    float acc[3];
-    m->getBody()->getAccel(acc);
-    Math::tmul33(s.orient, acc, acc);
+    float cd_min = 1e6;
+    int cd_min_kts = 0;
+    float acc[3] {0,0,0};
     
-    float drag = acc[0] * (-1/9.8);
-    
-    if (cd_min > drag) {
-      cd_min = drag;
-      cd_min_kts = kts;
+    printf("#kts, drag\n");    
+    for(int kts=15; kts<=150; kts++) {
+        _calculateAcceleration(a, aoa,kts * KTS2MPS, 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);
+}
+
+void findMinSpeed(Airplane* a, float alt)
+{
+    a->addControlSetting(Airplane::CRUISE, DEF_PROP_ELEVATOR_TRIM, 0.7f);
+    _setup(a, Airplane::CRUISE, alt);
+    float acc[3];
+
+    printf("aoa\tknots\tlift\n");
+    for(int deg=0; deg<=20; deg++) {
+        float aoa = deg * DEG2RAD;
+        for(int kts=15; kts<=180; kts++) {
+            _calculateAcceleration(a, aoa, kts * KTS2MPS, acc);
+            float lift = acc[2];
+            if (lift > 0) {
+                printf("%d\t%d\t%f\n", deg, kts, lift);
+                break;
+            }
+        }
     }
-    printf("%d %g\n", kts, drag);
-  }
-  printf("# cd_min %g at %d kts\n", cd_min, cd_min_kts);
 }
 
 void report(Airplane* a)
@@ -233,97 +231,108 @@ void report(Airplane* a)
 
 int usage()
 {
-  fprintf(stderr, "Usage: \n");
-  fprintf(stderr, "  yasim <aircraft.xml> [-g [-a meters] [-s kts] [-approach | -cruise] ]\n");
-  fprintf(stderr, "  yasim <aircraft.xml> [-d [-a meters] [-approach | -cruise] ]\n");
-  fprintf(stderr, "  yasim <aircraft.xml> [-m]\n");
-  fprintf(stderr, "  yasim <aircraft.xml> [-test] [-a meters] [-s kts] [-approach | -cruise] ]\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, "                       -a set altitude in meters!\n");
-  fprintf(stderr, "                       -s set speed in knots\n");
-  fprintf(stderr, "                       -m print mass distribution table: id, x, y, z, mass \n");
-  fprintf(stderr, "                       -test print summary and output like -g -m \n");
-  return 1;
+    fprintf(stderr, "Usage: \n");
+    fprintf(stderr, "  yasim <aircraft.xml> [-g [-a meters] [-s kts] [-approach | -cruise] ]\n");
+    fprintf(stderr, "  yasim <aircraft.xml> [-d [-a meters] [-approach | -cruise] ]\n");
+    fprintf(stderr, "  yasim <aircraft.xml> [-m]\n");
+    fprintf(stderr, "  yasim <aircraft.xml> [-test] [-a meters] [-s kts] [-approach | -cruise] ]\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, "                       -a set altitude in meters!\n");
+    fprintf(stderr, "                       -s set speed in knots\n");
+    fprintf(stderr, "                       -m print mass distribution table: id, x, y, z, mass \n");
+    fprintf(stderr, "                       -test print summary and output like -g -m \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();
-  // 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
-  bool verbose {false};
-  if (argc > 2 && strcmp(argv[2], "-v") == 0) {
-    verbose=true;
-  }
-  if ((argc == 4) && (strcmp(argv[2], "--tweak") == 0)) {
-    float tweak = std::atof(argv[3]);
-    a->setSolverTweak(tweak);
-    a->setSolverMaxIterations(2000);
-    verbose=true;
-  }
-  a->compile(verbose);
-  if(a->getFailureMsg()) {
-      printf("SOLUTION FAILURE: %s\n", a->getFailureMsg());
-  }
-  if(!a->getFailureMsg() && argc > 2 ) {
-    bool test = (strcmp(argv[2], "-test") == 0);
-    if((strcmp(argv[2], "-g") == 0) || test) 
-    {
-      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]);
-        }
-        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();
-      }
-      if (test) {
-        report(a);
-        printf("\n#-- lift, drag at altitude %.0f meters, %.0f knots, Config %d --\n", alt, kts, cfg);
-      }
-      yasim_graph(a, alt, kts, cfg);
-      if (test) {
-        printf("\n#-- mass distribution --\n");
-        yasim_masses(a);
-      }
+    if(argc < 2) return usage();
+    // Read
+    try {
+        string file = argv[1];
+        readXML(SGPath(file), *fdm);
     } 
-    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);
+    catch (const sg_exception &e) {
+        printf("XML parse error: %s (%s)\n", e.getFormattedMessage().c_str(), e.getOrigin());
     }
-  }
-  else {
-    report(a);
-  }
-  delete fdm;
-  return 0;
+    // ... and run
+    bool verbose {false};
+    if (argc > 2 && strcmp(argv[2], "-v") == 0) {
+        verbose=true;
+    }
+    if ((argc == 4) && (strcmp(argv[2], "--tweak") == 0)) {
+        float tweak = std::atof(argv[3]);
+        a->setSolverTweak(tweak);
+        a->setSolverMaxIterations(2000);
+        verbose=true;
+    }
+    a->compile(verbose);
+    if(a->getFailureMsg()) {
+        printf("SOLUTION FAILURE: %s\n", a->getFailureMsg());
+    }
+    if(!a->getFailureMsg() && argc > 2 ) {
+        bool test = (strcmp(argv[2], "-test") == 0);
+        Airplane::Configuration cfg = Airplane::NONE;
+        float alt = 5000, kts = 100;
+
+        if((strcmp(argv[2], "-g") == 0) || test) {
+            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], "-s") == 0) {
+                    if(i+1 < argc) kts = std::atof(argv[++i]);
+                }
+                else if(std::strcmp(argv[i], "-approach") == 0) cfg = Airplane::APPROACH;
+                else if(std::strcmp(argv[i], "-cruise") == 0) cfg = Airplane::CRUISE;
+                else return usage();
+            }
+            if (test) {
+                report(a);
+                printf("\n#-- lift, drag at altitude %.0f meters, %.0f knots, Config %d --\n", alt, kts, cfg);
+            }
+            yasim_graph(a, alt, kts, cfg);
+            if (test) {
+                printf("\n#-- mass distribution --\n");
+                yasim_masses(a);
+            }
+        } 
+        else if(strcmp(argv[2], "-d") == 0) {
+            float alt = 2000, aoa = a->getCruiseAoA();
+            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 = Airplane::APPROACH;
+                else if(std::strcmp(argv[i], "-cruise") == 0) cfg = Airplane::CRUISE;
+                else return usage();
+            }
+            yasim_drag(a, aoa, alt, cfg);
+        }
+        else if(strcmp(argv[2], "-m") == 0) {
+            yasim_masses(a);
+        }
+        else if(strcmp(argv[2], "--min-speed") == 0) {
+            alt = 10;
+            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 = Airplane::APPROACH;
+                else if(std::strcmp(argv[i], "-cruise") == 0) cfg = Airplane::CRUISE;
+                else return usage();
+            }
+            findMinSpeed(a, alt);
+        }
+    }
+    else {
+        report(a);
+    }
+    delete fdm;
+    return 0;
 }