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