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