Giant helicopter code update from Maik Justus.

src/FDM/YASim/Airplane.cpp

@@ -6,7 +6,6 @@
 #include "RigidBody.hpp"
 #include "Surface.hpp"
 #include "Rotorpart.hpp"
-#include "Rotorblade.hpp"
 #include "Thruster.hpp"
 #include "Airplane.hpp"
 
@@ -87,8 +86,6 @@ Airplane::~Airplane()
         delete (Wing*)_vstabs.get(i);
     for(i=0; i<_weights.size(); i++)
         delete (WeightRec*)_weights.get(i);
-    for(i=0; i<_rotors.size(); i++)
-        delete (Rotor*)_rotors.get(i);
 }
 
 void Airplane::iterate(float dt)
@@ -168,6 +165,11 @@ Launchbar* Airplane::getLaunchbar()
     return _model.getLaunchbar();
 }
 
+Rotorgear* Airplane::getRotorgear()
+{
+    return _model.getRotorgear();
+}
+
 void Airplane::updateGearState()
 {
     for(int i=0; i<_gears.size(); i++) {
@@ -276,11 +278,6 @@ 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)
 {
@@ -480,41 +477,9 @@ float Airplane::compileWing(Wing* w)
     return wgt;
 }
 
-float Airplane::compileRotor(Rotor* r)
+float Airplane::compileRotorgear()
 {
-    // Todo: add rotor to model!!!
+    return getRotorgear()->compile(_model.getBody());
-    // Todo: calc and add mass!!!
-    r->compile();
-    _model.addRotor(r);
-
-    float wgt = 0;
-    int i;
-    for(i=0; i<r->numRotorparts(); i++) {
-        Rotorpart* s = (Rotorpart*)r->getRotorpart(i);
-
-        _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<r->numRotorblades(); i++) {
-        Rotorblade* b = (Rotorblade*)r->getRotorblade(i);
-
-        _model.addRotorblade(b);
-        
-        float mass = b->getWeight();
-        mass = mass * Math::sqrt(mass);
-        float pos[3];
-        b->getPosition(pos);
-        _model.getBody()->addMass(mass, pos);
-        wgt += mass;
-    }
-    return wgt;
 }
 
 float Airplane::compileFuselage(Fuselage* f)
@@ -654,9 +619,10 @@ void Airplane::compile()
     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 rotor(s)
-    
+    aeroWgt += compileRotorgear(); 
+
     // The fuselage(s)
     for(i=0; i<_fuselages.size(); i++)
         aeroWgt += compileFuselage((Fuselage*)_fuselages.get(i));
@@ -1077,13 +1043,29 @@ void Airplane::solveHelicopter()
 {
     _solutionIterations = 0;
     _failureMsg = 0;
-
+    if (getRotorgear()!=0)
-    applyDragFactor(Math::pow(15.7/1000, 1/SOLVE_TWEAK));
+    {
-    applyLiftRatio(Math::pow(104, 1/SOLVE_TWEAK));
+        Rotorgear* rg = getRotorgear();
+        applyDragFactor(Math::pow(rg->getYasimDragFactor()/1000,
+            1/SOLVE_TWEAK));
+        applyLiftRatio(Math::pow(rg->getYasimLiftFactor(),
+            1/SOLVE_TWEAK));
+    }
+    else
+    //huh, no wing and no rotor? (_rotorgear is constructed, 
+    //if a rotor is defined
+    {
+        applyDragFactor(Math::pow(15.7/1000, 1/SOLVE_TWEAK));
+        applyLiftRatio(Math::pow(104, 1/SOLVE_TWEAK));
+    }
     setupState(0,0, &_cruiseState);
     _model.setState(&_cruiseState);
+    setupWeights(true);
     _controls.reset();
     _model.getBody()->reset();
+    _model.setAir(_cruiseP, _cruiseT,
+                  Atmosphere::calcStdDensity(_cruiseP, _cruiseT));
+    
 }
 
 }; // namespace yasim

src/FDM/YASim/Airplane.hpp

@@ -36,10 +36,6 @@ 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);
@@ -64,6 +60,7 @@ public:
     int numGear();
     Gear* getGear(int g);
     Hook* getHook();
+    Rotorgear* getRotorgear();
     Launchbar* getLaunchbar();
 
     int numThrusters() { return _thrusters.size(); }
@@ -110,7 +107,7 @@ private:
     void solve();
     void solveHelicopter();
     float compileWing(Wing* w);
-    float compileRotor(Rotor* w);
+    float compileRotorgear();
     float compileFuselage(Fuselage* f);
     void compileGear(GearRec* gr);
     void applyDragFactor(float factor);
@@ -142,8 +139,6 @@ private:
     Vector _weights;
     Vector _surfs; // NON-wing Surfaces
 
-    Vector _rotors;
-
     Vector _solveWeights;
 
     Vector _cruiseControls;

src/FDM/YASim/ControlMap.cpp

@@ -210,7 +210,8 @@ void ControlMap::applyControls(float dt)
         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 ROTORBRAKE:    ((Rotorgear*)obj)->setRotorBrake(lval); break;
+        case ROTORENGINEON: ((Rotorgear*)obj)->setEngineOn((int)lval); break;
 	case REVERSE_THRUST: ((Jet*)obj)->setReverse(lval != 0);   break;
 	case BOOST:
 	    ((PistonEngine*)((Thruster*)obj)->getEngine())->setBoost(lval);

src/FDM/YASim/ControlMap.hpp

@@ -15,6 +15,7 @@ public:
 		      INCIDENCE, FLAP0, FLAP1, SLAT, SPOILER, VECTOR,
                       BOOST, CASTERING, PROPPITCH, PROPFEATHER,
                       COLLECTIVE, CYCLICAIL, CYCLICELE, ROTORENGINEON,
+                      ROTORBRAKE,
                       REVERSE_THRUST, WASTEGATE };
 
     enum { OPT_SPLIT  = 0x01,

src/FDM/YASim/FGFDM.cpp

@@ -16,7 +16,6 @@
 #include "TurbineEngine.hpp"
 #include "Rotor.hpp"
 #include "Rotorpart.hpp"
-#include "Rotorblade.hpp"
 
 #include "FGFDM.hpp"
 
@@ -176,7 +175,19 @@ void FGFDM::startElement(const char* name, const XMLAttributes &atts)
 	v[2] = attrf(a, "z");
 	_airplane.setPilotPos(v);
     } else if(eq(name, "rotor")) {
-        _airplane.addRotor(parseRotor(a, name));
+        _airplane.getModel()->getRotorgear()->addRotor(parseRotor(a, name));
+    } else if(eq(name, "rotorgear")) {
+        Rotorgear* r = _airplane.getModel()->getRotorgear();
+	_currObj = r;
+        #define p(x) if (a->hasAttribute(#x)) r->setParameter((char *)#x,attrf(a,#x) );
+        p(max_power_engine)
+        p(engine_prop_factor)
+        p(yasimdragfactor)
+        p(yasimliftfactor)
+        p(max_power_rotor_brake)
+        p(engine_accell_limit)
+        #undef p
+        r->setInUse();
     } else if(eq(name, "wing")) {
 	_airplane.setWing(parseWing(a, name));
     } else if(eq(name, "hstab")) {
@@ -475,8 +486,8 @@ void FGFDM::setOutputProperties(float dt)
         p->prop->setFloatValue(val);
     }
 
-    for(i=0; i<_airplane.getNumRotors(); i++) {
+    for(i=0; i<_airplane.getRotorgear()->getNumRotors(); i++) {
-        Rotor*r=(Rotor*)_airplane.getRotor(i);
+        Rotor*r=(Rotor*)_airplane.getRotorgear()->getRotor(i);
         int j = 0;
         float f;
         char b[256];
@@ -492,15 +503,6 @@ void FGFDM::setOutputProperties(float dt)
                 if(b[0]) fgSetFloat(b, s->getAlpha(k));
             }
         }
-        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));
-            }
-        }
     }
 
     float fuelDensity = _airplane.getFuelDensity(0); // HACK
@@ -677,9 +679,34 @@ Rotor* FGFDM::parseRotor(XMLAttributes* a, const char* type)
     w->setPowerAtPitchB(attrf(a, "poweratpitch_b", 3000));
     if(attrb(a,"notorque"))
        w->setNotorque(1); 
-    if(attrb(a,"simblades"))
-       w->setSimBlades(1); 
 
+#define p(x) if (a->hasAttribute(#x)) w->setParameter((char *)#x,attrf(a,#x) );
+    p(translift_ve)
+    p(translift_maxfactor)
+    p(ground_effect_constant)
+    p(vortex_state_lift_factor)
+    p(vortex_state_c1)
+    p(vortex_state_c2)
+    p(vortex_state_c3)
+    p(vortex_state_e1)
+    p(vortex_state_e2)
+    p(twist)
+    p(number_of_segments)
+    p(rel_len_where_incidence_is_measured)
+    p(chord)
+    p(taper)
+    p(airfoil_incidence_no_lift)
+    p(rel_len_blade_start)
+    p(incidence_stall_zero_speed)
+    p(incidence_stall_half_sonic_speed)
+    p(lift_factor_stall)
+    p(stall_change_over)
+    p(drag_factor_stall)
+    p(airfoil_lift_coefficient)
+    p(airfoil_drag_coefficient0)
+    p(airfoil_drag_coefficient1)
+
+#undef p
     _currObj = w;
     return w;
 }
@@ -853,7 +880,8 @@ int FGFDM::parseOutput(const char* name)
     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;
+    if(eq(name, "ROTORGEARENGINEON")) return ControlMap::ROTORENGINEON;
+    if(eq(name, "ROTORBRAKE")) return ControlMap::ROTORBRAKE;
     if(eq(name, "REVERSE_THRUST")) return ControlMap::REVERSE_THRUST;
     if(eq(name, "WASTEGATE")) return ControlMap::WASTEGATE;
     SG_LOG(SG_FLIGHT,SG_ALERT,"Unrecognized control type '"

src/FDM/YASim/Model.cpp

@@ -11,7 +11,6 @@
 #include "Surface.hpp"
 #include "Rotor.hpp"
 #include "Rotorpart.hpp"
-#include "Rotorblade.hpp"
 #include "Glue.hpp"
 #include "Ground.hpp"
 
@@ -131,31 +130,22 @@ void Model::initIteration()
     
 }
 
