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