2 #include "Rotorblade.hpp"
5 //#include <Main/fg_props.hxx>
7 const float pi=3.14159;
9 Rotorblade::Rotorblade()
12 _orient[0] = 1; _orient[1] = 0; _orient[2] = 0;
13 _orient[3] = 0; _orient[4] = 1; _orient[5] = 0;
14 _orient[6] = 0; _orient[7] = 0; _orient[8] = 1;
19 #define set3(x,a,b,c) x[0]=a;x[1]=b;x[2]=c;
20 set3 (_directionofzentipetalforce,1,0,0);
29 _flapatpos[0]=_flapatpos[1]=_flapatpos[2]=_flapatpos[3]=0;
36 _calcforcesdone=false;
42 _alphaoutputbuf[0][0]=0;
43 _alphaoutputbuf[1][0]=0;
66 _rellenteeterhinge=0.01;
75 void Rotorblade::inititeration(float dt,float *rot)
77 //printf("init %5.3f",dt*1000);
79 _calcforcesdone=false;
80 float a=Math::dot3(rot,_normal);
83 while (_phi>(2*pi)) _phi-=2*pi;
84 while (_phi<(0 )) _phi+=2*pi;
85 //jetzt noch Drehung des Rumpfes in gleiche Richtung wie alpha bestimmen
86 //und zu _alphaalt hinzuf
\81gen
87 //alpha gibt drehung um normal cross dirofzentf an
90 Math::cross3(_lright,_normal,dir);
94 a=-Math::dot3(rot,dir);
95 float alphaneu= _alpha+a;
96 // alphaneu= Math::clamp(alphaneu,-.5,.5);
97 //_alphaomega=(alphaneu-_alphaalt)/_dt;//now calculated in calcforces
104 void Rotorblade::setTorque(float torque_max_force,float torque_no_force)
106 _torque_max_force=torque_max_force;
107 _torque_no_force=torque_no_force;
109 void Rotorblade::setAlpha0(float f)
113 void Rotorblade::setAlphamin(float f)
117 void Rotorblade::setAlphamax(float f)
121 void Rotorblade::setAlpha0factor(float f)
129 void Rotorblade::setWeight(float value)
133 float Rotorblade::getWeight(void)
135 return(_mass/.453); //_mass is in kg, returns pounds
138 void Rotorblade::setPosition(float* p)
141 for(i=0; i<3; i++) _pos[i] = p[i];
144 void Rotorblade::calcFrontRight()
146 float tmpcf[3],tmpsr[3],tmpsf[3],tmpcr[3];
147 Math::mul3(Math::cos(_phi),_right,tmpcr);
148 Math::mul3(Math::cos(_phi),_front,tmpcf);
149 Math::mul3(Math::sin(_phi),_right,tmpsr);
150 Math::mul3(Math::sin(_phi),_front,tmpsf);
152 Math::add3(tmpcf,tmpsr,_lfront);
153 Math::sub3(tmpcr,tmpsf,_lright);
157 void Rotorblade::getPosition(float* out)
160 Math::mul3(_len,_lfront,dir);
161 Math::add3(_pos,dir,out);
164 void Rotorblade::setPositionForceAttac(float* p)
167 for(i=0; i<3; i++) _posforceattac[i] = p[i];
170 void Rotorblade::getPositionForceAttac(float* out)
173 Math::mul3(_len*_rellenhinge*2,_lfront,dir);
174 Math::add3(_pos,dir,out);
176 void Rotorblade::setSpeed(float p)
180 void Rotorblade::setDirectionofZentipetalforce(float* p)
183 for(i=0; i<3; i++) _directionofzentipetalforce[i] = p[i];
186 void Rotorblade::setZentipetalForce(float f)
190 void Rotorblade::setMaxpitch(float f)
194 void Rotorblade::setMaxPitchForce(float f)
198 void Rotorblade::setDelta(float f)
202 void Rotorblade::setDeltaPhi(float f)
206 void Rotorblade::setDelta3(float f)
210 void Rotorblade::setTranslift(float f)
214 void Rotorblade::setDynamic(float f)
218 void Rotorblade::setC2(float f)
222 void Rotorblade::setStepspersecond(float steps)
224 _stepspersecond=steps;
226 void Rotorblade::setRelLenTeeterHinge(float f)
228 _rellenteeterhinge=f;
231 void Rotorblade::setTeeterdamp(float f)
235 void Rotorblade::setMaxteeterdamp(float f)
239 float Rotorblade::getAlpha(int i)
242 if ((i==0)&&(_first))
243 return _alpha*180/3.14;//in Grad = 1
251 float Rotorblade::getrealAlpha(void)
255 void Rotorblade::setAlphaoutput(char *text,int i)
257 printf("setAlphaoutput Rotorblade [%s] typ %i\n",text,i);
259 strncpy(_alphaoutputbuf[i>0],text,255);
261 if (i>0) _alpha2type=i;
265 char* Rotorblade::getAlphaoutput(int i)
268 if ((i==0)&&(_first))
270 int winkel=(int)(.5+_phi/pi*180/wstep);
272 sprintf(_alphaoutputbuf[0],"/blades/pos%03i",winkel*wstep);
278 int winkel=(int)(.5+_phi/pi*180/wstep);
281 sprintf(_alphaoutputbuf[i&1],"/blades/showa_%i_%03i",i,winkel*wstep);
284 sprintf(_alphaoutputbuf[i&1],"/blades/damp_%03i",winkel*wstep);
286 sprintf(_alphaoutputbuf[i&1],"/blades/showb_%i_%03i",i,winkel*wstep);
290 return _alphaoutputbuf[i&1];
294 void Rotorblade::setNormal(float* p)
297 for(i=0; i<3; i++) _normal[i] = p[i];
299 void Rotorblade::setFront(float* p)
302 for(i=0; i<3; i++) _lfront[i]=_front[i] = p[i];
303 printf("front: %5.3f %5.3f %5.3f\n",p[0],p[1],p[2]);
305 void Rotorblade::setRight(float* p)
308 for(i=0; i<3; i++) _lright[i]=_right[i] = p[i];
309 printf("right: %5.3f %5.3f %5.3f\n",p[0],p[1],p[2]);
312 void Rotorblade::getNormal(float* out)
315 for(i=0; i<3; i++) out[i] = _normal[i];
319 void Rotorblade::setCollective(float pos)
324 void Rotorblade::setCyclicele(float pos)
328 void Rotorblade::setCyclicail(float pos)
334 void Rotorblade::setPhi(float value)
337 if(value==0) _first=1; else _first =0;
339 float Rotorblade::getPhi()
343 void Rotorblade::setOmega(float value)
347 void Rotorblade::setOmegaN(float value)
351 void Rotorblade::setLen(float value)
355 void Rotorblade::setLenHinge(float value)
359 void Rotorblade::setLforceattac(float value)
363 float Rotorblade::getIncidence()
368 float Rotorblade::getFlapatPos(int k)
370 return _flapatpos[k%4];
376 void Rotorblade::setlastnextrp(Rotorblade*lastrp,Rotorblade*nextrp,Rotorblade *oppositerp)
380 _oppositerp=oppositerp;
384 void Rotorblade::strncpy(char *dest,const char *src,int maxlen)
387 while(src[n]&&n<(maxlen-1))
396 // Calculate the aerodynamic force given a wind vector v (in the
397 // aircraft's "local" coordinates) and an air density rho. Returns a
398 // torque about the Y axis, too.
399 void Rotorblade::calcForce(float* v, float rho, float* out, float* torque)
402 //printf("cf: alt:%g aw:%g ",_alphaalt,_alphaomega);
403 //if (_first) printf("p: %5.3f e:%5.3f a:%5.3f p:%5.3f",_collective,_cyclicele,_cyclicail,_phi);
408 torque[i] = _oldt[i];
419 if ((_phi<=(float(k)*pi/2))&&((_phi+_omega*_dt)>=(float(k)*pi/2)))
421 _flapatpos[k%4]=_alphaalt;
427 if ((_phi>=(float(k)*pi/2))&&((_phi+_omega*_dt)<=(float(k)*pi/2)))
429 _flapatpos[k%4]=_alphaalt;
435 int steps=int(_dt*_stepspersecond);
436 if (steps<=0) steps=1;
444 //_zentipetalforce=_mass*_omega*_omega*_len*(_rellenhinge+(1-_rellenhinge)*Math::cos(_alphalt));
445 //_zentipetalforce=_mass*_omega*_omega*_len/(_rellenhinge+(1-_rellenhinge)*Math::cos(_alphalt)); //incl teeter
446 _speed=_omega*_len*(1-_rellenhinge+_rellenhinge*Math::cos(_alphaalt));
448 float vrel[3],vreldir[3],speed[3];
449 Math::mul3(_speed,_lright,speed);
450 Math::sub3(speed,v,vrel);
451 Math::unit3(vrel,vreldir);//direction of blade-movement rel. to air
452 float delta=Math::asin(Math::dot3(_normal,vreldir));//Angle of blade which would produce no vertical force
453 float lalphaalt=_alphaalt;
455 float lalphaomega=_alphaomega;
456 if((_phi>0.01)&&(_first)&&(_phi<0.02))
458 printf("mass:%5.3f delta: %5.3f _dt: %5.7f ldt: %5.7f st:%i w: %5.3f w0: %5.3f\n",
459 _mass,_delta,_dt,ldt,steps,_omega,Math::sqrt(_zentipetalforce*(1-_rellenhinge)/_len/_mass));
461 for (int step=0;step<steps;step++)
463 lphi=_phi+(step-steps/2.)*ldt*_omega;
464 //_zentipetalforce=_mass*_omega*_omega*_len/(_rellenhinge+(1-_rellenhinge)*Math::cos(lalphaalt)); //incl teeter
465 _zentipetalforce=_mass*_omega*_omega*_len;
466 //printf("[%5.3f]",col);
467 float beta=-_cyclicele*Math::sin(lphi-0*_deltaphi)+_cyclicail*Math::cos(lphi-0*_deltaphi)+_collective-_delta3*lalphaalt;
468 if (step==(steps/2)) _incidence=beta;
469 //printf("be:%5.3f de:%5.3f ",beta,delta);
470 //printf("\nvd: %5.3f %5.3f %5.3f ",vreldir[0],vreldir[1],vreldir[2]);
471 //printf("lr: %5.3f %5.3f %5.3f\n",_lright[0],_lright[1],_lright[2]);
472 //printf("no: %5.3f %5.3f %5.3f ",_normal[0],_normal[1],_normal[2]);
473 //printf("sp: %5.3f %5.3f %5.3f\n ",speed[0],speed[1],speed[2]);
474 //printf("vr: %5.3f %5.3f %5.3f ",vrel[0],vrel[1],vrel[2]);
475 //printf("v : %5.3f %5.3f %5.3f ",v[0],v[1],v[2]);
477 //float c=_maxpitchforce/(_maxpitch*_zentipetalforce);
479 float zforcealph=(beta-delta)/_maxpitch*_maxpitchforce*_omega/_omegan;
480 float zforcezent=(1-_rellenhinge)*Math::sin(lalphaalt)*_zentipetalforce;
481 float zforcelowspeed=(_omegan-_omega)/_omegan*(lalpha-_alpha0)*_mass*_alpha0factor;
482 float zf=zforcealph-zforcezent-zforcelowspeed;
485 float vv=Math::sin(lalphaomega)*_len;
486 zf-=vv*_delta*2*_mass;
488 if ((_omega*10)<_omegan)
489 vv*=.5+5*(_omega/_omegan);//reduce if omega is low
490 //if (_first) _showb=vv*_delta*2*_mass;//for debugging output
491 lalpha=Math::asin(Math::sin(lalphaalt)+(vv/_len)*ldt);
492 lalpha=Math::clamp(lalpha,_alphamin,_alphamax);
493 float vblade=Math::abs(Math::dot3(_lfront,v));
494 float tliftfactor=Math::sqrt(1+vblade*_translift);
499 float xforce = Math::cos(lalpha)*_zentipetalforce;//
500 float zforce = tliftfactor*Math::sin(lalpha)*_zentipetalforce;//
501 float thetorque = _torque_no_force+_torque_max_force*Math::abs(zforce/_maxpitchforce);
503 printf("speed: %5.3f %5.3f %5.3f vwind: %5.3f %5.3f %5.3f sin %5.3f\n",
504 _speed[0],_speed[1],_speed[2],
505 v[0],v[1],v[2],Math::sin(alpha));
509 t[i] += _normal[i]*thetorque;
510 f[i] += _normal[i]*zforce+_lfront[i]*xforce;
512 lalphaomega=(lalpha-lalphaalt)/ldt;
516 _ddtteeter+=_len*_omega/(1-_rellenhinge)*lalphaomega*ldt;
518 float teeterforce=-_zentipetalforce*Math::sin(_teeter)*_c2;
519 teeterforce-=Math::clamp(_ddtteeter*_teeterdamp,-_maxteeterdamp,_maxteeterdamp);
520 _ddtteeter+=teeterforce/_mass;
522 _teeter+=_ddtteeter*ldt;
523 if (_first) _showb=_teeter*180/pi;
527 _alphaomega=lalphaomega;
529 if (_first) printf("aneu: %5.3f zfa:%5.3f vv:%g ao:%.3g xf:%.3g zf:%.3g \r",_alpha,zforcealph,vv,_alpha
534 torque[i] = _oldt[i]=t[i]/steps;
535 out[i] = _oldf[i]=f[i]/steps;
537 _calcforcesdone=true;
538 //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]);
542 }; // namespace yasim