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