src/FDM/YASim/Rotorpart.cpp

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