-// FIXME: This method looks to me like it's doing *integration*, not
+// This function initializes some variables for the rotor calculation
-// initialization.  Integration code should ideally go into
+// Furthermore it integrates in "void Rotorpart::inititeration
-// calcForces.  Only very "unstiff" problems can be solved well like
+// (float dt,float *rot)" the "rotor orientation" by omega*dt for the 
-// this (see the engine code for an example); I don't know if rotor
+// 3D-visualization of the heli only. and it compensates the rotation 
-// dynamics qualify or not.
+// of the fuselage. The rotor does not follow the rotation of the fuselage.
-// -Andy
+// Therefore its rotation must be subtracted from the orientation of the 
+// rotor.
+// Maik
 void Model::initRotorIteration()
 {
-    int i;
     float dt = _integrator.getInterval();
     float lrot[3];
+    if (!_rotorgear.isInUse()) return;
     Math::vmul33(_s->orient, _s->rot, lrot);
     Math::mul3(dt,lrot,lrot);
-    for(i=0; i<_rotors.size(); i++) {
+    _rotorgear.initRotorIteration(lrot,dt);
-        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()
@@ -207,17 +197,9 @@ Surface* Model::getSurface(int handle)
     return (Surface*)_surfaces.get(handle);
 }
 
-Rotorpart* Model::getRotorpart(int handle)
+Rotorgear* Model::getRotorgear(void)
 {
-    return (Rotorpart*)_rotorparts.get(handle);
+    return &_rotorgear;
-}
-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)
@@ -255,19 +237,6 @@ 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);
@@ -341,6 +310,24 @@ void Model::updateGround(State* s)
         _ground_cb->getGroundPlane(pt, global_ground, global_vel);
         g->setGlobalGround(global_ground, global_vel);
     }
+    for(i=0; i<_rotorgear.getRotors()->size(); i++) {
+        Rotor* r = (Rotor*)_rotorgear.getRotors()->get(i);
+
+        // Get the point of the rotor center
+        float pos[3];
+        r->getPosition(pos);
+
+        // Transform the local coordinates to
+        // global coordinates.
+        double pt[3];
+        s->posLocalToGlobal(pos, pt);
+
+        // Ask for the ground plane in the global coordinate system
+        double global_ground[4];
+        float global_vel[3];
+        _ground_cb->getGroundPlane(pt, global_ground, global_vel);
+        r->setGlobalGround(global_ground, global_vel);
+    }
 
     // The arrester hook
     if(_hook) {
@@ -370,7 +357,7 @@ void Model::calcForces(State* s)
     // step.
     _body.setGyro(_gyro);
     _body.addTorque(_torque);
-    int i;
+    int i,j;
     for(i=0; i<_thrusters.size(); i++) {
 	Thruster* t = (Thruster*)_thrusters.get(i);
 	float thrust[3], pos[3];
@@ -413,51 +400,55 @@ void Model::calcForces(State* s)
 	_body.addForce(pos, force);
 	_body.addTorque(torque);
     }
-    for(i=0; i<_rotorparts.size(); i++) {
+    for (j=0; j<_rotorgear.getRotors()->size();j++)
-        Rotorpart* sf = (Rotorpart*)_rotorparts.get(i);
+    {
-
+        Rotor* r = (Rotor *)_rotorgear.getRotors()->get(j);
-	// Vsurf = wind - velocity + (rot cross (cg - pos))
+        float vs[3], pos[3];
-	float vs[3], pos[3];
+        r->getPosition(pos);
-	sf->getPosition(pos);
         localWind(pos, s, vs, alt);
+        r->calcLiftFactor(vs, _rho,s);
+        float tq=0; 
+        // total torque of rotor (scalar) for calculating new rotor rpm
 
-	float force[3], torque[3];
+        for(i=0; i<r->_rotorparts.size(); i++) {
-	sf->calcForce(vs, _rho, force, torque);
+            float torque_scalar=0;
-        //Math::add3(faero, force, faero);
+            Rotorpart* rp = (Rotorpart*)r->_rotorparts.get(i);
 
-        sf->getPositionForceAttac(pos);
+            // Vsurf = wind - velocity + (rot cross (cg - pos))
+            float vs[3], pos[3];
+            rp->getPosition(pos);
+            localWind(pos, s, vs, alt,true);
 
-	_body.addForce(pos, force);
+            float force[3], torque[3];
-	_body.addTorque(torque);
+            rp->calcForce(vs, _rho, force, torque, &torque_scalar);
+            tq+=torque_scalar;
+            rp->getPositionForceAttac(pos);
+
+            _body.addForce(pos, force);
+            _body.addTorque(torque);
+        }
+        r->setTorque(tq);
     }
-    for(i=0; i<_rotorblades.size(); i++) {
+    if (_rotorgear.isInUse())
-        Rotorblade* sf = (Rotorblade*)_rotorblades.get(i);
+    {
-
+        float torque[3];
-	// Vsurf = wind - velocity + (rot cross (cg - pos))
+        _rotorgear.calcForces(torque);
-	float vs[3], pos[3];
+        _body.addTorque(torque);
-	sf->getPosition(pos);
-        localWind(pos, s, vs, alt);
-
-	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);
     }
 
     // Account for ground effect by multiplying the vertical force
     // component by an amount linear with the fraction of the wingspan
     // above the ground.
-    float dist = ground[3] - Math::dot3(ground, _wingCenter);
+    if ((_groundEffectSpan != 0) && (_groundEffect != 0 ))
-    if(dist > 0 && dist < _groundEffectSpan) {
+    {
-	float fz = Math::dot3(faero, ground);
+        float dist = ground[3] - Math::dot3(ground, _wingCenter);
-        fz *= (_groundEffectSpan - dist) / _groundEffectSpan;
+        if(dist > 0 && dist < _groundEffectSpan) {
-        fz *= _groundEffect;
+        float fz = Math::dot3(faero, ground);
-	Math::mul3(fz, ground, faero);
+            fz *= (_groundEffectSpan - dist) / _groundEffectSpan;
-	_body.addForce(faero);
+            fz *= _groundEffect;
+        Math::mul3(fz, ground, faero);
+        _body.addForce(faero);
+        }
     }
     
     // Convert the velocity and rotation vectors to local coordinates
@@ -528,7 +519,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)
+void Model::localWind(float* pos, State* s, float* out, float alt, bool is_rotor)
 {
     float tmp[3], lwind[3], lrot[3], lv[3];
 
@@ -556,6 +547,15 @@ void Model::localWind(float* pos, State* s, float* out, float alt)
     Math::mul3(-1, out, out);      //  (negated)
     Math::add3(lwind, out, out);    //  + wind
     Math::sub3(out, lv, out);       //  - velocity
+
+    //add the downwash of the rotors if it is not self a rotor
+    if (_rotorgear.isInUse()&&!is_rotor)
+    {
+        _rotorgear.getDownWash(pos,lv,tmp);
+        Math::add3(out,tmp, out);    //  + downwash
+    }
+
+
 }
 
 }; // namespace yasim

src/FDM/YASim/Model.hpp

@@ -6,6 +6,7 @@
 #include "BodyEnvironment.hpp"
 #include "Vector.hpp"
 #include "Turbulence.hpp"
+#include "Rotor.hpp"
 
 namespace yasim {
 
@@ -14,8 +15,6 @@ class Integrator;
 class Thruster;
 class Surface;
 class Rotorpart;
-class Rotorblade;
-class Rotor;
 class Gear;
 class Ground;
 class Hook;
@@ -43,16 +42,11 @@ public:
     // 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);
     void addHook(Hook* hook);
     void addLaunchbar(Launchbar* launchbar);
     Surface* getSurface(int handle);
-    Rotorpart* getRotorpart(int handle);
+    Rotorgear* getRotorgear(void);
-    Rotorblade* getRotorblade(int handle);
-    Rotor* getRotor(int handle);
     Gear* getGear(int handle);
     Hook* getHook(void);
     Launchbar* getLaunchbar(void);
@@ -84,7 +78,8 @@ 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);
+    void localWind(float* pos, State* s, float* out, float alt,
+        bool is_rotor = false);
 
     Integrator _integrator;
     RigidBody _body;
@@ -93,9 +88,7 @@ private:
 
     Vector _thrusters;
     Vector _surfaces;
-    Vector _rotorparts;
+    Rotorgear _rotorgear;
-    Vector _rotorblades;
-    Vector _rotors;
     Vector _gears;
     Hook* _hook;
     Launchbar* _launchbar;

src/FDM/YASim/RigidBody.cpp

@@ -172,8 +172,9 @@ void RigidBody::getAccel(float* pos, float* accelOut)
     float a[3];
     float rate = Math::mag3(_spin);
     Math::set3(_spin, a);
-    Math::mul3(1/rate, a, a);
+    if (rate !=0 )
-
+        Math::mul3(1/rate, a, a);
+    //an else branch is not neccesary. a, which is a=(0,0,0) in the else case, is only used in a dot product
     float v[3];
     Math::sub3(_cg, pos, v);             // v = cg - pos
     Math::mul3(Math::dot3(v, a), a, a);  // a = a * (v dot a)

src/FDM/YASim/Rotor.hpp

@@ -3,22 +3,39 @@
 
 #include "Vector.hpp"
 #include "Rotorpart.hpp"
-#include "Rotorblade.hpp"
+#include "Integrator.hpp"
+#include "RigidBody.hpp"
+#include "BodyEnvironment.hpp"
+
 namespace yasim {
 
 class Surface;
 class Rotorpart;
+const float rho_null=1.184f; //25DegC, 101325Pa
 
 class Rotor {
+private:
+    float _torque;
+    float _omega,_omegan,_omegarel,_ddt_omega,_omegarelneu;
+    float _chord;
+    float _taper;
+    float _airfoil_incidence_no_lift;
+    float _collective;
+    float _airfoil_lift_coefficient;
+    float _airfoil_drag_coefficient0;
+    float _airfoil_drag_coefficient1;
+    int _ccw;
+
 public:
     Rotor();
     ~Rotor();
 
-
     // Rotor geometry:
-    void setNormal(float* normal);    //the normal vector (direction of rotormast, pointing up)
+    void setNormal(float* normal);
-    void setForward(float* forward);    //the normal vector pointing forward (for ele and ail)
+    //the normal vector (direction of rotormast, pointing up)
-    //void setMaxPitchForce(float force);
+
+    void setForward(float* forward);
+    //the normal vector pointing forward (for ele and ail)
     void setForceAtPitchA(float force);
     void setPowerAtPitch0(float value);
     void setPowerAtPitchB(float value);
@@ -44,32 +61,28 @@ public:
     void setRPM(float value);
     void setRelLenHinge(float value);
     void setBase(float* base);        // in local coordinates
+    void getPosition(float* out);
     void setCyclicail(float lval,float rval);
     void setCyclicele(float lval,float rval);
     void setCollective(float lval);
     void setAlphaoutput(int i, const char *text);
     void setCcw(int ccw);
-    void setSimBlades(int value);
+    int getCcw() {return _ccw;};
-    void setEngineOn(int value);
+    void setParameter(char *parametername, float value);
-
+    void setGlobalGround(double* global_ground, float* global_vel);
+    float getTorqueOfInertia();
     int getValueforFGSet(int j,char *b,float *f);
     void setName(const char *text);
-    void inititeration(float dt);
+    void inititeration(float dt,float omegarel,float ddt_omegarel,float *rot);
     void compile();
-
     void getTip(float* tip);
-
+    void calcLiftFactor(float* v, float rho, State *s);
-
+    void getDownWash(float *pos, float * v_heli, float *downwash);
-
-    // Ground effect information, stil missing
     float getGroundEffect(float* posOut);
-    
+
     // Query the list of Rotorpart objects
     int numRotorparts();
     Rotorpart* getRotorpart(int n);
-    // Query the list of Rotorblade objects
-    int numRotorblades();
-    Rotorblade* getRotorblade(int n);
     void setAlpha0(float f);
     void setAlphamin(float f);
     void setAlphamax(float f);
@@ -77,28 +90,33 @@ public:
     void setMaxteeterdamp(float f);
     void setRelLenTeeterHinge(float value);
     void setAlpha0factor(float f);
+    void setTorque(float f);
+    void addTorque(float f);
+    float getTorque() {return _torque;}
+    float getLiftFactor();
+    float getLiftCoef(float incidence,float speed);
+    float getDragCoef(float incidence,float speed);
+    float getOmegaRel() {return _omegarel;}
+    float getOmegaRelNeu() {return _omegarelneu;}
+    void setOmegaRelNeu(float orn) {_omegarelneu=orn;}
+    float getOmegan() {return _omegan;}
+    float getTaper() { return _taper;}
+    float getChord() { return _chord;}
+    float getOverallStall() 
+        {if (_stall_v2sum !=0 ) return _stall_sum/_stall_v2sum; else return 0;}
+    Vector _rotorparts;
 
 private:
     void strncpy(char *dest,const char *src,int maxlen);
     void interp(float* v1, float* v2, float frac, float* out);
+    float calcStall(float incidence,float speed);
     Rotorpart* newRotorpart(float* pos, float *posforceattac, float *normal,
-         float* speed,float *dirzentforce, float zentforce,float maxpitchforce,float maxpitch, float minpitch, float mincyclic,float maxcyclic,
+        float* speed,float *dirzentforce, float zentforce,float maxpitchforce,
-         float delta3,float mass,float translift,float rellenhinge,float len);
+        float maxpitch, float minpitch, float mincyclic,float maxcyclic,
-
+        float delta3,float mass,float translift,float rellenhinge,float len);
-    Rotorblade* newRotorblade(
-         float* pos,  float *normal,float *front, float *right,
-         float lforceattac,float relenhinge,
-         float *dirzentforce, float zentforce,float maxpitchforce,float maxpitch, 
-         float delta3,float mass,float translift,float phi,float omega,float len,float speed);
-    
-
-
-    Vector _rotorparts;
-    Vector _rotorblades;
-
     float _base[3];
-
     float _normal[3];//the normal vector (direction of rotormast, pointing up)
+    float _normal_with_yaw_roll[3];//the normal vector (perpendicular to rotordisc)
     float _forward[3];
     float _diameter;
     int _number_of_blades;
@@ -106,7 +124,6 @@ private:
     float _rel_blade_center;
     float _min_pitch;
     float _max_pitch;
-    float _force_at_max_pitch;
     float _force_at_pitch_a;
     float _pitch_a;
     float _power_at_pitch_0;
@@ -128,18 +145,79 @@ private:
     float _stepspersecond;
     char _alphaoutput[8][256];
     char _name[256];
-    int _ccw;
     int _engineon;
-    float _omega,_omegan,_omegarel;
     float _alphamin,_alphamax,_alpha0,_alpha0factor;
     float _teeterdamp,_maxteeterdamp;
     float _rellenteeterhinge;
+    float _translift_ve;
+    float _translift_maxfactor;
+    float _ground_effect_constant;
+    float _vortex_state_lift_factor;
+    float _vortex_state_c1;
+    float _vortex_state_c2;
+    float _vortex_state_c3;
+    float _vortex_state_e1;
+    float _vortex_state_e2;
+    float _vortex_state_e3;
+    float _lift_factor,_f_ge,_f_vs,_f_tl;
+    float _vortex_state;
+    double _global_ground[4];
+    float _liftcoef;
+    float _dragcoef0;
+    float _dragcoef1;
+    float _twist; //outer incidence = inner inner incidence + _twist
+    int _number_of_segments;
+    float _rel_len_where_incidence_is_measured;
+    float _torque_of_inertia;
+    float _rel_len_blade_start;
+    float _incidence_stall_zero_speed;
+    float _incidence_stall_half_sonic_speed;
+    float _lift_factor_stall;
+    float _stall_change_over;
+    float _drag_factor_stall;
+    float _stall_sum;
+    float _stall_v2sum;
+    float _yaw;
+    float _roll;
+};
 
+class Rotorgear {
+private:
+    int _in_use;
+    int _engineon;
+    float _max_power_engine;
+    float _engine_prop_factor;
+    float _yasimdragfactor;
+    float _yasimliftfactor;
+    float _rotorbrake;
+    float _max_power_rotor_brake;
+    float _ddt_omegarel;
+    float _engine_accell_limit;
+    Vector _rotors;
 
-
+public:
-
+    Rotorgear();
-
+    ~Rotorgear();
-
+    int isInUse() {return _in_use;}
+    void setInUse() {_in_use = 1;}
+    float compile(RigidBody* body);
+    void addRotor(Rotor* rotor);
+    int getNumRotors() {return _rotors.size();}
+    Rotor* getRotor(int i) {return (Rotor*)_rotors.get(i);}
+    void calcForces(float* torqueOut);
+    void setParameter(char *parametername, float value);
+    void setEngineOn(int value);
+    int getEngineon();
+    void setRotorBrake(float lval);
+    float getYasimDragFactor() { return _yasimdragfactor;}
+    float getYasimLiftFactor() { return _yasimliftfactor;}
+    float getMaxPowerEngine() { return _max_power_engine;}
+    float getMaxPowerRotorBrake() { return _max_power_rotor_brake;}
+    float getRotorBrake() { return _rotorbrake;}
+    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);
 };
 
 }; // namespace yasim

