Maik Justus: modifications to add helicopter modeling to YASim.

src/FDM/YASim/Airplane.cpp

@@ -5,8 +5,9 @@
 #include "Glue.hpp"
 #include "RigidBody.hpp"
 #include "Surface.hpp"
+#include "Rotorpart.hpp"
+#include "Rotorblade.hpp"
 #include "Thruster.hpp"
-
 #include "Airplane.hpp"
 
 namespace yasim {
@@ -72,7 +73,7 @@ void Airplane::iterate(float dt)
     // The gear might have moved.  Change their aerodynamics.
     updateGearState();
 
-    _model.iterate();
+    _model.iterate(dt);
 }
 
 void Airplane::consumeFuel(float dt)
@@ -255,6 +256,11 @@ void Airplane::addVStab(Wing* vstab)
     _vstabs.add(vstab);
 }
 
+void Airplane::addRotor(Rotor* rotor)
+{
+    _rotors.add(rotor);
+}
+
 void Airplane::addFuselage(float* front, float* back, float width,
                            float taper, float mid)
 {
@@ -443,6 +449,69 @@ float Airplane::compileWing(Wing* w)
     return wgt;
 }
 
+float Airplane::compileRotor(Rotor* w)
+{
+    /* todo contact points
+    // 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();
+    _model.addRotor(w);
+
+    float wgt = 0;
+    int i;
+    /* Todo: add rotor to model!!!
+       Todo: calc and add mass!!!
+    */
+    for(i=0; i<w->numRotorparts(); i++) {
+        Rotorpart* s = (Rotorpart*)w->getRotorpart(i);
+
+        //float td = s->getTotalDrag();
+        //s->setTotalDrag(td);
+
+        _model.addRotorpart(s);
+
+        
+        float mass = s->getWeight();
+        mass = mass * Math::sqrt(mass);
+        float pos[3];
+        s->getPosition(pos);
+        _model.getBody()->addMass(mass, pos);
+        wgt += mass;
+        
+    }
+    
+    for(i=0; i<w->numRotorblades(); i++) {
+        Rotorblade* s = (Rotorblade*)w->getRotorblade(i);
+
+        //float td = s->getTotalDrag();
+        //s->setTotalDrag(td);
+
+        _model.addRotorblade(s);
+
+        
+        float mass = s->getWeight();
+        mass = mass * Math::sqrt(mass);
+        float pos[3];
+        s->getPosition(pos);
+        _model.getBody()->addMass(mass, pos);
+        wgt += mass;
+        
+    }
+    
+    return wgt;
+}
+
 float Airplane::compileFuselage(Fuselage* f)
 {
     // The front and back are contact points
@@ -576,12 +645,17 @@ void Airplane::compile()
     float aeroWgt = 0;
 
     // The Wing objects
-    aeroWgt += compileWing(_wing);
+    if (_wing)
-    aeroWgt += compileWing(_tail);
+      aeroWgt += compileWing(_wing);
+    if (_tail)
+      aeroWgt += compileWing(_tail);
     int i;
     for(i=0; i<_vstabs.size(); i++) {
         aeroWgt += compileWing((Wing*)_vstabs.get(i)); 
     }
+    for(i=0; i<_rotors.size(); i++) {
+        aeroWgt += compileRotor((Rotor*)_rotors.get(i)); 
+    }
     
     // The fuselage(s)
     for(i=0; i<_fuselages.size(); i++) {
@@ -628,10 +702,15 @@ void Airplane::compile()
 
     // Ground effect
     float gepos[3];
-    float gespan = _wing->getGroundEffect(gepos);
+    float gespan;
+    if(_wing)
+      gespan = _wing->getGroundEffect(gepos);
+    else
+      gespan=0;
     _model.setGroundEffect(gepos, gespan, 0.15f);
 
     solveGear();
+    
     solve();
 
     // Do this after solveGear, because it creates "gear" objects that
@@ -750,7 +829,7 @@ void Airplane::runCruise()
     // Precompute thrust in the model, and calculate aerodynamic forces
     _model.getBody()->recalc();
     _model.getBody()->reset();
-    _model.initIteration();
+    _model.initIteration(.01);//hier parameter egal
     _model.calcForces(&_cruiseState);
 }
 
@@ -793,7 +872,7 @@ void Airplane::runApproach()
     // Precompute thrust in the model, and calculate aerodynamic forces
     _model.getBody()->recalc();
     _model.getBody()->reset();
-    _model.initIteration();
+    _model.initIteration(.01);//hier parameter egal
     _model.calcForces(&_approachState);
 }
 
