From 78cad450e62eace367dd95b8bcc3434b83bb4cce Mon Sep 17 00:00:00 2001 From: curt Date: Thu, 16 Oct 2003 14:40:13 +0000 Subject: [PATCH] Initial revision. Maik Justus: First pass at helicopter support for YASim. --- src/FDM/YASim/Rotor.cpp | 814 +++++++++++++++++++++++++++++++++++ src/FDM/YASim/Rotor.hpp | 146 +++++++ src/FDM/YASim/Rotorblade.cpp | 542 +++++++++++++++++++++++ src/FDM/YASim/Rotorblade.hpp | 146 +++++++ src/FDM/YASim/Rotorpart.cpp | 399 +++++++++++++++++ src/FDM/YASim/Rotorpart.hpp | 116 +++++ 6 files changed, 2163 insertions(+) create mode 100644 src/FDM/YASim/Rotor.cpp create mode 100644 src/FDM/YASim/Rotor.hpp create mode 100644 src/FDM/YASim/Rotorblade.cpp create mode 100644 src/FDM/YASim/Rotorblade.hpp create mode 100644 src/FDM/YASim/Rotorpart.cpp create mode 100644 src/FDM/YASim/Rotorpart.hpp diff --git a/src/FDM/YASim/Rotor.cpp b/src/FDM/YASim/Rotor.cpp new file mode 100644 index 000000000..a141be262 --- /dev/null +++ b/src/FDM/YASim/Rotor.cpp @@ -0,0 +1,814 @@ + #include "Math.hpp" +#include "Surface.hpp" +#include "Rotorpart.hpp" +#include "Rotorblade.hpp" +#include "Rotor.hpp" +#include +//#include +namespace yasim { + +const float pi=3.14159; + +Rotor::Rotor() +{ + _alpha0=-.05; + _alpha0factor=1; + _alphamin=-.1; + _alphamax= .1; + _alphaoutput[0][0]=0; + _alphaoutput[1][0]=0; + _alphaoutput[2][0]=0; + _alphaoutput[3][0]=0; + _alphaoutput[4][0]=0; + _alphaoutput[5][0]=0; + _alphaoutput[6][0]=0; + _alphaoutput[7][0]=0; + _base[0] = _base[1] = _base[2] = 0; + _ccw=0; + _delta=1; + _delta3=0; + _diameter =10; + _dynamic=1; + _engineon=0; + _force_at_max_pitch=0; + _force_at_pitch_a=0; + _forward[0]=1; + _forward[1]=_forward[2]=0; + _max_pitch=13./180*pi; + _maxcyclicail=10./180*pi; + _maxcyclicele=10./180*pi; + _maxteeterdamp=0; + _mincyclicail=-10./180*pi; + _mincyclicele=-10./180*pi; + _min_pitch=-.5/180*pi; + _name[0] = 0; + _normal[0] = _normal[1] = 0; + _normal[2] = 1; + _number_of_blades=4; + _omega=_omegan=_omegarel=0; + _pitch_a=0; + _pitch_b=0; + _power_at_pitch_0=0; + _power_at_pitch_b=0; + _rel_blade_center=.7; + _rel_len_hinge=0.01; + _rellenteeterhinge=0.01; + _rotor_rpm=442; + _sim_blades=0; + _teeterdamp=0.00001; + _translift=0.05; + _weight_per_blade=42; + + + +} + +Rotor::~Rotor() +{ + int i; + for(i=0; i<_rotorparts.size(); i++) { + 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) +{ + if ((_engineon)&&(_omegarel>=1)) return; + if ((!_engineon)&&(_omegarel<=0)) return; + _omegarel+=dt*1/5.*(_engineon?1:-1); //hier 30 + _omegarel=Math::clamp(_omegarel,0,1); + _omega=_omegan*_omegarel; + int i; + for(i=0; i<_rotorparts.size(); i++) { + Rotorpart* r = (Rotorpart*)_rotorparts.get(i); + r->setOmega(_omega); + } + for(i=0; i<_rotorblades.size(); i++) { + Rotorblade* r = (Rotorblade*)_rotorblades.get(i); + r->setOmega(_omega); + } +} + +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) + { + 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=Math::sqr(Math::sin(rl*pi/2)); + rk=Math::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; + } + +} +void Rotor::setEngineOn(int 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(); +} + +Rotorpart* Rotor::getRotorpart(int n) +{ + return ((Rotorpart*)_rotorparts.get(n)); +} +int Rotor::numRotorblades() +{ + return _rotorblades.size(); +} + +Rotorblade* Rotor::getRotorblade(int n) +{ + return ((Rotorblade*)_rotorblades.get(n)); +} + +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; +} + + + +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); + else + invsum=1; + for(i=0; i<3; i++) { _normal[i] = normal[i]*invsum; } +} + +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); + else + invsum=1; + for(i=0; i<3; i++) { _forward[i] = forward[i]*invsum; } +} + + +void Rotor::setForceAtPitchA(float force) +{ + _force_at_pitch_a=force; +} +void Rotor::setPowerAtPitch0(float value) +{ + _power_at_pitch_0=value; +} +void Rotor::setPowerAtPitchB(float value) +{ + _power_at_pitch_b=value; +} +void Rotor::setPitchA(float value) +{ + _pitch_a=value/180*pi; +} +void Rotor::setPitchB(float value) +{ + _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; +} +void Rotor::setMaxCyclicele(float value) +{ + _maxcyclicele=value/180*pi; +} +void Rotor::setMinCyclicail(float value) +{ + _mincyclicail=value/180*pi; +} +void Rotor::setMinCyclicele(float value) +{ + _mincyclicele=value/180*pi; +} +void Rotor::setMinCollective(float value) +{ + _min_pitch=value/180*pi; +} +void Rotor::setMaxCollective(float value) +{ + _max_pitch=value/180*pi; +} +void Rotor::setDiameter(float value) +{ + _diameter=value; +} +void Rotor::setWeightPerBlade(float value) +{ + _weight_per_blade=value; +} +void Rotor::setNumberOfBlades(float value) +{ + _number_of_blades=int(value+.5); +} +void Rotor::setRelBladeCenter(float value) +{ + _rel_blade_center=value; +} +void Rotor::setDynamic(float value) +{ + _dynamic=value; +} +void Rotor::setDelta3(float value) +{ + _delta3=value; +} +void Rotor::setDelta(float value) +{ + _delta=value; +} +void Rotor::setTranslift(float value) +{ + _translift=value; +} +void Rotor::setC2(float value) +{ + _c2=value; +} +void Rotor::setStepspersecond(float steps) +{ + _stepspersecond=steps; +} +void Rotor::setRPM(float value) +{ + _rotor_rpm=value; +} +void Rotor::setRelLenHinge(float 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); +} +void Rotor::setName(const char *text) +{ + strncpy(_name,text,128);//128: some space needed for settings +} + +void Rotor::setCcw(int ccw) +{ + _ccw=ccw; +} +void Rotor::setNotorque(int value) +{ + _no_torque=value; +} +void Rotor::setSimBlades(int value) +{ + _sim_blades=value; +} + +void Rotor::setRelLenTeeterHinge(float f) +{ + _rellenteeterhinge=f; +} +void Rotor::setTeeterdamp(float f) +{ + _teeterdamp=f; +} +void Rotor::setMaxteeterdamp(float f) +{ + _maxteeterdamp=f; +} + + + + +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); + + } +} +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); +} + + +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; +} + +void Rotor::compile() +{ + // Have we already been compiled? + if(_rotorparts.size() != 0) return; + + //rotor is divided into 4 pointlike parts + + printf("debug: e %f...%f a%f...%f %f...%f\n", + _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 + directions[0][0]=_forward[0]; + directions[0][1]=_forward[1]; + directions[0][2]=_forward[2]; + int i; + printf("Rotor rotating ccw? %i\n",_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 rotorpartmass = _weight_per_blade*_number_of_blades/4*.453;//was pounds -> now kg + 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 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); + + 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(Math::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; + } + + } + + printf("spd: %5.3f lentoc: %5.3f dia: %5.3f rbl: %5.3f hing: %5.3f lfa:%5.3f\n" + "zf: %5.3f mpf: %5.3f\n" + "tq: %5.3f..%5.3f d3:%5.3f\n" + "o/o0: %5.3f phi: %5.3f relamp: %5.3f delta:%5.3f\n" + ,speed,lentocenter,_diameter,_rel_blade_center,_rel_len_hinge, + lentoforceattac,zentforce,maxpitchforce, + torque0,torquemax,_delta3, + omega/omega0,phi*180/pi,relamp,_delta); + + Rotorpart* rps[4]; + for (i=0;i<4;i++) + { + 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); + + float maxcyclic=(i&1)?_maxcyclicele:_maxcyclicail; + float mincyclic=(i&1)?_mincyclicele:_mincyclicail; + + + 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); + + + } + for (i=0;i<4;i++) + { + + rps[i]->setlastnextrp(rps[(i-1)%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; + printf("Rotor rotating ccw? %i\n",_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(Math::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; + } + + } + + printf("spd: %5.3f lentoc: %5.3f dia: %5.3f rbl: %5.3f hing: %5.3f lfa:%5.3f\n" + "zf: %5.3f mpf: %5.3f\n" + "tq: %5.3f..%5.3f d3:%5.3f\n" + "o/o0: %5.3f phi: %5.3f relamp:%5.3f delta:%5.3f\n" + ,speed,lentocenter,_diameter,_rel_blade_center,_rel_len_hinge, + lentoforceattac,zentforce,maxpitchforce, + torque0,torquemax,_delta3, + omega/omega0,float(phi*180/pi),relamp,_delta); + + float lspeed[3],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]); + } + */ + + } +} + + +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); + + + + 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); + + /* + #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; +} + +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) +{ + 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); + 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->setOmegaN(_omegan); + r->setAlpha0(_alpha0); + r->setAlphamin(_alphamin); + r->setAlphamax(_alphamax); + r->setAlpha0factor(_alpha0factor); + r->setLen(len); + + + + #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; +} +void Rotor::interp(float* v1, float* v2, float frac, float* out) +{ + out[0] = v1[0] + frac*(v2[0]-v1[0]); + out[1] = v1[1] + frac*(v2[1]-v1[1]); + out[2] = v1[2] + frac*(v2[2]-v1[2]); +} + +}; // namespace yasim + diff --git a/src/FDM/YASim/Rotor.hpp b/src/FDM/YASim/Rotor.hpp new file mode 100644 index 000000000..2f4a992df --- /dev/null +++ b/src/FDM/YASim/Rotor.hpp @@ -0,0 +1,146 @@ +#ifndef _ROTOR_HPP +#define _ROTOR_HPP + +#include "Vector.hpp" +#include "Rotorpart.hpp" +#include "Rotorblade.hpp" +namespace yasim { + +class Surface; +class Rotorpart; + +class Rotor { +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 setForceAtPitchA(float force); + void setPowerAtPitch0(float value); + void setPowerAtPitchB(float value); + void setNotorque(int value); + void setPitchA(float value); + void setPitchB(float value); + void setMinCyclicail(float value); + void setMinCyclicele(float value); + void setMaxCyclicail(float value); + void setMaxCyclicele(float value); + void setMaxCollective(float value); + void setMinCollective(float value); + void setDiameter(float value); + void setWeightPerBlade(float value); + void setNumberOfBlades(float value); + void setRelBladeCenter(float value); + void setDelta3(float value); + void setDelta(float value); + void setDynamic(float value); + void setTranslift(float value); + void setC2(float value); + void setStepspersecond(float steps); + void setRPM(float value); + void setRelLenHinge(float value); + void setBase(float* base); // in local coordinates + 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 getValueforFGSet(int j,char *b,float *f); + void setName(const char *text); + void inititeration(float dt); + void compile(); + + void getTip(float* tip); + + + + // Ground effect information, stil missing + float getGroundEffect(float* posOut); + + // Query the list of Rotorpart objects + int numRotorparts(); + Rotorpart* getRotorpart(int n); + // Query the list of Rotorblade objects + int numRotorblades(); + Rotorblade* getRotorblade(int n); + void setAlpha0(float f); + void setAlphamin(float f); + void setAlphamax(float f); + void setTeeterdamp(float f); + void setMaxteeterdamp(float f); + void setRelLenTeeterHinge(float value); + void setAlpha0factor(float f); + +private: + void strncpy(char *dest,const char *src,int maxlen); + void interp(float* v1, float* v2, float frac, float* out); + 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 _base[3]; + + float _normal[3];//the normal vector (direction of rotormast, pointing up) + float _forward[3]; + float _diameter; + int _number_of_blades; + float _weight_per_blade; + 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; + float _power_at_pitch_b; + int _no_torque; + int _sim_blades; + float _pitch_b; + float _rotor_rpm; + float _rel_len_hinge; + float _maxcyclicail; + float _maxcyclicele; + float _mincyclicail; + float _mincyclicele; + float _delta3; + float _delta; + float _dynamic; + float _translift; + float _c2; + 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; + + + + + + +}; + +}; // namespace yasim +#endif // _ROTOR_HPP diff --git a/src/FDM/YASim/Rotorblade.cpp b/src/FDM/YASim/Rotorblade.cpp new file mode 100644 index 000000000..d693a9cf9 --- /dev/null +++ b/src/FDM/YASim/Rotorblade.cpp @@ -0,0 +1,542 @@ +#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 new file mode 100644 index 000000000..4efe6e59d --- /dev/null +++ b/src/FDM/YASim/Rotorpart.cpp @@ -0,0 +1,399 @@ +#include "Math.hpp" +#include "Rotorpart.hpp" +#include +//#include +//#include
+namespace yasim { +const float pi=3.14159; + +Rotorpart::Rotorpart() +{ + _cyclic=0; + _collective=0; + _rellenhinge=0; + _dt=0; + #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 + _zentipetalforce=1; + _maxpitch=.02; + _minpitch=0; + _maxpitchforce=10; + _maxcyclic=0.02; + _mincyclic=-0.02; + _delta3=0.5; + _cyclic=0; + _collective=0; + _relamp=1; + _mass=10; + _incidence = 0; + _alpha=0; + _alphamin=-.1; + _alphamax= .1; + _alpha0=-.05; + _alpha0factor=1; + _alphaoutputbuf[0][0]=0; + _alphaoutputbuf[1][0]=0; + _alpha2type=0; + _alphaalt=0; + _lastrp=0; + _nextrp=0; + _oppositerp=0; + _translift=0; + _dynamic=100; + _c2=0; + _torque_max_force=0; + _torque_no_force=0; + _omega=0; + _omegan=1; + _phi=0; + _len=1; + + + + +} + + +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) + _alphaalt=_alpha*Math::cos(a) + +_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); + + +} +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::setWeight(float value) +{ + _mass=value; +} +float Rotorpart::getWeight(void) +{ + return(_mass/.453); //_mass is in kg, returns pounds +} + +void Rotorpart::setPosition(float* p) +{ + int i; + for(i=0; i<3; i++) _pos[i] = p[i]; +} +float Rotorpart::getIncidence() +{ + return(_incidence); +} + +void Rotorpart::getPosition(float* out) +{ + int i; + for(i=0; i<3; i++) out[i] = _pos[i]; +} + +void Rotorpart::setPositionForceAttac(float* p) +{ + int i; + for(i=0; i<3; i++) _posforceattac[i] = p[i]; +} + +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]; +} +void Rotorpart::setDirectionofZentipetalforce(float* p) +{ + int i; + for(i=0; i<3; i++) _directionofzentipetalforce[i] = p[i]; +} + +void Rotorpart::setOmega(float value) +{ + _omega=value; +} +void Rotorpart::setOmegaN(float value) +{ + _omegan=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; +} + + +float Rotorpart::getPhi() +{ + 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; + +} +float Rotorpart::getrealAlpha(void) +{ + return _alpha; +} +void Rotorpart::setAlphaoutput(char *text,int i) +{ + printf("setAlphaoutput rotorpart [%s] typ %i\n",text,i); + + strncpy(_alphaoutputbuf[i>0],text,255); + + if (i>0) _alpha2type=i; + + +} +char* Rotorpart::getAlphaoutput(int i) +{ + return _alphaoutputbuf[i&1]; +} + +void Rotorpart::setLen(float value) +{ + _len=value; +} + + +void Rotorpart::setNormal(float* p) +{ + int i; + for(i=0; i<3; i++) _normal[i] = p[i]; +} + +void Rotorpart::getNormal(float* out) +{ + int i; + for(i=0; i<3; i++) out[i] = _normal[i]; +} + + +void Rotorpart::setCollective(float pos) +{ + _collective = pos; +} + +void Rotorpart::setCyclic(float pos) +{ + _cyclic = pos; +} + +void Rotorpart::setlastnextrp(Rotorpart*lastrp,Rotorpart*nextrp,Rotorpart *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; +} + +// 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) +{ + { + _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 + + 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); + + factor=_dt*_dynamic; + if (factor>1) factor=1; + } + else + { + alpha=_alpha0; + factor=_dt*_dynamic/10; + if (factor>1) factor=1; + } + + 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); + + 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=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; + } +} + + +}; // namespace yasim diff --git a/src/FDM/YASim/Rotorpart.hpp b/src/FDM/YASim/Rotorpart.hpp new file mode 100644 index 000000000..0eaa6798f --- /dev/null +++ b/src/FDM/YASim/Rotorpart.hpp @@ -0,0 +1,116 @@ +#ifndef _ROTORPART_HPP +#define _ROTORPART_HPP + +namespace yasim { + +class Rotorpart +{ +public: + Rotorpart(); + + // Position of this surface in local coords + void setPosition(float* p); + void getPosition(float* out); + + + 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; + +}; + +}; // namespace yasim +#endif // _ROTORPART_HPP