src/FDM/YASim/Rotorblade.cpp

@@ -1,542 +0,0 @@
-#include "Math.hpp"
-#include "Rotorblade.hpp"
-#include <stdio.h>
-//#include <string.h>
-//#include <Main/fg_props.hxx>
-namespace yasim {
-const float pi=3.14159;
-
-Rotorblade::Rotorblade()
-{
-    /*
-    _orient[0] = 1; _orient[1] = 0; _orient[2] = 0;
-    _orient[3] = 0; _orient[4] = 1; _orient[5] = 0;
-    _orient[6] = 0; _orient[7] = 0; _orient[8] = 1;
-    */
-    _collective=0;
-    _dt=0;
-    _speed=0;
-    #define set3(x,a,b,c) x[0]=a;x[1]=b;x[2]=c;
-    set3 (_directionofzentipetalforce,1,0,0);
-    #undef set3
-    _zentipetalforce=1;
-    _maxpitch=.02;
-    _maxpitchforce=10;
-    _delta3=0.5;
-    _cyclicail=0;
-    _cyclicele=0;
-    _collective=0;
-    _flapatpos[0]=_flapatpos[1]=_flapatpos[2]=_flapatpos[3]=0;
-    _flapatpos[0]=.1;
-    _flapatpos[1]=.2;
-    _flapatpos[2]=.3;
-    _flapatpos[3]=.4;
-    _len=1;
-    _lforceattac=1;
-    _calcforcesdone=false;
-    _phi=0;
-    _omega=0;
-    _omegan=1;
-    _mass=10;
-    _alpha=0;
-    _alphaoutputbuf[0][0]=0;
-    _alphaoutputbuf[1][0]=0;
-    _alpha2type=0;
-    _alphaalt=0;
-    _alphaomega=0;
-    _lastrp=0;
-    _nextrp=0;
-    _oppositerp=0;
-    _translift=0;
-    _dynamic=100;
-    _c2=1;
-    _stepspersecond=240;
-    _torque_max_force=0;
-    _torque_no_force=0;
-    _deltaphi=0;
-    _alphamin=-.1;
-    _alphamax= .1;
-    _alpha0=-.05;
-    _alpha0factor=1;
-    _rellenhinge=0;
-    _teeter=0;
-    _ddtteeter=0;
-    _teeterdamp=0.00001;
-    _maxteeterdamp=0;
-    _rellenteeterhinge=0.01;
-
-
-
-
-
-}
-
-
-void Rotorblade::inititeration(float dt,float *rot)
-{
-     //printf("init %5.3f",dt*1000);
-     _dt=dt;
-     _calcforcesdone=false;
-     float a=Math::dot3(rot,_normal);
-   _phi+=a;
-     _phi+=_omega*dt;
-     while (_phi>(2*pi)) _phi-=2*pi;
-     while (_phi<(0   )) _phi+=2*pi;
-     //jetzt noch Drehung des Rumpfes in gleiche Richtung wie alpha bestimmen
-     //und zu _alphaalt hinzuf<75>gen
-     //alpha gibt drehung um normal cross dirofzentf an
-     
-     float dir[3];
-     Math::cross3(_lright,_normal,dir);
-     
-
-
-     a=-Math::dot3(rot,dir);
-   float alphaneu= _alpha+a;
-     // alphaneu= Math::clamp(alphaneu,-.5,.5);
-     //_alphaomega=(alphaneu-_alphaalt)/_dt;//now calculated in calcforces
-
-     _alphaalt = alphaneu;
-
-
-     calcFrontRight();
-}
-void Rotorblade::setTorque(float torque_max_force,float torque_no_force)
-{
-    _torque_max_force=torque_max_force;
-    _torque_no_force=torque_no_force;
-}
-void Rotorblade::setAlpha0(float f)
-{
-    _alpha0=f;
-}
-void Rotorblade::setAlphamin(float f)
-{
-    _alphamin=f;
-}
-void Rotorblade::setAlphamax(float f)
-{
-    _alphamax=f;
-}
-void Rotorblade::setAlpha0factor(float f)
-{
-    _alpha0factor=f;
-}
-
-
-
-
-void Rotorblade::setWeight(float value)
-{
-     _mass=value;
-}
-float Rotorblade::getWeight(void)
-{
-     return(_mass/.453); //_mass is in kg, returns pounds 
-}
-
-void Rotorblade::setPosition(float* p)
-{
-    int i;
-    for(i=0; i<3; i++) _pos[i] = p[i];
-}
-
-void Rotorblade::calcFrontRight()
-{
-    float tmpcf[3],tmpsr[3],tmpsf[3],tmpcr[3];
-    Math::mul3(Math::cos(_phi),_right,tmpcr);
-    Math::mul3(Math::cos(_phi),_front,tmpcf);
-    Math::mul3(Math::sin(_phi),_right,tmpsr);
-    Math::mul3(Math::sin(_phi),_front,tmpsf);
-
-    Math::add3(tmpcf,tmpsr,_lfront);
-    Math::sub3(tmpcr,tmpsf,_lright);
-
-}
-
-void Rotorblade::getPosition(float* out)
-{
-    float dir[3];
-    Math::mul3(_len,_lfront,dir);
-    Math::add3(_pos,dir,out);
-}
-
-void Rotorblade::setPositionForceAttac(float* p)
-{
-    int i;
-    for(i=0; i<3; i++) _posforceattac[i] = p[i];
-}
-
-void Rotorblade::getPositionForceAttac(float* out)
-{
-    float dir[3];
-    Math::mul3(_len*_rellenhinge*2,_lfront,dir);
-    Math::add3(_pos,dir,out);
-}
-void Rotorblade::setSpeed(float p)
-{
-    _speed = p;
-}
-void Rotorblade::setDirectionofZentipetalforce(float* p)
-{
-    int i;
-    for(i=0; i<3; i++) _directionofzentipetalforce[i] = p[i];
-}
-
-void Rotorblade::setZentipetalForce(float f)
-{
-    _zentipetalforce=f;
-} 
-void Rotorblade::setMaxpitch(float f)
-{
-    _maxpitch=f;
-} 
-void Rotorblade::setMaxPitchForce(float f)
-{
-    _maxpitchforce=f;
-} 
-void Rotorblade::setDelta(float f)
-{
-    _delta=f;
-} 
-void Rotorblade::setDeltaPhi(float f)
-{
-    _deltaphi=f;
-} 
-void Rotorblade::setDelta3(float f)
-{
-    _delta3=f;
-} 
-void Rotorblade::setTranslift(float f)
-{
-    _translift=f;
-}
-void Rotorblade::setDynamic(float f)
-{
-    _dynamic=f;
-}
-void Rotorblade::setC2(float f)
-{
-    _c2=f;
-}
-void Rotorblade::setStepspersecond(float steps)
-{
-    _stepspersecond=steps;
-}
-void Rotorblade::setRelLenTeeterHinge(float f)
-{
-   _rellenteeterhinge=f;
-}
-
-void Rotorblade::setTeeterdamp(float f)
-{
-    _teeterdamp=f;
-}
-void Rotorblade::setMaxteeterdamp(float f)
-{
-    _maxteeterdamp=f;
-}
-float Rotorblade::getAlpha(int i)
-{
-  i=i&1;
-  if ((i==0)&&(_first))
-    return _alpha*180/3.14;//in Grad = 1
-  else
-    if(i==0)
-      return _showa;
-    else
-      return _showb;
-
-}
-float Rotorblade::getrealAlpha(void)
-{
-    return _alpha;
-}
-void Rotorblade::setAlphaoutput(char *text,int i)
-{
-   printf("setAlphaoutput Rotorblade [%s] typ %i\n",text,i);
-
-   strncpy(_alphaoutputbuf[i>0],text,255);
-
-   if (i>0) _alpha2type=i;
-
-                             
-}
-char* Rotorblade::getAlphaoutput(int i)
-{
-   #define wstep 30
-    if ((i==0)&&(_first))
-    {
-       int winkel=(int)(.5+_phi/pi*180/wstep);
-       winkel%=(360/wstep);
-       sprintf(_alphaoutputbuf[0],"/blades/pos%03i",winkel*wstep);
-    }
-    
-    else 
-    {
-    
-       int winkel=(int)(.5+_phi/pi*180/wstep);
-       winkel%=(360/wstep);
-       if (i==0)
-         sprintf(_alphaoutputbuf[i&1],"/blades/showa_%i_%03i",i,winkel*wstep);
-       else
-         if (_first)
-           sprintf(_alphaoutputbuf[i&1],"/blades/damp_%03i",winkel*wstep);
-         else
-           sprintf(_alphaoutputbuf[i&1],"/blades/showb_%i_%03i",i,winkel*wstep);
-    
-
-    }
-    return _alphaoutputbuf[i&1];
-  #undef wstep
-}
-
-void Rotorblade::setNormal(float* p)
-{
-    int i;
-    for(i=0; i<3; i++) _normal[i] = p[i];
-}
-void Rotorblade::setFront(float* p)
-{
-    int i;
-    for(i=0; i<3; i++) _lfront[i]=_front[i] = p[i];
-    printf("front: %5.3f %5.3f %5.3f\n",p[0],p[1],p[2]);
-}
-void Rotorblade::setRight(float* p)
-{
-    int i;
-    for(i=0; i<3; i++) _lright[i]=_right[i] = p[i];
-    printf("right: %5.3f %5.3f %5.3f\n",p[0],p[1],p[2]);
-}
-
-void Rotorblade::getNormal(float* out)
-{
-    int i;
-    for(i=0; i<3; i++) out[i] = _normal[i];
-}
-
-
-void Rotorblade::setCollective(float pos)
-{
-    _collective = pos;
-}
-
-void Rotorblade::setCyclicele(float pos)
-{
-    _cyclicele = -pos;
-}
-void Rotorblade::setCyclicail(float pos)
-{
-    _cyclicail = -pos;
-}
-
-
-void Rotorblade::setPhi(float value)
-{
-  _phi=value;
-  if(value==0) _first=1; else _first =0;
-}
-float Rotorblade::getPhi()
-{
-  return(_phi);
-}
-void Rotorblade::setOmega(float value)
-{
-  _omega=value;
-}
-void Rotorblade::setOmegaN(float value)
-{
-  _omegan=value;
-}
-void Rotorblade::setLen(float value)
-{
-  _len=value;
-}
-void Rotorblade::setLenHinge(float value)
-{
-  _rellenhinge=value;
-}
-void Rotorblade::setLforceattac(float value)
-{
-  _lforceattac=value;
-}
-float Rotorblade::getIncidence()
-{
-    return(_incidence);
-}
-
-float Rotorblade::getFlapatPos(int k)
-{
-   return  _flapatpos[k%4];
-}
-
-
-
-/*
-void Rotorblade::setlastnextrp(Rotorblade*lastrp,Rotorblade*nextrp,Rotorblade *oppositerp)
-{
-  _lastrp=lastrp;
-  _nextrp=nextrp;
-  _oppositerp=oppositerp;
-}
-*/
-
-void Rotorblade::strncpy(char *dest,const char *src,int maxlen)
-{
-  int n=0;
-  while(src[n]&&n<(maxlen-1))
-  {
-    dest[n]=src[n];
-    n++;
-  }
-  dest[n]=0;
-}
-
-
-// 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 Rotorblade::calcForce(float* v, float rho, float* out, float* torque)
-{
-
-    //printf("cf: alt:%g aw:%g ",_alphaalt,_alphaomega);
-    //if (_first) printf("p: %5.3f e:%5.3f a:%5.3f p:%5.3f",_collective,_cyclicele,_cyclicail,_phi);
-    if (_calcforcesdone)
-    {
-        int i;
-        for(i=0; i<3; i++) {
-          torque[i] = _oldt[i];
-          out[i] = _oldf[i];
-        }
-        return;
-    }
-
-    {
-      int k;
-      if (_omega>0)
-          for (k=1;k<=4;k++)
-          {
-             if ((_phi<=(float(k)*pi/2))&&((_phi+_omega*_dt)>=(float(k)*pi/2)))
-             {
-                 _flapatpos[k%4]=_alphaalt;
-             }
-          }
-       else
-          for (k=0;k<4;k++)
-          {
-             if ((_phi>=(float(k)*pi/2))&&((_phi+_omega*_dt)<=(float(k)*pi/2)))
-             {
-                 _flapatpos[k%4]=_alphaalt;
-             }
-          }
-    }
-
-    float ldt;
-    int steps=int(_dt*_stepspersecond);
-    if (steps<=0) steps=1;
-    ldt=_dt/steps;
-    float lphi;
-    float f[3];
-    f[0]=f[1]=f[2]=0;
-    float t[3];
-    t[0]=t[1]=t[2]=0;
-
-    //_zentipetalforce=_mass*_omega*_omega*_len*(_rellenhinge+(1-_rellenhinge)*Math::cos(_alphalt));
-    //_zentipetalforce=_mass*_omega*_omega*_len/(_rellenhinge+(1-_rellenhinge)*Math::cos(_alphalt)); //incl teeter
-    _speed=_omega*_len*(1-_rellenhinge+_rellenhinge*Math::cos(_alphaalt));
-
-    float vrel[3],vreldir[3],speed[3];
-    Math::mul3(_speed,_lright,speed);
-    Math::sub3(speed,v,vrel);
-    Math::unit3(vrel,vreldir);//direction of blade-movement rel. to air
-    float delta=Math::asin(Math::dot3(_normal,vreldir));//Angle of blade which would produce no vertical force
-    float lalphaalt=_alphaalt;
-    float lalpha=_alpha;
-    float lalphaomega=_alphaomega;
-    if((_phi>0.01)&&(_first)&&(_phi<0.02))
-    {
-      printf("mass:%5.3f delta: %5.3f _dt: %5.7f ldt: %5.7f st:%i w: %5.3f w0: %5.3f\n",
-             _mass,_delta,_dt,ldt,steps,_omega,Math::sqrt(_zentipetalforce*(1-_rellenhinge)/_len/_mass));   
-    }
-    for (int step=0;step<steps;step++)
-    {
-       lphi=_phi+(step-steps/2.)*ldt*_omega;
-        //_zentipetalforce=_mass*_omega*_omega*_len/(_rellenhinge+(1-_rellenhinge)*Math::cos(lalphaalt)); //incl teeter
-        _zentipetalforce=_mass*_omega*_omega*_len; 
-        //printf("[%5.3f]",col);
-        float beta=-_cyclicele*Math::sin(lphi-0*_deltaphi)+_cyclicail*Math::cos(lphi-0*_deltaphi)+_collective-_delta3*lalphaalt;
-        if (step==(steps/2)) _incidence=beta;
-        //printf("be:%5.3f de:%5.3f ",beta,delta);
-        //printf("\nvd: %5.3f %5.3f %5.3f ",vreldir[0],vreldir[1],vreldir[2]);
-        //printf("lr: %5.3f %5.3f %5.3f\n",_lright[0],_lright[1],_lright[2]);
-        //printf("no: %5.3f %5.3f %5.3f ",_normal[0],_normal[1],_normal[2]);
-        //printf("sp: %5.3f %5.3f %5.3f\n ",speed[0],speed[1],speed[2]);
-        //printf("vr: %5.3f %5.3f %5.3f ",vrel[0],vrel[1],vrel[2]);
-        //printf("v : %5.3f %5.3f %5.3f ",v[0],v[1],v[2]);
-        
-        //float c=_maxpitchforce/(_maxpitch*_zentipetalforce);
-
-        float zforcealph=(beta-delta)/_maxpitch*_maxpitchforce*_omega/_omegan;
-        float zforcezent=(1-_rellenhinge)*Math::sin(lalphaalt)*_zentipetalforce;
-        float zforcelowspeed=(_omegan-_omega)/_omegan*(lalpha-_alpha0)*_mass*_alpha0factor;
-        float zf=zforcealph-zforcezent-zforcelowspeed;
-        _showa=zforcealph;
-        _showb=-zforcezent;
-        float vv=Math::sin(lalphaomega)*_len;
-        zf-=vv*_delta*2*_mass;
-        vv+=zf/_mass*ldt;
-        if ((_omega*10)<_omegan)
-          vv*=.5+5*(_omega/_omegan);//reduce if omega is low
-        //if (_first) _showb=vv*_delta*2*_mass;//for debugging output
-        lalpha=Math::asin(Math::sin(lalphaalt)+(vv/_len)*ldt);
-        lalpha=Math::clamp(lalpha,_alphamin,_alphamax);
-        float vblade=Math::abs(Math::dot3(_lfront,v));
-        float tliftfactor=Math::sqrt(1+vblade*_translift);
-
-
-
-
-        float xforce = Math::cos(lalpha)*_zentipetalforce;// 
-        float zforce = tliftfactor*Math::sin(lalpha)*_zentipetalforce;// 
-        float thetorque = _torque_no_force+_torque_max_force*Math::abs(zforce/_maxpitchforce);
-        /*
-        printf("speed: %5.3f %5.3f %5.3f vwind: %5.3f %5.3f %5.3f sin %5.3f\n",
-          _speed[0],_speed[1],_speed[2],
-          v[0],v[1],v[2],Math::sin(alpha));
-        */
-        int i;
-        for(i=0; i<3; i++) {
-          t[i] += _normal[i]*thetorque;
-          f[i] += _normal[i]*zforce+_lfront[i]*xforce;
-        }
-        lalphaomega=(lalpha-lalphaalt)/ldt;
-        lalphaalt=lalpha;
-
-        /*
-        _ddtteeter+=_len*_omega/(1-_rellenhinge)*lalphaomega*ldt;
-        
-        float teeterforce=-_zentipetalforce*Math::sin(_teeter)*_c2;
-        teeterforce-=Math::clamp(_ddtteeter*_teeterdamp,-_maxteeterdamp,_maxteeterdamp);
-        _ddtteeter+=teeterforce/_mass;
-        
-        _teeter+=_ddtteeter*ldt;
-        if (_first)  _showb=_teeter*180/pi;
-        */
-    }
-    _alpha=lalpha;
-    _alphaomega=lalphaomega;
-    /*
-        if (_first) printf("aneu: %5.3f zfa:%5.3f vv:%g ao:%.3g xf:%.3g zf:%.3g    \r",_alpha,zforcealph,vv,_alpha
-                          ,xforce,zforce);
-    */
-    int i;
-    for(i=0; i<3; i++) {
-          torque[i] = _oldt[i]=t[i]/steps;
-          out[i] = _oldf[i]=f[i]/steps;
-    }
-    _calcforcesdone=true;
-        //printf("alpha: %5.3f force: %5.3f %5.3f %5.3f %5.3f %5.3f\n",alpha*180/3.14,xforce,zforce,out[0],out[1],out[2]);
-}
-
-
-}; // namespace yasim

