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