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     lift_moment=-_mass*_len*9.81; //*cos yaw * cos roll
 365     *torque=0;//
 366     if((_nextrp==NULL)||(_lastrp==NULL)||(_rotor==NULL))
 367         return 0.0;//not initialized. Can happen during startupt of flightgear
 368     if (returnlift!=NULL) *returnlift=0;
 369     float flap_omega=(_next90rp->getrealAlpha()-_last90rp->getrealAlpha())
 370         *_omega / pi;
 371     float local_width=_diameter*(1-_rel_len_blade_start)/2.
 372         /(float (_number_of_segments));
 373     for (n=0;n<_number_of_segments;n++)
 374     {
 375         float rel = (n+.5)/(float (_number_of_segments));
 376         float r= _diameter *0.5 *(rel*(1-_rel_len_blade_start)
 377             +_rel_len_blade_start);
 378         float local_incidence=incidence+_twist *rel -
 379             _twist *_rel_len_where_incidence_is_measured;
 380         float local_chord = _rotor->getChord()*rel+_rotor->getChord()
 381             *_rotor->getTaper()*(1-rel);
 382         float A = local_chord * local_width;
 383         //calculate the local air speed and the incidence to this speed;
 384         Math::mul3(_omega * r , _direction_of_movement , v_local);
 385
 386         // add speed component due to flapping
 387         Math::mul3(flap_omega * r,_normal,v_flap);
 388         Math::add3(v_flap,v_local,v_local);
 389         Math::sub3(v_rel_air,v_local,v_local);
 390         //v_local is now the total airspeed at the blade
 391         //apparent missing: calculating the local_wind = v_rel_air at
 392         //every point of the rotor. It differs due to aircraft-rotation
 393         //it is considered in v_flap
 394
 395         //substract now the component of the air speed parallel to
 396         //the blade;
 397         Math::mul3(Math::dot3(v_local,_directionofrotorpart),
 398            _directionofrotorpart,v_help);
 399         Math::sub3(v_local,v_help,v_local);
 400
 401         //split into direction and magnitude
 402         v_local_scalar=Math::mag3(v_local);
 403         if (v_local_scalar!=0)
 404             //Math::unit3(v_local,v_help);
 405             Math::mul3(1/v_local_scalar,v_local,v_help);
 406         float incidence_of_airspeed = Math::asin(Math::clamp(
 407             Math::dot3(v_help,_normal),-1,1)) + local_incidence;
 408         ias = incidence_of_airspeed;
 409
 410         //reduce the ias (Prantl factor)
 411         float prantl_factor=2/pi*Math::acos(Math::exp(
 412             -_rotor->getNumberOfBlades()/2.*(1-rel)
 413              *Math::sqrt(1+1/Math::sqr(Math::tan(
 414                pi/2-Math::abs(incidence_of_airspeed-local_incidence))))));
 415         incidence_of_airspeed = (incidence_of_airspeed+
 416             _rotor->getAirfoilIncidenceNoLift())*prantl_factor
 417             *_rotor_correction_factor-_rotor->getAirfoilIncidenceNoLift();
 418         ias = incidence_of_airspeed;
 419         float lift_wo_cyc = _rotor->getLiftCoef(incidence_of_airspeed
 420             -cyc*_rotor_correction_factor*prantl_factor,v_local_scalar)
 421             * v_local_scalar * v_local_scalar * A *rho *0.5;
 422         float lift_with_cyc =
 423             _rotor->getLiftCoef(incidence_of_airspeed,v_local_scalar)
 424             * v_local_scalar * v_local_scalar *A *rho*0.5;
 425         float lift=lift_wo_cyc+_relamp*(lift_with_cyc-lift_wo_cyc);
 426         //take into account that the rotor is a resonant system where
 427         //the cyclic input hase increased result
 428         float drag = -_rotor->getDragCoef(incidence_of_airspeed,v_local_scalar)
 429             * v_local_scalar * v_local_scalar * A *rho*0.5;
 430         float angle = incidence_of_airspeed - incidence;
 431         //angle between blade movement caused by rotor-rotation and the
 432         //total movement of the blade
 433
 434         /* the next shold look like this, but this is the inner loop of
 435            the rotor simulation. For small angles (and we hav only small
 436            angles) the first order approximation works well
 437         lift_moment += r*(lift * Math::cos(angle)
 438             - drag * Math::sin(angle));
 439         *torque     += r*(drag * Math::cos(angle)
 440             + lift * Math::sin(angle));
 441             */
 442         lift_moment += r*(lift * (1-angle*angle)
 443             - drag * angle);
 444         *torque     += r*(drag * (1-angle*angle)
 445             + lift * angle);
 446
 447         if (returnlift!=NULL) *returnlift+=lift;
 448     }
 449     //as above, use 1st order approximation
 450     //float alpha=Math::atan2(lift_moment,_centripetalforce * _len);
 451     float alpha;
 452     if (_shared_flap_hinge)
 453     {
 454         float div=0;
 455         if (Math::abs(_alphaalt) >1e-6)
 456             div=(_centripetalforce * _len - _mass * _len * 9.81 /_alpha0*(_alphaalt+_oppositerp->getAlphaAlt())/(2.0*_alphaalt));
 457         if (Math::abs(div)>1e-6)
 458             alpha=lift_moment/div;
 459         else if(Math::abs(_alphaalt+_oppositerp->getAlphaAlt())>1e-6)
 460         {
 461             float div=(_centripetalforce * _len - _mass * _len * 9.81 *0.5)*(_alphaalt+_oppositerp->getAlphaAlt());
 462             if (Math::abs(div)>1e-6)
 463                 alpha=_oppositerp->getAlphaAlt()+lift_moment/div*_alphaalt;
 464         }
 465         else
 466             alpha=_alphaalt;
 467     }
 468     else
 469     {
 470         float div=(_centripetalforce * _len - _mass * _len * 9.81 /_alpha0);
 471         if (Math::abs(div)>1e-6)
 472             alpha=lift_moment/div;
 473         else
 474             alpha=_alphaalt;
 475     }
 476
 477     return (alpha);
 478 }
 479
 480 // Calculate the aerodynamic force given a wind vector v (in the
 481 // aircraft's "local" coordinates) and an air density rho.  Returns a
 482 // torque about the Y axis, too.
 483 void Rotorpart::calcForce(float* v, float rho,  float* out, float* torque,
 484     float* torque_scalar)
 485 {
 486     if (_compiled!=1)
 487     {
 488         for (int i=0;i<3;i++)
 489             torque[i]=out[i]=0;
 490         *torque_scalar=0;
 491         return;
 492     }
 493     _centripetalforce=_mass*_len*_omega*_omega;
 494     float vrel[3],vreldir[3];
 495     Math::sub3(_speed,v,vrel);
 496     float scalar_torque=0;
 497     Math::unit3(vrel,vreldir);//direction of blade-movement rel. to air
 498     //Angle of blade which would produce no vertical force (where the
 499     //effective incidence is zero)
 500
 501     float cyc=_cyclic;
 502     float col=_collective;
 503     if (_shared_flap_hinge)
 504         _incidence=(col+cyc)-_delta3*0.5*(_alphaalt-_oppositerp->getAlphaAlt());
 505     else
 506         _incidence=(col+cyc)-_delta3*_alphaalt;
 507     //the incidence of the rotorblade due to control input reduced by the
 508     //delta3 effect, see README.YASIM
 509     //float beta=_relamp*cyc+col;
 510     //the incidence of the rotorblade which is used for the calculation
 511
 512     float alpha,factor; //alpha is the flapping angle
 513     //the new flapping angle will be the old flapping angle
 514     //+ factor *(alpha - "old flapping angle")
 515     alpha=calculateAlpha(v,rho,_incidence,cyc,0,&scalar_torque);
 516     alpha=Math::clamp(alpha,_alphamin,_alphamax);
 517     //the incidence is a function of alpha (if _delta* != 0)
 518     //Therefore missing: wrap this function in an integrator
 519     //(runge kutta e. g.)
 520
 521     factor=_dt*_dynamic;
 522     if (factor>1) factor=1;
 523
 524     float dirblade[3];
 525     Math::cross3(_normal,_directionofcentripetalforce,dirblade);
 526     float vblade=Math::abs(Math::dot3(dirblade,v));
 527
 528     alpha=_alphaalt+(alpha-_alphaalt)*factor;
 529     _alpha=alpha;
 530     float meancosalpha=(1*Math::cos(_last90rp->getrealAlpha())
 531         +1*Math::cos(_next90rp->getrealAlpha())
 532         +1*Math::cos(_oppositerp->getrealAlpha())
 533         +1*Math::cos(alpha))/4;
 534     float schwenkfactor=1-(Math::cos(_lastrp->getrealAlpha())-meancosalpha)*_rotor->getNumberOfParts()/4;
 535
 536     //missing: consideration of rellenhinge
 537     float xforce = Math::cos(alpha)*_centripetalforce;
 538     float zforce = schwenkfactor*Math::sin(alpha)*_centripetalforce;
 539     *torque_scalar=scalar_torque;
 540     scalar_torque+= 0*_ddt_omega*_torque_of_inertia;
 541     float thetorque = scalar_torque;
 542     int i;
 543     float f=_rotor->getCcw()?1:-1;
 544     for(i=0; i<3; i++) {
 545         _last_torque[i]=torque[i] = f*_normal[i]*thetorque;
 546         out[i] = _normal[i]*zforce*_rotor->getLiftFactor()
 547             +_directionofcentripetalforce[i]*xforce;
 548     }
 549 }
 550
 551 void Rotorpart::getAccelTorque(float relaccel,float *t)
 552 {
 553     int i;
 554     float f=_rotor->getCcw()?1:-1;
 555     for(i=0; i<3; i++) {
 556         t[i]=f*-1* _normal[i]*relaccel*_omegan* _torque_of_inertia;// *_omeagan ?
 557         _rotor->addTorque(-relaccel*_omegan* _torque_of_inertia);
 558     }
 559 }
 560 std::ostream &  operator<<(std::ostream & out, const Rotorpart& rp)
 561 {
 562 #define i(x) << #x << ":" << rp.x << endl
 563 #define iv(x) << #x << ":" << rp.x[0] << ";" << rp.x[1] << ";" <<rp.x[2] << ";" << endl
 564     out << "Writing Info on Rotorpart " << endl
 565         i( _dt)
 566         iv( _last_torque)
 567         i( _compiled)
 568         iv( _pos)    // position in local coords
 569         iv( _posforceattac)    // position in local coords
 570         iv( _normal) //direcetion of the rotation axis
 571         i( _torque_max_force)
 572         i( _torque_no_force)
 573         iv( _speed)
 574         iv( _direction_of_movement)
 575         iv( _directionofcentripetalforce)
 576         iv( _directionofrotorpart)
 577         i( _centripetalforce)
 578         i( _cyclic)
 579         i( _collective)
 580         i( _delta3)
 581         i( _dynamic)
 582         i( _translift)
 583         i( _c2)
 584         i( _mass)
 585         i( _alpha)
 586         i( _alphaalt)
 587         i( _alphamin) i(_alphamax) i(_alpha0) i(_alpha0factor)
 588         i( _rellenhinge)
 589         i( _relamp)
 590         i( _omega) i(_omegan) i(_ddt_omega)
 591         i( _phi)
 592         i( _len)
 593         i( _incidence)
 594         i( _twist) //outer incidence = inner inner incidence + _twist
 595         i( _number_of_segments)
 596         i( _rel_len_where_incidence_is_measured)
 597         i( _rel_len_blade_start)
 598         i( _diameter)
 599         i( _torque_of_inertia)
 600         i( _torque)
 601         i (_alphaoutputbuf[0])
 602         i (_alphaoutputbuf[1])
 603         i( _alpha2type)
 604         i(_rotor_correction_factor)
 605     << endl;
 606 #undef i
 607 #undef iv
 608     return out;
 609 }
 610 }; // namespace yasim