src/FDM/YASim/Rotorblade.hpp

@@ -1,146 +0,0 @@
-#ifndef _ROTORBLADE_HPP
-#define _ROTORBLADE_HPP
-
-namespace yasim {
-
-class Rotorblade
-{
-public:
-    Rotorblade();
-
-    // Position of this surface in local coords
-    void setPosition(float* p);
-    void getPosition(float* out);
-    float getPhi();
-    void setPhi(float value);
-
-    void setPositionForceAttac(float* p);
-    void getPositionForceAttac(float* out);
-
-    void setNormal(float* p);
-    void setFront(float* p);
-    void setRight(float* p);
-    void getNormal(float* out);
-
-    void setMaxPitchForce(float force);
-
-    void setCollective(float pos);
-
-    void setCyclicele(float pos);
-    void setCyclicail(float pos);
-
-    void setOmega(float value);
-    void setOmegaN(float value);
-    void setLen(float value);
-    void setLenHinge(float value);
-    void setLforceattac(float value);
-
-    void setSpeed(float p);
-    void setDirectionofZentipetalforce(float* p);
-    void setZentipetalForce(float f);
-    void setMaxpitch(float f);
-    void setDelta3(float f);
-    void setDelta(float f);
-    void setDeltaPhi(float f);
-    void setDynamic(float f);
-    void setTranslift(float f);
-    void setC2(float f);
-    void setStepspersecond(float steps);
-    void setZentForce(float f);
-
-    float getAlpha(int i);
-    float getrealAlpha(void);
-    char* getAlphaoutput(int i);
-    void setAlphaoutput(char *text,int i);
-
-    void inititeration(float dt,float *rot);
-    
-    float getWeight(void);
-    void setWeight(float value);
-
-    float getFlapatPos(int k);
-
-
-
-
-
-
-
-    // local -> Rotorblade coords
-    //void setOrientation(float* o);
-
-
-    void calcForce(float* v, float rho, float* forceOut, float* torqueOut);
-    void setlastnextrp(Rotorblade*lastrp,Rotorblade*nextrp,Rotorblade *oppositerp);
-    void setTorque(float torque_max_force,float torque_no_force);
-    void calcFrontRight();
-    float getIncidence();
-    void setAlpha0(float f);
-    void setAlphamin(float f);
-    void setAlphamax(float f);
-    void setAlpha0factor(float f);
-    void setTeeterdamp(float f);
-    void setMaxteeterdamp(float f);
-    void setRelLenTeeterHinge(float value);
-
-private:
-    void strncpy(char *dest,const char *src,int maxlen);
-    Rotorblade *_lastrp,*_nextrp,*_oppositerp;
-
-    float _dt;
-    float _phi,_omega,_omegan;
-    float _delta;
-    float _deltaphi;
-    int _first;
-    
-    float _len;
-    float _lforceattac;
-    float _pos[3];    // position in local coords
-    float _posforceattac[3];    // position in local coords
-    float _normal[3]; //direcetion of the rotation axis
-    float _front[3],_right[3];
-    float _lright[3],_lfront[3];
-    float _torque_max_force;
-    float _torque_no_force;
-    float _speed;
-    float _directionofzentipetalforce[3];
-    float _zentipetalforce;
-    float _maxpitch;
-    float _maxpitchforce;
-    float _cyclicele;
-    float _cyclicail;
-    float _collective;
-    float _delta3;
-    float _dynamic;
-    float _flapatpos[4];//flapangle at 0, 90, 180 and 270 degree, for graphics
-    float _translift;
-    float _c2;
-    float _mass;
-    float _alpha;
-    float _alphaalt;
-    float _alphaomega;
-    float _rellenhinge;
-    float _incidence;
-    float _alphamin,_alphamax,_alpha0,_alpha0factor;
-    float _stepspersecond;
-    float _teeter,_ddtteeter;
-    float _teeterdamp,_maxteeterdamp;
-    float _rellenteeterhinge;
-
-
-
-          
-    char _alphaoutputbuf[2][256];
-    int _alpha2type;
-
-    //float _orient[9]; // local->surface orthonormal matrix
-
-    bool  _calcforcesdone;
-    float _oldt[3],_oldf[3];
-
-    float _showa,_showb;
-
-};
-
-}; // namespace yasim
-#endif // _ROTORBLADE_HPP

