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