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