src/FDM/YASim/Rotorpart.cpp

@@ -2,31 +2,34 @@
 
 #include "Math.hpp"
 #include "Rotorpart.hpp"
+#include "Rotor.hpp"
 #include <stdio.h>
-//#include <string.h>
+#include <string.h>
-//#include <Main/fg_props.hxx>
 namespace yasim {
 const float pi=3.14159;
-
+float _help = 0;
 Rotorpart::Rotorpart()
 {
+    _compiled=0;
     _cyclic=0;
     _collective=0;
     _rellenhinge=0;
     _dt=0;
-    #define set3(x,a,b,c) x[0]=a;x[1]=b;x[2]=c;
+#define set3(x,a,b,c) x[0]=a;x[1]=b;x[2]=c;
     set3 (_speed,1,0,0);
     set3 (_directionofzentipetalforce,1,0,0);
-    #undef set3
+    set3 (_directionofrotorpart,0,1,0);
+    set3 (_direction_of_movement,1,0,0);
+    set3 (_last_torque,0,0,0);
+#undef set3
     _zentipetalforce=1;
     _maxpitch=.02;
     _minpitch=0;
-    _maxpitchforce=10;
     _maxcyclic=0.02;
     _mincyclic=-0.02;
     _delta3=0.5;
     _cyclic=0;
-    _collective=0;
+    _collective=-1;
     _relamp=1;
     _mass=10;
     _incidence = 0;
@@ -49,64 +52,80 @@ Rotorpart::Rotorpart()
     _torque_no_force=0;
     _omega=0;
     _omegan=1;
+    _ddt_omega=0;
     _phi=0;
     _len=1;
-
+    _rotor=NULL;
-
+    _twist=0; 
-
+    _number_of_segments=1;
-
+    _rel_len_where_incidence_is_measured=0.7;
+    _diameter=10;
+    _torque_of_inertia=0;
+    _rel_len_blade_start=0;
 }
 
-
 void Rotorpart::inititeration(float dt,float *rot)
 {
-     //printf("init %5.3f",dt*1000);
+    _dt=dt;
-     _dt=dt;
+    _phi+=_omega*dt;
-     _phi+=_omega*dt;
+    while (_phi>(2*pi)) _phi-=2*pi;
-     while (_phi>(2*pi)) _phi-=2*pi;
+    while (_phi<(0   )) _phi+=2*pi;
-     while (_phi<(0   )) _phi+=2*pi;
+    float a=Math::dot3(rot,_normal);
-
+    if(a>0)
-     //_alphaalt=_alpha;
-     //a=skalarprdukt _normal mit rot ergibt drehung um Normale
-     //alphaalt=Math::cos(a)*alpha+.5*(Math::sin(a)*alphanachbarnlast-Math::sin(a)*alphanachbanext)
-     float a=Math::dot3(rot,_normal);
-     if(a>0)
         _alphaalt=_alpha*Math::cos(a)
-                  +_nextrp->getrealAlpha()*Math::sin(a);
+        +_nextrp->getrealAlpha()*Math::sin(a);
-      else
+    else
         _alphaalt=_alpha*Math::cos(a)
-                  +_lastrp->getrealAlpha()*Math::sin(-a);
+        +_lastrp->getrealAlpha()*Math::sin(-a);
-     //jetzt noch Drehung des Rumpfes in gleiche Richtung wie alpha bestimmen
+    //calculate the rotation of the fuselage, determine
-     //und zu _alphaalt hinzuf<75>gen
+    //the part in the same direction as alpha
-     //alpha gibt drehung um normal cross dirofzentf an
+    //and add it ro _alphaalt
-     
+    //alpha is rotation about "normal cross dirofzentf"
-     float dir[3];
-     Math::cross3(_directionofzentipetalforce,_normal,dir);
-     
-
-
-     a=Math::dot3(rot,dir);
-     _alphaalt -= a;
-
-     _alphaalt= Math::clamp(_alphaalt,_alphamin,_alphamax);
-
 
+    float dir[3];
+    Math::cross3(_directionofzentipetalforce,_normal,dir);
+    a=Math::dot3(rot,dir);
+    _alphaalt -= a;
+    _alphaalt= Math::clamp(_alphaalt,_alphamin,_alphamax);
 }
+
+void Rotorpart::setRotor(Rotor *rotor)
+{
+    _rotor=rotor;
+}
+
+void Rotorpart::setParameter(char *parametername, float value)
+{
+#define p(a) if (strcmp(parametername,#a)==0) _##a = value; else
+
+    p(twist)
+        p(number_of_segments)
+        p(rel_len_where_incidence_is_measured)
+        p(rel_len_blade_start)
+        cout << "internal error in parameter set up for rotorpart: '"
+            << parametername <<"'" << endl;
+#undef p
+}
+
 void Rotorpart::setTorque(float torque_max_force,float torque_no_force)
 {
     _torque_max_force=torque_max_force;
     _torque_no_force=torque_no_force;
 }
 
-
+void Rotorpart::setTorqueOfInertia(float toi)
+{
+    _torque_of_inertia=toi;
+}
 
 void Rotorpart::setWeight(float value)
 {
-     _mass=value;
+    _mass=value;
 }
+
 float Rotorpart::getWeight(void)
 {
-     return(_mass/.453); //_mass is in kg, returns pounds 
+    return(_mass/.453); //_mass is in kg, returns pounds 
 }
 
 void Rotorpart::setPosition(float* p)
@@ -114,6 +133,7 @@ void Rotorpart::setPosition(float* p)
     int i;
     for(i=0; i<3; i++) _pos[i] = p[i];
 }
