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 (_directionofcentripetalforce,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     _centripetalforce=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(_directionofcentripetalforce,_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++) _directionofcentripetalforce[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     _centripetalforce=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     if (f>-0.01) f=-0.01; //half a degree bending
 258     _alpha0=f;
 259 }
 260
 261 void Rotorpart::setAlphamin(float f)
 262 {
 263     _alphamin=f;
 264 }
 265
 266 void Rotorpart::setAlphamax(float f)
 267 {
 268     _alphamax=f;
 269 }
 270
 271 void Rotorpart::setAlpha0factor(float f)
 272 {
 273     _alpha0factor=f;
 274 }
 275
 276 void Rotorpart::setDiameter(float f)
 277 {
 278     _diameter=f;
 279 }
 280
 281 float Rotorpart::getPhi()
 282 {
 283     return(_phi);
 284 }
 285
 286 float Rotorpart::getAlpha(int i)
 287 {
 288     i=i&1;
 289
 290     if (i==0)
 291         return _alpha*180/pi;//in Grad = 1
 292     else
 293     {
 294         if (_alpha2type==1) //yaw or roll
 295             return (getAlpha(0)-_oppositerp->getAlpha(0))/2;
 296         else //collective
 297             return (getAlpha(0)+_oppositerp->getAlpha(0)+
 298             _next90rp->getAlpha(0)+_last90rp->getAlpha(0))/4;
 299     }
 300 }
 301 float Rotorpart::getrealAlpha(void)
 302 {
 303     return _alpha;
 304 }
 305
 306 void Rotorpart::setAlphaoutput(char *text,int i)
 307 {
 308     strncpy(_alphaoutputbuf[i>0],text,255);
 309     if (i>0) _alpha2type=i;
 310 }
 311
 312 char* Rotorpart::getAlphaoutput(int i)
 313 {
 314     return _alphaoutputbuf[i&1];
 315 }
 316
 317 void Rotorpart::setLen(float value)
 318 {
 319     _len=value;
 320 }
 321
 322 void Rotorpart::setNormal(float* p)
 323 {
 324     int i;
 325     for(i=0; i<3; i++) _normal[i] = p[i];
 326 }
 327
 328 void Rotorpart::getNormal(float* out)
 329 {
 330     int i;
 331     for(i=0; i<3; i++) out[i] = _normal[i];
 332 }
 333
 334 void Rotorpart::setCollective(float pos)
 335 {
 336     _collective = pos;
 337 }
 338
 339 void Rotorpart::setCyclic(float pos)
 340 {
 341     _cyclic = pos;
 342 }
 343
 344 void Rotorpart::setlastnextrp(Rotorpart*lastrp,Rotorpart*nextrp,
 345     Rotorpart *oppositerp,Rotorpart*last90rp,Rotorpart*next90rp)
 346 {
 347     _lastrp=lastrp;
 348     _nextrp=nextrp;
 349     _oppositerp=oppositerp;
 350     _last90rp=last90rp;
 351     _next90rp=next90rp;
 352 }
 353
 354 void Rotorpart::strncpy(char *dest,const char *src,int maxlen)
 355 {
 356     int n=0;
 357     while(src[n]&&n<(maxlen-1))
 358     {
 359         dest[n]=src[n];
 360         n++;
 361     }
 362     dest[n]=0;
 363 }
 364
 365 // Calculate the flapping angle, where zentripetal force and
 366 //lift compensate each other
 367 float Rotorpart::calculateAlpha(float* v_rel_air, float rho,
 368     float incidence, float cyc, float alphaalt, float *torque,
 369     float *returnlift)
 370 {
 371     float moment[3],v_local[3],v_local_scalar,lift_moment,v_flap[3],v_help[3];
 372     float ias;//nur f. dgb
 373     int i,n;
 374     for (i=0;i<3;i++)
 375         moment[i]=0;
 376     lift_moment=-_mass*_len; //*cos yaw * cos roll
 377     *torque=0;//
 378     if((_nextrp==NULL)||(_lastrp==NULL)||(_rotor==NULL))
 379         return 0.0;//not initialized. Can happen during startupt of flightgear
 380     if (returnlift!=NULL) *returnlift=0;
 381     /*float flap_omega=(_nextrp->getrealAlpha()-_lastrp->getrealAlpha())
 382         *_omega / pi*_rotor->getNumberOfParts()/4; hier mal die alte version probieren?
 383         */
 384     float flap_omega=(_next90rp->getrealAlpha()-_last90rp->getrealAlpha())
 385         *_omega / pi;
 386     float local_width=_diameter*(1-_rel_len_blade_start)/2.
 387         /(float (_number_of_segments));
 388     for (n=0;n<_number_of_segments;n++)
 389     {
 390         float rel = (n+.5)/(float (_number_of_segments));
 391         float r= _diameter *0.5 *(rel*(1-_rel_len_blade_start)
 392             +_rel_len_blade_start);
 393         float local_incidence=incidence+_twist *rel -
 394             _twist *_rel_len_where_incidence_is_measured;
 395         float local_chord = _rotor->getChord()*rel+_rotor->getChord()
 396             *_rotor->getTaper()*(1-rel);
 397         float A = local_chord * local_width;
 398         //calculate the local air speed and the incidence to this speed;
 399         Math::mul3(_omega * r , _direction_of_movement , v_local);
 400
 401         // add speed component due to flapping
 402         Math::mul3(flap_omega * r,_normal,v_flap);
 403         Math::add3(v_flap,v_local,v_local);
 404         Math::sub3(v_rel_air,v_local,v_local);
 405         //v_local is now the total airspeed at the blade
 406         //apparent missing: calculating the local_wind = v_rel_air at
 407         //every point of the rotor. It differs due to aircraft-rotation
 408         //it is considered in v_flap
 409
 410         //substract now the component of the air speed parallel to
 411         //the blade;
 412         Math::mul3(Math::dot3(v_local,_directionofrotorpart),
 413            _directionofrotorpart,v_help);
 414         Math::sub3(v_local,v_help,v_local);
 415
 416         //split into direction and magnitude
 417         v_local_scalar=Math::mag3(v_local);
 418         if (v_local_scalar!=0)
 419             //Math::unit3(v_local,v_help);
 420             Math::mul3(1/v_local_scalar,v_local,v_help);
 421         float incidence_of_airspeed = Math::asin(Math::clamp(
 422             Math::dot3(v_help,_normal),-1,1)) + local_incidence;
 423         ias = incidence_of_airspeed;
 424
 425         //reduce the ias (Prantl factor)
 426         float prantl_factor=2/pi*Math::acos(Math::exp(
 427             -_rotor->getNumberOfBlades()/2.*(1-rel)
 428              *Math::sqrt(1+1/Math::sqr(Math::tan(
 429                pi/2-Math::abs(incidence_of_airspeed-local_incidence))))));
 430         incidence_of_airspeed = (incidence_of_airspeed+
 431             _rotor->getAirfoilIncidenceNoLift())*prantl_factor
 432             *_rotor_correction_factor-_rotor->getAirfoilIncidenceNoLift();
 433         ias = incidence_of_airspeed;
 434         float lift_wo_cyc = _rotor->getLiftCoef(incidence_of_airspeed
 435             -cyc*_rotor_correction_factor,v_local_scalar)
 436             * v_local_scalar * v_local_scalar * A *rho *0.5;
 437         float lift_with_cyc =
 438             _rotor->getLiftCoef(incidence_of_airspeed,v_local_scalar)
 439             * v_local_scalar * v_local_scalar *A *rho*0.5;
 440         float lift=lift_wo_cyc+_relamp*(lift_with_cyc-lift_wo_cyc);
 441         //take into account that the rotor is a resonant system where
 442         //the cyclic input hase increased result
 443         float drag = -_rotor->getDragCoef(incidence_of_airspeed,v_local_scalar)
 444             * v_local_scalar * v_local_scalar * A *rho*0.5;
 445         float angle = incidence_of_airspeed - incidence;
 446         //angle between blade movement caused by rotor-rotation and the
 447         //total movement of the blade
 448
 449         /* the next shold look like this, but this is the inner loop of
 450            the rotor simulation. For small angles (and we hav only small
 451            angles) the first order approximation works well
 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             */
 457         lift_moment += r*(lift * (1-angle*angle)
 458             - drag * angle);
 459         *torque     += r*(drag * (1-angle*angle)
 460             + lift * angle);
 461
 462         if (returnlift!=NULL) *returnlift+=lift;
 463     }
 464     //as above, use 1st order approximation
 465     //float alpha=Math::atan2(lift_moment,_centripetalforce * _len);
 466     float alpha;
 467     alpha=lift_moment/(_centripetalforce * _len - _mass * _len/_alpha0);
 468     //centripetalforce is >=0 and _alpha0<-0.01
 469     return (alpha);
 470 }
 471
 472 // Calculate the aerodynamic force given a wind vector v (in the
 473 // aircraft's "local" coordinates) and an air density rho.  Returns a
 474 // torque about the Y axis, too.
 475 void Rotorpart::calcForce(float* v, float rho,  float* out, float* torque,
 476     float* torque_scalar)
 477 {
 478     if (_compiled!=1)
 479     {
 480         for (int i=0;i<3;i++)
 481             torque[i]=out[i]=0;
 482         *torque_scalar=0;
 483         return;
 484     }
 485     _centripetalforce=_mass*_len*_omega*_omega;
 486     float vrel[3],vreldir[3];
 487     Math::sub3(_speed,v,vrel);
 488     float scalar_torque=0;
 489     Math::unit3(vrel,vreldir);//direction of blade-movement rel. to air
 490     //Angle of blade which would produce no vertical force (where the
 491     //effective incidence is zero)
 492
 493     //float cyc=_mincyclic+(_cyclic+1)/2*(_maxcyclic-_mincyclic);
 494     float cyc=_cyclic;
 495     float col=_minpitch+(_collective+1)/2*(_maxpitch-_minpitch);
 496     _incidence=(col+cyc)-_delta3*_alphaalt;
 497     //the incidence of the rotorblade due to control input reduced by the
 498     //delta3 effect, see README.YASIM
 499     //float beta=_relamp*cyc+col;
 500     //the incidence of the rotorblade which is used for the calculation
 501
 502     float alpha,factor; //alpha is the flapping angle
 503     //the new flapping angle will be the old flapping angle
 504     //+ factor *(alpha - "old flapping angle")
 505     alpha=calculateAlpha(v,rho,_incidence,cyc,0,&scalar_torque);
 506     //the incidence is a function of alpha (if _delta* != 0)
 507     //Therefore missing: wrap this function in an integrator
 508     //(runge kutta e. g.)
 509
 510     factor=_dt*_dynamic;
 511     if (factor>1) factor=1;
 512
 513     float dirblade[3];
 514     Math::cross3(_normal,_directionofcentripetalforce,dirblade);
 515     float vblade=Math::abs(Math::dot3(dirblade,v));
 516
 517     alpha=_alphaalt+(alpha-_alphaalt)*factor;
 518     _alpha=alpha;
 519     float meancosalpha=(1*Math::cos(_last90rp->getrealAlpha())
 520         +1*Math::cos(_next90rp->getrealAlpha())
 521         +1*Math::cos(_oppositerp->getrealAlpha())
 522         +1*Math::cos(alpha))/4;
 523     float schwenkfactor=1-(Math::cos(_lastrp->getrealAlpha())-meancosalpha)*_rotor->getNumberOfParts()/4;
 524
 525     //missing: consideration of rellenhinge
 526     float xforce = Math::cos(alpha)*_centripetalforce;
 527     float zforce = schwenkfactor*Math::sin(alpha)*_centripetalforce;
 528     *torque_scalar=scalar_torque;
 529     scalar_torque+= 0*_ddt_omega*_torque_of_inertia;
 530     float thetorque = scalar_torque;
 531     int i;
 532     float f=_rotor->getCcw()?1:-1;
 533     for(i=0; i<3; i++) {
 534         _last_torque[i]=torque[i] = f*_normal[i]*thetorque;
 535         out[i] = _normal[i]*zforce*_rotor->getLiftFactor()
 536             +_directionofcentripetalforce[i]*xforce;
 537     }
 538 }
 539
 540 void Rotorpart::getAccelTorque(float relaccel,float *t)
 541 {
 542     int i;
 543     float f=_rotor->getCcw()?1:-1;
 544     for(i=0; i<3; i++) {
 545         t[i]=f*-1* _normal[i]*relaccel*_omegan* _torque_of_inertia;// *_omeagan ?
 546         _rotor->addTorque(-relaccel*_omegan* _torque_of_inertia);
 547     }
 548 }
 549 std::ostream &  operator<<(std::ostream & out, const Rotorpart& rp)
 550 {
 551 #define i(x) << #x << ":" << rp.x << endl
 552 #define iv(x) << #x << ":" << rp.x[0] << ";" << rp.x[1] << ";" <<rp.x[2] << ";" << endl
 553     out << "Writing Info on Rotorpart " << endl
 554         i( _dt)
 555         iv( _last_torque)
 556         i( _compiled)
 557         iv( _pos)    // position in local coords
 558         iv( _posforceattac)    // position in local coords
 559         iv( _normal) //direcetion of the rotation axis
 560         i( _torque_max_force)
 561         i( _torque_no_force)
 562         iv( _speed)
 563         iv( _direction_of_movement)
 564         iv( _directionofcentripetalforce)
 565         iv( _directionofrotorpart)
 566         i( _centripetalforce)
 567         i( _maxpitch)
 568         i( _minpitch)
 569         i( _maxcyclic)
 570         i( _mincyclic)
 571         i( _cyclic)
 572         i( _collective)
 573         i( _delta3)
 574         i( _dynamic)
 575         i( _translift)
 576         i( _c2)
 577         i( _mass)
 578         i( _alpha)
 579         i( _alphaalt)
 580         i( _alphamin) i(_alphamax) i(_alpha0) i(_alpha0factor)
 581         i( _rellenhinge)
 582         i( _relamp)
 583         i( _omega) i(_omegan) i(_ddt_omega)
 584         i( _phi)
 585         i( _len)
 586         i( _incidence)
 587         i( _twist) //outer incidence = inner inner incidence + _twist
 588         i( _number_of_segments)
 589         i( _rel_len_where_incidence_is_measured)
 590         i( _rel_len_blade_start)
 591         i( _diameter)
 592         i( _torque_of_inertia)
 593         i( _torque)
 594         i (_alphaoutputbuf[0])
 595         i (_alphaoutputbuf[1])
 596         i( _alpha2type)
 597         i(_rotor_correction_factor)
 598     << endl;
 599 #undef i
 600 #undef iv
 601     return out;
 602 }
 603 }; // namespace yasim