Extend yasim-test.cpp; add print mass distribution, add plot with different configs

src/FDM/YASim/Airplane.cpp

@@ -839,6 +839,17 @@ 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(); 
+}
+
 void Airplane::runCruise()
 {
     setupState(_cruiseAoA, _cruiseSpeed,_cruiseGlideAngle, &_cruiseState);
@@ -847,14 +858,8 @@ void Airplane::runCruise()
                   Atmosphere::calcStdDensity(_cruiseP, _cruiseT));
 
     // The control configuration
-    _controls.reset();
+    loadCruiseControls();
-    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
-
     // The local wind
     float wind[3];
     Math::mul3(-1, _cruiseState.v, wind);
@@ -865,7 +870,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 +887,17 @@ 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();
+}
+
 void Airplane::runApproach()
 {
     setupState(_approachAoA, _approachSpeed,_approachGlideAngle, &_approachState);
@@ -890,14 +906,8 @@ void Airplane::runApproach()
                   Atmosphere::calcStdDensity(_approachP, _approachT));
 
     // The control configuration
-    _controls.reset();
+    loadApproachControls();
-    int i;
+    
-    for(i=0; i<_approachControls.size(); i++) {
-	Control* c = (Control*)_approachControls.get(i);
-	_controls.setInput(c->control, c->val);
-    }
-    _controls.applyControls(1000000);
-
     // The local wind
     float wind[3];
     Math::mul3(-1, _approachState.v, wind);
@@ -909,7 +919,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,

src/FDM/YASim/Airplane.hpp

@@ -95,6 +95,8 @@ public:
     const char* getFailureMsg();
 
     static void setupState(float aoa, float speed, float gla, State* s); // utility
+    void loadApproachControls();
+    void loadCruiseControls();
 
 private:
     struct Tank { float pos[3]; float cap; float fill;

src/FDM/YASim/ControlMap.hpp

@@ -49,8 +49,9 @@ public:
     // specified value.
     void setInput(int input, float value);
 
-    // Calculates and applies the settings received since the last reset().
+    /// Calculates and applies the settings received since the last reset(). 
-    void applyControls(float dt);
+    /// 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

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;
@@ -30,6 +31,13 @@ static const float DEG2RAD = 0.0174532925199;
 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,15 +48,29 @@ 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, float alt, float kts, int cfg = CONFIG_NONE)
 {
     Model* m = a->getModel();
     State s;
-
+	
     m->setAir(Atmosphere::getStdPressure(alt),
               Atmosphere::getStdTemperature(alt),
               Atmosphere::getStdDensity(alt));
-    m->getBody()->recalc();
+
+	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()
+
+
+	m->getBody()->recalc();
 
     for(int deg=-179; deg<=179; deg++) {
         float aoa = deg * DEG2RAD;
@@ -58,7 +80,7 @@ void yasim_graph(Airplane* a, float alt, float kts)
         m->calcForces(&s);
 
         float acc[3];
-	m->getBody()->getAccel(acc);
+		m->getBody()->getAccel(acc);
         Math::tmul33(s.orient, acc, acc);
 
         float drag = acc[0] * (-1/9.8);
@@ -68,19 +90,39 @@ void yasim_graph(Airplane* a, float alt, float kts)
     }
 }
 
+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);	
+}
+
 int usage()
 {
-    fprintf(stderr, "Usage: yasim <ac.xml> [-g [-a alt] [-s kts]]\n");
+    fprintf(stderr, "Usage: yasim <ac.xml> [-g [-a alt] [-s kts] [-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, "       -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();
 
     if(argc < 2) return usage();
-
     // Read
     try {
         string file = argv[1];
@@ -94,38 +136,51 @@ int main(int argc, char** argv)
     a->compile();
     if(a->getFailureMsg())
         printf("SOLUTION FAILURE: %s\n", a->getFailureMsg());
-
+    if(!a->getFailureMsg() && argc > 2 ) {
-    if(!a->getFailureMsg() && argc > 2 && strcmp(argv[2], "-g") == 0) {
+		if(strcmp(argv[2], "-g") == 0) {
-        float alt = 5000, kts = 100;
+			float alt = 5000, kts = 100;
-        for(int i=3; i<argc; i++) {
+			int cfg = CONFIG_NONE;
-            if     (std::strcmp(argv[i], "-a") == 0) alt = std::atof(argv[++i]);
+			for(int i=3; i<argc; i++) {
-            else if(std::strcmp(argv[i], "-s") == 0) kts = std::atof(argv[++i]);
+				if (std::strcmp(argv[i], "-a") == 0) {
-            else return usage();
+          if (i+1 < argc) alt = std::atof(argv[++i]);
         }
-        yasim_graph(a, alt, kts);
+				else if(std::strcmp(argv[i], "-s") == 0) {
-    } else {
+          if(i+1 < argc) kts = std::atof(argv[++i]);
-        float aoa = a->getCruiseAoA() * RAD2DEG;
+        }
-        float tail = -1 * a->getTailIncidence() * RAD2DEG;
+				else if(std::strcmp(argv[i], "-approach") == 0) cfg = CONFIG_APPROACH;
-        float drag = 1000 * a->getDragCoefficient();
+				else if(std::strcmp(argv[i], "-cruise") == 0) cfg = CONFIG_CRUISE;
-        float cg[3];
+				else return usage();
-        a->getModel()->getBody()->getCG(cg);
+			}
-        a->getModel()->getBody()->recalc();
+			yasim_graph(a, alt, kts, 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];
+		float SI_inertia[9];
-        a->getModel()->getBody()->getInertiaMatrix(SI_inertia);
+		a->getModel()->getBody()->getInertiaMatrix(SI_inertia);
-        
+		
-        printf("Solution results:");
+		printf("       Iterations: %d\n", a->getSolutionIterations());
-        printf("       Iterations: %d\n", a->getSolutionIterations());
+		printf(" Drag Coefficient: %f\n", drag);
-        printf(" Drag Coefficient: %f\n", drag);
+		printf("       Lift Ratio: %f\n", a->getLiftRatio());
-        printf("       Lift Ratio: %f\n", a->getLiftRatio());
+		printf("       Cruise AoA: %f\n", aoa);
-        printf("       Cruise AoA: %f\n", aoa);
+		printf("   Tail Incidence: %f\n", tail);
-        printf("   Tail Incidence: %f\n", tail);
+		printf("Approach Elevator: %f\n", a->getApproachElevator());
-        printf("Approach Elevator: %f\n", a->getApproachElevator());
+		printf("               CG: x:%.3f, y:%.3f, z:%.3f\n\n", cg[0], cg[1], cg[2]);
-        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("  Inertia tensor : %.3f, %.3f, %.3f\n", SI_inertia[0], SI_inertia[1], SI_inertia[2]);
+		printf("  %7.3f, %7.3f, %7.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("  %7.3f, %7.3f, %7.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]);
+		printf("  %7.3f, %7.3f, %7.3f\n", SI_inertia[6], SI_inertia[7], SI_inertia[8]);
-    }
+	}
     delete fdm;
     return 0;
 }