2 #include "Rotorpart.hpp"
5 //#include <Main/fg_props.hxx>
7 const float pi=3.14159;
15 #define set3(x,a,b,c) x[0]=a;x[1]=b;x[2]=c;
17 set3 (_directionofzentipetalforce,1,0,0);
36 _alphaoutputbuf[0][0]=0;
37 _alphaoutputbuf[1][0]=0;
59 void Rotorpart::inititeration(float dt,float *rot)
61 //printf("init %5.3f",dt*1000);
64 while (_phi>(2*pi)) _phi-=2*pi;
65 while (_phi<(0 )) _phi+=2*pi;
68 //a=skalarprdukt _normal mit rot ergibt drehung um Normale
69 //alphaalt=Math::cos(a)*alpha+.5*(Math::sin(a)*alphanachbarnlast-Math::sin(a)*alphanachbanext)
70 float a=Math::dot3(rot,_normal);
72 _alphaalt=_alpha*Math::cos(a)
73 +_nextrp->getrealAlpha()*Math::sin(a);
75 _alphaalt=_alpha*Math::cos(a)
76 +_lastrp->getrealAlpha()*Math::sin(-a);
77 //jetzt noch Drehung des Rumpfes in gleiche Richtung wie alpha bestimmen
78 //und zu _alphaalt hinzuf
\81gen
79 //alpha gibt drehung um normal cross dirofzentf an
82 Math::cross3(_directionofzentipetalforce,_normal,dir);
86 a=Math::dot3(rot,dir);
89 _alphaalt= Math::clamp(_alphaalt,_alphamin,_alphamax);
93 void Rotorpart::setTorque(float torque_max_force,float torque_no_force)
95 _torque_max_force=torque_max_force;
96 _torque_no_force=torque_no_force;
101 void Rotorpart::setWeight(float value)
105 float Rotorpart::getWeight(void)
107 return(_mass/.453); //_mass is in kg, returns pounds
110 void Rotorpart::setPosition(float* p)
113 for(i=0; i<3; i++) _pos[i] = p[i];
115 float Rotorpart::getIncidence()
120 void Rotorpart::getPosition(float* out)
123 for(i=0; i<3; i++) out[i] = _pos[i];
126 void Rotorpart::setPositionForceAttac(float* p)
129 for(i=0; i<3; i++) _posforceattac[i] = p[i];
132 void Rotorpart::getPositionForceAttac(float* out)
135 for(i=0; i<3; i++) out[i] = _posforceattac[i];
136 //printf("posforce: %5.3f %5.3f %5.3f ",out[0],out[1],out[2]);
138 void Rotorpart::setSpeed(float* p)
141 for(i=0; i<3; i++) _speed[i] = p[i];
143 void Rotorpart::setDirectionofZentipetalforce(float* p)
146 for(i=0; i<3; i++) _directionofzentipetalforce[i] = p[i];
149 void Rotorpart::setOmega(float value)
153 void Rotorpart::setOmegaN(float value)
159 void Rotorpart::setZentipetalForce(float f)
163 void Rotorpart::setMinpitch(float f)
167 void Rotorpart::setMaxpitch(float f)
171 void Rotorpart::setMaxPitchForce(float f)
175 void Rotorpart::setMaxcyclic(float f)
179 void Rotorpart::setMincyclic(float f)
183 void Rotorpart::setDelta3(float f)
187 void Rotorpart::setRelamp(float f)
191 void Rotorpart::setTranslift(float f)
195 void Rotorpart::setDynamic(float f)
199 void Rotorpart::setRelLenHinge(float f)
203 void Rotorpart::setC2(float f)
207 void Rotorpart::setAlpha0(float f)
211 void Rotorpart::setAlphamin(float f)
215 void Rotorpart::setAlphamax(float f)
219 void Rotorpart::setAlpha0factor(float f)
225 float Rotorpart::getPhi()
230 float Rotorpart::getAlpha(int i)
234 return _alpha*180/3.14;//in Grad = 1
236 if (_alpha2type==1) //yaw or roll
237 return (getAlpha(0)-_oppositerp->getAlpha(0))/2;
239 return (getAlpha(0)+_oppositerp->getAlpha(0)+
240 _nextrp->getAlpha(0)+_lastrp->getAlpha(0))/4;
243 float Rotorpart::getrealAlpha(void)
247 void Rotorpart::setAlphaoutput(char *text,int i)
249 printf("setAlphaoutput rotorpart [%s] typ %i\n",text,i);
251 strncpy(_alphaoutputbuf[i>0],text,255);
253 if (i>0) _alpha2type=i;
257 char* Rotorpart::getAlphaoutput(int i)
259 return _alphaoutputbuf[i&1];
262 void Rotorpart::setLen(float value)
268 void Rotorpart::setNormal(float* p)
271 for(i=0; i<3; i++) _normal[i] = p[i];
274 void Rotorpart::getNormal(float* out)
277 for(i=0; i<3; i++) out[i] = _normal[i];
281 void Rotorpart::setCollective(float pos)
286 void Rotorpart::setCyclic(float pos)
291 void Rotorpart::setlastnextrp(Rotorpart*lastrp,Rotorpart*nextrp,Rotorpart *oppositerp)
295 _oppositerp=oppositerp;
298 void Rotorpart::strncpy(char *dest,const char *src,int maxlen)
301 while(src[n]&&n<(maxlen-1))
309 // Calculate the aerodynamic force given a wind vector v (in the
310 // aircraft's "local" coordinates) and an air density rho. Returns a
311 // torque about the Y axis, too.
312 void Rotorpart::calcForce(float* v, float rho, float* out, float* torque)
315 _zentipetalforce=_mass*_len*_omega*_omega;
316 float vrel[3],vreldir[3];
317 Math::sub3(_speed,v,vrel);
318 Math::unit3(vrel,vreldir);//direction of blade-movement rel. to air
319 float delta=Math::asin(Math::dot3(_normal,vreldir));//Angle of blade which would produce no vertical force
321 float cyc=_mincyclic+(_cyclic+1)/2*(_maxcyclic-_mincyclic);
322 float col=_minpitch+(_collective+1)/2*(_maxpitch-_minpitch);
323 //printf("[%5.3f]",col);
324 _incidence=(col+cyc)-_delta3*_alphaalt;
328 //float c=_maxpitchforce/(_maxpitch*_zentipetalforce);
329 float c,alpha,factor;
330 if((_omega*10)>_omegan)
332 c=_maxpitchforce/_omegan/(_maxpitch*_mass*_len*_omega);
333 alpha = c*(beta-delta)/(1+_delta3*c);
336 if (factor>1) factor=1;
341 factor=_dt*_dynamic/10;
342 if (factor>1) factor=1;
345 float vz=Math::dot3(_normal,v);
352 //fcw=vz/_c2*_maxpitchforce*_omega/_omegan;
353 fcw=vz*(_c2-1)*_maxpitchforce*_omega/(_omegan*_omegan*_len*_maxpitch);
356 Math::cross3(_normal,_directionofzentipetalforce,dirblade);
357 float vblade=Math::abs(Math::dot3(dirblade,v));
358 float tliftfactor=Math::sqrt(1+vblade*_translift);
361 alpha=_alphaalt+(alpha-_alphaalt)*factor;
362 //alpha=_alphaalt+(alpha-_lastrp->getrealAlpha())*factor;
368 //float schwenkfactor=1;// 1/Math::cos(_lastrp->getrealAlpha());
370 float meancosalpha=(1*Math::cos(_lastrp->getrealAlpha())
371 +1*Math::cos(_nextrp->getrealAlpha())
372 +1*Math::cos(_oppositerp->getrealAlpha())
373 +1*Math::cos(alpha))/4;
374 float schwenkfactor=1-(Math::cos(_lastrp->getrealAlpha())-meancosalpha);
377 //fehlt: verringerung um rellenhinge
378 float xforce = /*schwenkfactor*/ Math::cos(alpha)*_zentipetalforce;// /9.81*.453; //N->poundsofforce
379 float zforce = fcw+tliftfactor*schwenkfactor*Math::sin(alpha)*_zentipetalforce;// /9.81*.453;
381 float thetorque = _torque_no_force+_torque_max_force*Math::abs(zforce/_maxpitchforce);
383 printf("speed: %5.3f %5.3f %5.3f vwind: %5.3f %5.3f %5.3f sin %5.3f\n",
384 _speed[0],_speed[1],_speed[2],
385 v[0],v[1],v[2],Math::sin(alpha));
390 torque[i] = _normal[i]*thetorque;
391 out[i] = _normal[i]*zforce+_directionofzentipetalforce[i]*xforce;
393 //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]);
399 }; // namespace yasim