src/FDM/YASim/Rotor.cpp

   1 #include <simgear/debug/logstream.hxx>
   2
   3 #include "Math.hpp"
   4 #include "Surface.hpp"
   5 #include "Rotorpart.hpp"
   6 #include "Rotor.hpp"
   7
   8 #include STL_IOSTREAM
   9 #include STL_IOMANIP
  10
  11 SG_USING_STD(setprecision);
  12
  13 #include <stdio.h>
  14
  15 namespace yasim {
  16
  17 const float pi=3.14159;
  18
  19 static inline float sqr(float a) { return a * a; }
  20
  21 Rotor::Rotor()
  22 {
  23     int i;
  24     _alpha0=-.05;
  25     _alpha0factor=1;
  26     _alphamin=-.1;
  27     _alphamax= .1;
  28     _alphaoutput[0][0]=0;
  29     _alphaoutput[1][0]=0;
  30     _alphaoutput[2][0]=0;
  31     _alphaoutput[3][0]=0;
  32     _alphaoutput[4][0]=0;
  33     _alphaoutput[5][0]=0;
  34     _alphaoutput[6][0]=0;
  35     _alphaoutput[7][0]=0;
  36     _base[0] = _base[1] = _base[2] = 0;
  37     _ccw=0;
  38     _delta=1;
  39     _delta3=0;
  40     _diameter =10;
  41     _dynamic=1;
  42     _engineon=0;
  43     _force_at_pitch_a=0;
  44     _forward[0]=1;
  45     _forward[1]=_forward[2]=0;
  46     _max_pitch=13./180*pi;
  47     _maxcyclicail=10./180*pi;
  48     _maxcyclicele=10./180*pi;
  49     _maxteeterdamp=0;
  50     _mincyclicail=-10./180*pi;
  51     _mincyclicele=-10./180*pi;
  52     _min_pitch=-.5/180*pi;
  53     _name[0] = 0;
  54     _normal[0] = _normal[1] = 0;
  55     _normal[2] = 1;
  56     _normal_with_yaw_roll[0]= _normal_with_yaw_roll[1]=0;
  57     _normal_with_yaw_roll[2]=1;
  58     _number_of_blades=4;
  59     _omega=_omegan=_omegarel=_omegarelneu=0;
  60     _ddt_omega=0;
  61     _pitch_a=0;
  62     _pitch_b=0;
  63     _power_at_pitch_0=0;
  64     _power_at_pitch_b=0;
  65     _no_torque=0;
  66     _rel_blade_center=.7;
  67     _rel_len_hinge=0.01;
  68     _rellenteeterhinge=0.01;
  69     _rotor_rpm=442;
  70     _sim_blades=0;
  71     _teeterdamp=0.00001;
  72     _translift=0.05;
  73     _weight_per_blade=42;
  74     _translift_ve=20;
  75     _translift_maxfactor=1.3;
  76     _ground_effect_constant=0.1;
  77     _vortex_state_lift_factor=0.4;
  78     _vortex_state_c1=0.1;
  79     _vortex_state_c2=0;
  80     _vortex_state_c3=0;
  81     _vortex_state_e1=1;
  82     _vortex_state_e2=1;
  83     _vortex_state_e3=1;
  84     _lift_factor=1;
  85     _liftcoef=0.1;
  86     _dragcoef0=0.1;
  87     _dragcoef1=0.1;
  88     _twist=0;
  89     _number_of_segments=1;
  90     _rel_len_where_incidence_is_measured=0.7;
  91     _torque_of_inertia=1;
  92     _torque=0;
  93     _chord=0.3;
  94     _taper=1;
  95     _airfoil_incidence_no_lift=0;
  96     _rel_len_blade_start=0;
  97     _airfoil_lift_coefficient=0;
  98     _airfoil_drag_coefficient0=0;
  99     _airfoil_drag_coefficient1=0;
 100     for(i=0; i<2; i++)
 101         _global_ground[i] =  0;
 102     _global_ground[2] = 1;
 103     _global_ground[3] = -1e3;
 104     _incidence_stall_zero_speed=18*pi/180.;
 105     _incidence_stall_half_sonic_speed=14*pi/180.;
 106     _lift_factor_stall=0.28;
 107     _stall_change_over=2*pi/180.;
 108     _drag_factor_stall=8;
 109     _stall_sum=1;
 110     _stall_v2sum=1;
 111     _collective=0;
 112     _yaw=_roll=0;
 113 }
 114
 115 Rotor::~Rotor()
 116 {
 117     int i;
 118     for(i=0; i<_rotorparts.size(); i++) {
 119         Rotorpart* r = (Rotorpart*)_rotorparts.get(i);
 120         delete r;
 121     }
 122 }
 123
 124 void Rotor::inititeration(float dt,float omegarel,float ddt_omegarel,float *rot)
 125 {
 126     _stall_sum=0;
 127     _stall_v2sum=0;
 128     _omegarel=omegarel;
 129     _omega=_omegan*_omegarel;
 130     _ddt_omega=_omegan*ddt_omegarel;
 131     int i;
 132     for(i=0; i<_rotorparts.size(); i++) {
 133         Rotorpart* r = (Rotorpart*)_rotorparts.get(i);
 134         r->setOmega(_omega);
 135         r->setDdtOmega(_ddt_omega);
 136         r->inititeration(dt,rot);
 137     }
 138
 139     //calculate the normal of the rotor disc, for calcualtion of the downwash
 140     float side[3],help[3];
 141     Math::cross3(_normal,_forward,side);
 142     Math::mul3(Math::cos(_yaw)*Math::cos(_roll),_normal,_normal_with_yaw_roll);
 143
 144     Math::mul3(Math::sin(_yaw),_forward,help);
 145     Math::add3(_normal_with_yaw_roll,help,_normal_with_yaw_roll);
 146
 147     Math::mul3(Math::sin(_roll),side,help);
 148     Math::add3(_normal_with_yaw_roll,help,_normal_with_yaw_roll);
 149 }
 150
 151 float Rotor::calcStall(float incidence,float speed)
 152 {
 153     float stall_incidence=_incidence_stall_zero_speed
 154         +(_incidence_stall_half_sonic_speed
 155         -_incidence_stall_zero_speed)*speed/(343./2);
 156     //missing: Temeperature dependency of sonic speed
 157     incidence = Math::abs(incidence);
 158     if (incidence > (90./180.*pi))
 159         incidence = pi-incidence;
 160     float stall = (incidence-stall_incidence)/_stall_change_over;
 161     stall = Math::clamp(stall,0,1);
 162
 163     _stall_sum+=stall*speed*speed;
 164     _stall_v2sum+=speed*speed;
 165
 166     return stall;
 167 }
 168
 169 float Rotor::getLiftCoef(float incidence,float speed)
 170 {
 171     float stall=calcStall(incidence,speed);
 172     float c1=  Math::sin(incidence-_airfoil_incidence_no_lift)*_liftcoef;
 173     float c2=  Math::sin(2*(incidence-_airfoil_incidence_no_lift))
 174         *_liftcoef*_lift_factor_stall;
 175     return (1-stall)*c1 + stall *c2;
 176 }
 177
 178 float Rotor::getDragCoef(float incidence,float speed)
 179 {
 180     float stall=calcStall(incidence,speed);
 181     float c1= (Math::abs(Math::sin(incidence-_airfoil_incidence_no_lift))
 182         *_dragcoef1+_dragcoef0);
 183     float c2= c1*_drag_factor_stall;
 184     return (1-stall)*c1 + stall *c2;
 185 }
 186
 187 int Rotor::getValueforFGSet(int j,char *text,float *f)
 188 {
 189     if (_name[0]==0) return 0;
 190     if (4!=numRotorparts()) return 0; //compile first!
 191     if (j==0)
 192     {
 193         sprintf(text,"/rotors/%s/cone", _name);
 194         *f=( ((Rotorpart*)getRotorpart(0))->getrealAlpha()
 195             +((Rotorpart*)getRotorpart(1))->getrealAlpha()
 196             +((Rotorpart*)getRotorpart(2))->getrealAlpha()
 197             +((Rotorpart*)getRotorpart(3))->getrealAlpha()
 198             )/4*180/pi;
 199     }
 200     else
 201         if (j==1)
 202         {
 203             sprintf(text,"/rotors/%s/roll", _name);
 204             _roll = ( ((Rotorpart*)getRotorpart(0))->getrealAlpha()
 205                 -((Rotorpart*)getRotorpart(2))->getrealAlpha()
 206                 )/2*(_ccw?-1:1);
 207             *f=_roll *180/pi;
 208         }
 209         else
 210             if (j==2)
 211             {
 212                 sprintf(text,"/rotors/%s/yaw", _name);
 213                 _yaw=( ((Rotorpart*)getRotorpart(1))->getrealAlpha()
 214                     -((Rotorpart*)getRotorpart(3))->getrealAlpha()
 215                     )/2;
 216                 *f=_yaw*180/pi;
 217             }
 218             else
 219                 if (j==3)
 220                 {
 221                     sprintf(text,"/rotors/%s/rpm", _name);
 222                     *f=_omega/2/pi*60;
 223                 }
 224                 else
 225                     if (j==4)
 226                     {
 227                         sprintf(text,"/rotors/%s/debug/debugfge",_name);
 228                         *f=_f_ge;
 229                     }
 230                     else if (j==5)
 231                     {
 232                         sprintf(text,"/rotors/%s/debug/debugfvs",_name);
 233                         *f=_f_vs;
 234                     }
 235                     else if (j==6)
 236                     {
 237                         sprintf(text,"/rotors/%s/debug/debugftl",_name);
 238                         *f=_f_tl;
 239                     }
 240                     else if (j==7)
 241                     {
 242                         sprintf(text,"/rotors/%s/debug/vortexstate",_name);
 243                         *f=_vortex_state;
 244                     }
 245                     else if (j==8)
 246                     {
 247                         sprintf(text,"/rotors/%s/stall",_name);
 248                         *f=getOverallStall();
 249                     }
 250                     else if (j==9)
 251                     {
 252                         sprintf(text,"/rotors/%s/torque",_name);
 253                         *f=-_torque;;
 254                     }
 255                     else
 256                     {
 257                         int b=(j-10)/3;
 258                         if (b>=_number_of_blades)
 259                         {
 260                             return 0;
 261                         }
 262                         int w=j%3;
 263                         sprintf(text,"/rotors/%s/blade%i_%s",
 264                             _name,b+1,
 265                             w==0?"pos":(w==1?"flap":"incidence"));
 266                         *f=((Rotorpart*)getRotorpart(0))->getPhi()*180/pi
 267                             +360*b/_number_of_blades*(_ccw?1:-1);
 268                         if (*f>360) *f-=360;
 269                         if (*f<0) *f+=360;
 270                         int k,l;
 271                         float rk,rl,p;
 272                         p=(*f/90);
 273                         k=int(p);
 274                         l=int(p+1);
 275                         rk=l-p;
 276                         rl=1-rk;
 277                         if(w==2) {k+=2;l+=2;}
 278                         else
 279                             if(w==1) {k+=1;l+=1;}
 280                             k%=4;
 281                             l%=4;
 282                             if (w==1) *f=rk*((Rotorpart*) getRotorpart(k))->getrealAlpha()*180/pi
 283                                 +rl*((Rotorpart*) getRotorpart(l))->getrealAlpha()*180/pi;
 284                             else if(w==2) *f=rk*((Rotorpart*)getRotorpart(k))->getIncidence()*180/pi
 285                                 +rl*((Rotorpart*)getRotorpart(l))->getIncidence()*180/pi;
 286                     }
 287                     return j+1;
 288 }
 289
 290 void Rotorgear::setEngineOn(int value)
 291 {
 292     _engineon=value;
 293 }
 294
 295 void Rotor::setAlpha0(float f)
 296 {
 297     _alpha0=f;
 298 }
 299
 300 void Rotor::setAlphamin(float f)
 301 {
 302     _alphamin=f;
 303 }
 304
 305 void Rotor::setAlphamax(float f)
 306 {
 307     _alphamax=f;
 308 }
 309
 310 void Rotor::setAlpha0factor(float f)
 311 {
 312     _alpha0factor=f;
 313 }
 314
 315 int Rotor::numRotorparts()
 316 {
 317     return _rotorparts.size();
 318 }
 319
 320 Rotorpart* Rotor::getRotorpart(int n)
 321 {
 322     return ((Rotorpart*)_rotorparts.get(n));
 323 }
 324
 325 int Rotorgear::getEngineon()
 326 {
 327     return _engineon;
 328 }
 329
 330 float Rotor::getTorqueOfInertia()
 331 {
 332     return _torque_of_inertia;
 333 }
 334
 335 void Rotor::setTorque(float f)
 336 {
 337     _torque=f;
 338 }
 339
 340 void Rotor::addTorque(float f)
 341 {
 342     _torque+=f;
 343 }
 344
 345 void Rotor::strncpy(char *dest,const char *src,int maxlen)
 346 {
 347     int n=0;
 348     while(src[n]&&n<(maxlen-1))
 349     {
 350         dest[n]=src[n];
 351         n++;
 352     }
 353     dest[n]=0;
 354 }
 355
 356 void Rotor::setNormal(float* normal)
 357 {
 358     int i;
 359     float invsum,sqrsum=0;
 360     for(i=0; i<3; i++) { sqrsum+=normal[i]*normal[i];}
 361     if (sqrsum!=0)
 362         invsum=1/Math::sqrt(sqrsum);
 363     else
 364         invsum=1;
 365     for(i=0; i<3; i++)
 366     {
 367         _normal_with_yaw_roll[i]=_normal[i] = normal[i]*invsum;
 368     }
 369 }
 370
 371 void Rotor::setForward(float* forward)
 372 {
 373     int i;
 374     float invsum,sqrsum=0;
 375     for(i=0; i<3; i++) { sqrsum+=forward[i]*forward[i];}
 376     if (sqrsum!=0)
 377         invsum=1/Math::sqrt(sqrsum);
 378     else
 379         invsum=1;
 380     for(i=0; i<3; i++) { _forward[i] = forward[i]*invsum; }
 381 }
 382
 383 void Rotor::setForceAtPitchA(float force)
 384 {
 385     _force_at_pitch_a=force;
 386 }
 387
 388 void Rotor::setPowerAtPitch0(float value)
 389 {
 390     _power_at_pitch_0=value;
 391 }
 392
 393 void Rotor::setPowerAtPitchB(float value)
 394 {
 395     _power_at_pitch_b=value;
 396 }
 397
 398 void Rotor::setPitchA(float value)
 399 {
 400     _pitch_a=value/180*pi;
 401 }
 402
 403 void Rotor::setPitchB(float value)
 404 {
 405     _pitch_b=value/180*pi;
 406 }
 407
 408 void Rotor::setBase(float* base)
 409 {
 410     int i;
 411     for(i=0; i<3; i++) _base[i] = base[i];
 412 }
 413
 414 void Rotor::setMaxCyclicail(float value)
 415 {
 416     _maxcyclicail=value/180*pi;
 417 }
 418
 419 void Rotor::setMaxCyclicele(float value)
 420 {
 421     _maxcyclicele=value/180*pi;
 422 }
 423
 424 void Rotor::setMinCyclicail(float value)
 425 {
 426     _mincyclicail=value/180*pi;
 427 }
 428
 429 void Rotor::setMinCyclicele(float value)
 430 {
 431     _mincyclicele=value/180*pi;
 432 }
 433
 434 void Rotor::setMinCollective(float value)
 435 {
 436     _min_pitch=value/180*pi;
 437 }
 438
 439 void Rotor::setMaxCollective(float value)
 440 {
 441     _max_pitch=value/180*pi;
 442 }
 443
 444 void Rotor::setDiameter(float value)
 445 {
 446     _diameter=value;
 447 }
 448
 449 void Rotor::setWeightPerBlade(float value)
 450 {
 451     _weight_per_blade=value;
 452 }
 453
 454 void Rotor::setNumberOfBlades(float value)
 455 {
 456     _number_of_blades=int(value+.5);
 457 }
 458
 459 void Rotor::setRelBladeCenter(float value)
 460 {
 461     _rel_blade_center=value;
 462 }
 463
 464 void Rotor::setDynamic(float value)
 465 {
 466     _dynamic=value;
 467 }
 468
 469 void Rotor::setDelta3(float value)
 470 {
 471     _delta3=value;
 472 }
 473
 474 void Rotor::setDelta(float value)
 475 {
 476     _delta=value;
 477 }
 478
 479 void Rotor::setTranslift(float value)
 480 {
 481     _translift=value;
 482 }
 483
 484 void Rotor::setC2(float value)
 485 {
 486     _c2=value;
 487 }
 488
 489 void Rotor::setStepspersecond(float steps)
 490 {
 491     _stepspersecond=steps;
 492 }
 493
 494 void Rotor::setRPM(float value)
 495 {
 496     _rotor_rpm=value;
 497 }
 498
 499 void Rotor::setRelLenHinge(float value)
 500 {
 501     _rel_len_hinge=value;
 502 }
 503
 504 void Rotor::setAlphaoutput(int i, const char *text)
 505 {
 506     strncpy(_alphaoutput[i],text,255);
 507 }
 508
 509 void Rotor::setName(const char *text)
 510 {
 511     strncpy(_name,text,256);//256: some space needed for settings
 512 }
 513
 514 void Rotor::setCcw(int ccw)
 515 {
 516     _ccw=ccw;
 517 }
 518
 519 void Rotor::setNotorque(int value)
 520 {
 521     _no_torque=value;
 522 }
 523
 524 void Rotor::setRelLenTeeterHinge(float f)
 525 {
 526     _rellenteeterhinge=f;
 527 }
 528
 529 void Rotor::setTeeterdamp(float f)
 530 {
 531     _teeterdamp=f;
 532 }
 533
 534 void Rotor::setMaxteeterdamp(float f)
 535 {
 536     _maxteeterdamp=f;
 537 }
 538
 539 void Rotor::setGlobalGround(double *global_ground, float* global_vel)
 540 {
 541     int i;
 542     for(i=0; i<4; i++) _global_ground[i] = global_ground[i];
 543 }
 544
 545 void Rotor::setParameter(char *parametername, float value)
 546 {
 547 #define p(a,b) if (strcmp(parametername,#a)==0) _##a = (value * (b)); else
 548     p(translift_ve,1)
 549         p(translift_maxfactor,1)
 550         p(ground_effect_constant,1)
 551         p(vortex_state_lift_factor,1)
 552         p(vortex_state_c1,1)
 553         p(vortex_state_c2,1)
 554         p(vortex_state_c3,1)
 555         p(vortex_state_e1,1)
 556         p(vortex_state_e2,1)
 557         p(vortex_state_e3,1)
 558         p(twist,pi/180.)
 559         p(number_of_segments,1)
 560         p(rel_len_where_incidence_is_measured,1)
 561         p(chord,1)
 562         p(taper,1)
 563         p(airfoil_incidence_no_lift,pi/180.)
 564         p(rel_len_blade_start,1)
 565         p(incidence_stall_zero_speed,pi/180.)
 566         p(incidence_stall_half_sonic_speed,pi/180.)
 567         p(lift_factor_stall,1)
 568         p(stall_change_over,pi/180.)
 569         p(drag_factor_stall,1)
 570         p(airfoil_lift_coefficient,1)
 571         p(airfoil_drag_coefficient0,1)
 572         p(airfoil_drag_coefficient1,1)
 573         cout << "internal error in parameter set up for rotor: '"
 574             << parametername <<"'" << endl;
 575 #undef p
 576 }
 577
 578 float Rotor::getLiftFactor()
 579 {
 580     return _lift_factor;
 581 }
 582
 583 void Rotorgear::setRotorBrake(float lval)
 584 {
 585     lval = Math::clamp(lval, 0, 1);
 586     _rotorbrake=lval;
 587 }
 588
 589 void Rotor::setCollective(float lval)
 590 {
 591     lval = Math::clamp(lval, -1, 1);
 592     int i;
 593     for(i=0; i<_rotorparts.size(); i++) {
 594         ((Rotorpart*)_rotorparts.get(i))->setCollective(lval);
 595     }
 596     _collective=_min_pitch+(lval+1)/2*(_max_pitch-_min_pitch);
 597 }
 598
 599 void Rotor::setCyclicele(float lval,float rval)
 600 {
 601     rval = Math::clamp(rval, -1, 1);
 602     lval = Math::clamp(lval, -1, 1);
 603     float col=_mincyclicele+(lval+1)/2*(_maxcyclicele-_mincyclicele);
 604     if (_rotorparts.size()!=4) return;
 605     ((Rotorpart*)_rotorparts.get(1))->setCyclic(lval);
 606     ((Rotorpart*)_rotorparts.get(3))->setCyclic(-lval);
 607 }
 608
 609 void Rotor::setCyclicail(float lval,float rval)
 610 {
 611     lval = Math::clamp(lval, -1, 1);
 612     rval = Math::clamp(rval, -1, 1);
 613     float col=_mincyclicail+(lval+1)/2*(_maxcyclicail-_mincyclicail);
 614     if (_rotorparts.size()!=4) return;
 615     if (_ccw) lval *=-1;
 616     ((Rotorpart*)_rotorparts.get(0))->setCyclic(-lval);
 617     ((Rotorpart*)_rotorparts.get(2))->setCyclic( lval);
 618 }
 619
 620 void Rotor::getPosition(float* out)
 621 {
 622     int i;
 623     for(i=0; i<3; i++) out[i] = _base[i];
 624 }
 625
 626 void Rotor::calcLiftFactor(float* v, float rho, State *s)
 627 {
 628     //calculates _lift_factor, which is a foactor for the lift of the rotor
 629     //due to
 630     //- ground effect (_f_ge)
 631     //- vortex state (_f_vs)
 632     //- translational lift (_f_tl)
 633     _f_ge=1;
 634     _f_tl=1;
 635     _f_vs=1;
 636
 637     // find h, the distance to the ground
 638     // The ground plane transformed to the local frame.
 639     float ground[4];
 640     s->planeGlobalToLocal(_global_ground, ground);
 641
 642     float h = ground[3] - Math::dot3(_base, ground);
 643     // Now h is the distance from the rotor center to ground
 644
 645     // Calculate ground effect
 646     _f_ge=1+_diameter/h*_ground_effect_constant;
 647
 648     // Now calculate translational lift
 649     float v_vert = Math::dot3(v,_normal);
 650     float help[3];
 651     Math::cross3(v,_normal,help);
 652     float v_horiz = Math::mag3(help);
 653     _f_tl = ((1-Math::pow(2.7183,-v_horiz/_translift_ve))
 654         *(_translift_maxfactor-1)+1)/_translift_maxfactor;
 655
 656     _lift_factor = _f_ge*_f_tl*_f_vs;
 657 }
 658
 659 float Rotor::getGroundEffect(float* posOut)
 660 {
 661     return _diameter*0; //ground effect for rotor is calcualted not here
 662 }
 663
 664 void Rotor::getDownWash(float *pos, float *v_heli, float *downwash)
 665 {
 666     float pos2rotor[3],tmp[3];
 667     Math::sub3(_base,pos,pos2rotor);
 668     float dist=Math::dot3(pos2rotor,_normal_with_yaw_roll);
 669     //calculate incidence at 0.7r;
 670     float inc = _collective+_twist *0.7
 671                 - _twist*_rel_len_where_incidence_is_measured;
 672     if (inc < 0)
 673         dist *=-1;
 674     if (dist<0) // we are not in the downwash region
 675     {
 676         downwash[0]=downwash[1]=downwash[2]=0.;
 677         return;
 678     }
 679
 680     //calculate the mean downwash speed directly beneath the rotor disk
 681     float v1bar = Math::sin(inc) *_omega * 0.35 * _diameter * 0.8;
 682     //0.35 * d = 0.7 *r, a good position to calcualte the mean downwashd
 683     //0.8 the slip of the rotor.
 684
 685     //calculate the time the wind needed from thr rotor to here
 686     if (v1bar< 1) v1bar = 1;
 687     float time=dist/v1bar;
 688
 689     //calculate the pos2rotor, where the rotor was, "time" ago
 690     Math::mul3(time,v_heli,tmp);
 691     Math::sub3(pos2rotor,tmp,pos2rotor);
 692
 693     //and again calculate dist
 694     dist=Math::dot3(pos2rotor,_normal_with_yaw_roll);
 695     //missing the normal is offen not pointing to the normal of the rotor
 696     //disk. Rotate the normal by yaw and tilt angle
 697
 698     if (inc < 0)
 699         dist *=-1;
 700     if (dist<0) // we are not in the downwash region
 701     {
 702         downwash[0]=downwash[1]=downwash[2]=0.;
 703         return;
 704     }
 705     //of course this could be done in a runge kutta integrator, but it's such
 706     //a approximation that I beleave, it would'nt be more realistic
 707
 708     //calculate the dist to the rotor-axis
 709     Math::cross3(pos2rotor,_normal_with_yaw_roll,tmp);
 710     float r= Math::mag3(tmp);
 711     //calculate incidence at r;
 712     float rel_r = r *2 /_diameter;
 713     float inc_r = _collective+_twist * r /_diameter * 2
 714         - _twist*_rel_len_where_incidence_is_measured;
 715
 716     //calculate the downwash speed directly beneath the rotor disk
 717     float v1=0;
 718     if (rel_r<1)
 719         v1 = Math::sin(inc_r) *_omega * r * 0.8;
 720
 721     //calcualte the downwash speed in a distance "dist" to the rotor disc,
 722     //for large dist. The speed is assumed do follow a gausian distribution
 723     //with sigma increasing with dist^2:
 724     //sigma is assumed to be half of the rotor diameter directly beneath the
 725     //disc and is assumed to the rotor diameter at dist = (diameter * sqrt(2))
 726
 727     float sigma=_diameter/2 + dist * dist / _diameter /4.;
 728     float v2 = v1bar*_diameter/ (Math::sqrt(2 * pi) * sigma)
 729         * Math::pow(2.7183,-.5*r*r/(sigma*sigma))*_diameter/2/sigma;
 730
 731     //calculate the weight of the two downwash velocities.
 732     //Directly beneath the disc it is v1, far away it is v2
 733     float g = Math::pow(2.7183,-2*dist/_diameter);
 734     //at dist = rotor radius it is assumed to be 1/e * v1 + (1-1/e)* v2
 735
 736     float v = g * v1 + (1-g) * v2;
 737     Math::mul3(-v,_normal_with_yaw_roll,downwash);
 738     //the downwash is calculated in the opposite direction of the normal
 739 }
 740
 741 void Rotor::compile()
 742 {
 743     // Have we already been compiled?
 744     if(_rotorparts.size() != 0) return;
 745
 746     //rotor is divided into 4 pointlike parts
 747
 748     SG_LOG(SG_FLIGHT, SG_DEBUG, "debug: e "
 749         << _mincyclicele << "..." <<_maxcyclicele << ' '
 750         << _mincyclicail << "..." << _maxcyclicail << ' '
 751         << _min_pitch << "..." << _max_pitch);
 752
 753     _dynamic=_dynamic*(1/                          //inverse of the time
 754         ( (60/_rotor_rpm)/4         //for rotating 90 deg
 755         +(60/_rotor_rpm)/(2*_number_of_blades) //+ meantime a rotorblade
 756                                                //will pass a given point
 757         ));
 758     float directions[5][3];
 759     //pointing forward, right, ... the 5th is ony for calculation
 760     directions[0][0]=_forward[0];
 761     directions[0][1]=_forward[1];
 762     directions[0][2]=_forward[2];
 763     int i;
 764     SG_LOG(SG_FLIGHT, SG_DEBUG, "Rotor rotating ccw? " << _ccw);
 765     for (i=1;i<5;i++)
 766     {
 767         if (!_ccw)
 768             Math::cross3(directions[i-1],_normal,directions[i]);
 769         else
 770             Math::cross3(_normal,directions[i-1],directions[i]);
 771         Math::unit3(directions[i],directions[i]);
 772     }
 773     Math::set3(directions[4],directions[0]);
 774     float rotorpartmass = _weight_per_blade*_number_of_blades/4*.453;
 775     //was pounds -> now kg
 776
 777     _torque_of_inertia = 1/12. * ( 4 * rotorpartmass) * _diameter
 778         * _diameter * _rel_blade_center * _rel_blade_center /(0.5*0.5);
 779     float speed=_rotor_rpm/60*_diameter*_rel_blade_center*pi;
 780     float lentocenter=_diameter*_rel_blade_center*0.5;
 781     float lentoforceattac=_diameter*_rel_len_hinge*0.5;
 782     float zentforce=rotorpartmass*speed*speed/lentocenter;
 783     float pitchaforce=_force_at_pitch_a/4*.453*9.81;
 784     //was pounds of force, now N, devided by 4 (so its now per rotorpart)
 785
 786     float torque0=0,torquemax=0,torqueb=0;
 787     float omega=_rotor_rpm/60*2*pi;
 788     _omegan=omega;
 789     float omega0=omega*Math::sqrt(1/(1-_rel_len_hinge));
 790     _delta*=pitchaforce/(_pitch_a*omega*lentocenter*2*rotorpartmass);
 791
 792     float phi=Math::atan2(2*omega*_delta,omega0*omega0-omega*omega);
 793     float relamp=omega*omega/(2*_delta*Math::sqrt(sqr(omega0*omega0-omega*omega)
 794         +4*_delta*_delta*omega*omega));
 795     if (!_no_torque)
 796     {
 797         torque0=_power_at_pitch_0/4*1000/omega;
 798         // f*r=p/w ; p=f*s/t;  r=s/t/w ; r*w*t = s
 799         torqueb=_power_at_pitch_b/4*1000/omega;
 800         torquemax=_power_at_pitch_b/4*1000/omega/_pitch_b*_max_pitch;
 801
 802         if(_ccw)
 803         {
 804             torque0*=-1;
 805             torquemax*=-1;
 806             torqueb*=-1;
 807         }
 808     }
 809
 810     SG_LOG(SG_FLIGHT, SG_DEBUG,
 811         "spd: " << setprecision(8) << speed
 812         << " lentoc: " << lentocenter
 813         << " dia: " << _diameter
 814         << " rbl: " << _rel_blade_center
 815         << " hing: " << _rel_len_hinge
 816         << " lfa: " << lentoforceattac);
 817
 818     SG_LOG(SG_FLIGHT, SG_DEBUG,
 819         "tq: " << setprecision(8) << torque0 << ".." << torquemax
 820         << " d3: " << _delta3);
 821     SG_LOG(SG_FLIGHT, SG_DEBUG,
 822         "o/o0: " << setprecision(8) << omega/omega0
 823         << " phi: " << phi*180/pi
 824         << " relamp: " << relamp
 825         << " delta: " <<_delta);
 826
 827     Rotorpart* rps[4];
 828     for (i=0;i<4;i++)
 829     {
 830         float lpos[3],lforceattac[3],lspeed[3],dirzentforce[3];
 831
 832         Math::mul3(lentocenter,directions[i],lpos);
 833         Math::add3(lpos,_base,lpos);
 834         Math::mul3(lentoforceattac,directions[i+1],lforceattac);
 835         //i+1: +90deg (gyro)!!!
 836
 837         Math::add3(lforceattac,_base,lforceattac);
 838         Math::mul3(speed,directions[i+1],lspeed);
 839         Math::mul3(1,directions[i+1],dirzentforce);
 840
 841         float maxcyclic=(i&1)?_maxcyclicele:_maxcyclicail;
 842         float mincyclic=(i&1)?_mincyclicele:_mincyclicail;
 843
 844         Rotorpart* rp=rps[i]=newRotorpart(lpos, lforceattac, _normal,
 845             lspeed,dirzentforce,zentforce,pitchaforce, _max_pitch,_min_pitch,
 846             mincyclic,maxcyclic,_delta3,rotorpartmass,_translift,
 847             _rel_len_hinge,lentocenter);
 848         rp->setAlphaoutput(_alphaoutput[i&1?i:(_ccw?i^2:i)],0);
 849         rp->setAlphaoutput(_alphaoutput[4+(i&1?i:(_ccw?i^2:i))],1+(i>1));
 850         _rotorparts.add(rp);
 851         rp->setTorque(torquemax,torque0);
 852         rp->setRelamp(relamp);
 853         rp->setDirectionofRotorPart(directions[i]);
 854         rp->setTorqueOfInertia(_torque_of_inertia/4.);
 855     }
 856     for (i=0;i<4;i++)
 857     {
 858         rps[i]->setlastnextrp(rps[(i+3)%4],rps[(i+1)%4],rps[(i+2)%4]);
 859     }
 860     for (i=0;i<4;i++)
 861     {
 862         rps[i]->setCompiled();
 863     }
 864     float lift[4],torque[4], v_wind[3];
 865     v_wind[0]=v_wind[1]=v_wind[2]=0;
 866     rps[0]->setOmega(_omegan);
 867
 868     if (_airfoil_lift_coefficient==0)
 869     {
 870         //calculate the lift and drag coefficients now
 871         _liftcoef=0;
 872         _dragcoef0=1;
 873         _dragcoef1=0;
 874         rps[0]->calculateAlpha(v_wind,rho_null,0,0,0,&(torque[0]),&(lift[0]));
 875         //0 degree, c0
 876
 877         _liftcoef=0;
 878         _dragcoef0=0;
 879         _dragcoef1=1;
 880         rps[0]->calculateAlpha(v_wind,rho_null,0,0,0,&(torque[1]),&(lift[1]));
 881         //0 degree, c1
 882
 883         _liftcoef=0;
 884         _dragcoef0=1;
 885         _dragcoef1=0;
 886         rps[0]->calculateAlpha(v_wind,rho_null,_pitch_b,0,0,&(torque[2]),&(lift[2]));
 887         //picth b, c0
 888
 889         _liftcoef=0;
 890         _dragcoef0=0;
 891         _dragcoef1=1;
 892         rps[0]->calculateAlpha(v_wind,rho_null,_pitch_b,0,0,&(torque[3]),&(lift[3]));
 893         //picth b, c1
 894
 895         if (torque[0]==0)
 896         {
 897             _dragcoef1=torque0/torque[1];
 898             _dragcoef0=(torqueb-_dragcoef1*torque[3])/torque[2];
 899         }
 900         else
 901         {
 902             _dragcoef1=(torque0/torque[0]-torqueb/torque[2])
 903                 /(torque[1]/torque[0]-torque[3]/torque[2]);
 904             _dragcoef0=(torqueb-_dragcoef1*torque[3])/torque[2];
 905         }
 906
 907         _liftcoef=1;
 908         rps[0]->calculateAlpha(v_wind,rho_null,_pitch_a,0,0,
 909             &(torque[0]),&(lift[0])); //max_pitch a
 910         _liftcoef = pitchaforce/lift[0];
 911     }
 912     else
 913     {
 914         _liftcoef=_airfoil_lift_coefficient/4*_number_of_blades;
 915         _dragcoef0=_airfoil_drag_coefficient0/4*_number_of_blades;
 916         _dragcoef1=_airfoil_drag_coefficient1/4*_number_of_blades;
 917     }
 918
 919     //Check
 920     rps[0]->calculateAlpha(v_wind,rho_null,_pitch_a,0,0,
 921         &(torque[0]),&(lift[0])); //pitch a
 922     rps[0]->calculateAlpha(v_wind,rho_null,_pitch_b,0,0,
 923         &(torque[1]),&(lift[1])); //pitch b
 924     rps[0]->calculateAlpha(v_wind,rho_null,0,0,0,
 925         &(torque[3]),&(lift[3])); //pitch 0
 926     SG_LOG(SG_FLIGHT, SG_DEBUG,
 927         "Rotor: coefficients for airfoil:" << endl << setprecision(6)
 928         << " drag0: " << _dragcoef0*4/_number_of_blades
 929         << " drag1: " << _dragcoef1*4/_number_of_blades
 930         << " lift: " << _liftcoef*4/_number_of_blades
 931         << endl
 932         << "at 10 deg:" << endl
 933         << "drag: " << (Math::sin(10./180*pi)*_dragcoef1+_dragcoef0)
 934             *4/_number_of_blades
 935         << "lift: " << Math::sin(10./180*pi)*_liftcoef*4/_number_of_blades
 936         << endl
 937         << "Some results (Pitch [degree], Power [kW], Lift [N]" << endl
 938         << 0.0f << "deg " << Math::abs(torque[3]*4*_omegan/1000) << "kW "
 939             << lift[3] << endl
 940         << _pitch_a*180/pi << "deg " << Math::abs(torque[0]*4*_omegan/1000)
 941             << "kW " << lift[0] << endl
 942         << _pitch_b*180/pi << "deg " << Math::abs(torque[1]*4*_omegan/1000)
 943             << "kW " << lift[1] << endl << endl);
 944
 945     rps[0]->setOmega(0);
 946 }
 947
 948 Rotorpart* Rotor::newRotorpart(float* pos, float *posforceattac, float *normal,
 949     float* speed,float *dirzentforce, float zentforce,float maxpitchforce,
 950     float maxpitch, float minpitch, float mincyclic,float maxcyclic,
 951     float delta3,float mass,float translift,float rellenhinge,float len)
 952 {
 953     Rotorpart *r = new Rotorpart();
 954     r->setPosition(pos);
 955     r->setNormal(normal);
 956     r->setZentipetalForce(zentforce);
 957     r->setPositionForceAttac(posforceattac);
 958     r->setSpeed(speed);
 959     r->setDirectionofZentipetalforce(dirzentforce);
 960     r->setMaxpitch(maxpitch);
 961     r->setMinpitch(minpitch);
 962     r->setMaxcyclic(maxcyclic);
 963     r->setMincyclic(mincyclic);
 964     r->setDelta3(delta3);
 965     r->setDynamic(_dynamic);
 966     r->setTranslift(_translift);
 967     r->setC2(_c2);
 968     r->setWeight(mass);
 969     r->setRelLenHinge(rellenhinge);
 970     r->setOmegaN(_omegan);
 971     r->setAlpha0(_alpha0);
 972     r->setAlphamin(_alphamin);
 973     r->setAlphamax(_alphamax);
 974     r->setAlpha0factor(_alpha0factor);
 975     r->setLen(len);
 976     r->setDiameter(_diameter);
 977     r->setRotor(this);
 978 #define p(a) r->setParameter(#a,_##a);
 979     p(twist)
 980     p(number_of_segments)
 981     p(rel_len_where_incidence_is_measured)
 982     p(rel_len_blade_start)
 983 #undef p
 984
 985     SG_LOG(SG_FLIGHT, SG_DEBUG, setprecision(8)
 986         << "newrp: pos("
 987         << pos[0] << ' ' << pos[1] << ' ' << pos[2]
 988         << ") pfa ("
 989         << posforceattac[0] << ' ' << posforceattac[1] << ' '
 990         << posforceattac[2] << ')');
 991     SG_LOG(SG_FLIGHT, SG_DEBUG, setprecision(8)
 992         << "       nor("
 993         << normal[0] << ' ' << normal[1] << ' ' << normal[2]
 994         << ") spd ("
 995         << speed[0] << ' ' << speed[1] << ' ' << speed[2] << ')');
 996     SG_LOG(SG_FLIGHT, SG_DEBUG, setprecision(8)
 997         << "       dzf("
 998         << dirzentforce[0] << ' ' << dirzentforce[1] << dirzentforce[2]
 999         << ") zf  (" << zentforce << ") mpf (" << maxpitchforce << ')');
1000         SG_LOG(SG_FLIGHT, SG_DEBUG, setprecision(8)
1001         << "       pit(" << minpitch << ".." << maxpitch
1002         << ") mcy (" << mincyclic << ".." << maxcyclic
1003         << ") d3 (" << delta3 << ')');
1004     return r;
1005 }
1006
1007 void Rotor::interp(float* v1, float* v2, float frac, float* out)
1008 {
1009     out[0] = v1[0] + frac*(v2[0]-v1[0]);
1010     out[1] = v1[1] + frac*(v2[1]-v1[1]);
1011     out[2] = v1[2] + frac*(v2[2]-v1[2]);
1012 }
1013
1014 void Rotorgear::initRotorIteration(float *lrot,float dt)
1015 {
1016     int i;
1017     float omegarel;
1018     if (!_rotors.size()) return;
1019     Rotor* r0 = (Rotor*)_rotors.get(0);
1020     omegarel= r0->getOmegaRelNeu();
1021     for(i=0; i<_rotors.size(); i++) {
1022         Rotor* r = (Rotor*)_rotors.get(i);
1023         r->inititeration(dt,omegarel,0,lrot);
1024     }
1025 }
1026
1027 void Rotorgear::calcForces(float* torqueOut)
1028 {
1029     int i,j;
1030     torqueOut[0]=torqueOut[1]=torqueOut[2]=0;
1031     // check,<if the engine can handle the torque of the rotors.
1032     // If not reduce the torque to the fueselage and change rotational
1033     // speed of the rotors instead
1034     if (_rotors.size())
1035     {
1036         float omegarel,omegan;
1037         Rotor* r0 = (Rotor*)_rotors.get(0);
1038         omegarel= r0->getOmegaRel();
1039
1040         float total_torque_of_inertia=0;
1041         float total_torque=0;
1042         for(i=0; i<_rotors.size(); i++) {
1043             Rotor* r = (Rotor*)_rotors.get(i);
1044             omegan=r->getOmegan();
1045             total_torque_of_inertia+=r->getTorqueOfInertia()*omegan*omegan;
1046             //FIXME: this is constant, so this can be done in compile
1047
1048             total_torque+=r->getTorque()*omegan;
1049         }
1050         float max_torque_of_engine=0;
1051         if (_engineon)
1052         {
1053             max_torque_of_engine=_max_power_engine;
1054             float df=1-omegarel;
1055             df/=_engine_prop_factor;
1056             df = Math::clamp(df, 0, 1);
1057             max_torque_of_engine = df * _max_power_engine;
1058         }
1059         total_torque*=-1;
1060         _ddt_omegarel=0;
1061         float rel_torque_engine=1;
1062         if (total_torque<=0)
1063             rel_torque_engine=0;
1064         else
1065             if (max_torque_of_engine>0)
1066                 rel_torque_engine=1/max_torque_of_engine*total_torque;
1067             else
1068                 rel_torque_engine=0;
1069
1070         //add the rotor brake
1071         float dt=0.1f;
1072         if (r0->_rotorparts.size()) dt=((Rotorpart*)r0->_rotorparts.get(0))->getDt();
1073
1074         float rotor_brake_torque;
1075         rotor_brake_torque=_rotorbrake*_max_power_rotor_brake;
1076         //clamp it to the value you need to stop the rotor
1077         rotor_brake_torque=Math::clamp(rotor_brake_torque,0,
1078             total_torque_of_inertia/dt*omegarel);
1079         max_torque_of_engine-=rotor_brake_torque;
1080
1081         //change the rotation of the rotors
1082         if ((max_torque_of_engine<total_torque) //decreasing rotation
1083             ||((max_torque_of_engine>total_torque)&&(omegarel<1))
1084             //increasing rotation due to engine
1085             ||(total_torque<0) ) //increasing rotation due to autorotation
1086         {
1087             _ddt_omegarel=(max_torque_of_engine-total_torque)/total_torque_of_inertia;
1088             if(max_torque_of_engine>total_torque)
1089             {
1090                 //check if the acceleration is due to the engine. If yes,
1091                 //the engine self limits the accel.
1092                 float lim1=-total_torque/total_torque_of_inertia;
1093                 //accel. by autorotation
1094
1095                 if (lim1<_engine_accell_limit) lim1=_engine_accell_limit;
1096                 //if the accel by autorotation greater than the max. engine
1097                 //accel, then this is the limit, if not: the engine is the limit
1098                 if (_ddt_omegarel>lim1) _ddt_omegarel=lim1;
1099             }
1100             if (_ddt_omegarel>5.5)_ddt_omegarel=5.5;
1101             //clamp it to avoid overflow. Should never be reached
1102             if (_ddt_omegarel<-5.5)_ddt_omegarel=-5.5;
1103
1104             if (_max_power_engine<0.001) {omegarel=1;_ddt_omegarel=0;}
1105             //for debug: negative or no maxpower will result
1106             //in permanet 100% rotation
1107
1108             omegarel+=dt*_ddt_omegarel;
1109
1110             if (omegarel>2.5) omegarel=2.5;
1111             //clamp it to avoid overflow. Should never be reached
1112             if (omegarel<-.5) omegarel=-.5;
1113
1114             r0->setOmegaRelNeu(omegarel);
1115             //calculate the torque, which is needed to accelerate the rotors.
1116             //Add this additional torque to the body
1117             for(j=0; j<_rotors.size(); j++) {
1118                 Rotor* r = (Rotor*)_rotors.get(j);
1119                 for(i=0; i<r->_rotorparts.size(); i++) {
1120                     float torque_scalar=0;
1121                     Rotorpart* rp = (Rotorpart*)r->_rotorparts.get(i);
1122                     float torque[3];
1123                     rp->getAccelTorque(_ddt_omegarel,torque);
1124                     Math::add3(torque,torqueOut,torqueOut);
1125                 }
1126             }
1127         }
1128     }
1129 }
1130
1131 void Rotorgear::addRotor(Rotor* rotor)
1132 {
1133     _rotors.add(rotor);
1134     _in_use = 1;
1135 }
1136
1137 float Rotorgear::compile(RigidBody* body)
1138 {
1139     float wgt = 0;
1140     for(int j=0; j<_rotors.size(); j++) {
1141         Rotor* r = (Rotor*)_rotors.get(j);
1142         r->compile();
1143         int i;
1144         for(i=0; i<r->numRotorparts(); i++) {
1145             Rotorpart* rp = (Rotorpart*)r->getRotorpart(i);
1146             float mass = rp->getWeight();
1147             mass = mass * Math::sqrt(mass);
1148             float pos[3];
1149             rp->getPosition(pos);
1150             body->addMass(mass, pos);
1151             wgt += mass;
1152         }
1153     }
1154     return wgt;
1155 }
1156
1157 void Rotorgear::getDownWash(float *pos, float * v_heli, float *downwash)
1158 {
1159     float tmp[3];
1160     downwash[0]=downwash[1]=downwash[2]=0;
1161     for(int i=0; i<_rotors.size(); i++) {
1162         Rotor* ro = (Rotor*)_rotors.get(i);
1163         ro->getDownWash(pos,v_heli,tmp);
1164         Math::add3(downwash,tmp,downwash);    //  + downwash
1165     }
1166 }
1167
1168 void Rotorgear::setParameter(char *parametername, float value)
1169 {
1170 #define p(a,b) if (strcmp(parametername,#a)==0) _##a = (value * (b)); else
1171         p(max_power_engine,1000)
1172         p(engine_prop_factor,1)
1173         p(yasimdragfactor,1)
1174         p(yasimliftfactor,1)
1175         p(max_power_rotor_brake,1000)
1176         p(engine_accell_limit,0.01)
1177         cout << "internal error in parameter set up for rotorgear: '"
1178             << parametername <<"'" << endl;
1179 #undef p
1180 }
1181
1182 Rotorgear::Rotorgear()
1183 {
1184     _in_use=0;
1185     _engineon=0;
1186     _rotorbrake=0;
1187     _max_power_rotor_brake=1;
1188     _max_power_engine=1000*450;
1189     _engine_prop_factor=0.05f;
1190     _yasimdragfactor=1;
1191     _yasimliftfactor=1;
1192     _ddt_omegarel=0;
1193     _engine_accell_limit=0.05f;
1194 }
1195
1196 Rotorgear::~Rotorgear()
1197 {
1198     for(int i=0; i<_rotors.size(); i++)
1199         delete (Rotor*)_rotors.get(i);
1200 }
1201
1202 }; // namespace yasim