1 #include <simgear/debug/logstream.hxx>
4 #include "Rotorpart.hpp"
7 //#include <Main/fg_props.hxx>
9 const float pi=3.14159;
11 Rotorpart::Rotorpart()
17 #define set3(x,a,b,c) x[0]=a;x[1]=b;x[2]=c;
19 set3 (_directionofzentipetalforce,1,0,0);
38 _alphaoutputbuf[0][0]=0;
39 _alphaoutputbuf[1][0]=0;
61 void Rotorpart::inititeration(float dt,float *rot)
63 //printf("init %5.3f",dt*1000);
66 while (_phi>(2*pi)) _phi-=2*pi;
67 while (_phi<(0 )) _phi+=2*pi;
70 //a=skalarprdukt _normal mit rot ergibt drehung um Normale
71 //alphaalt=Math::cos(a)*alpha+.5*(Math::sin(a)*alphanachbarnlast-Math::sin(a)*alphanachbanext)
72 float a=Math::dot3(rot,_normal);
74 _alphaalt=_alpha*Math::cos(a)
75 +_nextrp->getrealAlpha()*Math::sin(a);
77 _alphaalt=_alpha*Math::cos(a)
78 +_lastrp->getrealAlpha()*Math::sin(-a);
79 //jetzt noch Drehung des Rumpfes in gleiche Richtung wie alpha bestimmen
80 //und zu _alphaalt hinzuf
\81gen
81 //alpha gibt drehung um normal cross dirofzentf an
84 Math::cross3(_directionofzentipetalforce,_normal,dir);
88 a=Math::dot3(rot,dir);
91 _alphaalt= Math::clamp(_alphaalt,_alphamin,_alphamax);
95 void Rotorpart::setTorque(float torque_max_force,float torque_no_force)
97 _torque_max_force=torque_max_force;
98 _torque_no_force=torque_no_force;
103 void Rotorpart::setWeight(float value)
107 float Rotorpart::getWeight(void)
109 return(_mass/.453); //_mass is in kg, returns pounds
112 void Rotorpart::setPosition(float* p)
115 for(i=0; i<3; i++) _pos[i] = p[i];
117 float Rotorpart::getIncidence()
122 void Rotorpart::getPosition(float* out)
125 for(i=0; i<3; i++) out[i] = _pos[i];
128 void Rotorpart::setPositionForceAttac(float* p)
131 for(i=0; i<3; i++) _posforceattac[i] = p[i];
134 void Rotorpart::getPositionForceAttac(float* out)
137 for(i=0; i<3; i++) out[i] = _posforceattac[i];
138 //printf("posforce: %5.3f %5.3f %5.3f ",out[0],out[1],out[2]);
140 void Rotorpart::setSpeed(float* p)
143 for(i=0; i<3; i++) _speed[i] = p[i];
145 void Rotorpart::setDirectionofZentipetalforce(float* p)
148 for(i=0; i<3; i++) _directionofzentipetalforce[i] = p[i];
151 void Rotorpart::setOmega(float value)
155 void Rotorpart::setOmegaN(float value)
161 void Rotorpart::setZentipetalForce(float f)
165 void Rotorpart::setMinpitch(float f)
169 void Rotorpart::setMaxpitch(float f)
173 void Rotorpart::setMaxPitchForce(float f)
177 void Rotorpart::setMaxcyclic(float f)
181 void Rotorpart::setMincyclic(float f)
185 void Rotorpart::setDelta3(float f)
189 void Rotorpart::setRelamp(float f)
193 void Rotorpart::setTranslift(float f)
197 void Rotorpart::setDynamic(float f)
201 void Rotorpart::setRelLenHinge(float f)
205 void Rotorpart::setC2(float f)
209 void Rotorpart::setAlpha0(float f)
213 void Rotorpart::setAlphamin(float f)
217 void Rotorpart::setAlphamax(float f)
221 void Rotorpart::setAlpha0factor(float f)
227 float Rotorpart::getPhi()
232 float Rotorpart::getAlpha(int i)
236 return _alpha*180/3.14;//in Grad = 1
238 if (_alpha2type==1) //yaw or roll
239 return (getAlpha(0)-_oppositerp->getAlpha(0))/2;
241 return (getAlpha(0)+_oppositerp->getAlpha(0)+
242 _nextrp->getAlpha(0)+_lastrp->getAlpha(0))/4;
245 float Rotorpart::getrealAlpha(void)
249 void Rotorpart::setAlphaoutput(char *text,int i)
251 SG_LOG(SG_FLIGHT, SG_DEBUG, "setAlphaoutput rotorpart ["
252 << text << "] typ" << i);
254 strncpy(_alphaoutputbuf[i>0],text,255);
256 if (i>0) _alpha2type=i;
260 char* Rotorpart::getAlphaoutput(int i)
262 return _alphaoutputbuf[i&1];
265 void Rotorpart::setLen(float value)
271 void Rotorpart::setNormal(float* p)
274 for(i=0; i<3; i++) _normal[i] = p[i];
277 void Rotorpart::getNormal(float* out)
280 for(i=0; i<3; i++) out[i] = _normal[i];
284 void Rotorpart::setCollective(float pos)
289 void Rotorpart::setCyclic(float pos)
294 void Rotorpart::setlastnextrp(Rotorpart*lastrp,Rotorpart*nextrp,Rotorpart *oppositerp)
298 _oppositerp=oppositerp;
301 void Rotorpart::strncpy(char *dest,const char *src,int maxlen)
304 while(src[n]&&n<(maxlen-1))
312 // Calculate the aerodynamic force given a wind vector v (in the
313 // aircraft's "local" coordinates) and an air density rho. Returns a
314 // torque about the Y axis, too.
315 void Rotorpart::calcForce(float* v, float rho, float* out, float* torque)
318 _zentipetalforce=_mass*_len*_omega*_omega;
319 float vrel[3],vreldir[3];
320 Math::sub3(_speed,v,vrel);
321 Math::unit3(vrel,vreldir);//direction of blade-movement rel. to air
322 float delta=Math::asin(Math::dot3(_normal,vreldir));//Angle of blade which would produce no vertical force
324 float cyc=_mincyclic+(_cyclic+1)/2*(_maxcyclic-_mincyclic);
325 float col=_minpitch+(_collective+1)/2*(_maxpitch-_minpitch);
326 //printf("[%5.3f]",col);
327 _incidence=(col+cyc)-_delta3*_alphaalt;
331 //float c=_maxpitchforce/(_maxpitch*_zentipetalforce);
332 float c,alpha,factor;
333 if((_omega*10)>_omegan)
335 c=_maxpitchforce/_omegan/(_maxpitch*_mass*_len*_omega);
336 alpha = c*(beta-delta)/(1+_delta3*c);
339 if (factor>1) factor=1;
344 factor=_dt*_dynamic/10;
345 if (factor>1) factor=1;
348 float vz=Math::dot3(_normal,v);
355 //fcw=vz/_c2*_maxpitchforce*_omega/_omegan;
356 fcw=vz*(_c2-1)*_maxpitchforce*_omega/(_omegan*_omegan*_len*_maxpitch);
359 Math::cross3(_normal,_directionofzentipetalforce,dirblade);
360 float vblade=Math::abs(Math::dot3(dirblade,v));
361 float tliftfactor=Math::sqrt(1+vblade*_translift);
364 alpha=_alphaalt+(alpha-_alphaalt)*factor;
365 //alpha=_alphaalt+(alpha-_lastrp->getrealAlpha())*factor;
371 //float schwenkfactor=1;// 1/Math::cos(_lastrp->getrealAlpha());
373 float meancosalpha=(1*Math::cos(_lastrp->getrealAlpha())
374 +1*Math::cos(_nextrp->getrealAlpha())
375 +1*Math::cos(_oppositerp->getrealAlpha())
376 +1*Math::cos(alpha))/4;
377 float schwenkfactor=1-(Math::cos(_lastrp->getrealAlpha())-meancosalpha);
380 //fehlt: verringerung um rellenhinge
381 float xforce = /*schwenkfactor*/ Math::cos(alpha)*_zentipetalforce;// /9.81*.453; //N->poundsofforce
382 float zforce = fcw+tliftfactor*schwenkfactor*Math::sin(alpha)*_zentipetalforce;// /9.81*.453;
384 float thetorque = _torque_no_force+_torque_max_force*Math::abs(zforce/_maxpitchforce);
386 printf("speed: %5.3f %5.3f %5.3f vwind: %5.3f %5.3f %5.3f sin %5.3f\n",
387 _speed[0],_speed[1],_speed[2],
388 v[0],v[1],v[2],Math::sin(alpha));
393 torque[i] = _normal[i]*thetorque;
394 out[i] = _normal[i]*zforce+_directionofzentipetalforce[i]*xforce;
396 //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]);
402 }; // namespace yasim