+
 float Rotorpart::getIncidence()
 {
     return(_incidence);
@@ -135,128 +155,157 @@ void Rotorpart::getPositionForceAttac(float* out)
 {
     int i;
     for(i=0; i<3; i++) out[i] = _posforceattac[i];
-    //printf("posforce: %5.3f %5.3f %5.3f ",out[0],out[1],out[2]);
 }
+
 void Rotorpart::setSpeed(float* p)
 {
     int i;
     for(i=0; i<3; i++) _speed[i] = p[i];
+    Math::unit3(_speed,_direction_of_movement); 
 }
+
 void Rotorpart::setDirectionofZentipetalforce(float* p)
 {
     int i;
     for(i=0; i<3; i++) _directionofzentipetalforce[i] = p[i];
 }
 
-void Rotorpart::setOmega(float value)
+void Rotorpart::setDirectionofRotorPart(float* p)
 {
-  _omega=value;
+    int i;
-}
+    for(i=0; i<3; i++) _directionofrotorpart[i] = p[i];
-void Rotorpart::setOmegaN(float value)
-{
-  _omegan=value;
 }
 
+void Rotorpart::setOmega(float value)
+{
+    _omega=value;
+}
+
+void Rotorpart::setOmegaN(float value)
+{
+    _omegan=value;
+}
+
+void Rotorpart::setDdtOmega(float value)
+{
+    _ddt_omega=value;
+}
 
 void Rotorpart::setZentipetalForce(float f)
 {
     _zentipetalforce=f;
 } 
+
 void Rotorpart::setMinpitch(float f)
 {
     _minpitch=f;
 } 
+
 void Rotorpart::setMaxpitch(float f)
 {
     _maxpitch=f;
 } 
-void Rotorpart::setMaxPitchForce(float f)
+
-{
-    _maxpitchforce=f;
-} 
 void Rotorpart::setMaxcyclic(float f)
 {
     _maxcyclic=f;
 } 
+
 void Rotorpart::setMincyclic(float f)
 {
     _mincyclic=f;
 } 
+
 void Rotorpart::setDelta3(float f)
 {
     _delta3=f;
 } 
+
 void Rotorpart::setRelamp(float f)
 {
     _relamp=f;
 } 
+
 void Rotorpart::setTranslift(float f)
 {
     _translift=f;
 }
+
 void Rotorpart::setDynamic(float f)
 {
     _dynamic=f;
 }
+
 void Rotorpart::setRelLenHinge(float f)
 {
     _rellenhinge=f;
 }
+
 void Rotorpart::setC2(float f)
 {
     _c2=f;
 }
+
 void Rotorpart::setAlpha0(float f)
 {
     _alpha0=f;
 }
+
 void Rotorpart::setAlphamin(float f)
 {
     _alphamin=f;
 }
+
 void Rotorpart::setAlphamax(float f)
 {
     _alphamax=f;
 }
+
 void Rotorpart::setAlpha0factor(float f)
 {
     _alpha0factor=f;
 }
 
+void Rotorpart::setDiameter(float f)
+{
+    _diameter=f;
+}
 
 float Rotorpart::getPhi()
 {
-  return(_phi);
+    return(_phi);
 }
 
 float Rotorpart::getAlpha(int i)
 {
-  i=i&1;
+    i=i&1;
-  if (i==0)
-    return _alpha*180/3.14;//in Grad = 1
-  else
-    if (_alpha2type==1) //yaw or roll
-      return (getAlpha(0)-_oppositerp->getAlpha(0))/2;
-     else //pitch
-      return (getAlpha(0)+_oppositerp->getAlpha(0)+
-              _nextrp->getAlpha(0)+_lastrp->getAlpha(0))/4;
 
+    if (i==0)
+        return _alpha*180/3.14;//in Grad = 1
+    else
+    {
+        if (_alpha2type==1) //yaw or roll
+            return (getAlpha(0)-_oppositerp->getAlpha(0))/2;
+        else //collective
+            return (getAlpha(0)+_oppositerp->getAlpha(0)+
+            _nextrp->getAlpha(0)+_lastrp->getAlpha(0))/4;
+    }
 }
 float Rotorpart::getrealAlpha(void)
 {
     return _alpha;
 }
+
 void Rotorpart::setAlphaoutput(char *text,int i)
 {
-   SG_LOG(SG_FLIGHT, SG_DEBUG, "setAlphaoutput rotorpart ["
+    SG_LOG(SG_FLIGHT, SG_DEBUG, "setAlphaoutput rotorpart ["
-          << text << "] typ" << i);
+        << text << "] typ" << i);
 
-   strncpy(_alphaoutputbuf[i>0],text,255);
+    strncpy(_alphaoutputbuf[i>0],text,255);
 
-   if (i>0) _alpha2type=i;
+    if (i>0) _alpha2type=i;
-
-                             
 }
+
 char* Rotorpart::getAlphaoutput(int i)
 {
     return _alphaoutputbuf[i&1];
@@ -264,10 +313,9 @@ char* Rotorpart::getAlphaoutput(int i)
 
 void Rotorpart::setLen(float value)
 {
-  _len=value;
+    _len=value;
 }
 
-
 void Rotorpart::setNormal(float* p)
 {
     int i;
@@ -280,7 +328,6 @@ void Rotorpart::getNormal(float* out)
     for(i=0; i<3; i++) out[i] = _normal[i];
 }
 
-
 void Rotorpart::setCollective(float pos)
 {
     _collective = pos;
@@ -291,112 +338,201 @@ void Rotorpart::setCyclic(float pos)
     _cyclic = pos;
 }
 
-void Rotorpart::setlastnextrp(Rotorpart*lastrp,Rotorpart*nextrp,Rotorpart *oppositerp)
+void Rotorpart::setlastnextrp(Rotorpart*lastrp,Rotorpart*nextrp,
+    Rotorpart *oppositerp)
 {
-  _lastrp=lastrp;
+    _lastrp=lastrp;
-  _nextrp=nextrp;
+    _nextrp=nextrp;
-  _oppositerp=oppositerp;
+    _oppositerp=oppositerp;
 }
 
 void Rotorpart::strncpy(char *dest,const char *src,int maxlen)
 {
-  int n=0;
+    int n=0;
-  while(src[n]&&n<(maxlen-1))
+    while(src[n]&&n<(maxlen-1))
-  {
+    {
-    dest[n]=src[n];
+        dest[n]=src[n];
-    n++;
+        n++;
-  }
+    }
-  dest[n]=0;
+    dest[n]=0;
+}
+
+// Calculate the flapping angle, where zentripetal force and
+//lift compensate each other
+float Rotorpart::calculateAlpha(float* v_rel_air, float rho, 
+    float incidence, float cyc, float alphaalt, float *torque,
+    float *returnlift)
+{
+    float moment[3],v_local[3],v_local_scalar,lift_moment,v_flap[3],v_help[3];
+    float ias;//nur f. dgb
+    int i,n;
+    for (i=0;i<3;i++)
+        moment[i]=0;
+    lift_moment=0;
+    *torque=0;//
+    if((_nextrp==NULL)||(_lastrp==NULL)||(_rotor==NULL)) 
+        return 0.0;//not initialized. Can happen during startupt of flightgear
+    if (returnlift!=NULL) *returnlift=0;
+    float flap_omega=(_nextrp->getrealAlpha()-_lastrp->getrealAlpha())
+        *_omega / pi;
+    float local_width=_diameter*(1-_rel_len_blade_start)/2.
+        /(float (_number_of_segments));
+    for (n=0;n<_number_of_segments;n++)
+    {
+        float rel = (n+.5)/(float (_number_of_segments));
+        float r= _diameter *0.5 *(rel*(1-_rel_len_blade_start)
+            +_rel_len_blade_start);
+        float local_incidence=incidence+_twist *rel - _twist
+            *_rel_len_where_incidence_is_measured;
+        float local_chord = _rotor->getChord()*rel+_rotor->getChord()
+            *_rotor->getTaper()*(1-rel);
+        float A = local_chord * local_width;
+        //calculate the local air speed and the incidence to this speed;
+        Math::mul3(_omega * r , _direction_of_movement , v_local);
+
+        // add speed component due to flapping
+        Math::mul3(flap_omega * r,_normal,v_flap);
+        Math::add3(v_flap,v_local,v_local);
+        Math::sub3(v_rel_air,v_local,v_local); 
+        //v_local is now the total airspeed at the blade 
+        //apparent missing: calculating the local_wind = v_rel_air at 
+        //every point of the rotor. It differs due to aircraft-rotation
+        //it is considered in v_flap
+
+        //substract now the component of the air speed parallel to 
+        //the blade;
+       Math::mul3(Math::dot3(v_local,_directionofrotorpart),
+           _directionofrotorpart,v_help);
+        Math::sub3(v_local,v_help,v_local);
+
+        //split into direction and magnitude
+        v_local_scalar=Math::mag3(v_local);
+        if (v_local_scalar!=0)
+            Math::unit3(v_local,v_local);
+        float incidence_of_airspeed = Math::asin(Math::clamp(
+            Math::dot3(v_local,_normal),-1,1)) + local_incidence;
+        ias = incidence_of_airspeed;
+        float lift_wo_cyc = 
+            _rotor->getLiftCoef(incidence_of_airspeed-cyc,v_local_scalar)
+            * v_local_scalar * v_local_scalar * A *rho *0.5;
+        float lift_with_cyc = 
+            _rotor->getLiftCoef(incidence_of_airspeed,v_local_scalar)
+            * v_local_scalar * v_local_scalar *A *rho*0.5;
+        float lift=lift_wo_cyc+_relamp*(lift_with_cyc-lift_wo_cyc);
+        //take into account that the rotor is a resonant system where
+        //the cyclic input hase increased result
+        float drag = -_rotor->getDragCoef(incidence_of_airspeed,v_local_scalar) 
+            * v_local_scalar * v_local_scalar * A *rho*0.5;
+        float angle = incidence_of_airspeed - incidence; 
+        //angle between blade movement caused by rotor-rotation and the
+        //total movement of the blade
+
+        lift_moment += r*(lift * Math::cos(angle) 
+            - drag * Math::sin(angle));
+        *torque     += r*(drag * Math::cos(angle) 
+            + lift * Math::sin(angle));
+
+        if (returnlift!=NULL) *returnlift+=lift;
+    }
+    float alpha=Math::atan2(lift_moment,_zentipetalforce * _len); 
+
+    return (alpha);
 }
 
 // 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 Rotorpart::calcForce(float* v, float rho, float* out, float* torque)
+void Rotorpart::calcForce(float* v, float rho,  float* out, float* torque,
+    float* torque_scalar)
 {
+    if (_compiled!=1)
     {
-        _zentipetalforce=_mass*_len*_omega*_omega;
+        for (int i=0;i<3;i++)
-        float vrel[3],vreldir[3];
+            torque[i]=out[i]=0;
-        Math::sub3(_speed,v,vrel);
+        *torque_scalar=0;
-        Math::unit3(vrel,vreldir);//direction of blade-movement rel. to air
+        return;
-        float delta=Math::asin(Math::dot3(_normal,vreldir));//Angle of blade which would produce no vertical force
+    }
+    _zentipetalforce=_mass*_len*_omega*_omega;
+    float vrel[3],vreldir[3];
+    Math::sub3(_speed,v,vrel);
+    float scalar_torque=0,alpha_alteberechnung=0;
+    Math::unit3(vrel,vreldir);//direction of blade-movement rel. to air
+    float delta=Math::asin(Math::dot3(_normal,vreldir));
+    //Angle of blade which would produce no vertical force (where the 
+    //effective incidence is zero)
 
-        float cyc=_mincyclic+(_cyclic+1)/2*(_maxcyclic-_mincyclic);
+    float cyc=_mincyclic+(_cyclic+1)/2*(_maxcyclic-_mincyclic);
-        float col=_minpitch+(_collective+1)/2*(_maxpitch-_minpitch);
+    float col=_minpitch+(_collective+1)/2*(_maxpitch-_minpitch);
-        //printf("[%5.3f]",col);
+    _incidence=(col+cyc)-_delta3*_alphaalt;
-        _incidence=(col+cyc)-_delta3*_alphaalt;
+    //the incidence of the rotorblade due to control input reduced by the
-        cyc*=_relamp;
+    //delta3 effect, see README.YASIM
-        float beta=cyc+col;
+    float beta=_relamp*cyc+col; 
-        
+    //the incidence of the rotorblade which is used for the calculation
-        //float c=_maxpitchforce/(_maxpitch*_zentipetalforce);
-        float c,alpha,factor;
-        if((_omega*10)>_omegan)
-        {
-          c=_maxpitchforce/_omegan/(_maxpitch*_mass*_len*_omega);
-          alpha = c*(beta-delta)/(1+_delta3*c);
 
-          factor=_dt*_dynamic;
+    float alpha,factor; //alpha is the flapping angle
-          if (factor>1) factor=1;
+    //the new flapping angle will be the old flapping angle
-        }
+    //+ factor *(alpha - "old flapping angle")
-        else
+    if((_omega*10)>_omegan) 
-        {
+    //the rotor is rotaing quite fast.
-          alpha=_alpha0;
+    //(at least 10% of the nominal rotational speed)
-          factor=_dt*_dynamic/10;
+    {
-          if (factor>1) factor=1;
+        alpha=calculateAlpha(v,rho,_incidence,cyc,0,&scalar_torque);
-        }
+        //the incidence is a function of alpha (if _delta* != 0)
+        //Therefore missing: wrap this function in an integrator
+        //(runge kutta e. g.)
 
-        float vz=Math::dot3(_normal,v);
+        factor=_dt*_dynamic;
-        //alpha+=_c2*vz;
+        if (factor>1) factor=1;
-        
+    }
-        float fcw;
+    else //the rotor is not rotating or rotating very slowly 
-        if(_c2==0)
+    {
-          fcw=0;
+        alpha=calculateAlpha(v,rho,_incidence,cyc,alpha_alteberechnung,
-        else
+            &scalar_torque);
-          //fcw=vz/_c2*_maxpitchforce*_omega/_omegan;
+        //calculate drag etc., e. g. for deccelrating the rotor if engine
-          fcw=vz*(_c2-1)*_maxpitchforce*_omega/(_omegan*_omegan*_len*_maxpitch);
+        //is off and omega <10%
 
-        float dirblade[3];
+        float rel =_omega*10 / _omegan;
-        Math::cross3(_normal,_directionofzentipetalforce,dirblade);
+        alpha=rel * alpha + (1-rel)* _alpha0;
-        float vblade=Math::abs(Math::dot3(dirblade,v));
+        factor=_dt*_dynamic/10;
-        float tliftfactor=Math::sqrt(1+vblade*_translift);
+        if (factor>1) factor=1;
+    }
 
+    float vz=Math::dot3(_normal,v); //the s
+    float dirblade[3];
+    Math::cross3(_normal,_directionofzentipetalforce,dirblade);
+    float vblade=Math::abs(Math::dot3(dirblade,v));
+    float tliftfactor=Math::sqrt(1+vblade*_translift);
 
-        alpha=_alphaalt+(alpha-_alphaalt)*factor;
+    alpha=_alphaalt+(alpha-_alphaalt)*factor;
-        //alpha=_alphaalt+(alpha-_lastrp->getrealAlpha())*factor;
+    _alpha=alpha;
+    float meancosalpha=(1*Math::cos(_lastrp->getrealAlpha())
+        +1*Math::cos(_nextrp->getrealAlpha())
+        +1*Math::cos(_oppositerp->getrealAlpha())
+        +1*Math::cos(alpha))/4;
+    float schwenkfactor=1-(Math::cos(_lastrp->getrealAlpha())-meancosalpha);
 
-
+    //missing: consideration of rellenhinge
-        _alpha=alpha;
+    float xforce = Math::cos(alpha)*_zentipetalforce;
-
+    float zforce = schwenkfactor*Math::sin(alpha)*_zentipetalforce;
-
+    *torque_scalar=scalar_torque;
-        //float schwenkfactor=1;//  1/Math::cos(_lastrp->getrealAlpha());
+    scalar_torque+= 0*_ddt_omega*_torque_of_inertia;
-
+    float thetorque = scalar_torque;
-        float meancosalpha=(1*Math::cos(_lastrp->getrealAlpha())
+    int i;
-                        +1*Math::cos(_nextrp->getrealAlpha())
+    float f=_rotor->getCcw()?1:-1;
-                        +1*Math::cos(_oppositerp->getrealAlpha())
+    for(i=0; i<3; i++) {
-                        +1*Math::cos(alpha))/4;
+        _last_torque[i]=torque[i] = f*_normal[i]*thetorque;
-        float schwenkfactor=1-(Math::cos(_lastrp->getrealAlpha())-meancosalpha);
+        out[i] = _normal[i]*zforce*_rotor->getLiftFactor()
-
+            +_directionofzentipetalforce[i]*xforce;
-
-        //fehlt: verringerung um rellenhinge
-        float xforce = /*schwenkfactor*/ Math::cos(alpha)*_zentipetalforce;// /9.81*.453; //N->poundsofforce
-        float zforce = fcw+tliftfactor*schwenkfactor*Math::sin(alpha)*_zentipetalforce;// /9.81*.453;
-
-        float thetorque = _torque_no_force+_torque_max_force*Math::abs(zforce/_maxpitchforce);
-        /*
-        printf("speed: %5.3f %5.3f %5.3f vwind: %5.3f %5.3f %5.3f sin %5.3f\n",
-          _speed[0],_speed[1],_speed[2],
-          v[0],v[1],v[2],Math::sin(alpha));
-        */
-
-        int i;
-        for(i=0; i<3; i++) {
-          torque[i] = _normal[i]*thetorque;
-          out[i] = _normal[i]*zforce+_directionofzentipetalforce[i]*xforce;
-        }
-        //printf("alpha: %5.3f force: %5.3f %5.3f %5.3f %5.3f %5.3f\n",alpha*180/3.14,xforce,zforce,out[0],out[1],out[2]);
-	return;
     }
 }
 
-
+void Rotorpart::getAccelTorque(float relaccel,float *t)
+{
+    int i;
+    float f=_rotor->getCcw()?1:-1;
+    for(i=0; i<3; i++) {
+        t[i]=f*-1* _normal[i]*relaccel*_omegan* _torque_of_inertia;
+        _rotor->addTorque(-relaccel*_omegan* _torque_of_inertia);
+    }
+}
 }; // namespace yasim

src/FDM/YASim/Rotorpart.hpp

@@ -2,115 +2,121 @@
 #define _ROTORPART_HPP
 
 namespace yasim {
+    class Rotor;
+    class Rotorpart
+    {
+    private:
+        float _dt;
+        float _last_torque[3];
+        int _compiled;
+    public:
+        Rotorpart();
 
-class Rotorpart
+        // Position of this surface in local coords
-{
+        void setPosition(float* p);
-public:
+        void getPosition(float* out);
-    Rotorpart();
+        void setCompiled() {_compiled=1;}
+        float getDt() {return _dt;}
+        void setPositionForceAttac(float* p);
+        void getPositionForceAttac(float* out);
+        void setNormal(float* p);
+        void getNormal(float* out);
+        void setCollective(float pos);
+        void setCyclic(float pos);
+        void getLastTorque(float *t)
+            {for (int i=0;i<3;i++) t[i]=_last_torque[i];}
+        void getAccelTorque(float relaccel,float *t);
+        void setSpeed(float* p);
+        void setDirectionofZentipetalforce(float* p);
+        void setDirectionofRotorPart(float* p);
+        void setZentipetalForce(float f);
+        void setMaxpitch(float f);
+        void setMinpitch(float f);
+        void setMaxcyclic(float f);
+        void setMincyclic(float f);
+        void setDelta3(float f);
+        void setDynamic(float f);
+        void setTranslift(float f);
+        void setC2(float f);
+        void setZentForce(float f);
+        void setRelLenHinge(float f);
+        void setRelamp(float f);
+        void setDiameter(float f);
+        float getAlpha(int i);
+        float getrealAlpha(void);
+        char* getAlphaoutput(int i);
+        void setAlphaoutput(char *text,int i);
+        void inititeration(float dt,float *rot);
+        float getWeight(void);
+        void setWeight(float value);
+        void calcForce(float* v, float rho, float* forceOut, float* torqueOut,
+            float* torque_scalar);
+        float calculateAlpha(float* v, float rho, float incidence, float cyc,
+            float alphaalt, float *torque,float *returnlift=0);
+        void setlastnextrp(Rotorpart*lastrp,Rotorpart*nextrp,
+            Rotorpart *oppositerp);
+        void setTorque(float torque_max_force,float torque_no_force);
+        void setOmega(float value);
+        void setOmegaN(float value);
+        void setDdtOmega(float value);
+        float getIncidence();
+        float getPhi();
+        void setAlphamin(float f);
+        void setAlphamax(float f);
+        void setAlpha0(float f);
+        void setAlpha0factor(float f);
+        void setLen(float value);
+        void setParameter(char *parametername, float value);
+        void setRotor(Rotor *rotor);
+        void setTorqueOfInertia(float toi);
 
-    // Position of this surface in local coords
+    private:
-    void setPosition(float* p);
+        void strncpy(char *dest,const char *src,int maxlen);
-    void getPosition(float* out);
+        Rotorpart *_lastrp,*_nextrp,*_oppositerp;
+        Rotor *_rotor;
 
-
+        float _pos[3];    // position in local coords
-    void setPositionForceAttac(float* p);
+        float _posforceattac[3];    // position in local coords
-    void getPositionForceAttac(float* out);
+        float _normal[3]; //direcetion of the rotation axis
-
+        float _torque_max_force;
-    void setNormal(float* p);
+        float _torque_no_force;
-    void getNormal(float* out);
+        float _speed[3];
-
+        float _direction_of_movement[3];
-    void setMaxPitchForce(float force);
+        float _directionofzentipetalforce[3];
-
+        float _directionofrotorpart[3];
-    void setCollective(float pos);
+        float _zentipetalforce;
-
+        float _maxpitch;
-    void setCyclic(float pos);
+        float _minpitch;
-
+        float _maxcyclic;
-    void setSpeed(float* p);
+        float _mincyclic;
-    void setDirectionofZentipetalforce(float* p);
+        float _cyclic;
-    void setZentipetalForce(float f);
+        float _collective;
-    void setMaxpitch(float f);
+        float _delta3;
-    void setMinpitch(float f);
+        float _dynamic;
-    void setMaxcyclic(float f);
+        float _translift;
-    void setMincyclic(float f);
+        float _c2;
-    void setDelta3(float f);
+        float _mass;
-    void setDynamic(float f);
+        float _alpha;
-    void setTranslift(float f);
+        float _alphaalt;
-    void setC2(float f);
+        float _alphamin,_alphamax,_alpha0,_alpha0factor;
-    void setZentForce(float f);
+        float _rellenhinge;
-    void setRelLenHinge(float f);
+        float _relamp;
-    void setRelamp(float f);
+        float _omega,_omegan,_ddt_omega;
-
+        float _phi;
-    float getAlpha(int i);
+        float _len;
-    float getrealAlpha(void);
+        float _incidence;
-    char* getAlphaoutput(int i);
+        float _twist; //outer incidence = inner inner incidence + _twist
-    void setAlphaoutput(char *text,int i);
+        int _number_of_segments;
-
+        float _rel_len_where_incidence_is_measured;
-    void inititeration(float dt,float *rot);
+        float _rel_len_blade_start;
-    
+        float _rel_len_blade_measured;
-    float getWeight(void);
+        float _diameter;
-    void setWeight(float value);
+        float _torque_of_inertia;
-
+        float _torque; 
-
+        // total torque of rotor (scalar) for calculating new rotor rpm
-
+        char _alphaoutputbuf[2][256];
-
+        int _alpha2type;
-
+    };
-
-    void calcForce(float* v, float rho, float* forceOut, float* torqueOut);
-    void setlastnextrp(Rotorpart*lastrp,Rotorpart*nextrp,Rotorpart *oppositerp);
-    void setTorque(float torque_max_force,float torque_no_force);
-    void setOmega(float value);
-    void setOmegaN(float value);
-    float getIncidence();
-    float getPhi();
-    void setAlphamin(float f);
-    void setAlphamax(float f);
-    void setAlpha0(float f);
-    void setAlpha0factor(float f);
-    void setLen(float value);
-
-
-private:
-    void strncpy(char *dest,const char *src,int maxlen);
-    Rotorpart *_lastrp,*_nextrp,*_oppositerp;
-
-    float _dt;
-    float _pos[3];    // position in local coords
-    float _posforceattac[3];    // position in local coords
-    float _normal[3]; //direcetion of the rotation axis
-    float _torque_max_force;
-    float _torque_no_force;
-    float _speed[3];
-    float _directionofzentipetalforce[3];
-    float _zentipetalforce;
-    float _maxpitch;
-    float _minpitch;
-    float _maxpitchforce;
-    float _maxcyclic;
-    float _mincyclic;
-    float _cyclic;
-    float _collective;
-    float _delta3;
-    float _dynamic;
-    float _translift;
-    float _c2;
-    float _mass;
-    float _alpha;
-    float _alphaalt;
-    float _alphamin,_alphamax,_alpha0,_alpha0factor;
-    float _rellenhinge;
-    float _relamp;
-    float _omega,_omegan;
-    float _phi;
-    float _len;
-    float _incidence;
-
-
-
-          
-    char _alphaoutputbuf[2][256];
-    int _alpha2type;
-
-};
 
 }; // namespace yasim
 #endif // _ROTORPART_HPP

src/FDM/YASim/Surface.cpp

@@ -295,6 +295,8 @@ float Surface::stallFunc(float* v)
 float Surface::flapLift(float alpha)
 {
     float flapLift = _cz * _flapPos * (_flapLift-1);
+    if(_stalls[0] == 0)
+        return 0;
 
     if(alpha < 0) alpha = -alpha;
     if(alpha < _stalls[0])