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         lift_moment += r*(lift * Math::cos(angle)
 453             - drag * Math::sin(angle));
 454         *torque     += r*(drag * Math::cos(angle)
 455             + lift * Math::sin(angle));
 456         if (returnlift!=NULL) *returnlift+=lift;
 457     }
 458     //use 1st order approximation for alpha
 459     //float alpha=Math::atan2(lift_moment,_centripetalforce * _len);
 460     float alpha;
 461     if (_shared_flap_hinge)
 462     {
 463         float div=0;
 464         if (Math::abs(_alphaalt) >1e-6)
 465             div=(_centripetalforce * _len - _mass * _len * 9.81 * relgrav /_alpha0*(_alphaalt+_oppositerp->getAlphaAlt())/(2.0*_alphaalt));
 466         if (Math::abs(div)>1e-6)
 467         {
 468             alpha=lift_moment/div;
 469         }
 470         else if(Math::abs(_alphaalt+_oppositerp->getAlphaAlt())>1e-6)
 471         {
 472             float div=(_centripetalforce * _len - _mass * _len * 9.81 *0.5 * relgrav)*(_alphaalt+_oppositerp->getAlphaAlt());
 473             if (Math::abs(div)>1e-6)
 474             {
 475                 alpha=_oppositerp->getAlphaAlt()+lift_moment/div*_alphaalt;
 476             }
 477             else
 478                 alpha=_alphaalt;
 479         }
 480         else
 481             alpha=_alphaalt;
 482         if (_omega/_omegan<0.2)
 483         {
 484             float frac = 0.001+_omega/_omegan*4.995;
 485             alpha=Math::clamp(alpha,_alphamin,_alphamax);
 486             alpha=_alphaalt*(1-frac)+frac*alpha;
 487         }
 488     }
 489     else
 490     {
 491         float div=(_centripetalforce * _len - _mass * _len * 9.81 /_alpha0);
 492         if (Math::abs(div)>1e-6)
 493             alpha=lift_moment/div;
 494         else
 495             alpha=_alphaalt;
 496     }
 497
 498     return (alpha);
 499 }
 500
 501 // Calculate the aerodynamic force given a wind vector v (in the
 502 // aircraft's "local" coordinates) and an air density rho.  Returns a
 503 // torque about the Y axis, too.
 504 void Rotorpart::calcForce(float* v, float rho,  float* out, float* torque,
 505     float* torque_scalar)
 506 {
 507     if (_compiled!=1)
 508     {
 509         for (int i=0;i<3;i++)
 510             torque[i]=out[i]=0;
 511         *torque_scalar=0;
 512         return;
 513     }
 514     _centripetalforce=_mass*_len*_omega*_omega;
 515     float vrel[3],vreldir[3];
 516     Math::sub3(_speed,v,vrel);
 517     float scalar_torque=0;
 518     Math::unit3(vrel,vreldir);//direction of blade-movement rel. to air
 519     //Angle of blade which would produce no vertical force (where the
 520     //effective incidence is zero)
 521
 522     float cyc=_cyclic;
 523     float col=_collective;
 524     if (_shared_flap_hinge)
 525         _incidence=(col+cyc)-_delta3*0.5*(_alphaalt-_oppositerp->getAlphaAlt());
 526     else
 527         _incidence=(col+cyc)-_delta3*_alphaalt;
 528     //the incidence of the rotorblade due to control input reduced by the
 529     //delta3 effect, see README.YASIM
 530     //float beta=_relamp*cyc+col;
 531     //the incidence of the rotorblade which is used for the calculation
 532
 533     float alpha,factor; //alpha is the flapping angle
 534     //the new flapping angle will be the old flapping angle
 535     //+ factor *(alpha - "old flapping angle")
 536     alpha=calculateAlpha(v,rho,_incidence,cyc,0,&scalar_torque);
 537     alpha=Math::clamp(alpha,_alphamin,_alphamax);
 538     //the incidence is a function of alpha (if _delta* != 0)
 539     //Therefore missing: wrap this function in an integrator
 540     //(runge kutta e. g.)
 541
 542     factor=_dt*_dynamic;
 543     if (factor>1) factor=1;
 544
 545     float dirblade[3];
 546     Math::cross3(_normal,_directionofcentripetalforce,dirblade);
 547     float vblade=Math::abs(Math::dot3(dirblade,v));
 548
 549     alpha=_alphaalt+(alpha-_alphaalt)*factor;
 550     _alpha=alpha;
 551     float meancosalpha=(1*Math::cos(_last90rp->getrealAlpha())
 552         +1*Math::cos(_next90rp->getrealAlpha())
 553         +1*Math::cos(_oppositerp->getrealAlpha())
 554         +1*Math::cos(alpha))/4;
 555     float schwenkfactor=1-(Math::cos(_lastrp->getrealAlpha())-meancosalpha)*_rotor->getNumberOfParts()/4;
 556
 557     //missing: consideration of rellenhinge
 558
 559     //add the unbalance
 560     _centripetalforce*=_balance;
 561     scalar_torque*=_balance;
 562
 563     float xforce = Math::cos(alpha)*_centripetalforce;
 564     float zforce = schwenkfactor*Math::sin(alpha)*_centripetalforce;
 565     *torque_scalar=scalar_torque;
 566     scalar_torque+= 0*_ddt_omega*_torque_of_inertia;
 567     float thetorque = scalar_torque;
 568     int i;
 569     float f=_rotor->getCcw()?1:-1;
 570     for(i=0; i<3; i++) {
 571         _last_torque[i]=torque[i] = f*_normal[i]*thetorque;
 572         out[i] = _normal[i]*zforce*_rotor->getLiftFactor()
 573             +_directionofcentripetalforce[i]*xforce;
 574     }
 575 }
 576
 577 void Rotorpart::getAccelTorque(float relaccel,float *t)
 578 {
 579     int i;
 580     float f=_rotor->getCcw()?1:-1;
 581     for(i=0; i<3; i++) {
 582         t[i]=f*-1* _normal[i]*relaccel*_omegan* _torque_of_inertia;// *_omeagan ?
 583         _rotor->addTorque(-relaccel*_omegan* _torque_of_inertia);
 584     }
 585 }
 586 std::ostream &  operator<<(std::ostream & out, const Rotorpart& rp)
 587 {
 588 #define i(x) << #x << ":" << rp.x << endl
 589 #define iv(x) << #x << ":" << rp.x[0] << ";" << rp.x[1] << ";" <<rp.x[2] << ";" << endl
 590     out << "Writing Info on Rotorpart " << endl
 591         i( _dt)
 592         iv( _last_torque)
 593         i( _compiled)
 594         iv( _pos)    // position in local coords
 595         iv( _posforceattac)    // position in local coords
 596         iv( _normal) //direcetion of the rotation axis
 597         i( _torque_max_force)
 598         i( _torque_no_force)
 599         iv( _speed)
 600         iv( _direction_of_movement)
 601         iv( _directionofcentripetalforce)
 602         iv( _directionofrotorpart)
 603         i( _centripetalforce)
 604         i( _cyclic)
 605         i( _collective)
 606         i( _delta3)
 607         i( _dynamic)
 608         i( _translift)
 609         i( _c2)
 610         i( _mass)
 611         i( _alpha)
 612         i( _alphaalt)
 613         i( _alphamin) i(_alphamax) i(_alpha0) i(_alpha0factor)
 614         i( _rellenhinge)
 615         i( _relamp)
 616         i( _omega) i(_omegan) i(_ddt_omega)
 617         i( _phi)
 618         i( _len)
 619         i( _incidence)
 620         i( _twist) //outer incidence = inner inner incidence + _twist
 621         i( _number_of_segments)
 622         i( _rel_len_where_incidence_is_measured)
 623         i( _rel_len_blade_start)
 624         i( _diameter)
 625         i( _torque_of_inertia)
 626         i( _torque)
 627         i (_alphaoutputbuf[0])
 628         i (_alphaoutputbuf[1])
 629         i( _alpha2type)
 630         i(_rotor_correction_factor)
 631     << endl;
 632 #undef i
 633 #undef iv
 634     return out;
 635 }
 636 }; // namespace yasim