@@ -801,8 +880,10 @@ void Airplane::applyDragFactor(float factor)
 {
     float applied = Math::pow(factor, SOLVE_TWEAK);
     _dragFactor *= applied;
-    _wing->setDragScale(_wing->getDragScale() * applied);
+    if(_wing)
-    _tail->setDragScale(_tail->getDragScale() * applied);
+      _wing->setDragScale(_wing->getDragScale() * applied);
+    if(_tail)
+      _tail->setDragScale(_tail->getDragScale() * applied);
     int i;
     for(i=0; i<_vstabs.size(); i++) {
 	Wing* w = (Wing*)_vstabs.get(i);
@@ -818,8 +899,10 @@ void Airplane::applyLiftRatio(float factor)
 {
     float applied = Math::pow(factor, SOLVE_TWEAK);
     _liftRatio *= applied;
-    _wing->setLiftRatio(_wing->getLiftRatio() * applied);
+    if(_wing)
-    _tail->setLiftRatio(_tail->getLiftRatio() * applied);
+      _wing->setLiftRatio(_wing->getLiftRatio() * applied);
+    if(_tail)
+      _tail->setLiftRatio(_tail->getLiftRatio() * applied);
     int i;
     for(i=0; i<_vstabs.size(); i++) {
         Wing* w = (Wing*)_vstabs.get(i);
@@ -848,6 +931,8 @@ void Airplane::solve()
     float tmp[3];
     _solutionIterations = 0;
     _failureMsg = 0;
+    if((_wing)&&(_tail))
+    {
     while(1) {
 #if 0
         printf("%d %f %f %f %f %f\n", //DEBUG
@@ -859,9 +944,9 @@ void Airplane::solve()
                _approachElevator.val);
 #endif
 
-	if(_solutionIterations++ > 10000) {
+        if(_solutionIterations++ > 10000) { 
             _failureMsg = "Solution failed to converge after 10000 iterations";
-	    return;
+            return;
         }
 
 	// Run an iteration at cruise, and extract the needed numbers:
@@ -986,5 +1071,19 @@ void Airplane::solve()
 	_failureMsg = "Tail incidence > 10 degrees";
 	return;
     }
+    }
+    else
+    {
+        applyDragFactor(Math::pow(15.7/1000, 1/SOLVE_TWEAK));
+        applyLiftRatio(Math::pow(104, 1/SOLVE_TWEAK));
+        setupState(0,0, &_cruiseState);
+        _model.setState(&_cruiseState);
+        _controls.reset();
+        _model.getBody()->reset();
+
+
+    }
+
+    return;
 }
 }; // namespace yasim

src/FDM/YASim/Airplane.hpp

@@ -4,6 +4,7 @@
 #include "ControlMap.hpp"
 #include "Model.hpp"
 #include "Wing.hpp"
+#include "Rotor.hpp"
 #include "Vector.hpp"
 
 namespace yasim {
@@ -33,6 +34,10 @@ public:
     void setTail(Wing* tail);
     void addVStab(Wing* vstab);
 
+    void addRotor(Rotor* Rotor);
+    int getNumRotors() {return _rotors.size();}
+    Rotor* getRotor(int i) {return (Rotor*)_rotors.get(i);}
+
     void addFuselage(float* front, float* back, float width,
                      float taper=1, float mid=0.5);
     int addTank(float* pos, float cap, float fuelDensity);
@@ -89,6 +94,7 @@ private:
     void solveGear();
     void solve();
     float compileWing(Wing* w);
+    float compileRotor(Rotor* w);
     float compileFuselage(Fuselage* f);
     void compileGear(GearRec* gr);
     void applyDragFactor(float factor);
@@ -119,6 +125,8 @@ private:
     Vector _weights;
     Vector _surfs; // NON-wing Surfaces
 
+    Vector _rotors;
+
     Vector _cruiseControls;
     State _cruiseState;
     float _cruiseP;

src/FDM/YASim/ControlMap.cpp

@@ -4,6 +4,7 @@
 #include "PistonEngine.hpp"
 #include "Gear.hpp"
 #include "Wing.hpp"
+#include "Rotor.hpp"
 #include "Math.hpp"
 #include "Propeller.hpp"
 
@@ -199,6 +200,10 @@ void ControlMap::applyControls(float dt)
 	case FLAP0:    ((Wing*)obj)->setFlap0(lval, rval);         break;
 	case FLAP1:    ((Wing*)obj)->setFlap1(lval, rval);         break;
 	case SPOILER:  ((Wing*)obj)->setSpoiler(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;
+        case ROTORENGINEON: ((Rotor*)obj)->setEngineOn((int)lval); break;
 	case BOOST:
 	    ((Thruster*)obj)->getPistonEngine()->setBoost(lval);
 	    break;
@@ -213,6 +218,9 @@ float ControlMap::rangeMin(int type)
     case FLAP0:    return -1;  // [-1:1]
     case FLAP1:    return -1;
     case STEER:    return -1;
+    case CYCLICELE: return -1;
+    case CYCLICAIL: return -1;
+    case COLLECTIVE: return -1;
     case MAGNETOS: return 0;   // [0:3]
     default:       return 0;   // [0:1]
     }

src/FDM/YASim/ControlMap.hpp

@@ -13,7 +13,8 @@ public:
 		      ADVANCE, REHEAT, PROP,
 		      BRAKE, STEER, EXTEND,
 		      INCIDENCE, FLAP0, FLAP1, SLAT, SPOILER, VECTOR,
-                      BOOST, CASTERING, PROPPITCH };
+                      BOOST, CASTERING, PROPPITCH, 
+                      COLLECTIVE, CYCLICAIL, CYCLICELE, ROTORENGINEON};
 
     enum { OPT_SPLIT  = 0x01,
            OPT_INVERT = 0x02,

src/FDM/YASim/FGFDM.cpp

@@ -10,8 +10,12 @@
 #include "PropEngine.hpp"
 #include "Propeller.hpp"
 #include "PistonEngine.hpp"
+#include "Rotor.hpp"
+#include "Rotorpart.hpp"
+#include "Rotorblade.hpp"
 
 #include "FGFDM.hpp"
+
 namespace yasim {
 
 // Some conversion factors
@@ -28,6 +32,7 @@ static const float INHG2PA = 3386.389;
 static const float K2DEGF = 1.8;
 static const float K2DEGFOFFSET = -459.4;
 static const float CIN2CM = 1.6387064e-5;
+static const float YASIM_PI = 3.14159265358979323846;
 
 // Stubs, so that this can be compiled without the FlightGear
 // binary.  What's the best way to handle this?
@@ -119,12 +124,16 @@ void FGFDM::startElement(const char* name, const XMLAttributes &atts)
 	v[1] = attrf(a, "y");
 	v[2] = attrf(a, "z");
 	_airplane.setPilotPos(v);
+    } else if(eq(name, "rotor")) {
+        _airplane.addRotor(parseRotor(a, name));
     } else if(eq(name, "wing")) {
 	_airplane.setWing(parseWing(a, name));
     } else if(eq(name, "hstab")) {
 	_airplane.setTail(parseWing(a, name));
     } else if(eq(name, "vstab")) {
 	_airplane.addVStab(parseWing(a, name));
+    } else if(eq(name, "mstab")) {
+	_airplane.addVStab(parseWing(a, name));
     } else if(eq(name, "propeller")) {
 	parsePropeller(a);
     } else if(eq(name, "thruster")) {
@@ -226,6 +235,11 @@ void FGFDM::startElement(const char* name, const XMLAttributes &atts)
     } else if(eq(name, "spoiler")) {
 	((Wing*)_currObj)->setSpoiler(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");
@@ -351,6 +365,49 @@ void FGFDM::setOutputProperties()
         fgSetFloat("/consumables/fuel/total-fuel-norm", totalFuel/totalCap);
     }
 
+    for(i=0; i<_airplane.getNumRotors(); i++) {
+        Rotor*r=(Rotor*)_airplane.getRotor(i);
+        int j=0;
+        float f;
+        char b[256];
+        while(j=r->getValueforFGSet(j,b,&f))
+        {
+              if (b[0])
+              {
+                 fgSetFloat(b,f);
+              }
+        }
+        
+        for(j=0; j<r->numRotorparts(); j++) {
+            Rotorpart* s = (Rotorpart*)r->getRotorpart(j);
+            char *b;
+            int k;
+            for (k=0;k<2;k++)
+            {
+              b=s->getAlphaoutput(k);
+              if (b[0])
+              {
+                 fgSetFloat(b,s->getAlpha(k));
+                 //printf("setting [%s]\n",b);
+              }
+            }
+        }
+        for(j=0; j<r->numRotorblades(); j++) {
+            Rotorblade* s = (Rotorblade*)r->getRotorblade(j);
+            char *b;
+            int k;
+            for (k=0;k<2;k++)
+            {
+              b=s->getAlphaoutput(k);
+              if (b[0])
+              {
+                 fgSetFloat(b,s->getAlpha(k));
+              }
+            }
+        }
+     }
+
+
     for(i=0; i<_thrusters.size(); i++) {
 	EngRec* er = (EngRec*)_thrusters.get(i);
         Thruster* t = er->eng;
@@ -425,6 +482,92 @@ Wing* FGFDM::parseWing(XMLAttributes* a, const char* type)
     float effect = attrf(a, "effectiveness", 1);
     w->setDragScale(w->getDragScale()*effect);
 
+    _currObj = w;
+    return w;
+}
+Rotor* FGFDM::parseRotor(XMLAttributes* a, const char* type)
+{
+    Rotor* w = new Rotor();
+
+    float defDihed = 0;
+
+    float pos[3];
+    pos[0] = attrf(a, "x");
+    pos[1] = attrf(a, "y");
+    pos[2] = attrf(a, "z");
+    w->setBase(pos);
+
+    float normal[3];
+    normal[0] = attrf(a, "nx");
+    normal[1] = attrf(a, "ny");
+    normal[2] = attrf(a, "nz");
+    w->setNormal(normal);
+
+    float forward[3];
+    forward[0] = attrf(a, "fx");
+    forward[1] = attrf(a, "fy");
+    forward[2] = attrf(a, "fz");
+    w->setForward(forward);
+
+
+
+    w->setMaxCyclicail(attrf(a, "maxcyclicail", 7.6));
+    w->setMaxCyclicele(attrf(a, "maxcyclicele", 4.94));
+    w->setMinCyclicail(attrf(a, "mincyclicail", -7.6));
+    w->setMinCyclicele(attrf(a, "mincyclicele", -4.94));
+    w->setMaxCollective(attrf(a, "maxcollective", 15.8));
+    w->setMinCollective(attrf(a, "mincollective", -0.2));
+    w->setDiameter(attrf(a, "diameter", 10.2));
+    w->setWeightPerBlade(attrf(a, "weightperblade", 44));
+    w->setNumberOfBlades(attrf(a, "numblades", 4));
+    w->setRelBladeCenter(attrf(a, "relbladecenter", 0.7));
+    w->setDynamic(attrf(a, "dynamic", 0.7));
+    w->setDelta3(attrf(a, "delta3", 0));
+    w->setDelta(attrf(a, "delta", 0));
+    w->setTranslift(attrf(a, "translift", 0.05));
+    w->setC2(attrf(a, "dragfactor", 1));
+    w->setStepspersecond(attrf(a, "stepspersecond", 120));
+    w->setRPM(attrf(a, "rpm", 424));
+    w->setRelLenHinge(attrf(a, "rellenflaphinge", 0.07));
+    w->setAlpha0((attrf(a, "flap0", -5))*YASIM_PI/180);
+    w->setAlphamin((attrf(a, "flapmin", -15))/180*YASIM_PI);
+    w->setAlphamax((attrf(a, "flapmax",  15))*YASIM_PI/180);
+    w->setAlpha0factor(attrf(a, "flap0factor", 1));
+    w->setTeeterdamp(attrf(a,"teeterdamp",.0001));
+    w->setMaxteeterdamp(attrf(a,"maxteeterdamp",1000));
+    w->setRelLenTeeterHinge(attrf(a,"rellenteeterhinge",0.01));
+    void setAlphamin(float f);
+    void setAlphamax(float f);
+    void setAlpha0factor(float f);
+
+    if(attristrue(a,"ccw"))
+       w->setCcw(1); 
+    
+    if(a->hasAttribute("name"))
+       w->setName(a->getValue("name") );
+    if(a->hasAttribute("alphaout0"))
+       w->setAlphaoutput(0,a->getValue("alphaout0") );
+    if(a->hasAttribute("alphaout1"))  w->setAlphaoutput(1,a->getValue("alphaout1") );
+    if(a->hasAttribute("alphaout2"))  w->setAlphaoutput(2,a->getValue("alphaout2") );
+    if(a->hasAttribute("alphaout3"))  w->setAlphaoutput(3,a->getValue("alphaout3") );
+    if(a->hasAttribute("coneout"))  w->setAlphaoutput(4,a->getValue("coneout") );
+    if(a->hasAttribute("yawout"))   w->setAlphaoutput(5,a->getValue("yawout") );
+    if(a->hasAttribute("rollout"))  w->setAlphaoutput(6,a->getValue("rollout") );
+
+    w->setPitchA(attrf(a, "pitch_a", 10));
+    w->setPitchB(attrf(a, "pitch_b", 10));
+    w->setForceAtPitchA(attrf(a, "forceatpitch_a", 3000));
+    w->setPowerAtPitch0(attrf(a, "poweratpitch_0", 300));
+    w->setPowerAtPitchB(attrf(a, "poweratpitch_b", 3000));
+    if(attristrue(a,"notorque"))
+       w->setNotorque(1); 
+    if(attristrue(a,"simblades"))
+       w->setSimBlades(1); 
+
+
+
+
+
     _currObj = w;
     return w;
 }
@@ -531,6 +674,10 @@ int FGFDM::parseOutput(const char* name)
     if(eq(name, "SPOILER"))   return ControlMap::SPOILER;
     if(eq(name, "CASTERING")) return ControlMap::CASTERING;
     if(eq(name, "PROPPITCH")) return ControlMap::PROPPITCH;
+    if(eq(name, "COLLECTIVE")) return ControlMap::COLLECTIVE;
+    if(eq(name, "CYCLICAIL")) return ControlMap::CYCLICAIL;
+    if(eq(name, "CYCLICELE")) return ControlMap::CYCLICELE;
+    if(eq(name, "ROTORENGINEON")) return ControlMap::ROTORENGINEON;
     SG_LOG(SG_FLIGHT,SG_ALERT,"Unrecognized control type '"
            << name << "' in YASim aircraft description.");
     exit(1);
@@ -605,4 +752,11 @@ float FGFDM::attrf(XMLAttributes* atts, char* attr, float def)
     else         return (float)atof(val);    
 }
 
+bool FGFDM::attristrue(XMLAttributes* atts, char* attr)
+{
+    const char* val = atts->getValue(attr);
+    if(val == 0) return false;
+    else         return eq(val,"true");    
+}
+
 }; // namespace yasim

src/FDM/YASim/FGFDM.hpp

@@ -36,6 +36,7 @@ private:
 
     void setOutputProperties();
 
+    Rotor* parseRotor(XMLAttributes* a, const char* name);
     Wing* parseWing(XMLAttributes* a, const char* name);
     int parseAxis(const char* name);
     int parseOutput(const char* name);
@@ -47,6 +48,7 @@ private:
     int attri(XMLAttributes* atts, char* attr, int def); 
     float attrf(XMLAttributes* atts, char* attr);
     float attrf(XMLAttributes* atts, char* attr, float def); 
+    bool attristrue(XMLAttributes* atts, char* attr);
 
     // The core Airplane object we manage.
     Airplane _airplane;

src/FDM/YASim/Makefile.am

@@ -22,6 +22,9 @@ SHARED_SOURCES = \
         PropEngine.cpp PropEngine.hpp \
         Propeller.cpp Propeller.hpp \
         RigidBody.cpp RigidBody.hpp \
+        Rotor.cpp Rotor.hpp \
+        Rotorblade.cpp Rotorblade.hpp \
+        Rotorpart.cpp Rotorpart.hpp \
         SimpleJet.cpp SimpleJet.hpp \
         Surface.cpp Surface.hpp \
         Thruster.cpp Thruster.hpp \

src/FDM/YASim/Math.cpp

@@ -19,12 +19,26 @@ float Math::sqrt(float f)
 {
     return (float)::sqrt(f);
 }
+float Math::sqr(float f)
+{
+    return f*f;
+}
 
 float Math::ceil(float f)
 {
     return (float)::ceil(f);
 }
 
+float Math::acos(float f)
+{
+    return (float)::acos(f);
+}
+
+float Math::asin(float f)
+{
+    return (float)::asin(f);
+}
+
 float Math::cos(float f)
 {
     return (float)::cos(f);
@@ -40,6 +54,11 @@ float Math::tan(float f)
     return (float)::tan(f);
 }
 
+float Math::atan(float f)
+{
+    return (float)::atan(f);
+}
+
 float Math::atan2(float y, float x)
 {
     return (float)::atan2(y, x);
@@ -126,7 +145,12 @@ float Math::mag3(float* v)
 
 void Math::unit3(float* v, float* out)
 {
-    float imag = 1/mag3(v);
+    float mag=mag3(v);
+    float imag;
+    if (mag!=0)
+      imag= 1/mag;
+    else
+      imag=1;
     mul3(imag, v, out);
 }
 

src/FDM/YASim/Math.hpp

@@ -12,11 +12,15 @@ public:
     // Simple wrappers around library routines
     static float abs(float f);
     static float sqrt(float f);
+    static float sqr(float f);
     static float ceil(float f);
     static float sin(float f);
     static float cos(float f);
     static float tan(float f);
+    static float atan(float f);
     static float atan2(float y, float x);
+    static float asin(float f);
+    static float acos(float f);
 
     // Takes two args and runs afoul of the Koenig rules.
     static float pow(double base, double exp);

src/FDM/YASim/Model.cpp

@@ -9,6 +9,9 @@
 #include "PistonEngine.hpp"
 #include "Gear.hpp"
 #include "Surface.hpp"
+#include "Rotor.hpp"
+#include "Rotorpart.hpp"
+#include "Rotorblade.hpp"
 #include "Glue.hpp"
 
 #include "Model.hpp"
@@ -69,7 +72,7 @@ void Model::getThrust(float* out)
     }
 }
 
-void Model::initIteration()
+void Model::initIteration(float dt)
 {
     // Precompute torque and angular momentum for the thrusters
     int i;
@@ -93,11 +96,31 @@ void Model::initIteration()
 	t->getGyro(v);
 	Math::add3(v, _gyro, _gyro);
     }
+
+
+    
+
+    float lrot[3];
+    Math::vmul33(_s->orient, _s->rot, lrot);
+    Math::mul3(dt,lrot,lrot);
+    for(i=0; i<_rotors.size(); i++) {
+        Rotor* r = (Rotor*)_rotors.get(i);
+        r->inititeration(dt);
+    }
+    for(i=0; i<_rotorparts.size(); i++) {
+        Rotorpart* rp = (Rotorpart*)_rotorparts.get(i);
+        rp->inititeration(dt,lrot);
+    }
+    for(i=0; i<_rotorblades.size(); i++) {
+        Rotorblade* rp = (Rotorblade*)_rotorblades.get(i);
+        rp->inititeration(dt,lrot);
+    }
+    
 }
 
