X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FFDM%2FYASim%2FRotor.cpp;h=2a93a4457f4be5e9468ddad21d4784c31f455a4d;hb=d66903e9ad63b91182ccc25d9bb82f18f8dd98b6;hp=d338ba5a71bb9cd057de8dbe9329ad2b1b6ec706;hpb=5aa142ee5f60796081e73e13089caed1224d83b7;p=flightgear.git diff --git a/src/FDM/YASim/Rotor.cpp b/src/FDM/YASim/Rotor.cpp index d338ba5a7..2a93a4457 100644 --- a/src/FDM/YASim/Rotor.cpp +++ b/src/FDM/YASim/Rotor.cpp @@ -1,15 +1,17 @@ #include #include "Math.hpp" +#include
#include "Surface.hpp" #include "Rotorpart.hpp" +#include "Glue.hpp" #include "Ground.hpp" #include "Rotor.hpp" -#include STL_IOSTREAM -#include STL_IOMANIP +#include +#include -SG_USING_STD(setprecision); +using std::setprecision; #ifdef TEST_DEBUG #include @@ -67,6 +69,7 @@ Rotor::Rotor() _normal_with_yaw_roll[2]=1; _number_of_blades=4; _omega=_omegan=_omegarel=_omegarelneu=0; + _phi_null=0; _ddt_omega=0; _pitch_a=0; _pitch_b=0; @@ -75,6 +78,7 @@ Rotor::Rotor() _no_torque=0; _rel_blade_center=.7; _rel_len_hinge=0.01; + _shared_flap_hinge=false; _rellenteeterhinge=0.01; _rotor_rpm=442; _sim_blades=0; @@ -110,7 +114,7 @@ Rotor::Rotor() _airfoil_drag_coefficient0=0; _airfoil_drag_coefficient1=0; for(i=0; i<2; i++) - _global_ground[i] = 0; + _global_ground[i] = _tilt_center[i] = 0; _global_ground[2] = 1; _global_ground[3] = -1e3; _incidence_stall_zero_speed=18*pi/180.; @@ -129,6 +133,24 @@ Rotor::Rotor() _cyclic_factor=1; _lift_factor=_f_ge=_f_vs=_f_tl=1; _rotor_correction_factor=.65; + _balance1=1; + _balance2=1; + _properties_tied=0; + _num_ground_contact_pos=0; + _directions_and_postions_dirty=true; + _tilt_yaw=0; + _tilt_roll=0; + _tilt_pitch=0; + _old_tilt_roll=0; + _old_tilt_pitch=0; + _old_tilt_yaw=0; + _min_tilt_yaw=0; + _min_tilt_pitch=0; + _min_tilt_roll=0; + _max_tilt_yaw=0; + _max_tilt_pitch=0; + _max_tilt_roll=0; + _downwash_factor=1; } Rotor::~Rotor() @@ -138,6 +160,14 @@ Rotor::~Rotor() Rotorpart* r = (Rotorpart*)_rotorparts.get(i); delete r; } + //untie the properties + if(_properties_tied) + { + SGPropertyNode * node = fgGetNode("/rotors", true)->getNode(_name,true); + node->untie("balance-ext"); + node->untie("balance-int"); + _properties_tied=0; + } } void Rotor::inititeration(float dt,float omegarel,float ddt_omegarel,float *rot) @@ -148,13 +178,16 @@ void Rotor::inititeration(float dt,float omegarel,float ddt_omegarel,float *rot) _omega=_omegan*_omegarel; _ddt_omega=_omegan*ddt_omegarel; int i; + float drot[3]; + updateDirectionsAndPositions(drot); + Math::add3(rot,drot,drot); for(i=0; i<_rotorparts.size(); i++) { - float s = Math::sin(2*pi*i/_number_of_parts); - float c = Math::cos(2*pi*i/_number_of_parts); + float s = Math::sin(float(2*pi*i/_number_of_parts+(_phi-pi/2.)*(_ccw?1:-1))); + float c = Math::cos(float(2*pi*i/_number_of_parts+(_phi-pi/2.)*(_ccw?1:-1))); Rotorpart* r = (Rotorpart*)_rotorparts.get(i); r->setOmega(_omega); r->setDdtOmega(_ddt_omega); - r->inititeration(dt,rot); + r->inititeration(dt,drot); r->setCyclic(_cyclicail*c+_cyclicele*s); } @@ -168,6 +201,13 @@ void Rotor::inititeration(float dt,float omegarel,float ddt_omegarel,float *rot) Math::mul3(Math::sin(_roll),side,help); Math::add3(_normal_with_yaw_roll,help,_normal_with_yaw_roll); + + //update balance + if ((_balance1*_balance2 < 0.97) && (_balance1>-1)) + { + _balance1-=(0.97-_balance1*_balance2)*(0.97-_balance1*_balance2)*0.005; + if (_balance1<-1) _balance1=-1; + } } float Rotor::calcStall(float incidence,float speed) @@ -231,11 +271,11 @@ int Rotor::getValueforFGSet(int j,char *text,float *f) if (j==0) { sprintf(text,"/rotors/%s/cone-deg", _name); - *f=( ((Rotorpart*)getRotorpart(0))->getrealAlpha() + *f=(_balance1>-1)?( ((Rotorpart*)getRotorpart(0))->getrealAlpha() +((Rotorpart*)getRotorpart(1*(_number_of_parts>>2)))->getrealAlpha() +((Rotorpart*)getRotorpart(2*(_number_of_parts>>2)))->getrealAlpha() +((Rotorpart*)getRotorpart(3*(_number_of_parts>>2)))->getrealAlpha() - )/4*180/pi; + )/4*180/pi:0; } else if (j==1) @@ -244,7 +284,7 @@ int Rotor::getValueforFGSet(int j,char *text,float *f) _roll = ( ((Rotorpart*)getRotorpart(0))->getrealAlpha() -((Rotorpart*)getRotorpart(2*(_number_of_parts>>2)))->getrealAlpha() )/2*(_ccw?-1:1); - *f=_roll *180/pi; + *f=(_balance1>-1)?_roll *180/pi:0; } else if (j==2) @@ -253,34 +293,34 @@ int Rotor::getValueforFGSet(int j,char *text,float *f) _yaw=( ((Rotorpart*)getRotorpart(1*(_number_of_parts>>2)))->getrealAlpha() -((Rotorpart*)getRotorpart(3*(_number_of_parts>>2)))->getrealAlpha() )/2; - *f=_yaw*180/pi; + *f=(_balance1>-1)?_yaw*180/pi:0; } else if (j==3) { sprintf(text,"/rotors/%s/rpm", _name); - *f=_omega/2/pi*60; + *f=(_balance1>-1)?_omega/2/pi*60:0; } else if (j==4) { - sprintf(text,"/rotors/%s/debug/debugfge",_name); - *f=_f_ge; + sprintf(text,"/rotors/%s/tilt/pitch-deg",_name); + *f=_tilt_pitch*180/pi; } else if (j==5) { - sprintf(text,"/rotors/%s/debug/debugfvs",_name); - *f=_f_vs; + sprintf(text,"/rotors/%s/tilt/roll-deg",_name); + *f=_tilt_roll*180/pi; } else if (j==6) { - sprintf(text,"/rotors/%s/debug/debugftl",_name); - *f=_f_tl; + sprintf(text,"/rotors/%s/tilt/yaw-deg",_name); + *f=_tilt_yaw*180/pi; } else if (j==7) { - sprintf(text,"/rotors/%s/debug/vortexstate",_name); - *f=_vortex_state; + sprintf(text,"/rotors/%s/balance", _name); + *f=_balance1; } else if (j==8) { @@ -307,6 +347,7 @@ int Rotor::getValueforFGSet(int j,char *text,float *f) +360*b/_number_of_blades*(_ccw?1:-1); if (*f>360) *f-=360; if (*f<0) *f+=360; + if (_balance1<=-1) *f=0; int k,l; float rk,rl,p; p=(*f/90); @@ -333,6 +374,16 @@ void Rotorgear::setEngineOn(int value) _engineon=value; } +void Rotorgear::setRotorEngineMaxRelTorque(float lval) +{ + _max_rel_torque=lval; +} + +void Rotorgear::setRotorRelTarget(float lval) +{ + _target_rel_rpm=lval; +} + void Rotor::setAlpha0(float f) { _alpha0=f; @@ -477,6 +528,51 @@ void Rotor::setMinCollective(float value) _min_pitch=value/180*pi; } +void Rotor::setMinTiltYaw(float value) +{ + _min_tilt_yaw=value/180*pi; +} + +void Rotor::setMinTiltPitch(float value) +{ + _min_tilt_pitch=value/180*pi; +} + +void Rotor::setMinTiltRoll(float value) +{ + _min_tilt_roll=value/180*pi; +} + +void Rotor::setMaxTiltYaw(float value) +{ + _max_tilt_yaw=value/180*pi; +} + +void Rotor::setMaxTiltPitch(float value) +{ + _max_tilt_pitch=value/180*pi; +} + +void Rotor::setMaxTiltRoll(float value) +{ + _max_tilt_roll=value/180*pi; +} + +void Rotor::setTiltCenterX(float value) +{ + _tilt_center[0] = value; +} + +void Rotor::setTiltCenterY(float value) +{ + _tilt_center[1] = value; +} + +void Rotor::setTiltCenterZ(float value) +{ + _tilt_center[2] = value; +} + void Rotor::setMaxCollective(float value) { _max_pitch=value/180*pi; @@ -522,6 +618,16 @@ void Rotor::setTranslift(float value) _translift=value; } +void Rotor::setSharedFlapHinge(bool s) +{ + _shared_flap_hinge=s; +} + +void Rotor::setBalance(float b) +{ + _balance1=b; +} + void Rotor::setC2(float value) { _c2=value; @@ -537,11 +643,21 @@ void Rotor::setRPM(float value) _rotor_rpm=value; } +void Rotor::setPhiNull(float value) +{ + _phi_null=value; +} + void Rotor::setRelLenHinge(float value) { _rel_len_hinge=value; } +void Rotor::setDownwashFactor(float value) +{ + _downwash_factor=value; +} + void Rotor::setAlphaoutput(int i, const char *text) { strncpy(_alphaoutput[i],text,255); @@ -583,7 +699,7 @@ void Rotor::setGlobalGround(double *global_ground, float* global_vel) for(i=0; i<4; i++) _global_ground[i] = global_ground[i]; } -void Rotor::setParameter(char *parametername, float value) +void Rotor::setParameter(const char *parametername, float value) { #define p(a,b) if (strcmp(parametername,#a)==0) _##a = (value * (b)); else p(translift_ve,1) @@ -631,14 +747,35 @@ void Rotorgear::setRotorBrake(float lval) _rotorbrake=lval; } +void Rotor::setTiltYaw(float lval) +{ + lval = Math::clamp(lval, -1, 1); + _tilt_yaw = _min_tilt_yaw+(lval+1)/2*(_max_tilt_yaw-_min_tilt_yaw); + _directions_and_postions_dirty = true; +} + +void Rotor::setTiltPitch(float lval) +{ + lval = Math::clamp(lval, -1, 1); + _tilt_pitch = _min_tilt_pitch+(lval+1)/2*(_max_tilt_pitch-_min_tilt_pitch); + _directions_and_postions_dirty = true; +} + +void Rotor::setTiltRoll(float lval) +{ + lval = Math::clamp(lval, -1, 1); + _tilt_roll = _min_tilt_roll+(lval+1)/2*(_max_tilt_roll-_min_tilt_roll); + _directions_and_postions_dirty = true; +} + void Rotor::setCollective(float lval) { lval = Math::clamp(lval, -1, 1); int i; + _collective=_min_pitch+(lval+1)/2*(_max_pitch-_min_pitch); for(i=0; i<_rotorparts.size(); i++) { - ((Rotorpart*)_rotorparts.get(i))->setCollective(lval); + ((Rotorpart*)_rotorparts.get(i))->setCollective(_collective); } - _collective=_min_pitch+(lval+1)/2*(_max_pitch-_min_pitch); } void Rotor::setCyclicele(float lval,float rval) @@ -654,6 +791,12 @@ void Rotor::setCyclicail(float lval,float rval) _cyclicail=-(_mincyclicail+(lval+1)/2*(_maxcyclicail-_mincyclicail)); } +void Rotor::setRotorBalance(float lval) +{ + lval = Math::clamp(lval, -1, 1); + _balance2 = lval; +} + void Rotor::getPosition(float* out) { int i; @@ -675,7 +818,7 @@ void Rotor::calcLiftFactor(float* v, float rho, State *s) _f_ge=1+_diameter/_ground_effect_altitude*_ground_effect_constant; // Now calculate translational lift - float v_vert = Math::dot3(v,_normal); + // float v_vert = Math::dot3(v,_normal); float help[3]; Math::cross3(v,_normal,help); float v_horiz = Math::mag3(help); @@ -683,6 +826,10 @@ void Rotor::calcLiftFactor(float* v, float rho, State *s) *(_translift_maxfactor-1)+1)/_translift_maxfactor; _lift_factor = _f_ge*_f_tl*_f_vs; + + //store the gravity direction + Glue::geodUp(s->pos, _grav_direction); + s->velGlobalToLocal(_grav_direction, _grav_direction); } void Rotor::findGroundEffectAltitude(Ground * ground_cb,State *s) @@ -690,8 +837,35 @@ void Rotor::findGroundEffectAltitude(Ground * ground_cb,State *s) _ground_effect_altitude=findGroundEffectAltitude(ground_cb,s, _groundeffectpos[0],_groundeffectpos[1], _groundeffectpos[2],_groundeffectpos[3]); + testForRotorGroundContact(ground_cb,s); } +void Rotor::testForRotorGroundContact(Ground * ground_cb,State *s) +{ + int i; + for (i=0;i<_num_ground_contact_pos;i++) + { + double pt[3],h; + s->posLocalToGlobal(_ground_contact_pos[i], 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); + // find h, the distance to the ground + // The ground plane transformed to the local frame. + float ground[4]; + s->planeGlobalToLocal(global_ground, ground); + + h = ground[3] - Math::dot3(_ground_contact_pos[i], ground); + // Now h is the distance from _ground_contact_pos[i] to ground + if (h<0) + { + _balance1 -= (-h)/_diameter/_num_ground_contact_pos; + _balance1 = (_balance1<-1)?-1:_balance1; + } + } +} float Rotor::findGroundEffectAltitude(Ground * ground_cb,State *s, float *pos0,float *pos1,float *pos2,float *pos3, int iteration,float a0,float a1,float a2,float a3) @@ -858,27 +1032,45 @@ void Rotor::getDownWash(float *pos, float *v_heli, float *downwash) //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); + Math::mul3(-v*_downwash_factor,_normal_with_yaw_roll,downwash); //the downwash is calculated in the opposite direction of the normal } -void Rotor::compile() +void Rotor::euler2orient(float roll, float pitch, float hdg, float* out) { - // Have we already been compiled? - if(_rotorparts.size() != 0) return; - - //rotor is divided into _number_of_parts parts - //each part is calcualted at _number_of_segments points + // the Glue::euler2orient, inverts y>2)<<2; - _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 - )); +void Rotor::updateDirectionsAndPositions(float *rot) +{ + if (!_directions_and_postions_dirty) + { + rot[0]=rot[1]=rot[2]=0; + return; + } + rot[0]=_old_tilt_roll-_tilt_roll; + rot[1]=_old_tilt_pitch-_tilt_pitch; + rot[2]=_old_tilt_yaw-_tilt_yaw; + _old_tilt_roll=_tilt_roll; + _old_tilt_pitch=_tilt_pitch; + _old_tilt_yaw=_tilt_yaw; + float orient[9]; + euler2orient(_tilt_roll, _tilt_pitch, _tilt_yaw, orient); + float forward[3]; + float normal[3]; + float base[3]; + Math::sub3(_base,_tilt_center,base); + Math::vmul33(orient, base, base); + Math::add3(base,_tilt_center,base); + Math::vmul33(orient, _forward, forward); + Math::vmul33(orient, _normal, normal); +#define _base base +#define _forward forward +#define _normal normal float directions[5][3]; //pointing forward, right, ... the 5th is ony for calculation directions[0][0]=_forward[0]; @@ -891,17 +1083,97 @@ void Rotor::compile() 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]); // now directions[0] is perpendicular to the _normal.and has a length // of 1. if _forward is already normalized and perpendicular to the // normal, directions[0] will be the same + //_num_ground_contact_pos=(_number_of_parts<16)?_number_of_parts:16; + for (i=0;i<_num_ground_contact_pos;i++) + { + float help[3]; + float s = Math::sin(pi*2*i/_num_ground_contact_pos); + float c = Math::cos(pi*2*i/_num_ground_contact_pos); + Math::mul3(c*_diameter*0.5,directions[0],_ground_contact_pos[i]); + Math::mul3(s*_diameter*0.5,directions[1],help); + Math::add3(help,_ground_contact_pos[i],_ground_contact_pos[i]); + Math::add3(_base,_ground_contact_pos[i],_ground_contact_pos[i]); + } for (i=0;i<4;i++) { Math::mul3(_diameter*0.7,directions[i],_groundeffectpos[i]); Math::add3(_base,_groundeffectpos[i],_groundeffectpos[i]); } + for (i=0;i<_number_of_parts;i++) + { + Rotorpart* rp = getRotorpart(i); + float lpos[3],lforceattac[3],lspeed[3],dirzentforce[3]; + float s = Math::sin(2*pi*i/_number_of_parts); + float c = Math::cos(2*pi*i/_number_of_parts); + float sp = Math::sin(float(2*pi*i/_number_of_parts-pi/2.+_phi)); + float cp = Math::cos(float(2*pi*i/_number_of_parts-pi/2.+_phi)); + float direction[3],nextdirection[3],help[3],direction90deg[3]; + float rotorpartmass = _weight_per_blade*_number_of_blades/_number_of_parts*.453; + 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; + + Math::mul3(c ,directions[0],help); + Math::mul3(s ,directions[1],direction); + Math::add3(help,direction,direction); + + Math::mul3(c ,directions[1],help); + Math::mul3(s ,directions[2],direction90deg); + Math::add3(help,direction90deg,direction90deg); + + Math::mul3(cp ,directions[1],help); + Math::mul3(sp ,directions[2],nextdirection); + Math::add3(help,nextdirection,nextdirection); + + Math::mul3(lentocenter,direction,lpos); + Math::add3(lpos,_base,lpos); + Math::mul3(lentoforceattac,nextdirection,lforceattac); + //nextdirection: +90deg (gyro)!!! + + Math::add3(lforceattac,_base,lforceattac); + Math::mul3(speed,direction90deg,lspeed); + Math::mul3(1,nextdirection,dirzentforce); + rp->setPosition(lpos); + rp->setNormal(_normal); + rp->setZentipetalForce(zentforce); + rp->setPositionForceAttac(lforceattac); + rp->setSpeed(lspeed); + rp->setDirectionofZentipetalforce(dirzentforce); + rp->setDirectionofRotorPart(direction); + } +#undef _base +#undef _forward +#undef _normal + _directions_and_postions_dirty=false; +} + +void Rotor::compile() +{ + // Have we already been compiled? + if(_rotorparts.size() != 0) return; + + //rotor is divided into _number_of_parts parts + //each part is calcualted at _number_of_segments points + + //clamp to 4..256 + //and make it a factor of 4 + _number_of_parts=(int(Math::clamp(_number_of_parts,4,256))>>2)<<2; + + _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 + )); + //normalize the directions + Math::unit3(_forward,_forward); + Math::unit3(_normal,_normal); + _num_ground_contact_pos=(_number_of_parts<16)?_number_of_parts:16; float rotorpartmass = _weight_per_blade*_number_of_blades/_number_of_parts*.453; //was pounds -> now kg @@ -909,7 +1181,7 @@ void Rotor::compile() * _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 lentoforceattac=_diameter*_rel_len_hinge*0.5; float zentforce=rotorpartmass*speed*speed/lentocenter; float pitchaforce=_force_at_pitch_a/_number_of_parts*.453*9.81; // was pounds of force, now N, devided by _number_of_parts @@ -919,11 +1191,15 @@ void Rotor::compile() float omega=_rotor_rpm/60*2*pi; _omegan=omega; float omega0=omega*Math::sqrt(1/(1-_rel_len_hinge)); - _delta*=pitchaforce/(_pitch_a*omega*lentocenter*2*rotorpartmass); + float delta_theoretical=pitchaforce/(_pitch_a*omega*lentocenter*2*rotorpartmass); + _delta*=delta_theoretical; - float phi=Math::atan2(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)))*_cyclic_factor; + //float relamp_theoretical=(omega*omega/(2*delta_theoretical*Math::sqrt(sqr(omega0*omega0-omega*omega) + // +4*delta_theoretical*delta_theoretical*omega*omega)))*_cyclic_factor; + _phi=Math::acos(_rel_len_hinge); + _phi-=Math::atan(_delta3); if (!_no_torque) { torque0=_power_at_pitch_0/_number_of_parts*1000/omega; @@ -940,45 +1216,19 @@ void Rotor::compile() } Rotorpart* rps[256]; + int i; for (i=0;i<_number_of_parts;i++) { - float lpos[3],lforceattac[3],lspeed[3],dirzentforce[3]; - float s = Math::sin(2*pi*i/_number_of_parts); - float c = Math::cos(2*pi*i/_number_of_parts); - float direction[3],nextdirection[3],help[3]; - Math::mul3(c ,directions[0],help); - Math::mul3(s ,directions[1],direction); - Math::add3(help,direction,direction); - - Math::mul3(c ,directions[1],help); - Math::mul3(s ,directions[2],nextdirection); - Math::add3(help,nextdirection,nextdirection); - - Math::mul3(lentocenter,direction,lpos); - Math::add3(lpos,_base,lpos); - Math::mul3(lentoforceattac,nextdirection,lforceattac); - //nextdirection: +90deg (gyro)!!! - - Math::add3(lforceattac,_base,lforceattac); - Math::mul3(speed,nextdirection,lspeed); - Math::mul3(1,nextdirection,dirzentforce); - - - 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); + Rotorpart* rp=rps[i]=newRotorpart(zentforce,pitchaforce,_delta3,rotorpartmass, + _translift,_rel_len_hinge,lentocenter); int k = i*4/_number_of_parts; rp->setAlphaoutput(_alphaoutput[k&1?k:(_ccw?k^2:k)],0); rp->setAlphaoutput(_alphaoutput[4+(k&1?k:(_ccw?k^2:k))],1+(k>1)); _rotorparts.add(rp); rp->setTorque(torquemax,torque0); rp->setRelamp(relamp); - rp->setDirectionofRotorPart(direction); rp->setTorqueOfInertia(_torque_of_inertia/_number_of_parts); + rp->setDirection(2*pi*i/_number_of_parts); } for (i=0;i<_number_of_parts;i++) { @@ -988,6 +1238,8 @@ void Rotor::compile() rps[(i-_number_of_parts/4+_number_of_parts)%_number_of_parts], rps[(i+_number_of_parts/4)%_number_of_parts]); } + float drot[3]; + updateDirectionsAndPositions(drot); for (i=0;i<_number_of_parts;i++) { rps[i]->setCompiled(); @@ -1081,9 +1333,21 @@ void Rotor::compile() rps[i]->setRelamp(relamp); } rps[0]->setOmega(0); + setCollective(0); + setCyclicail(0,0); + setCyclicele(0,0); + writeInfo(); + + //tie the properties + /* After reset these values are totally wrong. I have to find out why + SGPropertyNode * node = fgGetNode("/rotors", true)->getNode(_name,true); + node->tie("balance_ext",SGRawValuePointer(&_balance2),false); + node->tie("balance_int",SGRawValuePointer(&_balance1)); + _properties_tied=1; + */ } -std::ostream & operator<<(std::ostream & out, /*const*/ Rotor& r) +std::ostream & operator<<(std::ostream & out, Rotor& r) { #define i(x) << #x << ":" << r.x << endl #define iv(x) << #x << ":" << r.x[0] << ";" << r.x[1] << ";" <setPosition(pos); - r->setNormal(normal); - r->setZentipetalForce(zentforce); - r->setPositionForceAttac(posforceattac); - r->setSpeed(speed); - r->setDirectionofZentipetalforce(dirzentforce); - r->setMaxpitch(maxpitch); - r->setMinpitch(minpitch); - r->setMaxcyclic(maxcyclic); - r->setMincyclic(mincyclic); r->setDelta3(delta3); r->setDynamic(_dynamic); r->setTranslift(_translift); r->setC2(_c2); r->setWeight(mass); r->setRelLenHinge(rellenhinge); + r->setSharedFlapHinge(_shared_flap_hinge); r->setOmegaN(_omegan); + r->setPhi(_phi_null); r->setAlpha0(_alpha0); r->setAlphamin(_alphamin); r->setAlphamax(_alphamax); @@ -1274,13 +1528,14 @@ void Rotorgear::calcForces(float* torqueOut) total_torque+=r->getTorque()*omegan; } float max_torque_of_engine=0; + // SGPropertyNode * node=fgGetNode("/rotors/gear", true); if (_engineon) { - max_torque_of_engine=_max_power_engine; - float df=1-omegarel; + max_torque_of_engine=_max_power_engine*_max_rel_torque; + float df=_target_rel_rpm-omegarel; df/=_engine_prop_factor; df = Math::clamp(df, 0, 1); - max_torque_of_engine = df * _max_power_engine; + max_torque_of_engine = df * _max_power_engine*_max_rel_torque; } total_torque*=-1; _ddt_omegarel=0; @@ -1307,7 +1562,7 @@ void Rotorgear::calcForces(float* torqueOut) //change the rotation of the rotors if ((max_torque_of_enginetotal_torque)&&(omegarel<1)) + ||((max_torque_of_engine>total_torque)&&(omegarel<_target_rel_rpm)) //increasing rotation due to engine ||(total_torque<0) ) //increasing rotation due to autorotation { @@ -1344,7 +1599,7 @@ void Rotorgear::calcForces(float* torqueOut) for(j=0; j<_rotors.size(); j++) { Rotor* r = (Rotor*)_rotors.get(j); for(i=0; i_rotorparts.size(); i++) { - float torque_scalar=0; + // float torque_scalar=0; Rotorpart* rp = (Rotorpart*)r->_rotorparts.get(i); float torque[3]; rp->getAccelTorque(_ddt_omegarel,torque); @@ -1364,7 +1619,7 @@ void Rotorgear::addRotor(Rotor* rotor) void Rotorgear::compile() { - float wgt = 0; + // float wgt = 0; for(int j=0; j<_rotors.size(); j++) { Rotor* r = (Rotor*)_rotors.get(j); r->compile(); @@ -1420,6 +1675,8 @@ Rotorgear::Rotorgear() _ddt_omegarel=0; _engine_accel_limit=0.05f; _total_torque_on_engine=0; + _target_rel_rpm=1; + _max_rel_torque=1; } Rotorgear::~Rotorgear()