src/FDM/YASim/Rotor.cpp

   1 #include <simgear/debug/logstream.hxx>
   2
   3 #include "Math.hpp"
   4 #include <Main/fg_props.hxx>
   5 #include "Surface.hpp"
   6 #include "Rotorpart.hpp"
   7 #include "Glue.hpp"
   8 #include "Ground.hpp"
   9 #include "Rotor.hpp"
  10
  11 #include STL_IOSTREAM
  12 #include STL_IOMANIP
  13
  14 SG_USING_STD(setprecision);
  15
  16 #ifdef TEST_DEBUG
  17 #include <stdio.h>
  18 #endif
  19 #include <string.h>
  20 #include <iostream>
  21 #include <sstream>
  22
  23
  24
  25 namespace yasim {
  26
  27 const float pi=3.14159;
  28
  29 static inline float sqr(float a) { return a * a; }
  30
  31 Rotor::Rotor()
  32 {
  33     int i;
  34     _alpha0=-.05;
  35     _alpha0factor=1;
  36     _alphamin=-.1;
  37     _alphamax= .1;
  38     _alphaoutput[0][0]=0;
  39     _alphaoutput[1][0]=0;
  40     _alphaoutput[2][0]=0;
  41     _alphaoutput[3][0]=0;
  42     _alphaoutput[4][0]=0;
  43     _alphaoutput[5][0]=0;
  44     _alphaoutput[6][0]=0;
  45     _alphaoutput[7][0]=0;
  46     _base[0] = _base[1] = _base[2] = 0;
  47     _ccw=0;
  48     _delta=1;
  49     _delta3=0;
  50     _diameter =10;
  51     _dynamic=1;
  52     _engineon=0;
  53     _force_at_pitch_a=0;
  54     _forward[0]=1;
  55     _forward[1]=_forward[2]=0;
  56     _max_pitch=13./180*pi;
  57     _maxcyclicail=10./180*pi;
  58     _maxcyclicele=10./180*pi;
  59     _maxteeterdamp=0;
  60     _mincyclicail=-10./180*pi;
  61     _mincyclicele=-10./180*pi;
  62     _min_pitch=-.5/180*pi;
  63     _cyclicele=0;
  64     _cyclicail=0;
  65     _name[0] = 0;
  66     _normal[0] = _normal[1] = 0;
  67     _normal[2] = 1;
  68     _normal_with_yaw_roll[0]= _normal_with_yaw_roll[1]=0;
  69     _normal_with_yaw_roll[2]=1;
  70     _number_of_blades=4;
  71     _omega=_omegan=_omegarel=_omegarelneu=0;
  72     _phi_null=0;
  73     _ddt_omega=0;
  74     _pitch_a=0;
  75     _pitch_b=0;
  76     _power_at_pitch_0=0;
  77     _power_at_pitch_b=0;
  78     _no_torque=0;
  79     _rel_blade_center=.7;
  80     _rel_len_hinge=0.01;
  81     _shared_flap_hinge=false;
  82     _rellenteeterhinge=0.01;
  83     _rotor_rpm=442;
  84     _sim_blades=0;
  85     _teeterdamp=0.00001;
  86     _translift=0.05;
  87     _weight_per_blade=42;
  88     _translift_ve=20;
  89     _translift_maxfactor=1.3;
  90     _ground_effect_constant=0.1;
  91     _vortex_state_lift_factor=0.4;
  92     _vortex_state_c1=0.1;
  93     _vortex_state_c2=0;
  94     _vortex_state_c3=0;
  95     _vortex_state_e1=1;
  96     _vortex_state_e2=1;
  97     _vortex_state_e3=1;
  98     _vortex_state=0;
  99     _lift_factor=1;
 100     _liftcoef=0.1;
 101     _dragcoef0=0.1;
 102     _dragcoef1=0.1;
 103     _twist=0;
 104     _number_of_segments=1;
 105     _number_of_parts=4;
 106     _rel_len_where_incidence_is_measured=0.7;
 107     _torque_of_inertia=1;
 108     _torque=0;
 109     _chord=0.3;
 110     _taper=1;
 111     _airfoil_incidence_no_lift=0;
 112     _rel_len_blade_start=0;
 113     _airfoil_lift_coefficient=0;
 114     _airfoil_drag_coefficient0=0;
 115     _airfoil_drag_coefficient1=0;
 116     for(i=0; i<2; i++)
 117         _global_ground[i] =  0;
 118     _global_ground[2] = 1;
 119     _global_ground[3] = -1e3;
 120     _incidence_stall_zero_speed=18*pi/180.;
 121     _incidence_stall_half_sonic_speed=14*pi/180.;
 122     _lift_factor_stall=0.28;
 123     _stall_change_over=2*pi/180.;
 124     _drag_factor_stall=8;
 125     _stall_sum=1;
 126     _stall_v2sum=1;
 127     _collective=0;
 128     _yaw=_roll=0;
 129     for (int k=0;k<4;k++)
 130         for (i=0;i<3;i++)
 131             _groundeffectpos[k][i]=0;
 132     _ground_effect_altitude=1;
 133     _cyclic_factor=1;
 134     _lift_factor=_f_ge=_f_vs=_f_tl=1;
 135     _rotor_correction_factor=.65;
 136     _balance1=1;
 137     _balance2=1;
 138     _properties_tied=0;
 139     _num_ground_contact_pos=0;
 140 }
 141
 142 Rotor::~Rotor()
 143 {
 144     int i;
 145     for(i=0; i<_rotorparts.size(); i++) {
 146         Rotorpart* r = (Rotorpart*)_rotorparts.get(i);
 147         delete r;
 148     }
 149     //untie the properties
 150     if(_properties_tied)
 151     {
 152         SGPropertyNode * node = fgGetNode("/rotors", true)->getNode(_name,true);
 153         node->untie("balance-ext");
 154         node->untie("balance-int");
 155         _properties_tied=0;
 156     }
 157 }
 158
 159 void Rotor::inititeration(float dt,float omegarel,float ddt_omegarel,float *rot)
 160 {
 161     _stall_sum=0;
 162     _stall_v2sum=0;
 163     _omegarel=omegarel;
 164     _omega=_omegan*_omegarel;
 165     _ddt_omega=_omegan*ddt_omegarel;
 166     int i;
 167     for(i=0; i<_rotorparts.size(); i++) {
 168         float s = Math::sin(float(2*pi*i/_number_of_parts+(_phi-pi/2.)*(_ccw?1:-1)));
 169         float c = Math::cos(float(2*pi*i/_number_of_parts+(_phi-pi/2.)*(_ccw?1:-1)));
 170         Rotorpart* r = (Rotorpart*)_rotorparts.get(i);
 171         r->setOmega(_omega);
 172         r->setDdtOmega(_ddt_omega);
 173         r->inititeration(dt,rot);
 174         r->setCyclic(_cyclicail*c+_cyclicele*s);
 175     }
 176
 177     //calculate the normal of the rotor disc, for calcualtion of the downwash
 178     float side[3],help[3];
 179     Math::cross3(_normal,_forward,side);
 180     Math::mul3(Math::cos(_yaw)*Math::cos(_roll),_normal,_normal_with_yaw_roll);
 181
 182     Math::mul3(Math::sin(_yaw),_forward,help);
 183     Math::add3(_normal_with_yaw_roll,help,_normal_with_yaw_roll);
 184
 185     Math::mul3(Math::sin(_roll),side,help);
 186     Math::add3(_normal_with_yaw_roll,help,_normal_with_yaw_roll);
 187
 188     //update balance
 189     if ((_balance1*_balance2 < 0.97) && (_balance1>-1))
 190     {
 191         _balance1-=(0.97-_balance1*_balance2)*(0.97-_balance1*_balance2)*0.00001;
 192         if (_balance1<-1) _balance1=-1;
 193     }
 194 }
 195
 196 float Rotor::calcStall(float incidence,float speed)
 197 {
 198     float stall_incidence=_incidence_stall_zero_speed
 199         +(_incidence_stall_half_sonic_speed
 200         -_incidence_stall_zero_speed)*speed/(343./2);
 201     //missing: Temeperature dependency of sonic speed
 202     incidence = Math::abs(incidence);
 203     if (incidence > (90./180.*pi))
 204         incidence = pi-incidence;
 205     float stall = (incidence-stall_incidence)/_stall_change_over;
 206     stall = Math::clamp(stall,0,1);
 207
 208     _stall_sum+=stall*speed*speed;
 209     _stall_v2sum+=speed*speed;
 210
 211     return stall;
 212 }
 213
 214 float Rotor::getLiftCoef(float incidence,float speed)
 215 {
 216     float stall=calcStall(incidence,speed);
 217     /* the next shold look like this, but this is the inner loop of
 218            the rotor simulation. For small angles (and we hav only small
 219            angles) the first order approximation works well
 220     float c1=  Math::sin(incidence-_airfoil_incidence_no_lift)*_liftcoef;
 221     for c2 we would need higher order, because in stall the angle can be large
 222     */
 223     float i2;
 224     if (incidence > (pi/2))
 225         i2 = incidence-pi;
 226     else if (incidence <-(pi/2))
 227         i2 = (incidence+pi);
 228     else
 229         i2 = incidence;
 230     float c1=  (i2-_airfoil_incidence_no_lift)*_liftcoef;
 231     if (stall > 0)
 232     {
 233     float c2=  Math::sin(2*(incidence-_airfoil_incidence_no_lift))
 234         *_liftcoef*_lift_factor_stall;
 235     return (1-stall)*c1 + stall *c2;
 236     }
 237     else
 238         return c1;
 239 }
 240
 241 float Rotor::getDragCoef(float incidence,float speed)
 242 {
 243     float stall=calcStall(incidence,speed);
 244     float c1= (Math::abs(Math::sin(incidence-_airfoil_incidence_no_lift))
 245         *_dragcoef1+_dragcoef0);
 246     float c2= c1*_drag_factor_stall;
 247     return (1-stall)*c1 + stall *c2;
 248 }
 249
 250 int Rotor::getValueforFGSet(int j,char *text,float *f)
 251 {
 252     if (_name[0]==0) return 0;
 253     if (4>numRotorparts()) return 0; //compile first!
 254     if (j==0)
 255     {
 256         sprintf(text,"/rotors/%s/cone-deg", _name);
 257         *f=(_balance1>-1)?( ((Rotorpart*)getRotorpart(0))->getrealAlpha()
 258             +((Rotorpart*)getRotorpart(1*(_number_of_parts>>2)))->getrealAlpha()
 259             +((Rotorpart*)getRotorpart(2*(_number_of_parts>>2)))->getrealAlpha()
 260             +((Rotorpart*)getRotorpart(3*(_number_of_parts>>2)))->getrealAlpha()
 261             )/4*180/pi:0;
 262     }
 263     else
 264         if (j==1)
 265         {
 266             sprintf(text,"/rotors/%s/roll-deg", _name);
 267             _roll = ( ((Rotorpart*)getRotorpart(0))->getrealAlpha()
 268                 -((Rotorpart*)getRotorpart(2*(_number_of_parts>>2)))->getrealAlpha()
 269                 )/2*(_ccw?-1:1);
 270             *f=(_balance1>-1)?_roll *180/pi:0;
 271         }
 272         else
 273             if (j==2)
 274             {
 275                 sprintf(text,"/rotors/%s/yaw-deg", _name);
 276                 _yaw=( ((Rotorpart*)getRotorpart(1*(_number_of_parts>>2)))->getrealAlpha()
 277                     -((Rotorpart*)getRotorpart(3*(_number_of_parts>>2)))->getrealAlpha()
 278                     )/2;
 279                 *f=(_balance1>-1)?_yaw*180/pi:0;
 280             }
 281             else
 282                 if (j==3)
 283                 {
 284                     sprintf(text,"/rotors/%s/rpm", _name);
 285                     *f=(_balance1>-1)?_omega/2/pi*60:0;
 286                 }
 287                 else
 288                     if (j==4)
 289                     {
 290                         sprintf(text,"/rotors/%s/debug/debugfge",_name);
 291                         *f=_f_ge;
 292                     }
 293                     else if (j==5)
 294                     {
 295                         sprintf(text,"/rotors/%s/debug/debugfvs",_name);
 296                         *f=_f_vs;
 297                     }
 298                     else if (j==6)
 299                     {
 300                         sprintf(text,"/rotors/%s/debug/debugftl",_name);
 301                         *f=_f_tl;
 302                     }
 303                     else if (j==7)
 304                     {
 305                         sprintf(text,"/rotors/%s/balance", _name);
 306                         *f=_balance1;
 307                     }
 308                     else if (j==8)
 309                     {
 310                         sprintf(text,"/rotors/%s/stall",_name);
 311                         *f=getOverallStall();
 312                     }
 313                     else if (j==9)
 314                     {
 315                         sprintf(text,"/rotors/%s/torque",_name);
 316                         *f=-_torque;;
 317                     }
 318                     else
 319                     {
 320                         int b=(j-10)/3;
 321                         if (b>=_number_of_blades)
 322                         {
 323                             return 0;
 324                         }
 325                         int w=j%3;
 326                         sprintf(text,"/rotors/%s/blade[%i]/%s",
 327                             _name,b,
 328                             w==0?"position-deg":(w==1?"flap-deg":"incidence-deg"));
 329                         *f=((Rotorpart*)getRotorpart(0))->getPhi()*180/pi
 330                             +360*b/_number_of_blades*(_ccw?1:-1);
 331                         if (*f>360) *f-=360;
 332                         if (*f<0) *f+=360;
 333                         if (_balance1<=-1) *f=0;
 334                         int k,l;
 335                         float rk,rl,p;
 336                         p=(*f/90);
 337                         k=int(p);
 338                         l=k+1;
 339                         rk=l-p;
 340                         rk=Math::clamp(rk,0,1);//Delete this
 341                         rl=1-rk;
 342                         if(w==2) {k+=2;l+=2;}
 343                         else
 344                             if(w==1) {k+=1;l+=1;}
 345                             k%=4;
 346                             l%=4;
 347                             if (w==1) *f=rk*((Rotorpart*) getRotorpart(k*(_number_of_parts>>2)))->getrealAlpha()*180/pi
 348                                 +rl*((Rotorpart*) getRotorpart(l*(_number_of_parts>>2)))->getrealAlpha()*180/pi;
 349                             else if(w==2) *f=rk*((Rotorpart*)getRotorpart(k*(_number_of_parts>>2)))->getIncidence()*180/pi
 350                                 +rl*((Rotorpart*)getRotorpart(l*(_number_of_parts>>2)))->getIncidence()*180/pi;
 351                     }
 352                     return j+1;
 353 }
 354
 355 void Rotorgear::setEngineOn(int value)
 356 {
 357     _engineon=value;
 358 }
 359
 360 void Rotorgear::setRotorEngineMaxRelTorque(float lval)
 361 {
 362     SGPropertyNode * node = fgGetNode("/rotors/gear/max-rel-torque", true);
 363     if (node) node->setDoubleValue(lval);
 364 }
 365
 366 void Rotorgear::setRotorRelTarget(float lval)
 367 {
 368     SGPropertyNode * node = fgGetNode("/rotors/gear/target-rel-rpm", true);
 369     if (node) node->setDoubleValue(lval);
 370 }
 371
 372 void Rotor::setAlpha0(float f)
 373 {
 374     _alpha0=f;
 375 }
 376
 377 void Rotor::setAlphamin(float f)
 378 {
 379     _alphamin=f;
 380 }
 381
 382 void Rotor::setAlphamax(float f)
 383 {
 384     _alphamax=f;
 385 }
 386
 387 void Rotor::setAlpha0factor(float f)
 388 {
 389     _alpha0factor=f;
 390 }
 391
 392 int Rotor::numRotorparts()
 393 {
 394     return _rotorparts.size();
 395 }
 396
 397 Rotorpart* Rotor::getRotorpart(int n)
 398 {
 399     return ((Rotorpart*)_rotorparts.get(n));
 400 }
 401
 402 int Rotorgear::getEngineon()
 403 {
 404     return _engineon;
 405 }
 406
 407 float Rotor::getTorqueOfInertia()
 408 {
 409     return _torque_of_inertia;
 410 }
 411
 412 void Rotor::setTorque(float f)
 413 {
 414     _torque=f;
 415 }
 416
 417 void Rotor::addTorque(float f)
 418 {
 419     _torque+=f;
 420 }
 421
 422 void Rotor::strncpy(char *dest,const char *src,int maxlen)
 423 {
 424     int n=0;
 425     while(src[n]&&n<(maxlen-1))
 426     {
 427         dest[n]=src[n];
 428         n++;
 429     }
 430     dest[n]=0;
 431 }
 432
 433 void Rotor::setNormal(float* normal)
 434 {
 435     int i;
 436     float invsum,sqrsum=0;
 437     for(i=0; i<3; i++) { sqrsum+=normal[i]*normal[i];}
 438     if (sqrsum!=0)
 439         invsum=1/Math::sqrt(sqrsum);
 440     else
 441         invsum=1;
 442     for(i=0; i<3; i++)
 443     {
 444         _normal_with_yaw_roll[i]=_normal[i] = normal[i]*invsum;
 445     }
 446 }
 447
 448 void Rotor::setForward(float* forward)
 449 {
 450     int i;
 451     float invsum,sqrsum=0;
 452     for(i=0; i<3; i++) { sqrsum+=forward[i]*forward[i];}
 453     if (sqrsum!=0)
 454         invsum=1/Math::sqrt(sqrsum);
 455     else
 456         invsum=1;
 457     for(i=0; i<3; i++) { _forward[i] = forward[i]*invsum; }
 458 }
 459
 460 void Rotor::setForceAtPitchA(float force)
 461 {
 462     _force_at_pitch_a=force;
 463 }
 464
 465 void Rotor::setPowerAtPitch0(float value)
 466 {
 467     _power_at_pitch_0=value;
 468 }
 469
 470 void Rotor::setPowerAtPitchB(float value)
 471 {
 472     _power_at_pitch_b=value;
 473 }
 474
 475 void Rotor::setPitchA(float value)
 476 {
 477     _pitch_a=value/180*pi;
 478 }
 479
 480 void Rotor::setPitchB(float value)
 481 {
 482     _pitch_b=value/180*pi;
 483 }
 484
 485 void Rotor::setBase(float* base)
 486 {
 487     int i;
 488     for(i=0; i<3; i++) _base[i] = base[i];
 489 }
 490
 491 void Rotor::setMaxCyclicail(float value)
 492 {
 493     _maxcyclicail=value/180*pi;
 494 }
 495
 496 void Rotor::setMaxCyclicele(float value)
 497 {
 498     _maxcyclicele=value/180*pi;
 499 }
 500
 501 void Rotor::setMinCyclicail(float value)
 502 {
 503     _mincyclicail=value/180*pi;
 504 }
 505
 506 void Rotor::setMinCyclicele(float value)
 507 {
 508     _mincyclicele=value/180*pi;
 509 }
 510
 511 void Rotor::setMinCollective(float value)
 512 {
 513     _min_pitch=value/180*pi;
 514 }
 515
 516 void Rotor::setMaxCollective(float value)
 517 {
 518     _max_pitch=value/180*pi;
 519 }
 520
 521 void Rotor::setDiameter(float value)
 522 {
 523     _diameter=value;
 524 }
 525
 526 void Rotor::setWeightPerBlade(float value)
 527 {
 528     _weight_per_blade=value;
 529 }
 530
 531 void Rotor::setNumberOfBlades(float value)
 532 {
 533     _number_of_blades=int(value+.5);
 534 }
 535
 536 void Rotor::setRelBladeCenter(float value)
 537 {
 538     _rel_blade_center=value;
 539 }
 540
 541 void Rotor::setDynamic(float value)
 542 {
 543     _dynamic=value;
 544 }
 545
 546 void Rotor::setDelta3(float value)
 547 {
 548     _delta3=value;
 549 }
 550
 551 void Rotor::setDelta(float value)
 552 {
 553     _delta=value;
 554 }
 555
 556 void Rotor::setTranslift(float value)
 557 {
 558     _translift=value;
 559 }
 560
 561 void Rotor::setSharedFlapHinge(bool s)
 562 {
 563     _shared_flap_hinge=s;
 564 }
 565
 566 void Rotor::setBalance(float b)
 567 {
 568     _balance1=b;
 569 }
 570
 571 void Rotor::setC2(float value)
 572 {
 573     _c2=value;
 574 }
 575
 576 void Rotor::setStepspersecond(float steps)
 577 {
 578     _stepspersecond=steps;
 579 }
 580
 581 void Rotor::setRPM(float value)
 582 {
 583     _rotor_rpm=value;
 584 }
 585
 586 void Rotor::setPhiNull(float value)
 587 {
 588     _phi_null=value;
 589 }
 590
 591 void Rotor::setRelLenHinge(float value)
 592 {
 593     _rel_len_hinge=value;
 594 }
 595
 596 void Rotor::setAlphaoutput(int i, const char *text)
 597 {
 598     strncpy(_alphaoutput[i],text,255);
 599 }
 600
 601 void Rotor::setName(const char *text)
 602 {
 603     strncpy(_name,text,256);//256: some space needed for settings
 604 }
 605
 606 void Rotor::setCcw(int ccw)
 607 {
 608     _ccw=ccw;
 609 }
 610
 611 void Rotor::setNotorque(int value)
 612 {
 613     _no_torque=value;
 614 }
 615
 616 void Rotor::setRelLenTeeterHinge(float f)
 617 {
 618     _rellenteeterhinge=f;
 619 }
 620
 621 void Rotor::setTeeterdamp(float f)
 622 {
 623     _teeterdamp=f;
 624 }
 625
 626 void Rotor::setMaxteeterdamp(float f)
 627 {
 628     _maxteeterdamp=f;
 629 }
 630
 631 void Rotor::setGlobalGround(double *global_ground, float* global_vel)
 632 {
 633     int i;
 634     for(i=0; i<4; i++) _global_ground[i] = global_ground[i];
 635 }
 636
 637 void Rotor::setParameter(char *parametername, float value)
 638 {
 639 #define p(a,b) if (strcmp(parametername,#a)==0) _##a = (value * (b)); else
 640     p(translift_ve,1)
 641         p(translift_maxfactor,1)
 642         p(ground_effect_constant,1)
 643         p(vortex_state_lift_factor,1)
 644         p(vortex_state_c1,1)
 645         p(vortex_state_c2,1)
 646         p(vortex_state_c3,1)
 647         p(vortex_state_e1,1)
 648         p(vortex_state_e2,1)
 649         p(vortex_state_e3,1)
 650         p(twist,pi/180.)
 651         p(number_of_segments,1)
 652         p(number_of_parts,1)
 653         p(rel_len_where_incidence_is_measured,1)
 654         p(chord,1)
 655         p(taper,1)
 656         p(airfoil_incidence_no_lift,pi/180.)
 657         p(rel_len_blade_start,1)
 658         p(incidence_stall_zero_speed,pi/180.)
 659         p(incidence_stall_half_sonic_speed,pi/180.)
 660         p(lift_factor_stall,1)
 661         p(stall_change_over,pi/180.)
 662         p(drag_factor_stall,1)
 663         p(airfoil_lift_coefficient,1)
 664         p(airfoil_drag_coefficient0,1)
 665         p(airfoil_drag_coefficient1,1)
 666         p(cyclic_factor,1)
 667         p(rotor_correction_factor,1)
 668         SG_LOG(SG_INPUT, SG_ALERT,
 669             "internal error in parameter set up for rotor: '" <<
 670             parametername <<"'" << endl);
 671 #undef p
 672 }
 673
 674 float Rotor::getLiftFactor()
 675 {
 676     return _lift_factor;
 677 }
 678
 679 void Rotorgear::setRotorBrake(float lval)
 680 {
 681     lval = Math::clamp(lval, 0, 1);
 682     _rotorbrake=lval;
 683 }
 684
 685 void Rotor::setCollective(float lval)
 686 {
 687     lval = Math::clamp(lval, -1, 1);
 688     int i;
 689     _collective=_min_pitch+(lval+1)/2*(_max_pitch-_min_pitch);
 690     for(i=0; i<_rotorparts.size(); i++) {
 691         ((Rotorpart*)_rotorparts.get(i))->setCollective(_collective);
 692     }
 693 }
 694
 695 void Rotor::setCyclicele(float lval,float rval)
 696 {
 697     lval = Math::clamp(lval, -1, 1);
 698     _cyclicele=_mincyclicele+(lval+1)/2*(_maxcyclicele-_mincyclicele);
 699 }
 700
 701 void Rotor::setCyclicail(float lval,float rval)
 702 {
 703     lval = Math::clamp(lval, -1, 1);
 704     if (_ccw) lval *=-1;
 705     _cyclicail=-(_mincyclicail+(lval+1)/2*(_maxcyclicail-_mincyclicail));
 706 }
 707
 708 void Rotor::getPosition(float* out)
 709 {
 710     int i;
 711     for(i=0; i<3; i++) out[i] = _base[i];
 712 }
 713
 714 void Rotor::calcLiftFactor(float* v, float rho, State *s)
 715 {
 716     //calculates _lift_factor, which is a foactor for the lift of the rotor
 717     //due to
 718     //- ground effect (_f_ge)
 719     //- vortex state (_f_vs)
 720     //- translational lift (_f_tl)
 721     _f_ge=1;
 722     _f_tl=1;
 723     _f_vs=1;
 724
 725     // Calculate ground effect
 726     _f_ge=1+_diameter/_ground_effect_altitude*_ground_effect_constant;
 727
 728     // Now calculate translational lift
 729     float v_vert = Math::dot3(v,_normal);
 730     float help[3];
 731     Math::cross3(v,_normal,help);
 732     float v_horiz = Math::mag3(help);
 733     _f_tl = ((1-Math::pow(2.7183,-v_horiz/_translift_ve))
 734         *(_translift_maxfactor-1)+1)/_translift_maxfactor;
 735
 736     _lift_factor = _f_ge*_f_tl*_f_vs;
 737
 738     //store the gravity direction
 739     Glue::geodUp(s->pos, _grav_direction);
 740 }
 741
 742 void Rotor::findGroundEffectAltitude(Ground * ground_cb,State *s)
 743 {
 744     _ground_effect_altitude=findGroundEffectAltitude(ground_cb,s,
 745         _groundeffectpos[0],_groundeffectpos[1],
 746         _groundeffectpos[2],_groundeffectpos[3]);
 747     testForRotorGroundContact(ground_cb,s);
 748 }
 749
 750 void Rotor::testForRotorGroundContact(Ground * ground_cb,State *s)
 751 {
 752     int i;
 753     for (i=0;i<_num_ground_contact_pos;i++)
 754     {
 755         double pt[3],h;
 756         s->posLocalToGlobal(_ground_contact_pos[i], pt);
 757
 758         // Ask for the ground plane in the global coordinate system
 759         double global_ground[4];
 760         float global_vel[3];
 761         ground_cb->getGroundPlane(pt, global_ground, global_vel);
 762         // find h, the distance to the ground
 763         // The ground plane transformed to the local frame.
 764         float ground[4];
 765         s->planeGlobalToLocal(global_ground, ground);
 766
 767         h = ground[3] - Math::dot3(_ground_contact_pos[i], ground);
 768         // Now h is the distance from _ground_contact_pos[i] to ground
 769         if (h<0)
 770         {
 771             _balance1 -= (-h)/_diameter/_num_ground_contact_pos;
 772             _balance1 = (_balance1<-1)?-1:_balance1;
 773         }
 774     }
 775 }
 776 float Rotor::findGroundEffectAltitude(Ground * ground_cb,State *s,
 777         float *pos0,float *pos1,float *pos2,float *pos3,
 778         int iteration,float a0,float a1,float a2,float a3)
 779 {
 780     float a[5];
 781     float *p[5],pos4[3];
 782     a[0]=a0;
 783     a[1]=a1;
 784     a[2]=a2;
 785     a[3]=a3;
 786     a[4]=-1;
 787     p[0]=pos0;
 788     p[1]=pos1;
 789     p[2]=pos2;
 790     p[3]=pos3;
 791     p[4]=pos4;
 792     Math::add3(p[0],p[2],p[4]);
 793     Math::mul3(0.5,p[4],p[4]);//the center
 794
 795     float mina=100*_diameter;
 796     float suma=0;
 797     for (int i=0;i<5;i++)
 798     {
 799         if (a[i]==-1)//in the first iteration,(iteration==0) no height is
 800                      //passed to this function, these missing values are
 801                      //marked by ==-1
 802         {
 803             double pt[3];
 804             s->posLocalToGlobal(p[i], pt);
 805
 806             // Ask for the ground plane in the global coordinate system
 807             double global_ground[4];
 808             float global_vel[3];
 809             ground_cb->getGroundPlane(pt, global_ground, global_vel);
 810             // find h, the distance to the ground
 811             // The ground plane transformed to the local frame.
 812             float ground[4];
 813             s->planeGlobalToLocal(global_ground, ground);
 814
 815             a[i] = ground[3] - Math::dot3(p[i], ground);
 816             // Now a[i] is the distance from p[i] to ground
 817         }
 818         suma+=a[i];
 819         if (a[i]<mina)
 820             mina=a[i];
 821     }
 822     if (mina>=10*_diameter)
 823         return mina; //the ground effect will be zero
 824
 825     //check if further recursion is neccessary
 826     //if the height does not differ more than 20%, than
 827     //we can return then mean height, if not split
 828     //zhe square to four parts and calcualte the height
 829     //for each part
 830     //suma * 0.2 is the mean
 831     //0.15 is the maximum allowed difference from the mean
 832     //to the height at the center
 833     if ((iteration>2)
 834        ||(Math::abs(suma*0.2-a[4])<(0.15*0.2*suma*(1<<iteration))))
 835         return suma*0.2;
 836     suma=0;
 837     float pc[4][3],ac[4]; //pc[i]=center of pos[i] and pos[(i+1)&3]
 838     for (int i=0;i<4;i++)
 839     {
 840         Math::add3(p[i],p[(i+1)&3],pc[i]);
 841         Math::mul3(0.5,pc[i],pc[i]);
 842         double pt[3];
 843         s->posLocalToGlobal(pc[i], pt);
 844
 845         // Ask for the ground plane in the global coordinate system
 846         double global_ground[4];
 847         float global_vel[3];
 848         ground_cb->getGroundPlane(pt, global_ground, global_vel);
 849         // find h, the distance to the ground
 850         // The ground plane transformed to the local frame.
 851         float ground[4];
 852         s->planeGlobalToLocal(global_ground, ground);
 853
 854         ac[i] = ground[3] - Math::dot3(p[i], ground);
 855         // Now ac[i] is the distance from pc[i] to ground
 856     }
 857     return 0.25*
 858         (findGroundEffectAltitude(ground_cb,s,p[0],pc[1],p[4],pc[3],
 859             iteration+1,a[0],ac[0],a[4],ac[3])
 860         +findGroundEffectAltitude(ground_cb,s,p[1],pc[0],p[4],pc[1],
 861             iteration+1,a[1],ac[0],a[4],ac[1])
 862         +findGroundEffectAltitude(ground_cb,s,p[2],pc[1],p[4],pc[2],
 863             iteration+1,a[2],ac[1],a[4],ac[2])
 864         +findGroundEffectAltitude(ground_cb,s,p[3],pc[2],p[4],pc[3],
 865             iteration+1,a[3],ac[2],a[4],ac[3])
 866         );
 867 }
 868
 869 void Rotor::getDownWash(float *pos, float *v_heli, float *downwash)
 870 {
 871     float pos2rotor[3],tmp[3];
 872     Math::sub3(_base,pos,pos2rotor);
 873     float dist=Math::dot3(pos2rotor,_normal_with_yaw_roll);
 874     //calculate incidence at 0.7r;
 875     float inc = _collective+_twist *0.7
 876                 - _twist*_rel_len_where_incidence_is_measured;
 877     if (inc < 0)
 878         dist *=-1;
 879     if (dist<0) // we are not in the downwash region
 880     {
 881         downwash[0]=downwash[1]=downwash[2]=0.;
 882         return;
 883     }
 884
 885     //calculate the mean downwash speed directly beneath the rotor disk
 886     float v1bar = Math::sin(inc) *_omega * 0.35 * _diameter * 0.8;
 887     //0.35 * d = 0.7 *r, a good position to calcualte the mean downwashd
 888     //0.8 the slip of the rotor.
 889
 890     //calculate the time the wind needed from thr rotor to here
 891     if (v1bar< 1) v1bar = 1;
 892     float time=dist/v1bar;
 893
 894     //calculate the pos2rotor, where the rotor was, "time" ago
 895     Math::mul3(time,v_heli,tmp);
 896     Math::sub3(pos2rotor,tmp,pos2rotor);
 897
 898     //and again calculate dist
 899     dist=Math::dot3(pos2rotor,_normal_with_yaw_roll);
 900     //missing the normal is offen not pointing to the normal of the rotor
 901     //disk. Rotate the normal by yaw and tilt angle
 902
 903     if (inc < 0)
 904         dist *=-1;
 905     if (dist<0) // we are not in the downwash region
 906     {
 907         downwash[0]=downwash[1]=downwash[2]=0.;
 908         return;
 909     }
 910     //of course this could be done in a runge kutta integrator, but it's such
 911     //a approximation that I beleave, it would'nt be more realistic
 912
 913     //calculate the dist to the rotor-axis
 914     Math::cross3(pos2rotor,_normal_with_yaw_roll,tmp);
 915     float r= Math::mag3(tmp);
 916     //calculate incidence at r;
 917     float rel_r = r *2 /_diameter;
 918     float inc_r = _collective+_twist * r /_diameter * 2
 919         - _twist*_rel_len_where_incidence_is_measured;
 920
 921     //calculate the downwash speed directly beneath the rotor disk
 922     float v1=0;
 923     if (rel_r<1)
 924         v1 = Math::sin(inc_r) *_omega * r * 0.8;
 925
 926     //calcualte the downwash speed in a distance "dist" to the rotor disc,
 927     //for large dist. The speed is assumed do follow a gausian distribution
 928     //with sigma increasing with dist^2:
 929     //sigma is assumed to be half of the rotor diameter directly beneath the
 930     //disc and is assumed to the rotor diameter at dist = (diameter * sqrt(2))
 931
 932     float sigma=_diameter/2 + dist * dist / _diameter /4.;
 933     float v2 = v1bar*_diameter/ (Math::sqrt(2 * pi) * sigma)
 934         * Math::pow(2.7183,-.5*r*r/(sigma*sigma))*_diameter/2/sigma;
 935
 936     //calculate the weight of the two downwash velocities.
 937     //Directly beneath the disc it is v1, far away it is v2
 938     float g = Math::pow(2.7183,-2*dist/_diameter);
 939     //at dist = rotor radius it is assumed to be 1/e * v1 + (1-1/e)* v2
 940
 941     float v = g * v1 + (1-g) * v2;
 942     Math::mul3(-v,_normal_with_yaw_roll,downwash);
 943     //the downwash is calculated in the opposite direction of the normal
 944 }
 945
 946 void Rotor::compile()
 947 {
 948     // Have we already been compiled?
 949     if(_rotorparts.size() != 0) return;
 950
 951     //rotor is divided into _number_of_parts parts
 952     //each part is calcualted at _number_of_segments points
 953
 954     //clamp to 4..256
 955     //and make it a factor of 4
 956     _number_of_parts=(int(Math::clamp(_number_of_parts,4,256))>>2)<<2;
 957
 958     _dynamic=_dynamic*(1/                          //inverse of the time
 959         ( (60/_rotor_rpm)/4         //for rotating 90 deg
 960         +(60/_rotor_rpm)/(2*_number_of_blades) //+ meantime a rotorblade
 961                                                //will pass a given point
 962         ));
 963     float directions[5][3];
 964     //pointing forward, right, ... the 5th is ony for calculation
 965     directions[0][0]=_forward[0];
 966     directions[0][1]=_forward[1];
 967     directions[0][2]=_forward[2];
 968     int i;
 969     for (i=1;i<5;i++)
 970     {
 971         if (!_ccw)
 972             Math::cross3(directions[i-1],_normal,directions[i]);
 973         else
 974             Math::cross3(_normal,directions[i-1],directions[i]);
 975         Math::unit3(directions[i],directions[i]);
 976     }
 977     Math::set3(directions[4],directions[0]);
 978     // now directions[0] is perpendicular to the _normal.and has a length
 979     // of 1. if _forward is already normalized and perpendicular to the
 980     // normal, directions[0] will be the same
 981     _num_ground_contact_pos=(_number_of_parts<16)?_number_of_parts:16;
 982     for (i=0;i<_num_ground_contact_pos;i++)
 983     {
 984         float help[3];
 985         float s = Math::sin(pi*2*_num_ground_contact_pos/i);
 986         float c = Math::cos(pi*2*_num_ground_contact_pos/i);
 987         Math::mul3(c*_diameter*0.5,directions[0],_ground_contact_pos[i]);
 988         Math::mul3(s*_diameter*0.5,directions[1],help);
 989         Math::add3(help,_ground_contact_pos[i],_ground_contact_pos[i]);
 990         Math::add3(_base,_ground_contact_pos[i],_ground_contact_pos[i]);
 991     }
 992     for (i=0;i<4;i++)
 993     {
 994         Math::mul3(_diameter*0.7,directions[i],_groundeffectpos[i]);
 995         Math::add3(_base,_groundeffectpos[i],_groundeffectpos[i]);
 996     }
 997     float rotorpartmass = _weight_per_blade*_number_of_blades/_number_of_parts*.453;
 998     //was pounds -> now kg
 999
1000     _torque_of_inertia = 1/12. * ( _number_of_parts * rotorpartmass) * _diameter
1001         * _diameter * _rel_blade_center * _rel_blade_center /(0.5*0.5);
1002     float speed=_rotor_rpm/60*_diameter*_rel_blade_center*pi;
1003     float lentocenter=_diameter*_rel_blade_center*0.5;
1004     float lentoforceattac=_diameter*_rel_len_hinge*0.5;
1005     float zentforce=rotorpartmass*speed*speed/lentocenter;
1006     float pitchaforce=_force_at_pitch_a/_number_of_parts*.453*9.81;
1007     // was pounds of force, now N, devided by _number_of_parts
1008     //(so its now per rotorpart)
1009
1010     float torque0=0,torquemax=0,torqueb=0;
1011     float omega=_rotor_rpm/60*2*pi;
1012     _omegan=omega;
1013     float omega0=omega*Math::sqrt(1/(1-_rel_len_hinge));
1014     float delta_theoretical=pitchaforce/(_pitch_a*omega*lentocenter*2*rotorpartmass);
1015     _delta*=delta_theoretical;
1016
1017     float relamp=(omega*omega/(2*_delta*Math::sqrt(sqr(omega0*omega0-omega*omega)
1018         +4*_delta*_delta*omega*omega)))*_cyclic_factor;
1019     float relamp_theoretical=(omega*omega/(2*delta_theoretical*Math::sqrt(sqr(omega0*omega0-omega*omega)
1020         +4*delta_theoretical*delta_theoretical*omega*omega)))*_cyclic_factor;
1021     _phi=Math::acos(_rel_len_hinge);
1022     _phi-=Math::atan(_delta3);
1023     if (!_no_torque)
1024     {
1025         torque0=_power_at_pitch_0/_number_of_parts*1000/omega;
1026         // f*r=p/w ; p=f*s/t;  r=s/t/w ; r*w*t = s
1027         torqueb=_power_at_pitch_b/_number_of_parts*1000/omega;
1028         torquemax=_power_at_pitch_b/_number_of_parts*1000/omega/_pitch_b*_max_pitch;
1029
1030         if(_ccw)
1031         {
1032             torque0*=-1;
1033             torquemax*=-1;
1034             torqueb*=-1;
1035         }
1036     }
1037
1038     Rotorpart* rps[256];
1039     for (i=0;i<_number_of_parts;i++)
1040     {
1041         float lpos[3],lforceattac[3],lspeed[3],dirzentforce[3];
1042         float s = Math::sin(2*pi*i/_number_of_parts);
1043         float c = Math::cos(2*pi*i/_number_of_parts);
1044         float sp = Math::sin(float(2*pi*i/_number_of_parts-pi/2.+_phi));
1045         float cp = Math::cos(float(2*pi*i/_number_of_parts-pi/2.+_phi));
1046         float direction[3],nextdirection[3],help[3],direction90deg[3];
1047         Math::mul3(c ,directions[0],help);
1048         Math::mul3(s ,directions[1],direction);
1049         Math::add3(help,direction,direction);
1050
1051         Math::mul3(c ,directions[1],help);
1052         Math::mul3(s ,directions[2],direction90deg);
1053         Math::add3(help,direction90deg,direction90deg);
1054
1055         Math::mul3(cp ,directions[1],help);
1056         Math::mul3(sp ,directions[2],nextdirection);
1057         Math::add3(help,nextdirection,nextdirection);
1058
1059         Math::mul3(lentocenter,direction,lpos);
1060         Math::add3(lpos,_base,lpos);
1061         Math::mul3(lentoforceattac,nextdirection,lforceattac);
1062         //nextdirection: +90deg (gyro)!!!
1063
1064         Math::add3(lforceattac,_base,lforceattac);
1065         Math::mul3(speed,direction90deg,lspeed);
1066         Math::mul3(1,nextdirection,dirzentforce);
1067
1068         Rotorpart* rp=rps[i]=newRotorpart(lpos, lforceattac, _normal,
1069             lspeed,dirzentforce,zentforce,pitchaforce,_delta3,rotorpartmass,
1070             _translift,_rel_len_hinge,lentocenter);
1071         int k = i*4/_number_of_parts;
1072         rp->setAlphaoutput(_alphaoutput[k&1?k:(_ccw?k^2:k)],0);
1073         rp->setAlphaoutput(_alphaoutput[4+(k&1?k:(_ccw?k^2:k))],1+(k>1));
1074         _rotorparts.add(rp);
1075         rp->setTorque(torquemax,torque0);
1076         rp->setRelamp(relamp);
1077         rp->setDirectionofRotorPart(direction);
1078         rp->setTorqueOfInertia(_torque_of_inertia/_number_of_parts);
1079         rp->setDirection(2*pi*i/_number_of_parts);
1080     }
1081     for (i=0;i<_number_of_parts;i++)
1082     {
1083         rps[i]->setlastnextrp(rps[(i-1+_number_of_parts)%_number_of_parts],
1084             rps[(i+1)%_number_of_parts],
1085             rps[(i+_number_of_parts/2)%_number_of_parts],
1086             rps[(i-_number_of_parts/4+_number_of_parts)%_number_of_parts],
1087             rps[(i+_number_of_parts/4)%_number_of_parts]);
1088     }
1089     for (i=0;i<_number_of_parts;i++)
1090     {
1091         rps[i]->setCompiled();
1092     }
1093     float lift[4],torque[4], v_wind[3];
1094     v_wind[0]=v_wind[1]=v_wind[2]=0;
1095     rps[0]->setOmega(_omegan);
1096
1097     if (_airfoil_lift_coefficient==0)
1098     {
1099         //calculate the lift and drag coefficients now
1100         _dragcoef0=1;
1101         _dragcoef1=1;
1102         _liftcoef=1;
1103         rps[0]->calculateAlpha(v_wind,rho_null,_pitch_a,0,0,
1104             &(torque[0]),&(lift[0])); //max_pitch a
1105         _liftcoef = pitchaforce/lift[0];
1106         _dragcoef0=1;
1107         _dragcoef1=0;
1108         rps[0]->calculateAlpha(v_wind,rho_null,0,0,0,&(torque[0]),&(lift[0]));
1109         //0 degree, c0
1110
1111         _dragcoef0=0;
1112         _dragcoef1=1;
1113         rps[0]->calculateAlpha(v_wind,rho_null,0,0,0,&(torque[1]),&(lift[1]));
1114         //0 degree, c1
1115
1116         _dragcoef0=1;
1117         _dragcoef1=0;
1118         rps[0]->calculateAlpha(v_wind,rho_null,_pitch_b,0,0,&(torque[2]),&(lift[2]));
1119         //picth b, c0
1120
1121         _dragcoef0=0;
1122         _dragcoef1=1;
1123         rps[0]->calculateAlpha(v_wind,rho_null,_pitch_b,0,0,&(torque[3]),&(lift[3]));
1124         //picth b, c1
1125
1126         if (torque[0]==0)
1127         {
1128             _dragcoef1=torque0/torque[1];
1129             _dragcoef0=(torqueb-_dragcoef1*torque[3])/torque[2];
1130         }
1131         else
1132         {
1133             _dragcoef1=(torque0/torque[0]-torqueb/torque[2])
1134                 /(torque[1]/torque[0]-torque[3]/torque[2]);
1135             _dragcoef0=(torqueb-_dragcoef1*torque[3])/torque[2];
1136         }
1137     }
1138     else
1139     {
1140         _liftcoef=_airfoil_lift_coefficient/_number_of_parts*_number_of_blades;
1141         _dragcoef0=_airfoil_drag_coefficient0/_number_of_parts*_number_of_blades*_c2;
1142         _dragcoef1=_airfoil_drag_coefficient1/_number_of_parts*_number_of_blades*_c2;
1143     }
1144
1145     //Check
1146     rps[0]->calculateAlpha(v_wind,rho_null,_pitch_a,0,0,
1147         &(torque[0]),&(lift[0])); //pitch a
1148     rps[0]->calculateAlpha(v_wind,rho_null,_pitch_b,0,0,
1149         &(torque[1]),&(lift[1])); //pitch b
1150     rps[0]->calculateAlpha(v_wind,rho_null,0,0,0,
1151         &(torque[3]),&(lift[3])); //pitch 0
1152     SG_LOG(SG_GENERAL, SG_INFO,
1153         "Rotor: coefficients for airfoil:" << endl << setprecision(6)
1154         << " drag0: " << _dragcoef0*_number_of_parts/_number_of_blades/_c2
1155         << " drag1: " << _dragcoef1*_number_of_parts/_number_of_blades/_c2
1156         << " lift: " << _liftcoef*_number_of_parts/_number_of_blades
1157         << endl
1158         << "at 10 deg:" << endl
1159         << "drag: " << (Math::sin(10./180*pi)*_dragcoef1+_dragcoef0)
1160             *_number_of_parts/_number_of_blades/_c2
1161         << " lift: " << Math::sin(10./180*pi)*_liftcoef*_number_of_parts/_number_of_blades
1162         << endl
1163         << "Some results (Pitch [degree], Power [kW], Lift [N])" << endl
1164         << 0.0f << "deg " << Math::abs(torque[3]*_number_of_parts*_omegan/1000) << "kW "
1165             << lift[3]*_number_of_parts << endl
1166         << _pitch_a*180/pi << "deg " << Math::abs(torque[0]*_number_of_parts*_omegan/1000)
1167             << "kW " << lift[0]*_number_of_parts << endl
1168         << _pitch_b*180/pi << "deg " << Math::abs(torque[1]*_number_of_parts*_omegan/1000)
1169             << "kW " << lift[1]*_number_of_parts << endl << endl );
1170
1171     //first calculation of relamp is wrong
1172     //it used pitchaforce, but this was unknown and
1173     //on the default value
1174     _delta*=lift[0]/pitchaforce;
1175     relamp=(omega*omega/(2*_delta*Math::sqrt(sqr(omega0*omega0-omega*omega)
1176         +4*_delta*_delta*omega*omega)))*_cyclic_factor;
1177     for (i=0;i<_number_of_parts;i++)
1178     {
1179         rps[i]->setRelamp(relamp);
1180     }
1181     rps[0]->setOmega(0);
1182     writeInfo();
1183
1184     //tie the properties
1185     /* After reset these values are totally wrong. I have to find out why
1186     SGPropertyNode * node = fgGetNode("/rotors", true)->getNode(_name,true);
1187     node->tie("balance_ext",SGRawValuePointer<float>(&_balance2),false);
1188     node->tie("balance_int",SGRawValuePointer<float>(&_balance1));
1189     _properties_tied=1;
1190     */
1191 }
1192 std::ostream &  operator<<(std::ostream & out, Rotor& r)
1193 {
1194 #define i(x) << #x << ":" << r.x << endl
1195 #define iv(x) << #x << ":" << r.x[0] << ";" << r.x[1] << ";" <<r.x[2] << ";" << endl
1196     out << "Writing Info on Rotor "
1197     i(_name)
1198     i(_torque)
1199     i(_omega) i(_omegan) i(_omegarel) i(_ddt_omega) i(_omegarelneu)
1200     i (_chord)
1201     i( _taper)
1202     i( _airfoil_incidence_no_lift)
1203     i( _collective)
1204     i( _airfoil_lift_coefficient)
1205     i( _airfoil_drag_coefficient0)
1206     i( _airfoil_drag_coefficient1)
1207     i( _ccw)
1208     i( _number_of_segments)
1209     i( _number_of_parts)
1210     iv( _base)
1211     iv( _groundeffectpos[0])iv( _groundeffectpos[1])iv( _groundeffectpos[2])iv( _groundeffectpos[3])
1212     i( _ground_effect_altitude)
1213     iv( _normal)
1214     iv( _normal_with_yaw_roll)
1215     iv( _forward)
1216     i( _diameter)
1217     i( _number_of_blades)
1218     i( _weight_per_blade)
1219     i( _rel_blade_center)
1220     i( _min_pitch)
1221     i( _max_pitch)
1222     i( _force_at_pitch_a)
1223     i( _pitch_a)
1224     i( _power_at_pitch_0)
1225     i( _power_at_pitch_b)
1226     i( _no_torque)
1227     i( _sim_blades)
1228     i( _pitch_b)
1229     i( _rotor_rpm)
1230     i( _rel_len_hinge)
1231     i( _maxcyclicail)
1232     i( _maxcyclicele)
1233     i( _mincyclicail)
1234     i( _mincyclicele)
1235     i( _delta3)
1236     i( _delta)
1237     i( _dynamic)
1238     i( _translift)
1239     i( _c2)
1240     i( _stepspersecond)
1241     i( _engineon)
1242     i( _alphamin) i(_alphamax) i(_alpha0) i(_alpha0factor)
1243     i( _teeterdamp) i(_maxteeterdamp)
1244     i( _rellenteeterhinge)
1245     i( _translift_ve)
1246     i( _translift_maxfactor)
1247     i( _ground_effect_constant)
1248     i( _vortex_state_lift_factor)
1249     i( _vortex_state_c1)
1250     i( _vortex_state_c2)
1251     i( _vortex_state_c3)
1252     i( _vortex_state_e1)
1253     i( _vortex_state_e2)
1254     i( _vortex_state_e3)
1255     i( _lift_factor) i(_f_ge) i(_f_vs) i(_f_tl)
1256     i( _vortex_state)
1257     i( _liftcoef)
1258     i( _dragcoef0)
1259     i( _dragcoef1)
1260     i( _twist) //outer incidence = inner inner incidence + _twist
1261     i( _rel_len_where_incidence_is_measured)
1262     i( _torque_of_inertia)
1263     i( _rel_len_blade_start)
1264     i( _incidence_stall_zero_speed)
1265     i( _incidence_stall_half_sonic_speed)
1266     i( _lift_factor_stall)
1267     i( _stall_change_over)
1268     i( _drag_factor_stall)
1269     i( _stall_sum)
1270     i( _stall_v2sum)
1271     i( _yaw)
1272     i( _roll)
1273     i( _cyclicail)
1274     i( _cyclicele)
1275     i( _cyclic_factor) <<endl;
1276     int j;
1277     for(j=0; j<r._rotorparts.size(); j++) {
1278         out << *((Rotorpart*)r._rotorparts.get(j));
1279     }
1280     out <<endl << endl;
1281 #undef i
1282 #undef iv
1283     return out;
1284 }
1285 void Rotor:: writeInfo()
1286 {
1287 #ifdef TEST_DEBUG
1288     std::ostringstream buffer;
1289     buffer << *this;
1290     FILE*f=fopen("c:\\fgmsvc\\bat\\log.txt","at");
1291     if (!f) f=fopen("c:\\fgmsvc\\bat\\log.txt","wt");
1292     if (f)
1293     {
1294         fprintf(f,"%s",(const char *)buffer.str().c_str());
1295         fclose (f);
1296     }
1297 #endif
1298 }
1299 Rotorpart* Rotor::newRotorpart(float* pos, float *posforceattac, float *normal,
1300     float* speed,float *dirzentforce, float zentforce,float maxpitchforce,
1301     float delta3,float mass,float translift,float rellenhinge,float len)
1302 {
1303     Rotorpart *r = new Rotorpart();
1304     r->setPosition(pos);
1305     r->setNormal(normal);
1306     r->setZentipetalForce(zentforce);
1307     r->setPositionForceAttac(posforceattac);
1308     r->setSpeed(speed);
1309     r->setDirectionofZentipetalforce(dirzentforce);
1310     r->setDelta3(delta3);
1311     r->setDynamic(_dynamic);
1312     r->setTranslift(_translift);
1313     r->setC2(_c2);
1314     r->setWeight(mass);
1315     r->setRelLenHinge(rellenhinge);
1316     r->setSharedFlapHinge(_shared_flap_hinge);
1317     r->setOmegaN(_omegan);
1318     r->setPhi(_phi_null);
1319     r->setAlpha0(_alpha0);
1320     r->setAlphamin(_alphamin);
1321     r->setAlphamax(_alphamax);
1322     r->setAlpha0factor(_alpha0factor);
1323     r->setLen(len);
1324     r->setDiameter(_diameter);
1325     r->setRotor(this);
1326 #define p(a) r->setParameter(#a,_##a);
1327     p(twist)
1328     p(number_of_segments)
1329     p(rel_len_where_incidence_is_measured)
1330     p(rel_len_blade_start)
1331     p(rotor_correction_factor)
1332 #undef p
1333     return r;
1334 }
1335
1336 void Rotor::interp(float* v1, float* v2, float frac, float* out)
1337 {
1338     out[0] = v1[0] + frac*(v2[0]-v1[0]);
1339     out[1] = v1[1] + frac*(v2[1]-v1[1]);
1340     out[2] = v1[2] + frac*(v2[2]-v1[2]);
1341 }
1342
1343 void Rotorgear::initRotorIteration(float *lrot,float dt)
1344 {
1345     int i;
1346     float omegarel;
1347     if (!_rotors.size()) return;
1348     Rotor* r0 = (Rotor*)_rotors.get(0);
1349     omegarel= r0->getOmegaRelNeu();
1350     for(i=0; i<_rotors.size(); i++) {
1351         Rotor* r = (Rotor*)_rotors.get(i);
1352         r->inititeration(dt,omegarel,0,lrot);
1353     }
1354 }
1355
1356 void Rotorgear::calcForces(float* torqueOut)
1357 {
1358     int i,j;
1359     torqueOut[0]=torqueOut[1]=torqueOut[2]=0;
1360     // check,<if the engine can handle the torque of the rotors.
1361     // If not reduce the torque to the fueselage and change rotational
1362     // speed of the rotors instead
1363     if (_rotors.size())
1364     {
1365         float omegarel,omegan;
1366         Rotor* r0 = (Rotor*)_rotors.get(0);
1367         omegarel= r0->getOmegaRel();
1368
1369         float total_torque_of_inertia=0;
1370         float total_torque=0;
1371         for(i=0; i<_rotors.size(); i++) {
1372             Rotor* r = (Rotor*)_rotors.get(i);
1373             omegan=r->getOmegan();
1374             total_torque_of_inertia+=r->getTorqueOfInertia()*omegan*omegan;
1375             //FIXME: this is constant, so this can be done in compile
1376
1377             total_torque+=r->getTorque()*omegan;
1378         }
1379         float max_torque_of_engine=0;
1380         SGPropertyNode * node=fgGetNode("/rotors/gear", true);
1381         float target_rel_rpm=(node==0?1:node->getDoubleValue("target-rel-rpm",1));
1382         if (_engineon)
1383         {
1384             float max_rel_torque=(node==0?1:node->getDoubleValue("max-rel-torque",1));
1385             max_torque_of_engine=_max_power_engine*max_rel_torque;
1386             float df=target_rel_rpm-omegarel;
1387             df/=_engine_prop_factor;
1388             df = Math::clamp(df, 0, 1);
1389             max_torque_of_engine = df * _max_power_engine*max_rel_torque;
1390         }
1391         total_torque*=-1;
1392         _ddt_omegarel=0;
1393         float rel_torque_engine=1;
1394         if (total_torque<=0)
1395             rel_torque_engine=0;
1396         else
1397             if (max_torque_of_engine>0)
1398                 rel_torque_engine=1/max_torque_of_engine*total_torque;
1399             else
1400                 rel_torque_engine=0;
1401
1402         //add the rotor brake and the gear fritcion
1403         float dt=0.1f;
1404         if (r0->_rotorparts.size()) dt=((Rotorpart*)r0->_rotorparts.get(0))->getDt();
1405
1406         float rotor_brake_torque;
1407         rotor_brake_torque=_rotorbrake*_max_power_rotor_brake+_rotorgear_friction;
1408         //clamp it to the value you need to stop the rotor
1409         //to avod accelerate the rotor to neagtive rpm:
1410         rotor_brake_torque=Math::clamp(rotor_brake_torque,0,
1411             total_torque_of_inertia/dt*omegarel);
1412         max_torque_of_engine-=rotor_brake_torque;
1413
1414         //change the rotation of the rotors
1415         if ((max_torque_of_engine<total_torque) //decreasing rotation
1416             ||((max_torque_of_engine>total_torque)&&(omegarel<target_rel_rpm))
1417             //increasing rotation due to engine
1418             ||(total_torque<0) ) //increasing rotation due to autorotation
1419         {
1420             _ddt_omegarel=(max_torque_of_engine-total_torque)/total_torque_of_inertia;
1421             if(max_torque_of_engine>total_torque)
1422             {
1423                 //check if the acceleration is due to the engine. If yes,
1424                 //the engine self limits the accel.
1425                 float lim1=-total_torque/total_torque_of_inertia;
1426                 //accel. by autorotation
1427
1428                 if (lim1<_engine_accel_limit) lim1=_engine_accel_limit;
1429                 //if the accel by autorotation greater than the max. engine
1430                 //accel, then this is the limit, if not: the engine is the limit
1431                 if (_ddt_omegarel>lim1) _ddt_omegarel=lim1;
1432             }
1433             if (_ddt_omegarel>5.5)_ddt_omegarel=5.5;
1434             //clamp it to avoid overflow. Should never be reached
1435             if (_ddt_omegarel<-5.5)_ddt_omegarel=-5.5;
1436
1437             if (_max_power_engine<0.001) {omegarel=1;_ddt_omegarel=0;}
1438             //for debug: negative or no maxpower will result
1439             //in permanet 100% rotation
1440
1441             omegarel+=dt*_ddt_omegarel;
1442
1443             if (omegarel>2.5) omegarel=2.5;
1444             //clamp it to avoid overflow. Should never be reached
1445             if (omegarel<-.5) omegarel=-.5;
1446
1447             r0->setOmegaRelNeu(omegarel);
1448             //calculate the torque, which is needed to accelerate the rotors.
1449             //Add this additional torque to the body
1450             for(j=0; j<_rotors.size(); j++) {
1451                 Rotor* r = (Rotor*)_rotors.get(j);
1452                 for(i=0; i<r->_rotorparts.size(); i++) {
1453                     float torque_scalar=0;
1454                     Rotorpart* rp = (Rotorpart*)r->_rotorparts.get(i);
1455                     float torque[3];
1456                     rp->getAccelTorque(_ddt_omegarel,torque);
1457                     Math::add3(torque,torqueOut,torqueOut);
1458                 }
1459             }
1460         }
1461         _total_torque_on_engine=total_torque+_ddt_omegarel*total_torque_of_inertia;
1462     }
1463 }
1464
1465 void Rotorgear::addRotor(Rotor* rotor)
1466 {
1467     _rotors.add(rotor);
1468     _in_use = 1;
1469 }
1470
1471 void Rotorgear::compile()
1472 {
1473     float wgt = 0;
1474     for(int j=0; j<_rotors.size(); j++) {
1475         Rotor* r = (Rotor*)_rotors.get(j);
1476         r->compile();
1477     }
1478     SGPropertyNode * node = fgGetNode("/rotors/gear/target-rel-rpm", true);
1479     if (node) node->setDoubleValue(1);
1480     node =fgGetNode("/rotors/gear/max-rel-torque", true);
1481     if (node) node->setDoubleValue(1);
1482 }
1483
1484 void Rotorgear::getDownWash(float *pos, float * v_heli, float *downwash)
1485 {
1486     float tmp[3];
1487     downwash[0]=downwash[1]=downwash[2]=0;
1488     for(int i=0; i<_rotors.size(); i++) {
1489         Rotor* ro = (Rotor*)_rotors.get(i);
1490         ro->getDownWash(pos,v_heli,tmp);
1491         Math::add3(downwash,tmp,downwash);    //  + downwash
1492     }
1493 }
1494
1495 void Rotorgear::setParameter(char *parametername, float value)
1496 {
1497 #define p(a,b) if (strcmp(parametername,#a)==0) _##a = (value * (b)); else
1498         p(max_power_engine,1000)
1499         p(engine_prop_factor,1)
1500         p(yasimdragfactor,1)
1501         p(yasimliftfactor,1)
1502         p(max_power_rotor_brake,1000)
1503         p(rotorgear_friction,1000)
1504         p(engine_accel_limit,0.01)
1505         SG_LOG(SG_INPUT, SG_ALERT,
1506             "internal error in parameter set up for rotorgear: '"
1507             << parametername <<"'" << endl);
1508 #undef p
1509 }
1510 int Rotorgear::getValueforFGSet(int j,char *text,float *f)
1511 {
1512     if (j==0)
1513     {
1514         sprintf(text,"/rotors/gear/total-torque");
1515         *f=_total_torque_on_engine;
1516     } else return 0;
1517     return j+1;
1518 }
1519 Rotorgear::Rotorgear()
1520 {
1521     _in_use=0;
1522     _engineon=0;
1523     _rotorbrake=0;
1524     _max_power_rotor_brake=1;
1525     _rotorgear_friction=1;
1526     _max_power_engine=1000*450;
1527     _engine_prop_factor=0.05f;
1528     _yasimdragfactor=1;
1529     _yasimliftfactor=1;
1530     _ddt_omegarel=0;
1531     _engine_accel_limit=0.05f;
1532     _total_torque_on_engine=0;
1533 }
1534
1535 Rotorgear::~Rotorgear()
1536 {
1537     for(int i=0; i<_rotors.size(); i++)
1538         delete (Rotor*)_rotors.get(i);
1539 }
1540
1541 }; // namespace yasim