-void Model::iterate()
+void Model::iterate(float dt)
 {
-    initIteration();
+    initIteration(dt);
     _body.recalc(); // FIXME: amortize this, somehow
     _integrator.calcNewInterval();
 }
@@ -122,6 +145,12 @@ State* Model::getState()
     return _s;
 }
 
+void Model::resetState()
+{
+    //_s->resetState();
+}
+
+
 void Model::setState(State* s)
 {
     _integrator.setState(s);
@@ -143,6 +172,19 @@ Surface* Model::getSurface(int handle)
     return (Surface*)_surfaces.get(handle);
 }
 
+Rotorpart* Model::getRotorpart(int handle)
+{
+    return (Rotorpart*)_rotorparts.get(handle);
+}
+Rotorblade* Model::getRotorblade(int handle)
+{
+    return (Rotorblade*)_rotorblades.get(handle);
+}
+Rotor* Model::getRotor(int handle)
+{
+    return (Rotor*)_rotors.get(handle);
+}
+
 int Model::addThruster(Thruster* t)
 {
     return _thrusters.add(t);
@@ -168,6 +210,19 @@ int Model::addSurface(Surface* surf)
     return _surfaces.add(surf);
 }
 
+int Model::addRotorpart(Rotorpart* rpart)
+{
+    return _rotorparts.add(rpart);
+}
+int Model::addRotorblade(Rotorblade* rblade)
+{
+    return _rotorblades.add(rblade);
+}
+int Model::addRotor(Rotor* r)
+{
+    return _rotors.add(r);
+}
+
 int Model::addGear(Gear* gear)
 {
     return _gears.add(gear);
@@ -210,6 +265,7 @@ void Model::calcForces(State* s)
     // velocity), and are therefore constant across the four calls to
     // calcForces.  They get computed before we begin the integration
     // step.
+    //printf("f");
     _body.setGyro(_gyro);
     _body.addTorque(_torque);
     int i;
@@ -243,9 +299,53 @@ void Model::calcForces(State* s)
 	float force[3], torque[3];
 	sf->calcForce(vs, _rho, force, torque);
 	Math::add3(faero, force, faero);
+
+        
+
 	_body.addForce(pos, force);
 	_body.addTorque(torque);
     }
