diff --git a/src/FDM/YASim/Airplane.cpp b/src/FDM/YASim/Airplane.cpp index 4f49c9216..30bdf59f3 100644 --- a/src/FDM/YASim/Airplane.cpp +++ b/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!!! - // Todo: calc and add mass!!! - r->compile(); - _model.addRotor(r); - - float wgt = 0; - int i; - for(i=0; inumRotorparts(); 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; inumRotorblades(); 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; + return getRotorgear()->compile(_model.getBody()); } 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; - - applyDragFactor(Math::pow(15.7/1000, 1/SOLVE_TWEAK)); - applyLiftRatio(Math::pow(104, 1/SOLVE_TWEAK)); + if (getRotorgear()!=0) + { + 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 diff --git a/src/FDM/YASim/Airplane.hpp b/src/FDM/YASim/Airplane.hpp index 1e3291545..ad8b2c13e 100644 --- a/src/FDM/YASim/Airplane.hpp +++ b/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; diff --git a/src/FDM/YASim/ControlMap.cpp b/src/FDM/YASim/ControlMap.cpp index 77f9a3fd6..798000763 100644 --- a/src/FDM/YASim/ControlMap.cpp +++ b/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); diff --git a/src/FDM/YASim/ControlMap.hpp b/src/FDM/YASim/ControlMap.hpp index 8fdd2afba..a48768f08 100644 --- a/src/FDM/YASim/ControlMap.hpp +++ b/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, diff --git a/src/FDM/YASim/FGFDM.cpp b/src/FDM/YASim/FGFDM.cpp index 0284adf25..761a86375 100644 --- a/src/FDM/YASim/FGFDM.cpp +++ b/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++) { - Rotor*r=(Rotor*)_airplane.getRotor(i); + for(i=0; i<_airplane.getRotorgear()->getNumRotors(); 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 '" diff --git a/src/FDM/YASim/Model.cpp b/src/FDM/YASim/Model.cpp index 35cf6dea7..2fcdd5ca0 100644 --- a/src/FDM/YASim/Model.cpp +++ b/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 -// initialization. Integration code should ideally go into -// calcForces. Only very "unstiff" problems can be solved well like -// this (see the engine code for an example); I don't know if rotor -// dynamics qualify or not. -// -Andy +// This function initializes some variables for the rotor calculation +// Furthermore it integrates in "void Rotorpart::inititeration +// (float dt,float *rot)" the "rotor orientation" by omega*dt for the +// 3D-visualization of the heli only. and it compensates the rotation +// of the fuselage. The rotor does not follow the rotation of the fuselage. +// 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++) { - 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); - } + _rotorgear.initRotorIteration(lrot,dt); } 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); -} -Rotorblade* Model::getRotorblade(int handle) -{ - return (Rotorblade*)_rotorblades.get(handle); -} -Rotor* Model::getRotor(int handle) -{ - return (Rotor*)_rotors.get(handle); + return &_rotorgear; } 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++) { - Rotorpart* sf = (Rotorpart*)_rotorparts.get(i); - - // Vsurf = wind - velocity + (rot cross (cg - pos)) - float vs[3], pos[3]; - sf->getPosition(pos); + for (j=0; j<_rotorgear.getRotors()->size();j++) + { + Rotor* r = (Rotor *)_rotorgear.getRotors()->get(j); + float vs[3], pos[3]; + r->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]; - sf->calcForce(vs, _rho, force, torque); - //Math::add3(faero, force, faero); + for(i=0; i_rotorparts.size(); i++) { + float torque_scalar=0; + 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); - _body.addTorque(torque); + float force[3], torque[3]; + 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++) { - 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, 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); + if (_rotorgear.isInUse()) + { + float torque[3]; + _rotorgear.calcForces(torque); + _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(dist > 0 && dist < _groundEffectSpan) { - float fz = Math::dot3(faero, ground); - fz *= (_groundEffectSpan - dist) / _groundEffectSpan; - fz *= _groundEffect; - Math::mul3(fz, ground, faero); - _body.addForce(faero); + if ((_groundEffectSpan != 0) && (_groundEffect != 0 )) + { + float dist = ground[3] - Math::dot3(ground, _wingCenter); + if(dist > 0 && dist < _groundEffectSpan) { + float fz = Math::dot3(faero, ground); + fz *= (_groundEffectSpan - dist) / _groundEffectSpan; + fz *= _groundEffect; + Math::mul3(fz, ground, faero); + _body.addForce(faero); + } } // 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 diff --git a/src/FDM/YASim/Model.hpp b/src/FDM/YASim/Model.hpp index 64f98ba26..b0a81fb86 100644 --- a/src/FDM/YASim/Model.hpp +++ b/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); - Rotorblade* getRotorblade(int handle); - Rotor* getRotor(int handle); + Rotorgear* getRotorgear(void); 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; - Vector _rotorblades; - Vector _rotors; + Rotorgear _rotorgear; Vector _gears; Hook* _hook; Launchbar* _launchbar; diff --git a/src/FDM/YASim/RigidBody.cpp b/src/FDM/YASim/RigidBody.cpp index 145cfc105..5456aa56b 100644 --- a/src/FDM/YASim/RigidBody.cpp +++ b/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) diff --git a/src/FDM/YASim/Rotor.cpp b/src/FDM/YASim/Rotor.cpp index 142ff75eb..0094b2309 100644 --- a/src/FDM/YASim/Rotor.cpp +++ b/src/FDM/YASim/Rotor.cpp @@ -3,7 +3,6 @@ #include "Math.hpp" #include "Surface.hpp" #include "Rotorpart.hpp" -#include "Rotorblade.hpp" #include "Rotor.hpp" #include STL_IOSTREAM @@ -11,7 +10,6 @@ SG_USING_STD(setprecision); -//#include #include namespace yasim { @@ -22,6 +20,7 @@ static inline float sqr(float a) { return a * a; } Rotor::Rotor() { + int i; _alpha0=-.05; _alpha0factor=1; _alphamin=-.1; @@ -41,7 +40,6 @@ Rotor::Rotor() _diameter =10; _dynamic=1; _engineon=0; - _force_at_max_pitch=0; _force_at_pitch_a=0; _forward[0]=1; _forward[1]=_forward[2]=0; @@ -55,12 +53,16 @@ Rotor::Rotor() _name[0] = 0; _normal[0] = _normal[1] = 0; _normal[2] = 1; + _normal_with_yaw_roll[0]= _normal_with_yaw_roll[1]=0; + _normal_with_yaw_roll[2]=1; _number_of_blades=4; - _omega=_omegan=_omegarel=0; + _omega=_omegan=_omegarel=_omegarelneu=0; + _ddt_omega=0; _pitch_a=0; _pitch_b=0; _power_at_pitch_0=0; _power_at_pitch_b=0; + _no_torque=0; _rel_blade_center=.7; _rel_len_hinge=0.01; _rellenteeterhinge=0.01; @@ -69,9 +71,45 @@ Rotor::Rotor() _teeterdamp=0.00001; _translift=0.05; _weight_per_blade=42; - - - + _translift_ve=20; + _translift_maxfactor=1.3; + _ground_effect_constant=0.1; + _vortex_state_lift_factor=0.4; + _vortex_state_c1=0.1; + _vortex_state_c2=0; + _vortex_state_c3=0; + _vortex_state_e1=1; + _vortex_state_e2=1; + _vortex_state_e3=1; + _lift_factor=1; + _liftcoef=0.1; + _dragcoef0=0.1; + _dragcoef1=0.1; + _twist=0; + _number_of_segments=1; + _rel_len_where_incidence_is_measured=0.7; + _torque_of_inertia=1; + _torque=0; + _chord=0.3; + _taper=1; + _airfoil_incidence_no_lift=0; + _rel_len_blade_start=0; + _airfoil_lift_coefficient=0; + _airfoil_drag_coefficient0=0; + _airfoil_drag_coefficient1=0; + for(i=0; i<2; i++) + _global_ground[i] = 0; + _global_ground[2] = 1; + _global_ground[3] = -1e3; + _incidence_stall_zero_speed=18*pi/180.; + _incidence_stall_half_sonic_speed=14*pi/180.; + _lift_factor_stall=0.28; + _stall_change_over=2*pi/180.; + _drag_factor_stall=8; + _stall_sum=1; + _stall_v2sum=1; + _collective=0; + _yaw=_roll=0; } Rotor::~Rotor() @@ -81,185 +119,199 @@ Rotor::~Rotor() Rotorpart* r = (Rotorpart*)_rotorparts.get(i); delete r; } - for(i=0; i<_rotorblades.size(); i++) { - Rotorblade* r = (Rotorblade*)_rotorblades.get(i); - delete r; - } - } -void Rotor::inititeration(float dt) +void Rotor::inititeration(float dt,float omegarel,float ddt_omegarel,float *rot) { - if ((_engineon)&&(_omegarel>=1)) return; - if ((!_engineon)&&(_omegarel<=0)) return; - _omegarel+=dt*1/30.*(_engineon?1:-1); - _omegarel=Math::clamp(_omegarel,0,1); - _omega=_omegan*_omegarel; - int i; + _stall_sum=0; + _stall_v2sum=0; + _omegarel=omegarel; + _omega=_omegan*_omegarel; + _ddt_omega=_omegan*ddt_omegarel; + int i; for(i=0; i<_rotorparts.size(); i++) { Rotorpart* r = (Rotorpart*)_rotorparts.get(i); r->setOmega(_omega); + r->setDdtOmega(_ddt_omega); + r->inititeration(dt,rot); } - for(i=0; i<_rotorblades.size(); i++) { - Rotorblade* r = (Rotorblade*)_rotorblades.get(i); - r->setOmega(_omega); - } + + //calculate the normal of the rotor disc, for calcualtion of the downwash + float side[3],help[3]; + Math::cross3(_normal,_forward,side); + Math::mul3(Math::cos(_yaw)*Math::cos(_roll),_normal,_normal_with_yaw_roll); + + Math::mul3(Math::sin(_yaw),_forward,help); + Math::add3(_normal_with_yaw_roll,help,_normal_with_yaw_roll); + + Math::mul3(Math::sin(_roll),side,help); + Math::add3(_normal_with_yaw_roll,help,_normal_with_yaw_roll); +} + +float Rotor::calcStall(float incidence,float speed) +{ + float stall_incidence=_incidence_stall_zero_speed + +(_incidence_stall_half_sonic_speed + -_incidence_stall_zero_speed)*speed/(343./2); + //missing: Temeperature dependency of sonic speed + incidence = Math::abs(incidence); + if (incidence > (90./180.*pi)) + incidence = pi-incidence; + float stall = (incidence-stall_incidence)/_stall_change_over; + stall = Math::clamp(stall,0,1); + + _stall_sum+=stall*speed*speed; + _stall_v2sum+=speed*speed; + + return stall; +} + +float Rotor::getLiftCoef(float incidence,float speed) +{ + float stall=calcStall(incidence,speed); + float c1= Math::sin(incidence-_airfoil_incidence_no_lift)*_liftcoef; + float c2= Math::sin(2*(incidence-_airfoil_incidence_no_lift)) + *_liftcoef*_lift_factor_stall; + return (1-stall)*c1 + stall *c2; +} + +float Rotor::getDragCoef(float incidence,float speed) +{ + float stall=calcStall(incidence,speed); + float c1= (Math::abs(Math::sin(incidence-_airfoil_incidence_no_lift)) + *_dragcoef1+_dragcoef0); + float c2= c1*_drag_factor_stall; + return (1-stall)*c1 + stall *c2; } int Rotor::getValueforFGSet(int j,char *text,float *f) { - if (_name[0]==0) return 0; - - - if (_sim_blades) - { - if (!numRotorblades()) return 0; - if (j==0) - { - sprintf(text,"/rotors/%s/cone", _name); - - *f=( ((Rotorblade*)getRotorblade(0))->getFlapatPos(0) - +((Rotorblade*)getRotorblade(0))->getFlapatPos(1) - +((Rotorblade*)getRotorblade(0))->getFlapatPos(2) - +((Rotorblade*)getRotorblade(0))->getFlapatPos(3) - )/4*180/pi; - - } - else - if (j==1) - { - sprintf(text,"/rotors/%s/roll", _name); - - *f=( ((Rotorblade*)getRotorblade(0))->getFlapatPos(1) - -((Rotorblade*)getRotorblade(0))->getFlapatPos(3) - )/2*180/pi; - } - else - if (j==2) - { - sprintf(text,"/rotors/%s/yaw", _name); - - *f=( ((Rotorblade*)getRotorblade(0))->getFlapatPos(2) - -((Rotorblade*)getRotorblade(0))->getFlapatPos(0) - )/2*180/pi; - } - else - if (j==3) - { - sprintf(text,"/rotors/%s/rpm", _name); - - *f=_omega/2/pi*60; - } - else - { - - int b=(j-4)/3; - - if (b>=numRotorblades()) return 0; - int w=j%3; - sprintf(text,"/rotors/%s/blade%i_%s", - _name,b+1, - w==0?"pos":(w==1?"flap":"incidence")); - if (w==0) *f=((Rotorblade*)getRotorblade(b))->getPhi()*180/pi; - else if (w==1) *f=((Rotorblade*) getRotorblade(b))->getrealAlpha()*180/pi; - else *f=((Rotorblade*)getRotorblade(b))->getIncidence()*180/pi; - } - return j+1; - } - else - { - if (4!=numRotorparts()) return 0; //compile first! - if (j==0) - { + if (_name[0]==0) return 0; + if (4!=numRotorparts()) return 0; //compile first! + if (j==0) + { sprintf(text,"/rotors/%s/cone", _name); *f=( ((Rotorpart*)getRotorpart(0))->getrealAlpha() +((Rotorpart*)getRotorpart(1))->getrealAlpha() +((Rotorpart*)getRotorpart(2))->getrealAlpha() +((Rotorpart*)getRotorpart(3))->getrealAlpha() - )/4*180/pi; - } - else - if (j==1) - { - sprintf(text,"/rotors/%s/roll", _name); - *f=( ((Rotorpart*)getRotorpart(0))->getrealAlpha() - -((Rotorpart*)getRotorpart(2))->getrealAlpha() - )/2*180/pi*(_ccw?-1:1); - } - else - if (j==2) - { - sprintf(text,"/rotors/%s/yaw", _name); - *f=( ((Rotorpart*)getRotorpart(1))->getrealAlpha() - -((Rotorpart*)getRotorpart(3))->getrealAlpha() - )/2*180/pi; - } - else - if (j==3) - { - sprintf(text,"/rotors/%s/rpm", _name); - - *f=_omega/2/pi*60; - } - else - { - int b=(j-4)/3; - if (b>=_number_of_blades) return 0; - int w=j%3; - sprintf(text,"/rotors/%s/blade%i_%s", - _name,b+1, - w==0?"pos":(w==1?"flap":"incidence")); - *f=((Rotorpart*)getRotorpart(0))->getPhi()*180/pi+360*b/_number_of_blades*(_ccw?1:-1); - if (*f>360) *f-=360; - if (*f<0) *f+=360; - int k,l; - float rk,rl,p; - p=(*f/90); - k=int(p); - l=int(p+1); - rk=l-p; - rl=1-rk; - /* - rl=sqr(Math::sin(rl*pi/2)); - rk=sqr(Math::sin(rk*pi/2)); - */ - if(w==2) {k+=2;l+=2;} - else - if(w==1) {k+=1;l+=1;} - k%=4; - l%=4; - if (w==1) *f=rk*((Rotorpart*) getRotorpart(k))->getrealAlpha()*180/pi - +rl*((Rotorpart*) getRotorpart(l))->getrealAlpha()*180/pi; - else if(w==2) *f=rk*((Rotorpart*)getRotorpart(k))->getIncidence()*180/pi - +rl*((Rotorpart*)getRotorpart(l))->getIncidence()*180/pi; - } - return j+1; - } - + )/4*180/pi; + } + else + if (j==1) + { + sprintf(text,"/rotors/%s/roll", _name); + _roll = ( ((Rotorpart*)getRotorpart(0))->getrealAlpha() + -((Rotorpart*)getRotorpart(2))->getrealAlpha() + )/2*(_ccw?-1:1); + *f=_roll *180/pi; + } + else + if (j==2) + { + sprintf(text,"/rotors/%s/yaw", _name); + _yaw=( ((Rotorpart*)getRotorpart(1))->getrealAlpha() + -((Rotorpart*)getRotorpart(3))->getrealAlpha() + )/2; + *f=_yaw*180/pi; + } + else + if (j==3) + { + sprintf(text,"/rotors/%s/rpm", _name); + *f=_omega/2/pi*60; + } + else + if (j==4) + { + sprintf(text,"/rotors/%s/debug/debugfge",_name); + *f=_f_ge; + } + else if (j==5) + { + sprintf(text,"/rotors/%s/debug/debugfvs",_name); + *f=_f_vs; + } + else if (j==6) + { + sprintf(text,"/rotors/%s/debug/debugftl",_name); + *f=_f_tl; + } + else if (j==7) + { + sprintf(text,"/rotors/%s/debug/vortexstate",_name); + *f=_vortex_state; + } + else if (j==8) + { + sprintf(text,"/rotors/%s/stall",_name); + *f=getOverallStall(); + } + else if (j==9) + { + sprintf(text,"/rotors/%s/torque",_name); + *f=-_torque;; + } + else + { + int b=(j-10)/3; + if (b>=_number_of_blades) + { + return 0; + } + int w=j%3; + sprintf(text,"/rotors/%s/blade%i_%s", + _name,b+1, + w==0?"pos":(w==1?"flap":"incidence")); + *f=((Rotorpart*)getRotorpart(0))->getPhi()*180/pi + +360*b/_number_of_blades*(_ccw?1:-1); + if (*f>360) *f-=360; + if (*f<0) *f+=360; + int k,l; + float rk,rl,p; + p=(*f/90); + k=int(p); + l=int(p+1); + rk=l-p; + rl=1-rk; + if(w==2) {k+=2;l+=2;} + else + if(w==1) {k+=1;l+=1;} + k%=4; + l%=4; + if (w==1) *f=rk*((Rotorpart*) getRotorpart(k))->getrealAlpha()*180/pi + +rl*((Rotorpart*) getRotorpart(l))->getrealAlpha()*180/pi; + else if(w==2) *f=rk*((Rotorpart*)getRotorpart(k))->getIncidence()*180/pi + +rl*((Rotorpart*)getRotorpart(l))->getIncidence()*180/pi; + } + return j+1; } -void Rotor::setEngineOn(int value) + +void Rotorgear::setEngineOn(int value) { - _engineon=value; + _engineon=value; } void Rotor::setAlpha0(float f) { _alpha0=f; } + void Rotor::setAlphamin(float f) { _alphamin=f; } + void Rotor::setAlphamax(float f) { _alphamax=f; } + void Rotor::setAlpha0factor(float f) { _alpha0factor=f; } - int Rotor::numRotorparts() { return _rotorparts.size(); @@ -269,40 +321,51 @@ Rotorpart* Rotor::getRotorpart(int n) { return ((Rotorpart*)_rotorparts.get(n)); } -int Rotor::numRotorblades() + +int Rotorgear::getEngineon() { - return _rotorblades.size(); + return _engineon; } -Rotorblade* Rotor::getRotorblade(int n) +float Rotor::getTorqueOfInertia() { - return ((Rotorblade*)_rotorblades.get(n)); + return _torque_of_inertia; +} + +void Rotor::setTorque(float f) +{ + _torque=f; +} + +void Rotor::addTorque(float f) +{ + _torque+=f; } void Rotor::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; + int n=0; + while(src[n]&&n<(maxlen-1)) + { + dest[n]=src[n]; + n++; + } + dest[n]=0; } - - void Rotor::setNormal(float* normal) { int i; float invsum,sqrsum=0; for(i=0; i<3; i++) { sqrsum+=normal[i]*normal[i];} - if (sqrsum!=0) - invsum=1/Math::sqrt(sqrsum); + invsum=1/Math::sqrt(sqrsum); else - invsum=1; - for(i=0; i<3; i++) { _normal[i] = normal[i]*invsum; } + invsum=1; + for(i=0; i<3; i++) + { + _normal_with_yaw_roll[i]=_normal[i] = normal[i]*invsum; + } } void Rotor::setForward(float* forward) @@ -310,498 +373,588 @@ void Rotor::setForward(float* forward) int i; float invsum,sqrsum=0; for(i=0; i<3; i++) { sqrsum+=forward[i]*forward[i];} - if (sqrsum!=0) - invsum=1/Math::sqrt(sqrsum); + invsum=1/Math::sqrt(sqrsum); else - invsum=1; + invsum=1; for(i=0; i<3; i++) { _forward[i] = forward[i]*invsum; } } - void Rotor::setForceAtPitchA(float force) { - _force_at_pitch_a=force; + _force_at_pitch_a=force; } + void Rotor::setPowerAtPitch0(float value) { - _power_at_pitch_0=value; + _power_at_pitch_0=value; } + void Rotor::setPowerAtPitchB(float value) { - _power_at_pitch_b=value; + _power_at_pitch_b=value; } + void Rotor::setPitchA(float value) { - _pitch_a=value/180*pi; + _pitch_a=value/180*pi; } + void Rotor::setPitchB(float value) { - _pitch_b=value/180*pi; + _pitch_b=value/180*pi; } + void Rotor::setBase(float* base) { int i; for(i=0; i<3; i++) _base[i] = base[i]; } - void Rotor::setMaxCyclicail(float value) { - _maxcyclicail=value/180*pi; + _maxcyclicail=value/180*pi; } + void Rotor::setMaxCyclicele(float value) { - _maxcyclicele=value/180*pi; + _maxcyclicele=value/180*pi; } + void Rotor::setMinCyclicail(float value) { - _mincyclicail=value/180*pi; + _mincyclicail=value/180*pi; } + void Rotor::setMinCyclicele(float value) { - _mincyclicele=value/180*pi; + _mincyclicele=value/180*pi; } + void Rotor::setMinCollective(float value) { - _min_pitch=value/180*pi; + _min_pitch=value/180*pi; } + void Rotor::setMaxCollective(float value) { - _max_pitch=value/180*pi; + _max_pitch=value/180*pi; } + void Rotor::setDiameter(float value) { - _diameter=value; + _diameter=value; } + void Rotor::setWeightPerBlade(float value) { - _weight_per_blade=value; + _weight_per_blade=value; } + void Rotor::setNumberOfBlades(float value) { - _number_of_blades=int(value+.5); + _number_of_blades=int(value+.5); } + void Rotor::setRelBladeCenter(float value) { - _rel_blade_center=value; + _rel_blade_center=value; } + void Rotor::setDynamic(float value) { - _dynamic=value; + _dynamic=value; } + void Rotor::setDelta3(float value) { - _delta3=value; + _delta3=value; } + void Rotor::setDelta(float value) { - _delta=value; + _delta=value; } + void Rotor::setTranslift(float value) { - _translift=value; + _translift=value; } + void Rotor::setC2(float value) { - _c2=value; + _c2=value; } + void Rotor::setStepspersecond(float steps) { - _stepspersecond=steps; + _stepspersecond=steps; } + void Rotor::setRPM(float value) { - _rotor_rpm=value; + _rotor_rpm=value; } + void Rotor::setRelLenHinge(float value) { - _rel_len_hinge=value; + _rel_len_hinge=value; } void Rotor::setAlphaoutput(int i, const char *text) { - //printf("SetAlphaoutput %i [%s]\n",i,text); - strncpy(_alphaoutput[i],text,255); + strncpy(_alphaoutput[i],text,255); } + void Rotor::setName(const char *text) { - strncpy(_name,text,128);//128: some space needed for settings + strncpy(_name,text,256);//256: some space needed for settings } void Rotor::setCcw(int ccw) -{ - _ccw=ccw; +{ + _ccw=ccw; } + void Rotor::setNotorque(int value) { - _no_torque=value; -} -void Rotor::setSimBlades(int value) -{ - _sim_blades=value; + _no_torque=value; } void Rotor::setRelLenTeeterHinge(float f) { - _rellenteeterhinge=f; + _rellenteeterhinge=f; } + void Rotor::setTeeterdamp(float f) { _teeterdamp=f; } + void Rotor::setMaxteeterdamp(float f) { _maxteeterdamp=f; } +void Rotor::setGlobalGround(double *global_ground, float* global_vel) +{ + int i; + for(i=0; i<4; i++) _global_ground[i] = global_ground[i]; +} +void Rotor::setParameter(char *parametername, float value) +{ +#define p(a,b) if (strcmp(parametername,#a)==0) _##a = (value * (b)); else + p(translift_ve,1) + p(translift_maxfactor,1) + p(ground_effect_constant,1) + p(vortex_state_lift_factor,1) + p(vortex_state_c1,1) + p(vortex_state_c2,1) + p(vortex_state_c3,1) + p(vortex_state_e1,1) + p(vortex_state_e2,1) + p(vortex_state_e3,1) + p(twist,pi/180.) + p(number_of_segments,1) + p(rel_len_where_incidence_is_measured,1) + p(chord,1) + p(taper,1) + p(airfoil_incidence_no_lift,pi/180.) + p(rel_len_blade_start,1) + p(incidence_stall_zero_speed,pi/180.) + p(incidence_stall_half_sonic_speed,pi/180.) + p(lift_factor_stall,1) + p(stall_change_over,pi/180.) + p(drag_factor_stall,1) + p(airfoil_lift_coefficient,1) + p(airfoil_drag_coefficient0,1) + p(airfoil_drag_coefficient1,1) + cout << "internal error in parameter set up for rotor: '" + << parametername <<"'" << endl; +#undef p +} +float Rotor::getLiftFactor() +{ + return _lift_factor; +} + +void Rotorgear::setRotorBrake(float lval) +{ + lval = Math::clamp(lval, 0, 1); + _rotorbrake=lval; +} void Rotor::setCollective(float lval) { lval = Math::clamp(lval, -1, 1); int i; - //printf("col: %5.3f\n",lval); for(i=0; i<_rotorparts.size(); i++) { ((Rotorpart*)_rotorparts.get(i))->setCollective(lval); - - } - float col=_min_pitch+(lval+1)/2*(_max_pitch-_min_pitch); - for(i=0; i<_rotorblades.size(); i++) { - ((Rotorblade*)_rotorblades.get(i))->setCollective(col); - } + _collective=_min_pitch+(lval+1)/2*(_max_pitch-_min_pitch); } + void Rotor::setCyclicele(float lval,float rval) { rval = Math::clamp(rval, -1, 1); lval = Math::clamp(lval, -1, 1); float col=_mincyclicele+(lval+1)/2*(_maxcyclicele-_mincyclicele); - int i; - for(i=0; i<_rotorblades.size(); i++) { - //((Rotorblade*)_rotorblades.get(i))->setCyclicele(col*Math::sin(((Rotorblade*)_rotorblades.get(i))->getPhi())); - ((Rotorblade*)_rotorblades.get(i))->setCyclicele(col); - } if (_rotorparts.size()!=4) return; - //printf(" ele: %5.3f %5.3f\n",lval,rval); ((Rotorpart*)_rotorparts.get(1))->setCyclic(lval); ((Rotorpart*)_rotorparts.get(3))->setCyclic(-lval); } + void Rotor::setCyclicail(float lval,float rval) { lval = Math::clamp(lval, -1, 1); rval = Math::clamp(rval, -1, 1); float col=_mincyclicail+(lval+1)/2*(_maxcyclicail-_mincyclicail); - int i; - for(i=0; i<_rotorblades.size(); i++) { - ((Rotorblade*)_rotorblades.get(i))->setCyclicail(col); - } if (_rotorparts.size()!=4) return; - //printf("ail: %5.3f %5.3f\n",lval,rval); if (_ccw) lval *=-1; ((Rotorpart*)_rotorparts.get(0))->setCyclic(-lval); ((Rotorpart*)_rotorparts.get(2))->setCyclic( lval); } +void Rotor::getPosition(float* out) +{ + int i; + for(i=0; i<3; i++) out[i] = _base[i]; +} + +void Rotor::calcLiftFactor(float* v, float rho, State *s) +{ + //calculates _lift_factor, which is a foactor for the lift of the rotor + //due to + //- ground effect (_f_ge) + //- vortex state (_f_vs) + //- translational lift (_f_tl) + _f_ge=1; + _f_tl=1; + _f_vs=1; + + // find h, the distance to the ground + // The ground plane transformed to the local frame. + float ground[4]; + s->planeGlobalToLocal(_global_ground, ground); + + float h = ground[3] - Math::dot3(_base, ground); + // Now h is the distance from the rotor center to ground + + // Calculate ground effect + _f_ge=1+_diameter/h*_ground_effect_constant; + + // Now calculate translational lift + float v_vert = Math::dot3(v,_normal); + float help[3]; + Math::cross3(v,_normal,help); + float v_horiz = Math::mag3(help); + _f_tl = ((1-Math::pow(2.7183,-v_horiz/_translift_ve)) + *(_translift_maxfactor-1)+1)/_translift_maxfactor; + + _lift_factor = _f_ge*_f_tl*_f_vs; +} float Rotor::getGroundEffect(float* posOut) { - /* - int i; - for(i=0; i<3; i++) posOut[i] = _base[i]; - float span = _length * Math::cos(_sweep) * Math::cos(_dihedral); - span = 2*(span + Math::abs(_base[2])); - */ - return _diameter; + return _diameter*0; //ground effect for rotor is calcualted not here +} + +void Rotor::getDownWash(float *pos, float *v_heli, float *downwash) +{ + float pos2rotor[3],tmp[3]; + Math::sub3(_base,pos,pos2rotor); + float dist=Math::dot3(pos2rotor,_normal_with_yaw_roll); + //calculate incidence at 0.7r; + float inc = _collective+_twist *0.7 + - _twist*_rel_len_where_incidence_is_measured; + if (inc < 0) + dist *=-1; + if (dist<0) // we are not in the downwash region + { + downwash[0]=downwash[1]=downwash[2]=0.; + return; + } + + //calculate the mean downwash speed directly beneath the rotor disk + float v1bar = Math::sin(inc) *_omega * 0.35 * _diameter * 0.8; + //0.35 * d = 0.7 *r, a good position to calcualte the mean downwashd + //0.8 the slip of the rotor. + + //calculate the time the wind needed from thr rotor to here + if (v1bar< 1) v1bar = 1; + float time=dist/v1bar; + + //calculate the pos2rotor, where the rotor was, "time" ago + Math::mul3(time,v_heli,tmp); + Math::sub3(pos2rotor,tmp,pos2rotor); + + //and again calculate dist + dist=Math::dot3(pos2rotor,_normal_with_yaw_roll); + //missing the normal is offen not pointing to the normal of the rotor + //disk. Rotate the normal by yaw and tilt angle + + if (inc < 0) + dist *=-1; + if (dist<0) // we are not in the downwash region + { + downwash[0]=downwash[1]=downwash[2]=0.; + return; + } + //of course this could be done in a runge kutta integrator, but it's such + //a approximation that I beleave, it would'nt be more realistic + + //calculate the dist to the rotor-axis + Math::cross3(pos2rotor,_normal_with_yaw_roll,tmp); + float r= Math::mag3(tmp); + //calculate incidence at r; + float rel_r = r *2 /_diameter; + float inc_r = _collective+_twist * r /_diameter * 2 + - _twist*_rel_len_where_incidence_is_measured; + + //calculate the downwash speed directly beneath the rotor disk + float v1=0; + if (rel_r<1) + v1 = Math::sin(inc_r) *_omega * r * 0.8; + + //calcualte the downwash speed in a distance "dist" to the rotor disc, + //for large dist. The speed is assumed do follow a gausian distribution + //with sigma increasing with dist^2: + //sigma is assumed to be half of the rotor diameter directly beneath the + //disc and is assumed to the rotor diameter at dist = (diameter * sqrt(2)) + + float sigma=_diameter/2 + dist * dist / _diameter /4.; + float v2 = v1bar*_diameter/ (Math::sqrt(2 * pi) * sigma) + * Math::pow(2.7183,-.5*r*r/(sigma*sigma))*_diameter/2/sigma; + + //calculate the weight of the two downwash velocities. + //Directly beneath the disc it is v1, far away it is v2 + float g = Math::pow(2.7183,-2*dist/_diameter); + //at dist = rotor radius it is assumed to be 1/e * v1 + (1-1/e)* v2 + + float v = g * v1 + (1-g) * v2; + Math::mul3(-v,_normal_with_yaw_roll,downwash); + //the downwash is calculated in the opposite direction of the normal } void Rotor::compile() { - // Have we already been compiled? - if(_rotorparts.size() != 0) return; + // Have we already been compiled? + if(_rotorparts.size() != 0) return; - //rotor is divided into 4 pointlike parts + //rotor is divided into 4 pointlike parts + + SG_LOG(SG_FLIGHT, SG_DEBUG, "debug: e " + << _mincyclicele << "..." <<_maxcyclicele << ' ' + << _mincyclicail << "..." << _maxcyclicail << ' ' + << _min_pitch << "..." << _max_pitch); - SG_LOG(SG_FLIGHT, SG_DEBUG, "debug: e " - << _mincyclicele << "..." <<_maxcyclicele << ' ' - << _mincyclicail << "..." << _maxcyclicail << ' ' - << _min_pitch << "..." << _max_pitch); - - if(!_sim_blades) - { _dynamic=_dynamic*(1/ //inverse of the time - ( (60/_rotor_rpm)/4 //for rotating 90 deg - +(60/_rotor_rpm)/(2*_number_of_blades) //+ meantime a rotorblade will pass a given point - )); - float directions[5][3];//pointing forward, right, ... the 5th is ony for calculation + ( (60/_rotor_rpm)/4 //for rotating 90 deg + +(60/_rotor_rpm)/(2*_number_of_blades) //+ meantime a rotorblade + //will pass a given point + )); + float directions[5][3]; + //pointing forward, right, ... the 5th is ony for calculation directions[0][0]=_forward[0]; directions[0][1]=_forward[1]; directions[0][2]=_forward[2]; int i; SG_LOG(SG_FLIGHT, SG_DEBUG, "Rotor rotating ccw? " << _ccw); for (i=1;i<5;i++) - { - if (!_ccw) - Math::cross3(directions[i-1],_normal,directions[i]); - else - Math::cross3(_normal,directions[i-1],directions[i]); - Math::unit3(directions[i],directions[i]); + if (!_ccw) + Math::cross3(directions[i-1],_normal,directions[i]); + else + Math::cross3(_normal,directions[i-1],directions[i]); + Math::unit3(directions[i],directions[i]); } Math::set3(directions[4],directions[0]); - float rotorpartmass = _weight_per_blade*_number_of_blades/4*.453;//was pounds -> now kg + float rotorpartmass = _weight_per_blade*_number_of_blades/4*.453; + //was pounds -> now kg + + _torque_of_inertia = 1/12. * ( 4 * rotorpartmass) * _diameter + * _diameter * _rel_blade_center * _rel_blade_center /(0.5*0.5); float speed=_rotor_rpm/60*_diameter*_rel_blade_center*pi; float lentocenter=_diameter*_rel_blade_center*0.5; float lentoforceattac=_diameter*_rel_len_hinge*0.5; float zentforce=rotorpartmass*speed*speed/lentocenter; - _force_at_max_pitch=_force_at_pitch_a/_pitch_a*_max_pitch; - float maxpitchforce=_force_at_max_pitch/4*.453*9.81;//was pounds of force, now N - float torque0=0,torquemax=0; + float pitchaforce=_force_at_pitch_a/4*.453*9.81; + //was pounds of force, now N, devided by 4 (so its now per rotorpart) + + float torque0=0,torquemax=0,torqueb=0; float omega=_rotor_rpm/60*2*pi; _omegan=omega; float omega0=omega*Math::sqrt(1/(1-_rel_len_hinge)); - //float omega0=omega*Math::sqrt((1-_rel_len_hinge)); - //_delta=omega*_delta; - _delta*=maxpitchforce/(_max_pitch*omega*lentocenter*2*rotorpartmass); + _delta*=pitchaforce/(_pitch_a*omega*lentocenter*2*rotorpartmass); float phi=Math::atan2(2*omega*_delta,omega0*omega0-omega*omega); - //float relamp=omega*omega/(2*_delta*Math::sqrt(omega0*omega0-_delta*_delta)); - float relamp=omega*omega/(2*_delta*Math::sqrt(sqr(omega0*omega0-omega*omega)+4*_delta*_delta*omega*omega)); + float relamp=omega*omega/(2*_delta*Math::sqrt(sqr(omega0*omega0-omega*omega) + +4*_delta*_delta*omega*omega)); if (!_no_torque) { - torque0=_power_at_pitch_0/4*1000/omega; - torquemax=_power_at_pitch_b/4*1000/omega/_pitch_b*_max_pitch; - - if(_ccw) - { - torque0*=-1; - torquemax*=-1; - } + torque0=_power_at_pitch_0/4*1000/omega; + // f*r=p/w ; p=f*s/t; r=s/t/w ; r*w*t = s + torqueb=_power_at_pitch_b/4*1000/omega; + torquemax=_power_at_pitch_b/4*1000/omega/_pitch_b*_max_pitch; + if(_ccw) + { + torque0*=-1; + torquemax*=-1; + torqueb*=-1; + } } SG_LOG(SG_FLIGHT, SG_DEBUG, - "spd: " << setprecision(8) << speed - << " lentoc: " << lentocenter - << " dia: " << _diameter - << " rbl: " << _rel_blade_center - << " hing: " << _rel_len_hinge - << " lfa: " << lentoforceattac); + "spd: " << setprecision(8) << speed + << " lentoc: " << lentocenter + << " dia: " << _diameter + << " rbl: " << _rel_blade_center + << " hing: " << _rel_len_hinge + << " lfa: " << lentoforceattac); + SG_LOG(SG_FLIGHT, SG_DEBUG, - "zf: " << setprecision(8) << zentforce - << " mpf: " << maxpitchforce); + "tq: " << setprecision(8) << torque0 << ".." << torquemax + << " d3: " << _delta3); SG_LOG(SG_FLIGHT, SG_DEBUG, - "tq: " << setprecision(8) << torque0 << ".." << torquemax - << " d3: " << _delta3); - SG_LOG(SG_FLIGHT, SG_DEBUG, - "o/o0: " << setprecision(8) << omega/omega0 - << " phi: " << phi*180/pi - << " relamp: " << relamp - << " delta: " <<_delta); + "o/o0: " << setprecision(8) << omega/omega0 + << " phi: " << phi*180/pi + << " relamp: " << relamp + << " delta: " <<_delta); Rotorpart* rps[4]; for (i=0;i<4;i++) { - float lpos[3],lforceattac[3],lspeed[3],dirzentforce[3]; + float lpos[3],lforceattac[3],lspeed[3],dirzentforce[3]; - Math::mul3(lentocenter,directions[i],lpos); - Math::add3(lpos,_base,lpos); - Math::mul3(lentoforceattac,directions[i+1],lforceattac);//i+1: +90deg (gyro)!!! - Math::add3(lforceattac,_base,lforceattac); - Math::mul3(speed,directions[i+1],lspeed); - Math::mul3(1,directions[i+1],dirzentforce); + Math::mul3(lentocenter,directions[i],lpos); + Math::add3(lpos,_base,lpos); + Math::mul3(lentoforceattac,directions[i+1],lforceattac); + //i+1: +90deg (gyro)!!! - float maxcyclic=(i&1)?_maxcyclicele:_maxcyclicail; - float mincyclic=(i&1)?_mincyclicele:_mincyclicail; - + Math::add3(lforceattac,_base,lforceattac); + Math::mul3(speed,directions[i+1],lspeed); + Math::mul3(1,directions[i+1],dirzentforce); - Rotorpart* rp=rps[i]=newRotorpart(lpos, lforceattac, _normal, - lspeed,dirzentforce,zentforce,maxpitchforce, _max_pitch,_min_pitch,mincyclic,maxcyclic, - _delta3,rotorpartmass,_translift,_rel_len_hinge,lentocenter); - rp->setAlphaoutput(_alphaoutput[i&1?i:(_ccw?i^2:i)],0); - rp->setAlphaoutput(_alphaoutput[4+(i&1?i:(_ccw?i^2:i))],1+(i>1)); - _rotorparts.add(rp); - rp->setTorque(torquemax,torque0); - rp->setRelamp(relamp); + float maxcyclic=(i&1)?_maxcyclicele:_maxcyclicail; + float mincyclic=(i&1)?_mincyclicele:_mincyclicail; - + Rotorpart* rp=rps[i]=newRotorpart(lpos, lforceattac, _normal, + lspeed,dirzentforce,zentforce,pitchaforce, _max_pitch,_min_pitch, + mincyclic,maxcyclic,_delta3,rotorpartmass,_translift, + _rel_len_hinge,lentocenter); + rp->setAlphaoutput(_alphaoutput[i&1?i:(_ccw?i^2:i)],0); + rp->setAlphaoutput(_alphaoutput[4+(i&1?i:(_ccw?i^2:i))],1+(i>1)); + _rotorparts.add(rp); + rp->setTorque(torquemax,torque0); + rp->setRelamp(relamp); + rp->setDirectionofRotorPart(directions[i]); + rp->setTorqueOfInertia(_torque_of_inertia/4.); } for (i=0;i<4;i++) { - - rps[i]->setlastnextrp(rps[(i+3)%4],rps[(i+1)%4],rps[(i+2)%4]); + rps[i]->setlastnextrp(rps[(i+3)%4],rps[(i+1)%4],rps[(i+2)%4]); } - } - else - { - float directions[5][3];//pointing forward, right, ... the 5th is ony for calculation - directions[0][0]=_forward[0]; - directions[0][1]=_forward[1]; - directions[0][2]=_forward[2]; - int i; - SG_LOG(SG_FLIGHT, SG_DEBUG, "Rotor rotating ccw? " <<_ccw); - for (i=1;i<5;i++) - - { - //if (!_ccw) - Math::cross3(directions[i-1],_normal,directions[i]); - //else - // Math::cross3(_normal,directions[i-1],directions[i]); - Math::unit3(directions[i],directions[i]); - } - Math::set3(directions[4],directions[0]); - float speed=_rotor_rpm/60*_diameter*_rel_blade_center*pi; - float lentocenter=_diameter*_rel_blade_center*0.5; - float lentoforceattac=_diameter*_rel_len_hinge*0.5; - float zentforce=_weight_per_blade*.453*speed*speed/lentocenter; - _force_at_max_pitch=_force_at_pitch_a/_pitch_a*_max_pitch; - float maxpitchforce=_force_at_max_pitch/_number_of_blades*.453*9.81;//was pounds of force, now N - float torque0=0,torquemax=0; - float omega=_rotor_rpm/60*2*pi; - _omegan=omega; - float omega0=omega*Math::sqrt(1/(1-_rel_len_hinge)); - //float omega0=omega*Math::sqrt(1-_rel_len_hinge); - //_delta=omega*_delta; - _delta*=maxpitchforce/(_max_pitch*omega*lentocenter*2*_weight_per_blade*.453); - float phi=Math::atan2(2*omega*_delta,omega0*omega0-omega*omega); - // float phi2=Math::abs(omega0-omega)<.000000001?pi/2:Math::atan(2*omega*_delta/(omega0*omega0-omega*omega)); - float relamp=omega*omega/(2*_delta*Math::sqrt(sqr(omega0*omega0-omega*omega)+4*_delta*_delta*omega*omega)); - if (!_no_torque) - { - torque0=_power_at_pitch_0/_number_of_blades*1000/omega; - torquemax=_power_at_pitch_b/_number_of_blades*1000/omega/_pitch_b*_max_pitch; - - if(_ccw) - { - torque0*=-1; - torquemax*=-1; - } - - } - - SG_LOG(SG_FLIGHT, SG_DEBUG, - "spd: " << setprecision(8) << speed - << " lentoc: " << lentocenter - << " dia: " << _diameter - << " rbl: " << _rel_blade_center - << " hing: " << _rel_len_hinge - << " lfa: " << lentoforceattac); - SG_LOG(SG_FLIGHT, SG_DEBUG, - "zf: " << setprecision(8) << zentforce - << " mpf: " << maxpitchforce); - SG_LOG(SG_FLIGHT, SG_DEBUG, - "tq: " << setprecision(8) << torque0 << ".." << torquemax - << " d3: " << _delta3); - SG_LOG(SG_FLIGHT, SG_DEBUG, - "o/o0: " << setprecision(8) << omega/omega0 - << " phi: " << phi*180/pi - << " relamp: " << relamp - << " delta: " <<_delta); - - // float lspeed[3]; - float dirzentforce[3]; - - float f=(!_ccw)?1:-1; - //Math::mul3(f*speed,directions[1],lspeed); - Math::mul3(f,directions[1],dirzentforce); - for (i=0;i<_number_of_blades;i++) - { - - - - - Rotorblade* rb=newRotorblade(_base,_normal,directions[0],directions[1], - lentoforceattac,_rel_len_hinge, - dirzentforce,zentforce,maxpitchforce, _max_pitch, - _delta3,_weight_per_blade*.453,_translift,2*pi/_number_of_blades*i, - omega,lentocenter,/*f* */speed); - //rp->setAlphaoutput(_alphaoutput[i&1?i:(_ccw?i^2:i)],0); - //rp->setAlphaoutput(_alphaoutput[4+(i&1?i:(_ccw?i^2:i))],1+(i>1)); - _rotorblades.add(rb); - rb->setTorque(torquemax,torque0); - rb->setDeltaPhi(pi/2.-phi); - rb->setDelta(_delta); - - rb->calcFrontRight(); - - } - /* for (i=0;i<4;i++) { - - rps[i]->setlastnextrp(rps[(i-1)%4],rps[(i+1)%4],rps[(i+2)%4]); + rps[i]->setCompiled(); } - */ + float lift[4],torque[4], v_wind[3]; + v_wind[0]=v_wind[1]=v_wind[2]=0; + rps[0]->setOmega(_omegan); - } -} + if (_airfoil_lift_coefficient==0) + { + //calculate the lift and drag coefficients now + _liftcoef=0; + _dragcoef0=1; + _dragcoef1=0; + rps[0]->calculateAlpha(v_wind,rho_null,0,0,0,&(torque[0]),&(lift[0])); + //0 degree, c0 + _liftcoef=0; + _dragcoef0=0; + _dragcoef1=1; + rps[0]->calculateAlpha(v_wind,rho_null,0,0,0,&(torque[1]),&(lift[1])); + //0 degree, c1 -Rotorblade* Rotor::newRotorblade(float* pos, float *normal, float *front, float *right, - float lforceattac,float rellenhinge, - float *dirzentforce, float zentforce,float maxpitchforce,float maxpitch, - float delta3,float mass,float translift,float phi,float omega,float len,float speed) -{ - Rotorblade *r = new Rotorblade(); - r->setPosition(pos); - r->setNormal(normal); - r->setFront(front); - r->setRight(right); - r->setMaxPitchForce(maxpitchforce); - r->setZentipetalForce(zentforce); - r->setAlpha0(_alpha0); - r->setAlphamin(_alphamin); - r->setAlphamax(_alphamax); - r->setAlpha0factor(_alpha0factor); + _liftcoef=0; + _dragcoef0=1; + _dragcoef1=0; + rps[0]->calculateAlpha(v_wind,rho_null,_pitch_b,0,0,&(torque[2]),&(lift[2])); + //picth b, c0 + _liftcoef=0; + _dragcoef0=0; + _dragcoef1=1; + rps[0]->calculateAlpha(v_wind,rho_null,_pitch_b,0,0,&(torque[3]),&(lift[3])); + //picth b, c1 + if (torque[0]==0) + { + _dragcoef1=torque0/torque[1]; + _dragcoef0=(torqueb-_dragcoef1*torque[3])/torque[2]; + } + else + { + _dragcoef1=(torque0/torque[0]-torqueb/torque[2]) + /(torque[1]/torque[0]-torque[3]/torque[2]); + _dragcoef0=(torqueb-_dragcoef1*torque[3])/torque[2]; + } - r->setSpeed(speed); - r->setDirectionofZentipetalforce(dirzentforce); - r->setMaxpitch(maxpitch); - r->setDelta3(delta3); - r->setDynamic(_dynamic); - r->setTranslift(_translift); - r->setC2(_c2); - r->setStepspersecond(_stepspersecond); - r->setWeight(mass); - r->setOmegaN(omega); - r->setPhi(phi); - r->setLforceattac(lforceattac); - r->setLen(len); - r->setLenHinge(rellenhinge); - r->setRelLenTeeterHinge(_rellenteeterhinge); - r->setTeeterdamp(_teeterdamp); - r->setMaxteeterdamp(_maxteeterdamp); + _liftcoef=1; + rps[0]->calculateAlpha(v_wind,rho_null,_pitch_a,0,0, + &(torque[0]),&(lift[0])); //max_pitch a + _liftcoef = pitchaforce/lift[0]; + } + else + { + _liftcoef=_airfoil_lift_coefficient/4*_number_of_blades; + _dragcoef0=_airfoil_drag_coefficient0/4*_number_of_blades; + _dragcoef1=_airfoil_drag_coefficient1/4*_number_of_blades; + } - /* - #define a(x) x[0],x[1],x[2] - printf("newrp: pos(%5.3f %5.3f %5.3f) pfa (%5.3f %5.3f %5.3f)\n" - " nor(%5.3f %5.3f %5.3f) spd (%5.3f %5.3f %5.3f)\n" - " dzf(%5.3f %5.3f %5.3f) zf (%5.3f) mpf (%5.3f)\n" - " pit (%5.3f..%5.3f) mcy (%5.3f..%5.3f) d3 (%5.3f)\n" - ,a(pos),a(posforceattac),a(normal), - a(speed),a(dirzentforce),zentforce,maxpitchforce,minpitch,maxpitch,mincyclic,maxcyclic, - delta3); - #undef a - */ - return r; + //Check + rps[0]->calculateAlpha(v_wind,rho_null,_pitch_a,0,0, + &(torque[0]),&(lift[0])); //pitch a + rps[0]->calculateAlpha(v_wind,rho_null,_pitch_b,0,0, + &(torque[1]),&(lift[1])); //pitch b + rps[0]->calculateAlpha(v_wind,rho_null,0,0,0, + &(torque[3]),&(lift[3])); //pitch 0 + SG_LOG(SG_FLIGHT, SG_DEBUG, + "Rotor: coefficients for airfoil:" << endl << setprecision(6) + << " drag0: " << _dragcoef0*4/_number_of_blades + << " drag1: " << _dragcoef1*4/_number_of_blades + << " lift: " << _liftcoef*4/_number_of_blades + << endl + << "at 10 deg:" << endl + << "drag: " << (Math::sin(10./180*pi)*_dragcoef1+_dragcoef0) + *4/_number_of_blades + << "lift: " << Math::sin(10./180*pi)*_liftcoef*4/_number_of_blades + << endl + << "Some results (Pitch [degree], Power [kW], Lift [N]" << endl + << 0.0f << "deg " << Math::abs(torque[3]*4*_omegan/1000) << "kW " + << lift[3] << endl + << _pitch_a*180/pi << "deg " << Math::abs(torque[0]*4*_omegan/1000) + << "kW " << lift[0] << endl + << _pitch_b*180/pi << "deg " << Math::abs(torque[1]*4*_omegan/1000) + << "kW " << lift[1] << endl << endl); + + rps[0]->setOmega(0); } Rotorpart* Rotor::newRotorpart(float* pos, float *posforceattac, float *normal, - float* speed,float *dirzentforce, float zentforce,float maxpitchforce, - float maxpitch, float minpitch, float mincyclic,float maxcyclic, - float delta3,float mass,float translift,float rellenhinge,float len) + float* speed,float *dirzentforce, float zentforce,float maxpitchforce, + float maxpitch, float minpitch, float mincyclic,float maxcyclic, + float delta3,float mass,float translift,float rellenhinge,float len) { Rotorpart *r = new Rotorpart(); r->setPosition(pos); r->setNormal(normal); - r->setMaxPitchForce(maxpitchforce); r->setZentipetalForce(zentforce); - r->setPositionForceAttac(posforceattac); - r->setSpeed(speed); r->setDirectionofZentipetalforce(dirzentforce); r->setMaxpitch(maxpitch); @@ -820,30 +973,37 @@ Rotorpart* Rotor::newRotorpart(float* pos, float *posforceattac, float *normal, r->setAlphamax(_alphamax); r->setAlpha0factor(_alpha0factor); r->setLen(len); - + r->setDiameter(_diameter); + r->setRotor(this); +#define p(a) r->setParameter(#a,_##a); + p(twist) + p(number_of_segments) + p(rel_len_where_incidence_is_measured) + p(rel_len_blade_start) +#undef p SG_LOG(SG_FLIGHT, SG_DEBUG, setprecision(8) - << "newrp: pos(" - << pos[0] << ' ' << pos[1] << ' ' << pos[2] - << ") pfa (" - << posforceattac[0] << ' ' << posforceattac[1] << ' ' - << posforceattac[2] << ')'); + << "newrp: pos(" + << pos[0] << ' ' << pos[1] << ' ' << pos[2] + << ") pfa (" + << posforceattac[0] << ' ' << posforceattac[1] << ' ' + << posforceattac[2] << ')'); SG_LOG(SG_FLIGHT, SG_DEBUG, setprecision(8) - << " nor(" - << normal[0] << ' ' << normal[1] << ' ' << normal[2] - << ") spd (" - << speed[0] << ' ' << speed[1] << ' ' << speed[2] << ')'); + << " nor(" + << normal[0] << ' ' << normal[1] << ' ' << normal[2] + << ") spd (" + << speed[0] << ' ' << speed[1] << ' ' << speed[2] << ')'); SG_LOG(SG_FLIGHT, SG_DEBUG, setprecision(8) - << " dzf(" - << dirzentforce[0] << ' ' << dirzentforce[1] << dirzentforce[2] - << ") zf (" << zentforce << ") mpf (" << maxpitchforce << ')'); - SG_LOG(SG_FLIGHT, SG_DEBUG, setprecision(8) - << " pit(" << minpitch << ".." << maxpitch - << ") mcy (" << mincyclic << ".." << maxcyclic - << ") d3 (" << delta3 << ')'); - + << " dzf(" + << dirzentforce[0] << ' ' << dirzentforce[1] << dirzentforce[2] + << ") zf (" << zentforce << ") mpf (" << maxpitchforce << ')'); + SG_LOG(SG_FLIGHT, SG_DEBUG, setprecision(8) + << " pit(" << minpitch << ".." << maxpitch + << ") mcy (" << mincyclic << ".." << maxcyclic + << ") d3 (" << delta3 << ')'); return r; } + void Rotor::interp(float* v1, float* v2, float frac, float* out) { out[0] = v1[0] + frac*(v2[0]-v1[0]); @@ -851,5 +1011,192 @@ void Rotor::interp(float* v1, float* v2, float frac, float* out) out[2] = v1[2] + frac*(v2[2]-v1[2]); } +void Rotorgear::initRotorIteration(float *lrot,float dt) +{ + int i; + float omegarel; + if (!_rotors.size()) return; + Rotor* r0 = (Rotor*)_rotors.get(0); + omegarel= r0->getOmegaRelNeu(); + for(i=0; i<_rotors.size(); i++) { + Rotor* r = (Rotor*)_rotors.get(i); + r->inititeration(dt,omegarel,0,lrot); + } +} + +void Rotorgear::calcForces(float* torqueOut) +{ + int i,j; + torqueOut[0]=torqueOut[1]=torqueOut[2]=0; + // check,getOmegaRel(); + + float total_torque_of_inertia=0; + float total_torque=0; + for(i=0; i<_rotors.size(); i++) { + Rotor* r = (Rotor*)_rotors.get(i); + omegan=r->getOmegan(); + total_torque_of_inertia+=r->getTorqueOfInertia()*omegan*omegan; + //FIXME: this is constant, so this can be done in compile + + total_torque+=r->getTorque()*omegan; + } + float max_torque_of_engine=0; + if (_engineon) + { + max_torque_of_engine=_max_power_engine; + float df=1-omegarel; + df/=_engine_prop_factor; + df = Math::clamp(df, 0, 1); + max_torque_of_engine = df * _max_power_engine; + } + total_torque*=-1; + _ddt_omegarel=0; + float rel_torque_engine=1; + if (total_torque<=0) + rel_torque_engine=0; + else + if (max_torque_of_engine>0) + rel_torque_engine=1/max_torque_of_engine*total_torque; + else + rel_torque_engine=0; + + //add the rotor brake + float dt=0.1f; + if (r0->_rotorparts.size()) dt=((Rotorpart*)r0->_rotorparts.get(0))->getDt(); + + float rotor_brake_torque; + rotor_brake_torque=_rotorbrake*_max_power_rotor_brake; + //clamp it to the value you need to stop the rotor + rotor_brake_torque=Math::clamp(rotor_brake_torque,0, + total_torque_of_inertia/dt*omegarel); + max_torque_of_engine-=rotor_brake_torque; + + //change the rotation of the rotors + if ((max_torque_of_enginetotal_torque)&&(omegarel<1)) + //increasing rotation due to engine + ||(total_torque<0) ) //increasing rotation due to autorotation + { + _ddt_omegarel=(max_torque_of_engine-total_torque)/total_torque_of_inertia; + if(max_torque_of_engine>total_torque) + { + //check if the acceleration is due to the engine. If yes, + //the engine self limits the accel. + float lim1=-total_torque/total_torque_of_inertia; + //accel. by autorotation + + if (lim1<_engine_accell_limit) lim1=_engine_accell_limit; + //if the accel by autorotation greater than the max. engine + //accel, then this is the limit, if not: the engine is the limit + if (_ddt_omegarel>lim1) _ddt_omegarel=lim1; + } + if (_ddt_omegarel>5.5)_ddt_omegarel=5.5; + //clamp it to avoid overflow. Should never be reached + if (_ddt_omegarel<-5.5)_ddt_omegarel=-5.5; + + if (_max_power_engine<0.001) {omegarel=1;_ddt_omegarel=0;} + //for debug: negative or no maxpower will result + //in permanet 100% rotation + + omegarel+=dt*_ddt_omegarel; + + if (omegarel>2.5) omegarel=2.5; + //clamp it to avoid overflow. Should never be reached + if (omegarel<-.5) omegarel=-.5; + + r0->setOmegaRelNeu(omegarel); + //calculate the torque, which is needed to accelerate the rotors. + //Add this additional torque to the body + for(j=0; j<_rotors.size(); j++) { + Rotor* r = (Rotor*)_rotors.get(j); + for(i=0; i_rotorparts.size(); i++) { + float torque_scalar=0; + Rotorpart* rp = (Rotorpart*)r->_rotorparts.get(i); + float torque[3]; + rp->getAccelTorque(_ddt_omegarel,torque); + Math::add3(torque,torqueOut,torqueOut); + } + } + } + } +} + +void Rotorgear::addRotor(Rotor* rotor) +{ + _rotors.add(rotor); + _in_use = 1; +} + +float Rotorgear::compile(RigidBody* body) +{ + float wgt = 0; + for(int j=0; j<_rotors.size(); j++) { + Rotor* r = (Rotor*)_rotors.get(j); + r->compile(); + int i; + for(i=0; inumRotorparts(); i++) { + Rotorpart* rp = (Rotorpart*)r->getRotorpart(i); + float mass = rp->getWeight(); + mass = mass * Math::sqrt(mass); + float pos[3]; + rp->getPosition(pos); + body->addMass(mass, pos); + wgt += mass; + } + } + return wgt; +} + +void Rotorgear::getDownWash(float *pos, float * v_heli, float *downwash) +{ + float tmp[3]; + downwash[0]=downwash[1]=downwash[2]=0; + for(int i=0; i<_rotors.size(); i++) { + Rotor* ro = (Rotor*)_rotors.get(i); + ro->getDownWash(pos,v_heli,tmp); + Math::add3(downwash,tmp,downwash); // + downwash + } +} + +void Rotorgear::setParameter(char *parametername, float value) +{ +#define p(a,b) if (strcmp(parametername,#a)==0) _##a = (value * (b)); else + p(max_power_engine,1000) + p(engine_prop_factor,1) + p(yasimdragfactor,1) + p(yasimliftfactor,1) + p(max_power_rotor_brake,1000) + p(engine_accell_limit,0.01) + cout << "internal error in parameter set up for rotorgear: '" + << parametername <<"'" << endl; +#undef p +} + +Rotorgear::Rotorgear() +{ + _in_use=0; + _engineon=0; + _rotorbrake=0; + _max_power_rotor_brake=1; + _max_power_engine=1000*450; + _engine_prop_factor=0.05f; + _yasimdragfactor=1; + _yasimliftfactor=1; + _ddt_omegarel=0; + _engine_accell_limit=0.05f; +} + +Rotorgear::~Rotorgear() +{ + for(int i=0; i<_rotors.size(); i++) + delete (Rotor*)_rotors.get(i); +} + }; // namespace yasim - diff --git a/src/FDM/YASim/Rotor.hpp b/src/FDM/YASim/Rotor.hpp index 2f4a992df..df8bdae87 100644 --- a/src/FDM/YASim/Rotor.hpp +++ b/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 setForward(float* forward); //the normal vector pointing forward (for ele and ail) - //void setMaxPitchForce(float force); + void setNormal(float* normal); + //the normal vector (direction of rotormast, pointing up) + + 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); - void setEngineOn(int value); - + int getCcw() {return _ccw;}; + 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); - - - - // Ground effect information, stil missing + void calcLiftFactor(float* v, float rho, State *s); + void getDownWash(float *pos, float * v_heli, float *downwash); 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 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* speed,float *dirzentforce, float zentforce,float maxpitchforce, + float maxpitch, float minpitch, float mincyclic,float maxcyclic, + float delta3,float mass,float translift,float rellenhinge,float len); 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 diff --git a/src/FDM/YASim/Rotorblade.cpp b/src/FDM/YASim/Rotorblade.cpp index d693a9cf9..e69de29bb 100644 --- a/src/FDM/YASim/Rotorblade.cpp +++ b/src/FDM/YASim/Rotorblade.cpp @@ -1,542 +0,0 @@ -#include "Math.hpp" -#include "Rotorblade.hpp" -#include -//#include -//#include
-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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 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 diff --git a/src/FDM/YASim/Rotorpart.cpp b/src/FDM/YASim/Rotorpart.cpp index 5cc302eed..708065d68 100644 --- a/src/FDM/YASim/Rotorpart.cpp +++ b/src/FDM/YASim/Rotorpart.cpp @@ -2,31 +2,34 @@ #include "Math.hpp" #include "Rotorpart.hpp" +#include "Rotor.hpp" #include -//#include -//#include
+#include 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; - _phi+=_omega*dt; - while (_phi>(2*pi)) _phi-=2*pi; - while (_phi<(0 )) _phi+=2*pi; - - //_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) + _dt=dt; + _phi+=_omega*dt; + while (_phi>(2*pi)) _phi-=2*pi; + while (_phi<(0 )) _phi+=2*pi; + float a=Math::dot3(rot,_normal); + if(a>0) _alphaalt=_alpha*Math::cos(a) - +_nextrp->getrealAlpha()*Math::sin(a); - else + +_nextrp->getrealAlpha()*Math::sin(a); + else _alphaalt=_alpha*Math::cos(a) - +_lastrp->getrealAlpha()*Math::sin(-a); - //jetzt noch Drehung des Rumpfes in gleiche Richtung wie alpha bestimmen - //und zu _alphaalt hinzufgen - //alpha gibt drehung um normal cross dirofzentf an - - float dir[3]; - Math::cross3(_directionofzentipetalforce,_normal,dir); - - - - a=Math::dot3(rot,dir); - _alphaalt -= a; - - _alphaalt= Math::clamp(_alphaalt,_alphamin,_alphamax); - + +_lastrp->getrealAlpha()*Math::sin(-a); + //calculate the rotation of the fuselage, determine + //the part in the same direction as alpha + //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); } + +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; -} -void Rotorpart::setOmegaN(float value) -{ - _omegan=value; + int i; + for(i=0; i<3; i++) _directionofrotorpart[i] = p[i]; } +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; - 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; + 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 //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 [" - << text << "] typ" << i); + SG_LOG(SG_FLIGHT, SG_DEBUG, "setAlphaoutput rotorpart [" + << 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; - _nextrp=nextrp; - _oppositerp=oppositerp; + _lastrp=lastrp; + _nextrp=nextrp; + _oppositerp=oppositerp; } void Rotorpart::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; + int n=0; + while(src[n]&&n<(maxlen-1)) + { + dest[n]=src[n]; + n++; + } + 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; - float vrel[3],vreldir[3]; - 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 + for (int i=0;i<3;i++) + torque[i]=out[i]=0; + *torque_scalar=0; + return; + } + _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 col=_minpitch+(_collective+1)/2*(_maxpitch-_minpitch); - //printf("[%5.3f]",col); - _incidence=(col+cyc)-_delta3*_alphaalt; - cyc*=_relamp; - float beta=cyc+col; - - //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); + float cyc=_mincyclic+(_cyclic+1)/2*(_maxcyclic-_mincyclic); + float col=_minpitch+(_collective+1)/2*(_maxpitch-_minpitch); + _incidence=(col+cyc)-_delta3*_alphaalt; + //the incidence of the rotorblade due to control input reduced by the + //delta3 effect, see README.YASIM + float beta=_relamp*cyc+col; + //the incidence of the rotorblade which is used for the calculation - factor=_dt*_dynamic; - if (factor>1) factor=1; - } - else - { - alpha=_alpha0; - factor=_dt*_dynamic/10; - if (factor>1) factor=1; - } + float alpha,factor; //alpha is the flapping angle + //the new flapping angle will be the old flapping angle + //+ factor *(alpha - "old flapping angle") + if((_omega*10)>_omegan) + //the rotor is rotaing quite fast. + //(at least 10% of the nominal rotational speed) + { + 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); - //alpha+=_c2*vz; - - float fcw; - if(_c2==0) - fcw=0; - else - //fcw=vz/_c2*_maxpitchforce*_omega/_omegan; - fcw=vz*(_c2-1)*_maxpitchforce*_omega/(_omegan*_omegan*_len*_maxpitch); + factor=_dt*_dynamic; + if (factor>1) factor=1; + } + else //the rotor is not rotating or rotating very slowly + { + alpha=calculateAlpha(v,rho,_incidence,cyc,alpha_alteberechnung, + &scalar_torque); + //calculate drag etc., e. g. for deccelrating the rotor if engine + //is off and omega <10% - float dirblade[3]; - Math::cross3(_normal,_directionofzentipetalforce,dirblade); - float vblade=Math::abs(Math::dot3(dirblade,v)); - float tliftfactor=Math::sqrt(1+vblade*_translift); + float rel =_omega*10 / _omegan; + alpha=rel * alpha + (1-rel)* _alpha0; + factor=_dt*_dynamic/10; + 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-_lastrp->getrealAlpha())*factor; + alpha=_alphaalt+(alpha-_alphaalt)*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); - - _alpha=alpha; - - - //float schwenkfactor=1;// 1/Math::cos(_lastrp->getrealAlpha()); - - 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); - - - //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; + //missing: consideration of rellenhinge + float xforce = Math::cos(alpha)*_zentipetalforce; + float zforce = schwenkfactor*Math::sin(alpha)*_zentipetalforce; + *torque_scalar=scalar_torque; + scalar_torque+= 0*_ddt_omega*_torque_of_inertia; + float thetorque = scalar_torque; + int i; + float f=_rotor->getCcw()?1:-1; + for(i=0; i<3; i++) { + _last_torque[i]=torque[i] = f*_normal[i]*thetorque; + out[i] = _normal[i]*zforce*_rotor->getLiftFactor() + +_directionofzentipetalforce[i]*xforce; } } - +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 diff --git a/src/FDM/YASim/Rotorpart.hpp b/src/FDM/YASim/Rotorpart.hpp index 0eaa6798f..87dee27c5 100644 --- a/src/FDM/YASim/Rotorpart.hpp +++ b/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 -{ -public: - Rotorpart(); + // Position of this surface in local coords + void setPosition(float* p); + void getPosition(float* out); + 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 - void setPosition(float* p); - void getPosition(float* out); + private: + void strncpy(char *dest,const char *src,int maxlen); + Rotorpart *_lastrp,*_nextrp,*_oppositerp; + Rotor *_rotor; - - void setPositionForceAttac(float* p); - void getPositionForceAttac(float* out); - - void setNormal(float* p); - void getNormal(float* out); - - void setMaxPitchForce(float force); - - void setCollective(float pos); - - void setCyclic(float pos); - - void setSpeed(float* p); - void setDirectionofZentipetalforce(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); - - 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); - 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; - -}; + 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 _direction_of_movement[3]; + float _directionofzentipetalforce[3]; + float _directionofrotorpart[3]; + float _zentipetalforce; + float _maxpitch; + float _minpitch; + 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,_ddt_omega; + float _phi; + float _len; + float _incidence; + float _twist; //outer incidence = inner inner incidence + _twist + int _number_of_segments; + float _rel_len_where_incidence_is_measured; + float _rel_len_blade_start; + float _rel_len_blade_measured; + float _diameter; + float _torque_of_inertia; + float _torque; + // total torque of rotor (scalar) for calculating new rotor rpm + char _alphaoutputbuf[2][256]; + int _alpha2type; + }; }; // namespace yasim #endif // _ROTORPART_HPP diff --git a/src/FDM/YASim/Surface.cpp b/src/FDM/YASim/Surface.cpp index 0f0a40877..19922c438 100644 --- a/src/FDM/YASim/Surface.cpp +++ b/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])