src/FDM/YASim/Rotorblade.cpp

   1 #include "Math.hpp"
   2 #include "Rotorblade.hpp"
   3 #include <stdio.h>
   4 //#include <string.h>
   5 //#include <Main/fg_props.hxx>
   6 namespace yasim {
   7 const float pi=3.14159;
   8
   9 Rotorblade::Rotorblade()
  10 {
  11     /*
  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;
  15     */
  16     _collective=0;
  17     _dt=0;
  18     _speed=0;
  19     #define set3(x,a,b,c) x[0]=a;x[1]=b;x[2]=c;
  20     set3 (_directionofzentipetalforce,1,0,0);
  21     #undef set3
  22     _zentipetalforce=1;
  23     _maxpitch=.02;
  24     _maxpitchforce=10;
  25     _delta3=0.5;
  26     _cyclicail=0;
  27     _cyclicele=0;
  28     _collective=0;
  29     _flapatpos[0]=_flapatpos[1]=_flapatpos[2]=_flapatpos[3]=0;
  30     _flapatpos[0]=.1;
  31     _flapatpos[1]=.2;
  32     _flapatpos[2]=.3;
  33     _flapatpos[3]=.4;
  34     _len=1;
  35     _lforceattac=1;
  36     _calcforcesdone=false;
  37     _phi=0;
  38     _omega=0;
  39     _omegan=1;
  40     _mass=10;
  41     _alpha=0;
  42     _alphaoutputbuf[0][0]=0;
  43     _alphaoutputbuf[1][0]=0;
  44     _alpha2type=0;
  45     _alphaalt=0;
  46     _alphaomega=0;
  47     _lastrp=0;
  48     _nextrp=0;
  49     _oppositerp=0;
  50     _translift=0;
  51     _dynamic=100;
  52     _c2=1;
  53     _stepspersecond=240;
  54     _torque_max_force=0;
  55     _torque_no_force=0;
  56     _deltaphi=0;
  57     _alphamin=-.1;
  58     _alphamax= .1;
  59     _alpha0=-.05;
  60     _alpha0factor=1;
  61     _rellenhinge=0;
  62     _teeter=0;
  63     _ddtteeter=0;
  64     _teeterdamp=0.00001;
  65     _maxteeterdamp=0;
  66     _rellenteeterhinge=0.01;
  67
  68
  69
  70
  71
  72 }
  73
  74
  75 void Rotorblade::inititeration(float dt,float *rot)
  76 {
  77      //printf("init %5.3f",dt*1000);
  78      _dt=dt;
  79      _calcforcesdone=false;
  80      float a=Math::dot3(rot,_normal);
  81    _phi+=a;
  82      _phi+=_omega*dt;
  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
  88
  89      float dir[3];
  90      Math::cross3(_lright,_normal,dir);
  91
  92
  93
  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
  98
  99      _alphaalt = alphaneu;
 100
 101
 102      calcFrontRight();
 103 }
 104 void Rotorblade::setTorque(float torque_max_force,float torque_no_force)
 105 {
 106     _torque_max_force=torque_max_force;
 107     _torque_no_force=torque_no_force;
 108 }
 109 void Rotorblade::setAlpha0(float f)
 110 {
 111     _alpha0=f;
 112 }
 113 void Rotorblade::setAlphamin(float f)
 114 {
 115     _alphamin=f;
 116 }
 117 void Rotorblade::setAlphamax(float f)
 118 {
 119     _alphamax=f;
 120 }
 121 void Rotorblade::setAlpha0factor(float f)
 122 {
 123     _alpha0factor=f;
 124 }
 125
 126
 127
 128
 129 void Rotorblade::setWeight(float value)
 130 {
 131      _mass=value;
 132 }
 133 float Rotorblade::getWeight(void)
 134 {
 135      return(_mass/.453); //_mass is in kg, returns pounds
 136 }
 137
 138 void Rotorblade::setPosition(float* p)
 139 {
 140     int i;
 141     for(i=0; i<3; i++) _pos[i] = p[i];
 142 }
 143
 144 void Rotorblade::calcFrontRight()
 145 {
 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);
 151
 152     Math::add3(tmpcf,tmpsr,_lfront);
 153     Math::sub3(tmpcr,tmpsf,_lright);
 154
 155 }
 156
 157 void Rotorblade::getPosition(float* out)
 158 {
 159     float dir[3];
 160     Math::mul3(_len,_lfront,dir);
 161     Math::add3(_pos,dir,out);
 162 }
 163
 164 void Rotorblade::setPositionForceAttac(float* p)
 165 {
 166     int i;
 167     for(i=0; i<3; i++) _posforceattac[i] = p[i];
 168 }
 169
 170 void Rotorblade::getPositionForceAttac(float* out)
 171 {
 172     float dir[3];
 173     Math::mul3(_len*_rellenhinge*2,_lfront,dir);
 174     Math::add3(_pos,dir,out);
 175 }
 176 void Rotorblade::setSpeed(float p)
 177 {
 178     _speed = p;
 179 }
 180 void Rotorblade::setDirectionofZentipetalforce(float* p)
 181 {
 182     int i;
 183     for(i=0; i<3; i++) _directionofzentipetalforce[i] = p[i];
 184 }
 185
 186 void Rotorblade::setZentipetalForce(float f)
 187 {
 188     _zentipetalforce=f;
 189 }
 190 void Rotorblade::setMaxpitch(float f)
 191 {
 192     _maxpitch=f;
 193 }
 194 void Rotorblade::setMaxPitchForce(float f)
 195 {
 196     _maxpitchforce=f;
 197 }
 198 void Rotorblade::setDelta(float f)
 199 {
 200     _delta=f;
 201 }
 202 void Rotorblade::setDeltaPhi(float f)
 203 {
 204     _deltaphi=f;
 205 }
 206 void Rotorblade::setDelta3(float f)
 207 {
 208     _delta3=f;
 209 }
 210 void Rotorblade::setTranslift(float f)
 211 {
 212     _translift=f;
 213 }
 214 void Rotorblade::setDynamic(float f)
 215 {
 216     _dynamic=f;
 217 }
 218 void Rotorblade::setC2(float f)
 219 {
 220     _c2=f;
 221 }
 222 void Rotorblade::setStepspersecond(float steps)
 223 {
 224     _stepspersecond=steps;
 225 }
 226 void Rotorblade::setRelLenTeeterHinge(float f)
 227 {
 228    _rellenteeterhinge=f;
 229 }
 230
 231 void Rotorblade::setTeeterdamp(float f)
 232 {
 233     _teeterdamp=f;
 234 }
 235 void Rotorblade::setMaxteeterdamp(float f)
 236 {
 237     _maxteeterdamp=f;
 238 }
 239 float Rotorblade::getAlpha(int i)
 240 {
 241   i=i&1;
 242   if ((i==0)&&(_first))
 243     return _alpha*180/3.14;//in Grad = 1
 244   else
 245     if(i==0)
 246       return _showa;
 247     else
 248       return _showb;
 249
 250 }
 251 float Rotorblade::getrealAlpha(void)
 252 {
 253     return _alpha;
 254 }
 255 void Rotorblade::setAlphaoutput(char *text,int i)
 256 {
 257    printf("setAlphaoutput Rotorblade [%s] typ %i\n",text,i);
 258
 259    strncpy(_alphaoutputbuf[i>0],text,255);
 260
 261    if (i>0) _alpha2type=i;
 262
 263
 264 }
 265 char* Rotorblade::getAlphaoutput(int i)
 266 {
 267    #define wstep 30
 268     if ((i==0)&&(_first))
 269     {
 270        int winkel=(int)(.5+_phi/pi*180/wstep);
 271        winkel%=(360/wstep);
 272        sprintf(_alphaoutputbuf[0],"/blades/pos%03i",winkel*wstep);
 273     }
 274
 275     else
 276     {
 277
 278        int winkel=(int)(.5+_phi/pi*180/wstep);
 279        winkel%=(360/wstep);
 280        if (i==0)
 281          sprintf(_alphaoutputbuf[i&1],"/blades/showa_%i_%03i",i,winkel*wstep);
 282        else
 283          if (_first)
 284            sprintf(_alphaoutputbuf[i&1],"/blades/damp_%03i",winkel*wstep);
 285          else
 286            sprintf(_alphaoutputbuf[i&1],"/blades/showb_%i_%03i",i,winkel*wstep);
 287
 288
 289     }
 290     return _alphaoutputbuf[i&1];
 291   #undef wstep
 292 }
 293
 294 void Rotorblade::setNormal(float* p)
 295 {
 296     int i;
 297     for(i=0; i<3; i++) _normal[i] = p[i];
 298 }
 299 void Rotorblade::setFront(float* p)
 300 {
 301     int i;
 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]);
 304 }
 305 void Rotorblade::setRight(float* p)
 306 {
 307     int i;
 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]);
 310 }
 311
 312 void Rotorblade::getNormal(float* out)
 313 {
 314     int i;
 315     for(i=0; i<3; i++) out[i] = _normal[i];
 316 }
 317
 318
 319 void Rotorblade::setCollective(float pos)
 320 {
 321     _collective = pos;
 322 }
 323
 324 void Rotorblade::setCyclicele(float pos)
 325 {
 326     _cyclicele = -pos;
 327 }
 328 void Rotorblade::setCyclicail(float pos)
 329 {
 330     _cyclicail = -pos;
 331 }
 332
 333
 334 void Rotorblade::setPhi(float value)
 335 {
 336   _phi=value;
 337   if(value==0) _first=1; else _first =0;
 338 }
 339 float Rotorblade::getPhi()
 340 {
 341   return(_phi);
 342 }
 343 void Rotorblade::setOmega(float value)
 344 {
 345   _omega=value;
 346 }
 347 void Rotorblade::setOmegaN(float value)
 348 {
 349   _omegan=value;
 350 }
 351 void Rotorblade::setLen(float value)
 352 {
 353   _len=value;
 354 }
 355 void Rotorblade::setLenHinge(float value)
 356 {
 357   _rellenhinge=value;
 358 }
 359 void Rotorblade::setLforceattac(float value)
 360 {
 361   _lforceattac=value;
 362 }
 363 float Rotorblade::getIncidence()
 364 {
 365     return(_incidence);
 366 }
 367
 368 float Rotorblade::getFlapatPos(int k)
 369 {
 370    return  _flapatpos[k%4];
 371 }
 372
 373
 374
 375 /*
 376 void Rotorblade::setlastnextrp(Rotorblade*lastrp,Rotorblade*nextrp,Rotorblade *oppositerp)
 377 {
 378   _lastrp=lastrp;
 379   _nextrp=nextrp;
 380   _oppositerp=oppositerp;
 381 }
 382 */
 383
 384 void Rotorblade::strncpy(char *dest,const char *src,int maxlen)
 385 {
 386   int n=0;
 387   while(src[n]&&n<(maxlen-1))
 388   {
 389     dest[n]=src[n];
 390     n++;
 391   }
 392   dest[n]=0;
 393 }
 394
 395
 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)
 400 {
 401
 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);
 404     if (_calcforcesdone)
 405     {
 406         int i;
 407         for(i=0; i<3; i++) {
 408           torque[i] = _oldt[i];
 409           out[i] = _oldf[i];
 410         }
 411         return;
 412     }
 413
 414     {
 415       int k;
 416       if (_omega>0)
 417           for (k=1;k<=4;k++)
 418           {
 419              if ((_phi<=(float(k)*pi/2))&&((_phi+_omega*_dt)>=(float(k)*pi/2)))
 420              {
 421                  _flapatpos[k%4]=_alphaalt;
 422              }
 423           }
 424        else
 425           for (k=0;k<4;k++)
 426           {
 427              if ((_phi>=(float(k)*pi/2))&&((_phi+_omega*_dt)<=(float(k)*pi/2)))
 428              {
 429                  _flapatpos[k%4]=_alphaalt;
 430              }
 431           }
 432     }
 433
 434     float ldt;
 435     int steps=int(_dt*_stepspersecond);
 436     if (steps<=0) steps=1;
 437     ldt=_dt/steps;
 438     float lphi;
 439     float f[3];
 440     f[0]=f[1]=f[2]=0;
 441     float t[3];
 442     t[0]=t[1]=t[2]=0;
 443
 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));
 447
 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;
 454     float lalpha=_alpha;
 455     float lalphaomega=_alphaomega;
 456     if((_phi>0.01)&&(_first)&&(_phi<0.02))
 457     {
 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));
 460     }
 461     for (int step=0;step<steps;step++)
 462     {
 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]);
 476
 477         //float c=_maxpitchforce/(_maxpitch*_zentipetalforce);
 478
 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;
 483         _showa=zforcealph;
 484         _showb=-zforcezent;
 485         float vv=Math::sin(lalphaomega)*_len;
 486         zf-=vv*_delta*2*_mass;
 487         vv+=zf/_mass*ldt;
 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);
 495
 496
 497
 498
 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);
 502         /*
 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));
 506         */
 507         int i;
 508         for(i=0; i<3; i++) {
 509           t[i] += _normal[i]*thetorque;
 510           f[i] += _normal[i]*zforce+_lfront[i]*xforce;
 511         }
 512         lalphaomega=(lalpha-lalphaalt)/ldt;
 513         lalphaalt=lalpha;
 514
 515         /*
 516         _ddtteeter+=_len*_omega/(1-_rellenhinge)*lalphaomega*ldt;
 517
 518         float teeterforce=-_zentipetalforce*Math::sin(_teeter)*_c2;
 519         teeterforce-=Math::clamp(_ddtteeter*_teeterdamp,-_maxteeterdamp,_maxteeterdamp);
 520         _ddtteeter+=teeterforce/_mass;
 521
 522         _teeter+=_ddtteeter*ldt;
 523         if (_first)  _showb=_teeter*180/pi;
 524         */
 525     }
 526     _alpha=lalpha;
 527     _alphaomega=lalphaomega;
 528     /*
 529         if (_first) printf("aneu: %5.3f zfa:%5.3f vv:%g ao:%.3g xf:%.3g zf:%.3g    \r",_alpha,zforcealph,vv,_alpha
 530                           ,xforce,zforce);
 531     */
 532     int i;
 533     for(i=0; i<3; i++) {
 534           torque[i] = _oldt[i]=t[i]/steps;
 535           out[i] = _oldf[i]=f[i]/steps;
 536     }
 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]);
 539 }
 540
 541
 542 }; // namespace yasim