+    for(i=0; i<_rotorparts.size(); i++) {
+        Rotorpart* sf = (Rotorpart*)_rotorparts.get(i);
+
+	// Vsurf = wind - velocity + (rot cross (cg - pos))
+	float vs[3], pos[3];
+	sf->getPosition(pos);
+        localWind(pos, s, vs);
+
+	float force[3], torque[3];
+	sf->calcForce(vs, _rho, force, torque);
+        //Math::add3(faero, force, faero);
+
+        sf->getPositionForceAttac(pos);
+
+	_body.addForce(pos, force);
+	_body.addTorque(torque);
+    }
+    for(i=0; i<_rotorblades.size(); i++) {
+        Rotorblade* sf = (Rotorblade*)_rotorblades.get(i);
+
+	// Vsurf = wind - velocity + (rot cross (cg - pos))
+	float vs[3], pos[3];
+	sf->getPosition(pos);
+        localWind(pos, s, vs);
+
+	float force[3], torque[3];
+	sf->calcForce(vs, _rho, force, torque);
+        //Math::add3(faero, force, faero);
+
+        sf->getPositionForceAttac(pos);
+
+	_body.addForce(pos, force);
+	_body.addTorque(torque);
+    }
+    /*
+    {
+      float cg[3];
+      _body.getCG(cg);
+      //printf("cg: %5.3lf %5.3lf %5.3lf ",cg[0],cg[1],cg[2]);
+    }
+    */
 
     // Get a ground plane in local coordinates.  The first three
     // elements are the normal vector, the final one is the distance

