From d0550441bb16858591b1817580657d8ee1d47313 Mon Sep 17 00:00:00 2001 From: andy Date: Wed, 13 Jun 2007 21:10:23 +0000 Subject: [PATCH] Maik: Adding support for tilting of the rotor. Can be used for small autogyros or even for the Osprey. --- src/FDM/YASim/ControlMap.cpp | 3 + src/FDM/YASim/ControlMap.hpp | 1 + src/FDM/YASim/FGFDM.cpp | 12 ++ src/FDM/YASim/Rotor.cpp | 253 ++++++++++++++++++++++++++--------- src/FDM/YASim/Rotor.hpp | 30 ++++- 5 files changed, 233 insertions(+), 66 deletions(-) diff --git a/src/FDM/YASim/ControlMap.cpp b/src/FDM/YASim/ControlMap.cpp index f68a1be8f..104f67806 100644 --- a/src/FDM/YASim/ControlMap.cpp +++ b/src/FDM/YASim/ControlMap.cpp @@ -211,6 +211,9 @@ 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 TILTPITCH: ((Rotor*)obj)->setTiltPitch(lval); break; + case TILTYAW: ((Rotor*)obj)->setTiltYaw(lval); break; + case TILTROLL: ((Rotor*)obj)->setTiltRoll(lval); break; case ROTORBRAKE: ((Rotorgear*)obj)->setRotorBrake(lval); break; case ROTORENGINEON: ((Rotorgear*)obj)->setEngineOn((int)lval); break; diff --git a/src/FDM/YASim/ControlMap.hpp b/src/FDM/YASim/ControlMap.hpp index 05d2cbb67..e9235c65c 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, + TILTYAW, TILTPITCH, TILTROLL, ROTORBRAKE, ROTORENGINEMAXRELTORQUE, ROTORELTARGET, REVERSE_THRUST, WASTEGATE, WINCHRELSPEED, HITCHOPEN, PLACEWINCH, FINDAITOW}; diff --git a/src/FDM/YASim/FGFDM.cpp b/src/FDM/YASim/FGFDM.cpp index c705fecfc..53a59707e 100644 --- a/src/FDM/YASim/FGFDM.cpp +++ b/src/FDM/YASim/FGFDM.cpp @@ -719,6 +719,15 @@ Rotor* FGFDM::parseRotor(XMLAttributes* a, const char* type) w->setMaxteeterdamp(attrf(a,"maxteeterdamp",1000)); w->setRelLenTeeterHinge(attrf(a,"rellenteeterhinge",0.01)); w->setBalance(attrf(a,"balance",1.0)); + w->setMinTiltYaw(attrf(a,"mintiltyaw",0.0)); + w->setMinTiltPitch(attrf(a,"mintiltpitch",0.0)); + w->setMinTiltRoll(attrf(a,"mintiltroll",0.0)); + w->setMaxTiltYaw(attrf(a,"maxtiltyaw",0.0)); + w->setMaxTiltPitch(attrf(a,"maxtiltpitch",0.0)); + w->setMaxTiltRoll(attrf(a,"maxtiltroll",0.0)); + w->setTiltCenterX(attrf(a,"tiltcenterx",0.0)); + w->setTiltCenterY(attrf(a,"tiltcentery",0.0)); + w->setTiltCenterZ(attrf(a,"tiltcenterz",0.0)); if(attrb(a,"ccw")) w->setCcw(1); if(attrb(a,"sharedflaphinge")) @@ -947,6 +956,9 @@ 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, "TILTROLL")) return ControlMap::TILTROLL; + if(eq(name, "TILTPITCH")) return ControlMap::TILTPITCH; + if(eq(name, "TILTYAW")) return ControlMap::TILTYAW; if(eq(name, "ROTORGEARENGINEON")) return ControlMap::ROTORENGINEON; if(eq(name, "ROTORBRAKE")) return ControlMap::ROTORBRAKE; if(eq(name, "ROTORENGINEMAXRELTORQUE")) diff --git a/src/FDM/YASim/Rotor.cpp b/src/FDM/YASim/Rotor.cpp index 83a57b839..bdc3aa62c 100644 --- a/src/FDM/YASim/Rotor.cpp +++ b/src/FDM/YASim/Rotor.cpp @@ -114,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.; @@ -137,6 +137,16 @@ Rotor::Rotor() _balance2=1; _properties_tied=0; _num_ground_contact_pos=0; + _directions_and_postions_dirty=true; + _tilt_yaw=0; + _tilt_roll=0; + _tilt_pitch=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; } Rotor::~Rotor() @@ -164,6 +174,7 @@ void Rotor::inititeration(float dt,float omegarel,float ddt_omegarel,float *rot) _omega=_omegan*_omegarel; _ddt_omega=_omegan*ddt_omegarel; int i; + updateDirectionsAndPositions(); for(i=0; i<_rotorparts.size(); i++) { 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))); @@ -188,7 +199,7 @@ void Rotor::inititeration(float dt,float omegarel,float ddt_omegarel,float *rot) //update balance if ((_balance1*_balance2 < 0.97) && (_balance1>-1)) { - _balance1-=(0.97-_balance1*_balance2)*(0.97-_balance1*_balance2)*0.00001; + _balance1-=(0.97-_balance1*_balance2)*(0.97-_balance1*_balance2)*0.005; if (_balance1<-1) _balance1=-1; } } @@ -287,18 +298,18 @@ int Rotor::getValueforFGSet(int j,char *text,float *f) 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) { @@ -513,6 +524,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; @@ -682,6 +738,27 @@ 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); @@ -943,23 +1020,32 @@ void Rotor::getDownWash(float *pos, float *v_heli, float *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() +{ + if (!_directions_and_postions_dirty) + return; + 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]; @@ -972,18 +1058,17 @@ 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; + //_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*_num_ground_contact_pos/i); - float c = Math::cos(pi*2*_num_ground_contact_pos/i); + 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]); @@ -994,6 +1079,76 @@ void Rotor::compile() 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 @@ -1036,37 +1191,10 @@ 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 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]; - 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); - - Rotorpart* rp=rps[i]=newRotorpart(lpos, lforceattac, _normal, - lspeed,dirzentforce,zentforce,pitchaforce,_delta3,rotorpartmass, + 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); @@ -1074,7 +1202,6 @@ void Rotor::compile() _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); } @@ -1086,6 +1213,7 @@ void Rotor::compile() rps[(i-_number_of_parts/4+_number_of_parts)%_number_of_parts], rps[(i+_number_of_parts/4)%_number_of_parts]); } + updateDirectionsAndPositions(); for (i=0;i<_number_of_parts;i++) { rps[i]->setCompiled(); @@ -1179,6 +1307,10 @@ void Rotor::compile() rps[i]->setRelamp(relamp); } rps[0]->setOmega(0); + setCollective(0); + setCyclicail(0,0); + setCyclicele(0,0); + writeInfo(); //tie the properties @@ -1296,17 +1428,10 @@ void Rotor:: writeInfo() } #endif } -Rotorpart* Rotor::newRotorpart(float* pos, float *posforceattac, float *normal, - float* speed,float *dirzentforce, float zentforce,float maxpitchforce, +Rotorpart* Rotor::newRotorpart(float zentforce,float maxpitchforce, float delta3,float mass,float translift,float rellenhinge,float len) { Rotorpart *r = new Rotorpart(); - r->setPosition(pos); - r->setNormal(normal); - r->setZentipetalForce(zentforce); - r->setPositionForceAttac(posforceattac); - r->setSpeed(speed); - r->setDirectionofZentipetalforce(dirzentforce); r->setDelta3(delta3); r->setDynamic(_dynamic); r->setTranslift(_translift); diff --git a/src/FDM/YASim/Rotor.hpp b/src/FDM/YASim/Rotor.hpp index 1b31df0ab..8a27a465c 100644 --- a/src/FDM/YASim/Rotor.hpp +++ b/src/FDM/YASim/Rotor.hpp @@ -33,6 +33,9 @@ private: int _number_of_parts; float _balance1; float _balance2; + float _tilt_yaw; + float _tilt_roll; + float _tilt_pitch; public: Rotor(); @@ -56,6 +59,18 @@ public: void setMaxCyclicele(float value); void setMaxCollective(float value); void setMinCollective(float value); + void setMinTiltYaw(float value); + void setMinTiltPitch(float value); + void setMinTiltRoll(float value); + void setMaxTiltYaw(float value); + void setMaxTiltPitch(float value); + void setMaxTiltRoll(float value); + void setTiltCenterX(float value); + void setTiltCenterY(float value); + void setTiltCenterZ(float value); + void setTiltYaw(float lval); + void setTiltPitch(float lval); + void setTiltRoll(float lval); void setDiameter(float value); void setWeightPerBlade(float value); void setNumberOfBlades(float value); @@ -84,6 +99,7 @@ public: void setName(const char *text); void inititeration(float dt,float omegarel,float ddt_omegarel,float *rot); void compile(); + void updateDirectionsAndPositions(); void getTip(float* tip); void calcLiftFactor(float* v, float rho, State *s); void getDownWash(float *pos, float * v_heli, float *downwash); @@ -131,8 +147,10 @@ private: float findGroundEffectAltitude(Ground * ground_cb,State *s, float *pos0,float *pos1,float *pos2,float *pos3, int iteration=0,float a0=-1,float a1=-1,float a2=-1,float a3=-1); - Rotorpart* newRotorpart(float* pos, float *posforceattac, float *normal, - float* speed,float *dirzentforce, float zentforce,float maxpitchforce, + static void euler2orient(float roll, float pitch, float hdg, + float* out); + Rotorpart* newRotorpart(/*float* pos, float *posforceattac, float *normal, + float* speed,float *dirzentforce, */float zentforce,float maxpitchforce, float delta3,float mass,float translift,float rellenhinge,float len); float _base[3]; float _groundeffectpos[4][3]; @@ -146,6 +164,13 @@ private: float _diameter; float _weight_per_blade; float _rel_blade_center; + float _tilt_center[3]; + float _min_tilt_yaw; + float _min_tilt_pitch; + float _min_tilt_roll; + float _max_tilt_yaw; + float _max_tilt_pitch; + float _max_tilt_roll; float _min_pitch; float _max_pitch; float _force_at_pitch_a; @@ -210,6 +235,7 @@ private: bool _shared_flap_hinge; float _grav_direction[3]; int _properties_tied; + bool _directions_and_postions_dirty; }; std::ostream & operator<<(std::ostream & out, /*const*/ Rotor& r); -- 2.39.5