src/FDM/YASim/Rotor.cpp

   1 #include <simgear/debug/logstream.hxx>
   2
   3 #include "Math.hpp"
   4 #include "Surface.hpp"
   5 #include "Rotorpart.hpp"
   6 #include "Rotorblade.hpp"
   7 #include "Rotor.hpp"
   8
   9 #include STL_IOSTREAM
  10 #include STL_IOMANIP
  11
  12 SG_USING_STD(setprecision);
  13
  14 //#include <string.h>
  15 #include <stdio.h>
  16
  17 namespace yasim {
  18
  19 const float pi=3.14159;
  20
  21 Rotor::Rotor()
  22 {
  23     _alpha0=-.05;
  24     _alpha0factor=1;
  25     _alphamin=-.1;
  26     _alphamax= .1;
  27     _alphaoutput[0][0]=0;
  28     _alphaoutput[1][0]=0;
  29     _alphaoutput[2][0]=0;
  30     _alphaoutput[3][0]=0;
  31     _alphaoutput[4][0]=0;
  32     _alphaoutput[5][0]=0;
  33     _alphaoutput[6][0]=0;
  34     _alphaoutput[7][0]=0;
  35     _base[0] = _base[1] = _base[2] = 0;
  36     _ccw=0;
  37     _delta=1;
  38     _delta3=0;
  39     _diameter =10;
  40     _dynamic=1;
  41     _engineon=0;
  42     _force_at_max_pitch=0;
  43     _force_at_pitch_a=0;
  44     _forward[0]=1;
  45     _forward[1]=_forward[2]=0;
  46     _max_pitch=13./180*pi;
  47     _maxcyclicail=10./180*pi;
  48     _maxcyclicele=10./180*pi;
  49     _maxteeterdamp=0;
  50     _mincyclicail=-10./180*pi;
  51     _mincyclicele=-10./180*pi;
  52     _min_pitch=-.5/180*pi;
  53     _name[0] = 0;
  54     _normal[0] = _normal[1] = 0;
  55     _normal[2] = 1;
  56     _number_of_blades=4;
  57     _omega=_omegan=_omegarel=0;
  58     _pitch_a=0;
  59     _pitch_b=0;
  60     _power_at_pitch_0=0;
  61     _power_at_pitch_b=0;
  62     _rel_blade_center=.7;
  63     _rel_len_hinge=0.01;
  64     _rellenteeterhinge=0.01;
  65     _rotor_rpm=442;
  66     _sim_blades=0;
  67     _teeterdamp=0.00001;
  68     _translift=0.05;
  69     _weight_per_blade=42;
  70
  71
  72
  73 }
  74
  75 Rotor::~Rotor()
  76 {
  77     int i;
  78     for(i=0; i<_rotorparts.size(); i++) {
  79         Rotorpart* r = (Rotorpart*)_rotorparts.get(i);
  80         delete r;
  81     }
  82     for(i=0; i<_rotorblades.size(); i++) {
  83         Rotorblade* r = (Rotorblade*)_rotorblades.get(i);
  84         delete r;
  85     }
  86
  87 }
  88
  89 void Rotor::inititeration(float dt)
  90 {
  91    if ((_engineon)&&(_omegarel>=1)) return;
  92    if ((!_engineon)&&(_omegarel<=0)) return;
  93    _omegarel+=dt*1/5.*(_engineon?1:-1); //hier 30
  94    _omegarel=Math::clamp(_omegarel,0,1);
  95    _omega=_omegan*_omegarel;
  96    int i;
  97     for(i=0; i<_rotorparts.size(); i++) {
  98         Rotorpart* r = (Rotorpart*)_rotorparts.get(i);
  99         r->setOmega(_omega);
 100     }
 101     for(i=0; i<_rotorblades.size(); i++) {
 102         Rotorblade* r = (Rotorblade*)_rotorblades.get(i);
 103         r->setOmega(_omega);
 104     }
 105 }
 106
 107 int Rotor::getValueforFGSet(int j,char *text,float *f)
 108 {
 109   if (_name[0]==0) return 0;
 110
 111
 112   if (_sim_blades)
 113   {
 114      if (!numRotorblades()) return 0;
 115      if (j==0)
 116      {
 117         sprintf(text,"/rotors/%s/cone", _name);
 118
 119         *f=( ((Rotorblade*)getRotorblade(0))->getFlapatPos(0)
 120             +((Rotorblade*)getRotorblade(0))->getFlapatPos(1)
 121             +((Rotorblade*)getRotorblade(0))->getFlapatPos(2)
 122             +((Rotorblade*)getRotorblade(0))->getFlapatPos(3)
 123            )/4*180/pi;
 124
 125      }
 126      else
 127      if (j==1)
 128      {
 129         sprintf(text,"/rotors/%s/roll", _name);
 130
 131         *f=( ((Rotorblade*)getRotorblade(0))->getFlapatPos(1)
 132             -((Rotorblade*)getRotorblade(0))->getFlapatPos(3)
 133            )/2*180/pi;
 134      }
 135      else
 136      if (j==2)
 137      {
 138         sprintf(text,"/rotors/%s/yaw", _name);
 139
 140         *f=( ((Rotorblade*)getRotorblade(0))->getFlapatPos(2)
 141             -((Rotorblade*)getRotorblade(0))->getFlapatPos(0)
 142            )/2*180/pi;
 143      }
 144      else
 145      if (j==3)
 146      {
 147         sprintf(text,"/rotors/%s/rpm", _name);
 148
 149         *f=_omega/2/pi*60;
 150      }
 151      else
 152      {
 153
 154          int b=(j-4)/3;
 155
 156          if (b>=numRotorblades()) return 0;
 157          int w=j%3;
 158          sprintf(text,"/rotors/%s/blade%i_%s",
 159             _name,b+1,
 160             w==0?"pos":(w==1?"flap":"incidence"));
 161          if (w==0) *f=((Rotorblade*)getRotorblade(b))->getPhi()*180/pi;
 162          else if (w==1) *f=((Rotorblade*) getRotorblade(b))->getrealAlpha()*180/pi;
 163          else *f=((Rotorblade*)getRotorblade(b))->getIncidence()*180/pi;
 164      }
 165      return j+1;
 166   }
 167   else
 168   {
 169      if (4!=numRotorparts()) return 0; //compile first!
 170      if (j==0)
 171      {
 172         sprintf(text,"/rotors/%s/cone", _name);
 173         *f=( ((Rotorpart*)getRotorpart(0))->getrealAlpha()
 174             +((Rotorpart*)getRotorpart(1))->getrealAlpha()
 175             +((Rotorpart*)getRotorpart(2))->getrealAlpha()
 176             +((Rotorpart*)getRotorpart(3))->getrealAlpha()
 177            )/4*180/pi;
 178      }
 179      else
 180      if (j==1)
 181      {
 182         sprintf(text,"/rotors/%s/roll", _name);
 183         *f=( ((Rotorpart*)getRotorpart(0))->getrealAlpha()
 184             -((Rotorpart*)getRotorpart(2))->getrealAlpha()
 185            )/2*180/pi*(_ccw?-1:1);
 186      }
 187      else
 188      if (j==2)
 189      {
 190         sprintf(text,"/rotors/%s/yaw", _name);
 191         *f=( ((Rotorpart*)getRotorpart(1))->getrealAlpha()
 192             -((Rotorpart*)getRotorpart(3))->getrealAlpha()
 193            )/2*180/pi;
 194      }
 195      else
 196      if (j==3)
 197      {
 198         sprintf(text,"/rotors/%s/rpm", _name);
 199
 200         *f=_omega/2/pi*60;
 201      }
 202      else
 203      {
 204        int b=(j-4)/3;
 205        if (b>=_number_of_blades) return 0;
 206        int w=j%3;
 207        sprintf(text,"/rotors/%s/blade%i_%s",
 208             _name,b+1,
 209           w==0?"pos":(w==1?"flap":"incidence"));
 210        *f=((Rotorpart*)getRotorpart(0))->getPhi()*180/pi+360*b/_number_of_blades*(_ccw?1:-1);
 211        if (*f>360) *f-=360;
 212        if (*f<0) *f+=360;
 213        int k,l;
 214        float rk,rl,p;
 215        p=(*f/90);
 216        k=int(p);
 217        l=int(p+1);
 218        rk=l-p;
 219        rl=1-rk;
 220        /*
 221        rl=Math::sqr(Math::sin(rl*pi/2));
 222        rk=Math::sqr(Math::sin(rk*pi/2));
 223        */
 224        if(w==2) {k+=2;l+=2;}
 225        else
 226        if(w==1) {k+=1;l+=1;}
 227        k%=4;
 228        l%=4;
 229        if (w==1) *f=rk*((Rotorpart*) getRotorpart(k))->getrealAlpha()*180/pi
 230                    +rl*((Rotorpart*) getRotorpart(l))->getrealAlpha()*180/pi;
 231        else if(w==2) *f=rk*((Rotorpart*)getRotorpart(k))->getIncidence()*180/pi
 232                        +rl*((Rotorpart*)getRotorpart(l))->getIncidence()*180/pi;
 233      }
 234      return j+1;
 235   }
 236
 237 }
 238 void Rotor::setEngineOn(int value)
 239 {
 240   _engineon=value;
 241 }
 242
 243 void Rotor::setAlpha0(float f)
 244 {
 245     _alpha0=f;
 246 }
 247 void Rotor::setAlphamin(float f)
 248 {
 249     _alphamin=f;
 250 }
 251 void Rotor::setAlphamax(float f)
 252 {
 253     _alphamax=f;
 254 }
 255 void Rotor::setAlpha0factor(float f)
 256 {
 257     _alpha0factor=f;
 258 }
 259
 260
 261 int Rotor::numRotorparts()
 262 {
 263     return _rotorparts.size();
 264 }
 265
 266 Rotorpart* Rotor::getRotorpart(int n)
 267 {
 268     return ((Rotorpart*)_rotorparts.get(n));
 269 }
 270 int Rotor::numRotorblades()
 271 {
 272     return _rotorblades.size();
 273 }
 274
 275 Rotorblade* Rotor::getRotorblade(int n)
 276 {
 277     return ((Rotorblade*)_rotorblades.get(n));
 278 }
 279
 280 void Rotor::strncpy(char *dest,const char *src,int maxlen)
 281 {
 282   int n=0;
 283   while(src[n]&&n<(maxlen-1))
 284   {
 285     dest[n]=src[n];
 286     n++;
 287   }
 288   dest[n]=0;
 289 }
 290
 291
 292
 293 void Rotor::setNormal(float* normal)
 294 {
 295     int i;
 296     float invsum,sqrsum=0;
 297     for(i=0; i<3; i++) { sqrsum+=normal[i]*normal[i];}
 298
 299     if (sqrsum!=0)
 300       invsum=1/Math::sqrt(sqrsum);
 301     else
 302       invsum=1;
 303     for(i=0; i<3; i++) { _normal[i] = normal[i]*invsum; }
 304 }
 305
 306 void Rotor::setForward(float* forward)
 307 {
 308     int i;
 309     float invsum,sqrsum=0;
 310     for(i=0; i<3; i++) { sqrsum+=forward[i]*forward[i];}
 311
 312     if (sqrsum!=0)
 313       invsum=1/Math::sqrt(sqrsum);
 314     else
 315       invsum=1;
 316     for(i=0; i<3; i++) { _forward[i] = forward[i]*invsum; }
 317 }
 318
 319
 320 void Rotor::setForceAtPitchA(float force)
 321 {
 322      _force_at_pitch_a=force;
 323 }
 324 void Rotor::setPowerAtPitch0(float value)
 325 {
 326      _power_at_pitch_0=value;
 327 }
 328 void Rotor::setPowerAtPitchB(float value)
 329 {
 330      _power_at_pitch_b=value;
 331 }
 332 void Rotor::setPitchA(float value)
 333 {
 334      _pitch_a=value/180*pi;
 335 }
 336 void Rotor::setPitchB(float value)
 337 {
 338      _pitch_b=value/180*pi;
 339 }
 340 void Rotor::setBase(float* base)
 341 {
 342     int i;
 343     for(i=0; i<3; i++) _base[i] = base[i];
 344 }
 345
 346
 347 void Rotor::setMaxCyclicail(float value)
 348 {
 349   _maxcyclicail=value/180*pi;
 350 }
 351 void Rotor::setMaxCyclicele(float value)
 352 {
 353   _maxcyclicele=value/180*pi;
 354 }
 355 void Rotor::setMinCyclicail(float value)
 356 {
 357   _mincyclicail=value/180*pi;
 358 }
 359 void Rotor::setMinCyclicele(float value)
 360 {
 361   _mincyclicele=value/180*pi;
 362 }
 363 void Rotor::setMinCollective(float value)
 364 {
 365   _min_pitch=value/180*pi;
 366 }
 367 void Rotor::setMaxCollective(float value)
 368 {
 369   _max_pitch=value/180*pi;
 370 }
 371 void Rotor::setDiameter(float value)
 372 {
 373   _diameter=value;
 374 }
 375 void Rotor::setWeightPerBlade(float value)
 376 {
 377   _weight_per_blade=value;
 378 }
 379 void Rotor::setNumberOfBlades(float value)
 380 {
 381   _number_of_blades=int(value+.5);
 382 }
 383 void Rotor::setRelBladeCenter(float value)
 384 {
 385   _rel_blade_center=value;
 386 }
 387 void Rotor::setDynamic(float value)
 388 {
 389   _dynamic=value;
 390 }
 391 void Rotor::setDelta3(float value)
 392 {
 393   _delta3=value;
 394 }
 395 void Rotor::setDelta(float value)
 396 {
 397   _delta=value;
 398 }
 399 void Rotor::setTranslift(float value)
 400 {
 401   _translift=value;
 402 }
 403 void Rotor::setC2(float value)
 404 {
 405   _c2=value;
 406 }
 407 void Rotor::setStepspersecond(float steps)
 408 {
 409   _stepspersecond=steps;
 410 }
 411 void Rotor::setRPM(float value)
 412 {
 413   _rotor_rpm=value;
 414 }
 415 void Rotor::setRelLenHinge(float value)
 416 {
 417   _rel_len_hinge=value;
 418 }
 419
 420 void Rotor::setAlphaoutput(int i, const char *text)
 421 {
 422   //printf("SetAlphaoutput %i [%s]\n",i,text);
 423   strncpy(_alphaoutput[i],text,255);
 424 }
 425 void Rotor::setName(const char *text)
 426 {
 427   strncpy(_name,text,128);//128: some space needed for settings
 428 }
 429
 430 void Rotor::setCcw(int ccw)
 431 {
 432      _ccw=ccw;
 433 }
 434 void Rotor::setNotorque(int value)
 435 {
 436    _no_torque=value;
 437 }
 438 void Rotor::setSimBlades(int value)
 439 {
 440    _sim_blades=value;
 441 }
 442
 443 void Rotor::setRelLenTeeterHinge(float f)
 444 {
 445    _rellenteeterhinge=f;
 446 }
 447 void Rotor::setTeeterdamp(float f)
 448 {
 449     _teeterdamp=f;
 450 }
 451 void Rotor::setMaxteeterdamp(float f)
 452 {
 453     _maxteeterdamp=f;
 454 }
 455
 456
 457
 458
 459 void Rotor::setCollective(float lval)
 460 {
 461     lval = Math::clamp(lval, -1, 1);
 462     int i;
 463     //printf("col: %5.3f\n",lval);
 464     for(i=0; i<_rotorparts.size(); i++) {
 465         ((Rotorpart*)_rotorparts.get(i))->setCollective(lval);
 466
 467     }
 468     float col=_min_pitch+(lval+1)/2*(_max_pitch-_min_pitch);
 469     for(i=0; i<_rotorblades.size(); i++) {
 470         ((Rotorblade*)_rotorblades.get(i))->setCollective(col);
 471
 472     }
 473 }
 474 void Rotor::setCyclicele(float lval,float rval)
 475 {
 476     rval = Math::clamp(rval, -1, 1);
 477     lval = Math::clamp(lval, -1, 1);
 478     float col=_mincyclicele+(lval+1)/2*(_maxcyclicele-_mincyclicele);
 479     int i;
 480     for(i=0; i<_rotorblades.size(); i++) {
 481         //((Rotorblade*)_rotorblades.get(i))->setCyclicele(col*Math::sin(((Rotorblade*)_rotorblades.get(i))->getPhi()));
 482         ((Rotorblade*)_rotorblades.get(i))->setCyclicele(col);
 483     }
 484     if (_rotorparts.size()!=4) return;
 485     //printf("                     ele: %5.3f  %5.3f\n",lval,rval);
 486     ((Rotorpart*)_rotorparts.get(1))->setCyclic(lval);
 487     ((Rotorpart*)_rotorparts.get(3))->setCyclic(-lval);
 488 }
 489 void Rotor::setCyclicail(float lval,float rval)
 490 {
 491     lval = Math::clamp(lval, -1, 1);
 492     rval = Math::clamp(rval, -1, 1);
 493     float col=_mincyclicail+(lval+1)/2*(_maxcyclicail-_mincyclicail);
 494     int i;
 495     for(i=0; i<_rotorblades.size(); i++) {
 496         ((Rotorblade*)_rotorblades.get(i))->setCyclicail(col);
 497     }
 498     if (_rotorparts.size()!=4) return;
 499     //printf("ail: %5.3f %5.3f\n",lval,rval);
 500     if (_ccw) lval *=-1;
 501     ((Rotorpart*)_rotorparts.get(0))->setCyclic(-lval);
 502     ((Rotorpart*)_rotorparts.get(2))->setCyclic( lval);
 503 }
 504
 505
 506 float Rotor::getGroundEffect(float* posOut)
 507 {
 508     /*
 509     int i;
 510     for(i=0; i<3; i++) posOut[i] = _base[i];
 511     float span = _length * Math::cos(_sweep) * Math::cos(_dihedral);
 512     span = 2*(span + Math::abs(_base[2]));
 513     */
 514     return _diameter;
 515 }
 516
 517 void Rotor::compile()
 518 {
 519   // Have we already been compiled?
 520   if(_rotorparts.size() != 0) return;
 521
 522   //rotor is divided into 4 pointlike parts
 523
 524   SG_LOG(SG_FLIGHT, SG_DEBUG, "debug: e "
 525          << _mincyclicele << "..." <<_maxcyclicele << ' '
 526          << _mincyclicail << "..." << _maxcyclicail << ' '
 527          << _min_pitch << "..." << _max_pitch);
 528
 529   if(!_sim_blades)
 530   {
 531     _dynamic=_dynamic*(1/                          //inverse of the time
 532              ( (60/_rotor_rpm)/4         //for rotating 90 deg
 533               +(60/_rotor_rpm)/(2*_number_of_blades) //+ meantime a rotorblade will pass a given point
 534              ));
 535     float directions[5][3];//pointing forward, right, ... the 5th is ony for calculation
 536     directions[0][0]=_forward[0];
 537     directions[0][1]=_forward[1];
 538     directions[0][2]=_forward[2];
 539     int i;
 540     SG_LOG(SG_FLIGHT, SG_DEBUG, "Rotor rotating ccw? " << _ccw);
 541     for (i=1;i<5;i++)
 542
 543     {
 544          if (!_ccw)
 545            Math::cross3(directions[i-1],_normal,directions[i]);
 546          else
 547            Math::cross3(_normal,directions[i-1],directions[i]);
 548          Math::unit3(directions[i],directions[i]);
 549     }
 550     Math::set3(directions[4],directions[0]);
 551     float rotorpartmass = _weight_per_blade*_number_of_blades/4*.453;//was pounds -> now kg
 552     float speed=_rotor_rpm/60*_diameter*_rel_blade_center*pi;
 553     float lentocenter=_diameter*_rel_blade_center*0.5;
 554     float lentoforceattac=_diameter*_rel_len_hinge*0.5;
 555     float zentforce=rotorpartmass*speed*speed/lentocenter;
 556     _force_at_max_pitch=_force_at_pitch_a/_pitch_a*_max_pitch;
 557     float maxpitchforce=_force_at_max_pitch/4*.453*9.81;//was pounds of force, now N
 558     float torque0=0,torquemax=0;
 559     float omega=_rotor_rpm/60*2*pi;
 560     _omegan=omega;
 561     float omega0=omega*Math::sqrt(1/(1-_rel_len_hinge));
 562     //float omega0=omega*Math::sqrt((1-_rel_len_hinge));
 563     //_delta=omega*_delta;
 564     _delta*=maxpitchforce/(_max_pitch*omega*lentocenter*2*rotorpartmass);
 565
 566     float phi=Math::atan2(2*omega*_delta,omega0*omega0-omega*omega);
 567     //float relamp=omega*omega/(2*_delta*Math::sqrt(omega0*omega0-_delta*_delta));
 568     float relamp=omega*omega/(2*_delta*Math::sqrt(Math::sqr(omega0*omega0-omega*omega)+4*_delta*_delta*omega*omega));
 569     if (!_no_torque)
 570     {
 571        torque0=_power_at_pitch_0/4*1000/omega;
 572        torquemax=_power_at_pitch_b/4*1000/omega/_pitch_b*_max_pitch;
 573
 574        if(_ccw)
 575        {
 576          torque0*=-1;
 577          torquemax*=-1;
 578        }
 579
 580     }
 581
 582     SG_LOG(SG_FLIGHT, SG_DEBUG,
 583            "spd: " << setprecision(8) << speed
 584            << " lentoc: " << lentocenter
 585            << " dia: " << _diameter
 586            << " rbl: " << _rel_blade_center
 587            << " hing: " << _rel_len_hinge
 588            << " lfa: " << lentoforceattac);
 589     SG_LOG(SG_FLIGHT, SG_DEBUG,
 590            "zf: " << setprecision(8) << zentforce
 591            << " mpf: " << maxpitchforce);
 592     SG_LOG(SG_FLIGHT, SG_DEBUG,
 593            "tq: " << setprecision(8) << torque0 << ".." << torquemax
 594            << " d3: " << _delta3);
 595     SG_LOG(SG_FLIGHT, SG_DEBUG,
 596            "o/o0: " << setprecision(8) << omega/omega0
 597            << " phi: " << phi*180/pi
 598            << " relamp: " << relamp
 599            << " delta: " <<_delta);
 600
 601     Rotorpart* rps[4];
 602     for (i=0;i<4;i++)
 603     {
 604          float lpos[3],lforceattac[3],lspeed[3],dirzentforce[3];
 605
 606          Math::mul3(lentocenter,directions[i],lpos);
 607          Math::add3(lpos,_base,lpos);
 608          Math::mul3(lentoforceattac,directions[i+1],lforceattac);//i+1: +90deg (gyro)!!!
 609          Math::add3(lforceattac,_base,lforceattac);
 610          Math::mul3(speed,directions[i+1],lspeed);
 611          Math::mul3(1,directions[i+1],dirzentforce);
 612
 613          float maxcyclic=(i&1)?_maxcyclicele:_maxcyclicail;
 614          float mincyclic=(i&1)?_mincyclicele:_mincyclicail;
 615
 616
 617          Rotorpart* rp=rps[i]=newRotorpart(lpos, lforceattac, _normal,
 618                        lspeed,dirzentforce,zentforce,maxpitchforce, _max_pitch,_min_pitch,mincyclic,maxcyclic,
 619                        _delta3,rotorpartmass,_translift,_rel_len_hinge,lentocenter);
 620          rp->setAlphaoutput(_alphaoutput[i&1?i:(_ccw?i^2:i)],0);
 621          rp->setAlphaoutput(_alphaoutput[4+(i&1?i:(_ccw?i^2:i))],1+(i>1));
 622          _rotorparts.add(rp);
 623          rp->setTorque(torquemax,torque0);
 624          rp->setRelamp(relamp);
 625
 626
 627     }
 628     for (i=0;i<4;i++)
 629     {
 630
 631       rps[i]->setlastnextrp(rps[(i+3)%4],rps[(i+1)%4],rps[(i+2)%4]);
 632     }
 633   }
 634   else
 635   {
 636     float directions[5][3];//pointing forward, right, ... the 5th is ony for calculation
 637     directions[0][0]=_forward[0];
 638     directions[0][1]=_forward[1];
 639     directions[0][2]=_forward[2];
 640     int i;
 641     SG_LOG(SG_FLIGHT, SG_DEBUG, "Rotor rotating ccw? " <<_ccw);
 642     for (i=1;i<5;i++)
 643
 644     {
 645          //if (!_ccw)
 646            Math::cross3(directions[i-1],_normal,directions[i]);
 647          //else
 648          //  Math::cross3(_normal,directions[i-1],directions[i]);
 649          Math::unit3(directions[i],directions[i]);
 650     }
 651     Math::set3(directions[4],directions[0]);
 652     float speed=_rotor_rpm/60*_diameter*_rel_blade_center*pi;
 653     float lentocenter=_diameter*_rel_blade_center*0.5;
 654     float lentoforceattac=_diameter*_rel_len_hinge*0.5;
 655     float zentforce=_weight_per_blade*.453*speed*speed/lentocenter;
 656     _force_at_max_pitch=_force_at_pitch_a/_pitch_a*_max_pitch;
 657     float maxpitchforce=_force_at_max_pitch/_number_of_blades*.453*9.81;//was pounds of force, now N
 658     float torque0=0,torquemax=0;
 659     float omega=_rotor_rpm/60*2*pi;
 660     _omegan=omega;
 661     float omega0=omega*Math::sqrt(1/(1-_rel_len_hinge));
 662     //float omega0=omega*Math::sqrt(1-_rel_len_hinge);
 663     //_delta=omega*_delta;
 664     _delta*=maxpitchforce/(_max_pitch*omega*lentocenter*2*_weight_per_blade*.453);
 665     float phi=Math::atan2(2*omega*_delta,omega0*omega0-omega*omega);
 666     float phi2=Math::abs(omega0-omega)<.000000001?pi/2:Math::atan(2*omega*_delta/(omega0*omega0-omega*omega));
 667     float relamp=omega*omega/(2*_delta*Math::sqrt(Math::sqr(omega0*omega0-omega*omega)+4*_delta*_delta*omega*omega));
 668     if (!_no_torque)
 669     {
 670        torque0=_power_at_pitch_0/_number_of_blades*1000/omega;
 671        torquemax=_power_at_pitch_b/_number_of_blades*1000/omega/_pitch_b*_max_pitch;
 672
 673        if(_ccw)
 674        {
 675          torque0*=-1;
 676          torquemax*=-1;
 677        }
 678
 679     }
 680
 681     SG_LOG(SG_FLIGHT, SG_DEBUG,
 682            "spd: " << setprecision(8) << speed
 683            << " lentoc: " << lentocenter
 684            << " dia: " << _diameter
 685            << " rbl: " << _rel_blade_center
 686            << " hing: " << _rel_len_hinge
 687            << " lfa: " << lentoforceattac);
 688     SG_LOG(SG_FLIGHT, SG_DEBUG,
 689            "zf: " << setprecision(8) << zentforce
 690            << " mpf: " << maxpitchforce);
 691     SG_LOG(SG_FLIGHT, SG_DEBUG,
 692            "tq: " << setprecision(8) << torque0 << ".." << torquemax
 693            << " d3: " << _delta3);
 694     SG_LOG(SG_FLIGHT, SG_DEBUG,
 695            "o/o0: " << setprecision(8) << omega/omega0
 696            << " phi: " << phi*180/pi
 697            << " relamp: " << relamp
 698            << " delta: " <<_delta);
 699
 700     float lspeed[3],dirzentforce[3];
 701
 702     float f=(!_ccw)?1:-1;
 703     //Math::mul3(f*speed,directions[1],lspeed);
 704     Math::mul3(f,directions[1],dirzentforce);
 705     for (i=0;i<_number_of_blades;i++)
 706     {
 707
 708
 709
 710
 711          Rotorblade* rb=newRotorblade(_base,_normal,directions[0],directions[1],
 712                        lentoforceattac,_rel_len_hinge,
 713                        dirzentforce,zentforce,maxpitchforce, _max_pitch,
 714                        _delta3,_weight_per_blade*.453,_translift,2*pi/_number_of_blades*i,
 715                        omega,lentocenter,/*f* */speed);
 716          //rp->setAlphaoutput(_alphaoutput[i&1?i:(_ccw?i^2:i)],0);
 717          //rp->setAlphaoutput(_alphaoutput[4+(i&1?i:(_ccw?i^2:i))],1+(i>1));
 718          _rotorblades.add(rb);
 719          rb->setTorque(torquemax,torque0);
 720          rb->setDeltaPhi(pi/2.-phi);
 721          rb->setDelta(_delta);
 722
 723          rb->calcFrontRight();
 724
 725     }
 726     /*
 727     for (i=0;i<4;i++)
 728     {
 729
 730       rps[i]->setlastnextrp(rps[(i-1)%4],rps[(i+1)%4],rps[(i+2)%4]);
 731     }
 732     */
 733
 734   }
 735 }
 736
 737
 738 Rotorblade* Rotor::newRotorblade(float* pos,  float *normal, float *front, float *right,
 739          float lforceattac,float rellenhinge,
 740          float *dirzentforce, float zentforce,float maxpitchforce,float maxpitch,
 741          float delta3,float mass,float translift,float phi,float omega,float len,float speed)
 742 {
 743     Rotorblade *r = new Rotorblade();
 744     r->setPosition(pos);
 745     r->setNormal(normal);
 746     r->setFront(front);
 747     r->setRight(right);
 748     r->setMaxPitchForce(maxpitchforce);
 749     r->setZentipetalForce(zentforce);
 750     r->setAlpha0(_alpha0);
 751     r->setAlphamin(_alphamin);
 752     r->setAlphamax(_alphamax);
 753     r->setAlpha0factor(_alpha0factor);
 754
 755
 756
 757     r->setSpeed(speed);
 758     r->setDirectionofZentipetalforce(dirzentforce);
 759     r->setMaxpitch(maxpitch);
 760     r->setDelta3(delta3);
 761     r->setDynamic(_dynamic);
 762     r->setTranslift(_translift);
 763     r->setC2(_c2);
 764     r->setStepspersecond(_stepspersecond);
 765     r->setWeight(mass);
 766     r->setOmegaN(omega);
 767     r->setPhi(phi);
 768     r->setLforceattac(lforceattac);
 769     r->setLen(len);
 770     r->setLenHinge(rellenhinge);
 771     r->setRelLenTeeterHinge(_rellenteeterhinge);
 772     r->setTeeterdamp(_teeterdamp);
 773     r->setMaxteeterdamp(_maxteeterdamp);
 774
 775     /*
 776     #define a(x) x[0],x[1],x[2]
 777     printf("newrp: pos(%5.3f %5.3f %5.3f) pfa (%5.3f %5.3f %5.3f)\n"
 778            "       nor(%5.3f %5.3f %5.3f) spd (%5.3f %5.3f %5.3f)\n"
 779            "       dzf(%5.3f %5.3f %5.3f) zf  (%5.3f) mpf (%5.3f)\n"
 780            "       pit (%5.3f..%5.3f) mcy (%5.3f..%5.3f) d3 (%5.3f)\n"
 781            ,a(pos),a(posforceattac),a(normal),
 782           a(speed),a(dirzentforce),zentforce,maxpitchforce,minpitch,maxpitch,mincyclic,maxcyclic,
 783           delta3);
 784     #undef a
 785     */
 786     return r;
 787 }
 788
 789 Rotorpart* Rotor::newRotorpart(float* pos, float *posforceattac, float *normal,
 790          float* speed,float *dirzentforce, float zentforce,float maxpitchforce,
 791          float maxpitch, float minpitch, float mincyclic,float maxcyclic,
 792          float delta3,float mass,float translift,float rellenhinge,float len)
 793 {
 794     Rotorpart *r = new Rotorpart();
 795     r->setPosition(pos);
 796     r->setNormal(normal);
 797     r->setMaxPitchForce(maxpitchforce);
 798     r->setZentipetalForce(zentforce);
 799
 800     r->setPositionForceAttac(posforceattac);
 801
 802     r->setSpeed(speed);
 803     r->setDirectionofZentipetalforce(dirzentforce);
 804     r->setMaxpitch(maxpitch);
 805     r->setMinpitch(minpitch);
 806     r->setMaxcyclic(maxcyclic);
 807     r->setMincyclic(mincyclic);
 808     r->setDelta3(delta3);
 809     r->setDynamic(_dynamic);
 810     r->setTranslift(_translift);
 811     r->setC2(_c2);
 812     r->setWeight(mass);
 813     r->setRelLenHinge(rellenhinge);
 814     r->setOmegaN(_omegan);
 815     r->setAlpha0(_alpha0);
 816     r->setAlphamin(_alphamin);
 817     r->setAlphamax(_alphamax);
 818     r->setAlpha0factor(_alpha0factor);
 819     r->setLen(len);
 820
 821
 822     SG_LOG(SG_FLIGHT, SG_DEBUG, setprecision(8)
 823            << "newrp: pos("
 824            << pos[0] << ' ' << pos[1] << ' ' << pos[2]
 825            << ") pfa ("
 826            << posforceattac[0] << ' ' << posforceattac[1] << ' '
 827            << posforceattac[2] << ')');
 828     SG_LOG(SG_FLIGHT, SG_DEBUG, setprecision(8)
 829            << "       nor("
 830            << normal[0] << ' ' << normal[1] << ' ' << normal[2]
 831            << ") spd ("
 832            << speed[0] << ' ' << speed[1] << ' ' << speed[2] << ')');
 833     SG_LOG(SG_FLIGHT, SG_DEBUG, setprecision(8)
 834            << "       dzf("
 835            << dirzentforce[0] << ' ' << dirzentforce[1] << dirzentforce[2]
 836            << ") zf  (" << zentforce << ") mpf (" << maxpitchforce << ')');
 837     SG_LOG(SG_FLIGHT, SG_DEBUG, setprecision(8)
 838            << "       pit(" << minpitch << ".." << maxpitch
 839            << ") mcy (" << mincyclic << ".." << maxcyclic
 840            << ") d3 (" << delta3 << ')');
 841
 842     return r;
 843 }
 844 void Rotor::interp(float* v1, float* v2, float frac, float* out)
 845 {
 846     out[0] = v1[0] + frac*(v2[0]-v1[0]);
 847     out[1] = v1[1] + frac*(v2[1]-v1[1]);
 848     out[2] = v1[2] + frac*(v2[2]-v1[2]);
 849 }
 850
 851 }; // namespace yasim
 852