src/FDM/YASim/Model.hpp

@@ -12,6 +12,9 @@ namespace yasim {
 class Integrator;
 class Thruster;
 class Surface;
+class Rotorpart;
+class Rotorblade;
+class Rotor;
 class Gear;
 
 class Model : public BodyEnvironment {
@@ -24,24 +27,32 @@ public:
 
     State* getState();
     void setState(State* s);
+
+    void resetState();
     bool isCrashed();
     void setCrashed(bool crashed);
     float getAGL();
 
-    void iterate();
+    void iterate(float dt);
 
     // Externally-managed subcomponents
     int addThruster(Thruster* t);
     int addSurface(Surface* surf);
+    int addRotorpart(Rotorpart* rpart);
+    int addRotorblade(Rotorblade* rblade);
+    int addRotor(Rotor* rotor);
     int addGear(Gear* gear);
     Surface* getSurface(int handle);
+    Rotorpart* getRotorpart(int handle);
+    Rotorblade* getRotorblade(int handle);
+    Rotor* getRotor(int handle);
     Gear* getGear(int handle);
 
     // Semi-private methods for use by the Airplane solver.
     int numThrusters();
     Thruster* getThruster(int handle);
     void setThruster(int handle, Thruster* t);
-    void initIteration();
+    void initIteration(float dt);
     void getThrust(float* out);
 
     //
@@ -67,6 +78,9 @@ private:
 
     Vector _thrusters;
     Vector _surfaces;
+    Vector _rotorparts;
+    Vector _rotorblades;
+    Vector _rotors;
     Vector _gears;
 
     float _groundEffectSpan;

src/FDM/YASim/YASim.cxx

@@ -23,8 +23,9 @@
 
 using namespace yasim;
 
-static const float RAD2DEG = 180/3.14159265358979323846;
+static const float YASIM_PI = 3.14159265358979323846;
-static const float PI2 = 3.14159265358979323846*2;
+static const float RAD2DEG = 180/YASIM_PI;
+static const float PI2 = YASIM_PI*2;
 static const float RAD2RPM = 9.54929658551;
 static const float M2FT = 3.2808399;
 static const float FT2M = 0.3048;
@@ -127,7 +128,6 @@ void YASim::init()
 
     // Superclass hook
     common_init();
-
     m->setCrashed(false);
 
     // Figure out the initial speed type