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