src/FDM/YASim/Rotor.cpp

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