src/FDM/YASim/Rotorpart.cpp

   1 #include <ostream>
   2
   3 #include <simgear/debug/logstream.hxx>
   4
   5 #include "Math.hpp"
   6 #include "Rotorpart.hpp"
   7 #include "Rotor.hpp"
   8 #include <stdio.h>
   9 #include <string.h>
  10 namespace yasim {
  11 using std::endl;
  12
  13 const float pi=3.14159;
  14 float _help = 0;
  15 Rotorpart::Rotorpart()
  16 {
  17     _compiled=0;
  18     _cyclic=0;
  19     _collective=0;
  20     _rellenhinge=0;
  21     _shared_flap_hinge=false;
  22     _dt=0;
  23 #define set3(x,a,b,c) x[0]=a;x[1]=b;x[2]=c;
  24     set3 (_speed,1,0,0);
  25     set3 (_directionofcentripetalforce,1,0,0);
  26     set3 (_directionofrotorpart,0,1,0);
  27     set3 (_direction_of_movement,1,0,0);
  28     set3 (_last_torque,0,0,0);
  29 #undef set3
  30     _centripetalforce=1;
  31     _delta3=0.5;
  32     _cyclic=0;
  33     _collective=-1;
  34     _relamp=1;
  35     _mass=10;
  36     _incidence = 0;
  37     _alpha=0;
  38     _alphamin=-.1;
  39     _alphamax= .1;
  40     _alpha0=-.05;
  41     _alpha0factor=1;
  42     _alphaoutputbuf[0][0]=0;
  43     _alphaoutputbuf[1][0]=0;
  44     _alpha2type=0;
  45     _alphaalt=0;
  46     _lastrp=0;
  47     _nextrp=0;
  48     _oppositerp=0;
  49     _last90rp=0;
  50     _next90rp=0;
  51     _translift=0;
  52     _dynamic=100;
  53     _c2=0;
  54     _torque_max_force=0;
  55     _torque_no_force=0;
  56     _omega=0;
  57     _omegan=1;
  58     _ddt_omega=0;
  59     _phi=0;
  60     _len=1;
  61     _rotor=NULL;
  62     _twist=0;
  63     _number_of_segments=1;
  64     _rel_len_where_incidence_is_measured=0.7;
  65     _diameter=10;
  66     _torque_of_inertia=0;
  67     _rel_len_blade_start=0;
  68     _torque=0;
  69     _rotor_correction_factor=0.6;
  70     _direction=0;
  71     _balance=1;
  72 }
  73
  74 void Rotorpart::inititeration(float dt,float *rot)
  75 {
  76     _dt=dt;
  77     _phi+=_omega*dt;
  78     while (_phi>(2*pi)) _phi-=2*pi;
  79     while (_phi<(0   )) _phi+=2*pi;
  80     float a=Math::dot3(rot,_normal);
  81     if(a>0)
  82         _alphaalt=_alpha*Math::cos(a)
  83         +_next90rp->getrealAlpha()*Math::sin(a);
  84     else
  85         _alphaalt=_alpha*Math::cos(a)
  86         +_last90rp->getrealAlpha()*Math::sin(-a);
  87     //calculate the rotation of the fuselage, determine
  88     //the part in the same direction as alpha
  89     //and add it ro _alphaalt
  90     //alpha is rotation about "normal cross dirofzentf"
  91
  92     float dir[3];
  93     Math::cross3(_directionofcentripetalforce,_normal,dir);
  94     a=Math::dot3(rot,dir);
  95     _alphaalt -= a;
  96     _alphaalt= Math::clamp(_alphaalt,_alphamin,_alphamax);
  97
  98     //unbalance
  99     float b;
 100     b=_rotor->getBalance();
 101     float s =Math::sin(_phi+_direction);
 102     //float c =Math::cos(_phi+_direction);
 103     if (s>0)
 104         _balance=(b>0)?(1.-s*(1.-b)):(1.-s)*(1.+b);
 105     else
 106         _balance=(b>0)?1.:1.+b;
 107 }
 108
 109 void Rotorpart::setRotor(Rotor *rotor)
 110 {
 111     _rotor=rotor;
 112 }
 113
 114 void Rotorpart::setParameter(const char *parametername, float value)
 115 {
 116 #define p(a) if (strcmp(parametername,#a)==0) _##a = value; else
 117
 118     p(twist)
 119         p(number_of_segments)
 120         p(rel_len_where_incidence_is_measured)
 121         p(rel_len_blade_start)
 122         p(rotor_correction_factor)
 123         SG_LOG(SG_INPUT, SG_ALERT,
 124             "internal error in parameter set up for rotorpart: '"
 125             << parametername <<"'" << endl);
 126 #undef p
 127 }
 128
 129 void Rotorpart::setTorque(float torque_max_force,float torque_no_force)
 130 {
 131     _torque_max_force=torque_max_force;
 132     _torque_no_force=torque_no_force;
 133 }
 134
 135 void Rotorpart::setTorqueOfInertia(float toi)
 136 {
 137     _torque_of_inertia=toi;
 138 }
 139
 140 void Rotorpart::setWeight(float value)
 141 {
 142     _mass=value;
 143 }
 144
 145 float Rotorpart::getWeight(void)
 146 {
 147     return(_mass/.453); //_mass is in kg, returns pounds
 148 }
 149
 150 void Rotorpart::setPosition(float* p)
 151 {
 152     int i;
 153     for(i=0; i<3; i++) _pos[i] = p[i];
 154 }
 155
 156 float Rotorpart::getIncidence()
 157 {
 158     return(_incidence);
 159 }
 160
 161 void Rotorpart::getPosition(float* out)
 162 {
 163     int i;
 164     for(i=0; i<3; i++) out[i] = _pos[i];
 165 }
 166
 167 void Rotorpart::setPositionForceAttac(float* p)
 168 {
 169     int i;
 170     for(i=0; i<3; i++) _posforceattac[i] = p[i];
 171 }
 172
 173 void Rotorpart::getPositionForceAttac(float* out)
 174 {
 175     int i;
 176     for(i=0; i<3; i++) out[i] = _posforceattac[i];
 177 }
 178
 179 void Rotorpart::setSpeed(float* p)
 180 {
 181     int i;
 182     for(i=0; i<3; i++) _speed[i] = p[i];
 183     Math::unit3(_speed,_direction_of_movement);
 184 }
 185
 186 void Rotorpart::setDirectionofZentipetalforce(float* p)
 187 {
 188     int i;
 189     for(i=0; i<3; i++) _directionofcentripetalforce[i] = p[i];
 190 }
 191
 192 void Rotorpart::setDirectionofRotorPart(float* p)
 193 {
 194     int i;
 195     for(i=0; i<3; i++) _directionofrotorpart[i] = p[i];
 196 }
 197
 198 void Rotorpart::setDirection(float direction)
 199 {
 200     _direction=direction;
 201 }
 202
 203 void Rotorpart::setOmega(float value)
 204 {
 205     _omega=value;
 206 }
 207
 208 void Rotorpart::setPhi(float value)
 209 {
 210     _phi=value;
 211 }
 212
 213 void Rotorpart::setOmegaN(float value)
 214 {
 215     _omegan=value;
 216 }
 217
 218 void Rotorpart::setDdtOmega(float value)
 219 {
 220     _ddt_omega=value;
 221 }
 222
 223 void Rotorpart::setZentipetalForce(float f)
 224 {
 225     _centripetalforce=f;
 226 }
 227
 228
 229 void Rotorpart::setDelta3(float f)
 230 {
 231     _delta3=f;
 232 }
 233
 234 void Rotorpart::setRelamp(float f)
 235 {
 236     _relamp=f;
 237 }
 238
 239 void Rotorpart::setTranslift(float f)
 240 {
 241     _translift=f;
 242 }
 243
 244 void Rotorpart::setDynamic(float f)
 245 {
 246     _dynamic=f;
 247 }
 248
 249 void Rotorpart::setRelLenHinge(float f)
 250 {
 251     _rellenhinge=f;
 252 }
 253
 254 void Rotorpart::setSharedFlapHinge(bool s)
 255 {
 256     _shared_flap_hinge=s;
 257 }
 258
 259 void Rotorpart::setC2(float f)
 260 {
 261     _c2=f;
 262 }
 263
 264 void Rotorpart::setAlpha0(float f)
 265 {
 266     if (f>-0.01) f=-0.01; //half a degree bending
 267     _alpha0=f;
 268 }
 269
 270 void Rotorpart::setAlphamin(float f)
 271 {
 272     _alphamin=f;
 273 }
 274
 275 void Rotorpart::setAlphamax(float f)
 276 {
 277     _alphamax=f;
 278 }
 279
 280 void Rotorpart::setAlpha0factor(float f)
 281 {
 282     _alpha0factor=f;
 283 }
 284
 285 void Rotorpart::setDiameter(float f)
 286 {
 287     _diameter=f;
 288 }
 289
 290 float Rotorpart::getPhi()
 291 {
 292     return(_phi);
 293 }
 294
 295 float Rotorpart::getAlpha(int i)
 296 {
 297     i=i&1;
 298
 299     if (i==0)
 300         return _alpha*180/pi;//in Grad = 1
 301     else
 302     {
 303         if (_alpha2type==1) //yaw or roll
 304             return (getAlpha(0)-_oppositerp->getAlpha(0))/2;
 305         else //collective
 306             return (getAlpha(0)+_oppositerp->getAlpha(0)+
 307             _next90rp->getAlpha(0)+_last90rp->getAlpha(0))/4;
 308     }
 309 }
 310 float Rotorpart::getrealAlpha(void)
 311 {
 312     return _alpha;
 313 }
 314
 315 void Rotorpart::setAlphaoutput(char *text,int i)
 316 {
 317     strncpy(_alphaoutputbuf[i>0],text,255);
 318     if (i>0) _alpha2type=i;
 319 }
 320
 321 char* Rotorpart::getAlphaoutput(int i)
 322 {
 323     return _alphaoutputbuf[i&1];
 324 }
 325
 326 void Rotorpart::setLen(float value)
 327 {
 328     _len=value;
 329 }
 330
 331 void Rotorpart::setNormal(float* p)
 332 {
 333     int i;
 334     for(i=0; i<3; i++) _normal[i] = p[i];
 335 }
 336
 337 void Rotorpart::getNormal(float* out)
 338 {
 339     int i;
 340     for(i=0; i<3; i++) out[i] = _normal[i];
 341 }
 342
 343 void Rotorpart::setCollective(float pos)
 344 {
 345     _collective = pos;
 346 }
 347
 348 void Rotorpart::setCyclic(float pos)
 349 {
 350     _cyclic = pos;
 351 }
 352
 353 void Rotorpart::setlastnextrp(Rotorpart*lastrp,Rotorpart*nextrp,
 354     Rotorpart *oppositerp,Rotorpart*last90rp,Rotorpart*next90rp)
 355 {
 356     _lastrp=lastrp;
 357     _nextrp=nextrp;
 358     _oppositerp=oppositerp;
 359     _last90rp=last90rp;
 360     _next90rp=next90rp;
 361 }
 362
 363 void Rotorpart::strncpy(char *dest,const char *src,int maxlen)
 364 {
 365     int n=0;
 366     while(src[n]&&n<(maxlen-1))
 367     {
 368         dest[n]=src[n];
 369         n++;
 370     }
 371     dest[n]=0;
 372 }
 373
 374 // Calculate the flapping angle, where zentripetal force and
 375 //lift compensate each other
 376 float Rotorpart::calculateAlpha(float* v_rel_air, float rho,
 377     float incidence, float cyc, float alphaalt, float *torque,
 378     float *returnlift)
 379 {
 380     float moment[3],v_local[3],v_local_scalar,lift_moment,v_flap[3],v_help[3];
 381     float ias;//nur f. dgb
 382     int i,n;
 383     for (i=0;i<3;i++)
 384         moment[i]=0;
 385     float relgrav = Math::dot3(_normal,_rotor->getGravDirection());
 386     lift_moment=-_mass*_len*9.81*relgrav;
 387     *torque=0;//
 388     if((_nextrp==NULL)||(_lastrp==NULL)||(_rotor==NULL))
 389         return 0.0;//not initialized. Can happen during startupt of flightgear
 390     if (returnlift!=NULL) *returnlift=0;
 391     float flap_omega=(_next90rp->getrealAlpha()-_last90rp->getrealAlpha())
 392         *_omega / pi;
 393     float local_width=_diameter*(1-_rel_len_blade_start)/2.
 394         /(float (_number_of_segments));
 395     for (n=0;n<_number_of_segments;n++)
 396     {
 397         float rel = (n+.5)/(float (_number_of_segments));
 398         float r= _diameter *0.5 *(rel*(1-_rel_len_blade_start)
 399             +_rel_len_blade_start);
 400         float local_incidence=incidence+_twist *rel -
 401             _twist *_rel_len_where_incidence_is_measured;
 402         float local_chord = _rotor->getChord()*rel+_rotor->getChord()
 403             *_rotor->getTaper()*(1-rel);
 404         float A = local_chord * local_width;
 405         //calculate the local air speed and the incidence to this speed;
 406         Math::mul3(_omega * r , _direction_of_movement , v_local);
 407
 408         // add speed component due to flapping
 409         Math::mul3(flap_omega * r,_normal,v_flap);
 410         Math::add3(v_flap,v_local,v_local);
 411         Math::sub3(v_rel_air,v_local,v_local);
 412         //v_local is now the total airspeed at the blade
 413         //apparent missing: calculating the local_wind = v_rel_air at
 414         //every point of the rotor. It differs due to aircraft-rotation
 415         //it is considered in v_flap
 416
 417         //substract now the component of the air speed parallel to
 418         //the blade;
 419         Math::mul3(Math::dot3(v_local,_directionofrotorpart),
 420            _directionofrotorpart,v_help);
 421         Math::sub3(v_local,v_help,v_local);
 422
 423         //split into direction and magnitude
 424         v_local_scalar=Math::mag3(v_local);
 425         if (v_local_scalar!=0)
 426             //Math::unit3(v_local,v_help);
 427             Math::mul3(1/v_local_scalar,v_local,v_help);
 428         float incidence_of_airspeed = Math::asin(Math::clamp(
 429             Math::dot3(v_help,_normal),-1,1)) + local_incidence;
 430         ias = incidence_of_airspeed;
 431
 432         //reduce the ias (Prantl factor)
 433         float prantl_factor=2/pi*Math::acos(Math::exp(
 434             -_rotor->getNumberOfBlades()/2.*(1-rel)
 435              *Math::sqrt(1+1/Math::sqr(Math::tan(
 436                pi/2-Math::abs(incidence_of_airspeed-local_incidence))))));
 437         incidence_of_airspeed = (incidence_of_airspeed+
 438             _rotor->getAirfoilIncidenceNoLift())*prantl_factor
 439             *_rotor_correction_factor-_rotor->getAirfoilIncidenceNoLift();
 440         ias = incidence_of_airspeed;
 441         float lift_wo_cyc = _rotor->getLiftCoef(incidence_of_airspeed
 442             -cyc*_rotor_correction_factor*prantl_factor,v_local_scalar)
 443             * v_local_scalar * v_local_scalar * A *rho *0.5;
 444         float lift_with_cyc =
 445             _rotor->getLiftCoef(incidence_of_airspeed,v_local_scalar)
 446             * v_local_scalar * v_local_scalar *A *rho*0.5;
 447         float lift=lift_wo_cyc+_relamp*(lift_with_cyc-lift_wo_cyc);
 448         //take into account that the rotor is a resonant system where
 449         //the cyclic input hase increased result
 450         float drag = -_rotor->getDragCoef(incidence_of_airspeed,v_local_scalar)
 451             * v_local_scalar * v_local_scalar * A *rho*0.5;
 452         float angle = incidence_of_airspeed - incidence;
 453         //angle between blade movement caused by rotor-rotation and the
 454         //total movement of the blade
 455
 456         lift_moment += r*(lift * Math::cos(angle)
 457             - drag * Math::sin(angle));
 458         *torque     += r*(drag * Math::cos(angle)
 459             + lift * Math::sin(angle));
 460         if (returnlift!=NULL) *returnlift+=lift;
 461     }
 462     //use 1st order approximation for alpha
 463     //float alpha=Math::atan2(lift_moment,_centripetalforce * _len);
 464     float alpha;
 465     if (_shared_flap_hinge)
 466     {
 467         float div=0;
 468         if (Math::abs(_alphaalt) >1e-6)
 469             div=(_centripetalforce * _len - _mass * _len * 9.81 * relgrav /_alpha0*(_alphaalt+_oppositerp->getAlphaAlt())/(2.0*_alphaalt));
 470         if (Math::abs(div)>1e-6)
 471         {
 472             alpha=lift_moment/div;
 473         }
 474         else if(Math::abs(_alphaalt+_oppositerp->getAlphaAlt())>1e-6)
 475         {
 476             float div=(_centripetalforce * _len - _mass * _len * 9.81 *0.5 * relgrav)*(_alphaalt+_oppositerp->getAlphaAlt());
 477             if (Math::abs(div)>1e-6)
 478             {
 479                 alpha=_oppositerp->getAlphaAlt()+lift_moment/div*_alphaalt;
 480             }
 481             else
 482                 alpha=_alphaalt;
 483         }
 484         else
 485             alpha=_alphaalt;
 486         if (_omega/_omegan<0.2)
 487         {
 488             float frac = 0.001+_omega/_omegan*4.995;
 489             alpha=Math::clamp(alpha,_alphamin,_alphamax);
 490             alpha=_alphaalt*(1-frac)+frac*alpha;
 491         }
 492     }
 493     else
 494     {
 495         float div=(_centripetalforce * _len - _mass * _len * 9.81 /_alpha0);
 496         if (Math::abs(div)>1e-6)
 497             alpha=lift_moment/div;
 498         else
 499             alpha=_alphaalt;
 500     }
 501
 502     return (alpha);
 503 }
 504
 505 // Calculate the aerodynamic force given a wind vector v (in the
 506 // aircraft's "local" coordinates) and an air density rho.  Returns a
 507 // torque about the Y axis, too.
 508 void Rotorpart::calcForce(float* v, float rho,  float* out, float* torque,
 509     float* torque_scalar)
 510 {
 511     if (_compiled!=1)
 512     {
 513         for (int i=0;i<3;i++)
 514             torque[i]=out[i]=0;
 515         *torque_scalar=0;
 516         return;
 517     }
 518     _centripetalforce=_mass*_len*_omega*_omega;
 519     float vrel[3],vreldir[3];
 520     Math::sub3(_speed,v,vrel);
 521     float scalar_torque=0;
 522     Math::unit3(vrel,vreldir);//direction of blade-movement rel. to air
 523     //Angle of blade which would produce no vertical force (where the
 524     //effective incidence is zero)
 525
 526     float cyc=_cyclic;
 527     float col=_collective;
 528     if (_shared_flap_hinge)
 529         _incidence=(col+cyc)-_delta3*0.5*(_alphaalt-_oppositerp->getAlphaAlt());
 530     else
 531         _incidence=(col+cyc)-_delta3*_alphaalt;
 532     //the incidence of the rotorblade due to control input reduced by the
 533     //delta3 effect, see README.YASIM
 534     //float beta=_relamp*cyc+col;
 535     //the incidence of the rotorblade which is used for the calculation
 536
 537     float alpha,factor; //alpha is the flapping angle
 538     //the new flapping angle will be the old flapping angle
 539     //+ factor *(alpha - "old flapping angle")
 540     alpha=calculateAlpha(v,rho,_incidence,cyc,0,&scalar_torque);
 541     alpha=Math::clamp(alpha,_alphamin,_alphamax);
 542     //the incidence is a function of alpha (if _delta* != 0)
 543     //Therefore missing: wrap this function in an integrator
 544     //(runge kutta e. g.)
 545
 546     factor=_dt*_dynamic;
 547     if (factor>1) factor=1;
 548
 549     float dirblade[3];
 550     Math::cross3(_normal,_directionofcentripetalforce,dirblade);
 551     //float vblade=Math::abs(Math::dot3(dirblade,v));
 552
 553     alpha=_alphaalt+(alpha-_alphaalt)*factor;
 554     _alpha=alpha;
 555     float meancosalpha=(1*Math::cos(_last90rp->getrealAlpha())
 556         +1*Math::cos(_next90rp->getrealAlpha())
 557         +1*Math::cos(_oppositerp->getrealAlpha())
 558         +1*Math::cos(alpha))/4;
 559     float schwenkfactor=1-(Math::cos(_lastrp->getrealAlpha())-meancosalpha)*_rotor->getNumberOfParts()/4;
 560
 561     //missing: consideration of rellenhinge
 562
 563     //add the unbalance
 564     _centripetalforce*=_balance;
 565     scalar_torque*=_balance;
 566
 567     float xforce = Math::cos(alpha)*_centripetalforce;
 568     float zforce = schwenkfactor*Math::sin(alpha)*_centripetalforce;
 569     *torque_scalar=scalar_torque;
 570     scalar_torque+= 0*_ddt_omega*_torque_of_inertia;
 571     float thetorque = scalar_torque;
 572     int i;
 573     float f=_rotor->getCcw()?1:-1;
 574     for(i=0; i<3; i++) {
 575         _last_torque[i]=torque[i] = f*_normal[i]*thetorque;
 576         out[i] = _normal[i]*zforce*_rotor->getLiftFactor()
 577             +_directionofcentripetalforce[i]*xforce;
 578     }
 579 }
 580
 581 void Rotorpart::getAccelTorque(float relaccel,float *t)
 582 {
 583     int i;
 584     float f=_rotor->getCcw()?1:-1;
 585     for(i=0; i<3; i++) {
 586         t[i]=f*-1* _normal[i]*relaccel*_omegan* _torque_of_inertia;// *_omeagan ?
 587         _rotor->addTorque(-relaccel*_omegan* _torque_of_inertia);
 588     }
 589 }
 590 std::ostream &  operator<<(std::ostream & out, const Rotorpart& rp)
 591 {
 592 #define i(x) << #x << ":" << rp.x << endl
 593 #define iv(x) << #x << ":" << rp.x[0] << ";" << rp.x[1] << ";" <<rp.x[2] << ";" << endl
 594     out << "Writing Info on Rotorpart " << endl
 595         i( _dt)
 596         iv( _last_torque)
 597         i( _compiled)
 598         iv( _pos)    // position in local coords
 599         iv( _posforceattac)    // position in local coords
 600         iv( _normal) //direcetion of the rotation axis
 601         i( _torque_max_force)
 602         i( _torque_no_force)
 603         iv( _speed)
 604         iv( _direction_of_movement)
 605         iv( _directionofcentripetalforce)
 606         iv( _directionofrotorpart)
 607         i( _centripetalforce)
 608         i( _cyclic)
 609         i( _collective)
 610         i( _delta3)
 611         i( _dynamic)
 612         i( _translift)
 613         i( _c2)
 614         i( _mass)
 615         i( _alpha)
 616         i( _alphaalt)
 617         i( _alphamin) i(_alphamax) i(_alpha0) i(_alpha0factor)
 618         i( _rellenhinge)
 619         i( _relamp)
 620         i( _omega) i(_omegan) i(_ddt_omega)
 621         i( _phi)
 622         i( _len)
 623         i( _incidence)
 624         i( _twist) //outer incidence = inner inner incidence + _twist
 625         i( _number_of_segments)
 626         i( _rel_len_where_incidence_is_measured)
 627         i( _rel_len_blade_start)
 628         i( _diameter)
 629         i( _torque_of_inertia)
 630         i( _torque)
 631         i (_alphaoutputbuf[0])
 632         i (_alphaoutputbuf[1])
 633         i( _alpha2type)
 634         i(_rotor_correction_factor)
 635     << endl;
 636 #undef i
 637 #undef iv
 638     return out;
 639 }
 640 }; // namespace yasim