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     _translift=0;
  49     _dynamic=100;
  50     _c2=0;
  51     _torque_max_force=0;
  52     _torque_no_force=0;
  53     _omega=0;
  54     _omegan=1;
  55     _ddt_omega=0;
  56     _phi=0;
  57     _len=1;
  58     _rotor=NULL;
  59     _twist=0;
  60     _number_of_segments=1;
  61     _rel_len_where_incidence_is_measured=0.7;
  62     _diameter=10;
  63     _torque_of_inertia=0;
  64     _rel_len_blade_start=0;
  65 }
  66
  67 void Rotorpart::inititeration(float dt,float *rot)
  68 {
  69     _dt=dt;
  70     _phi+=_omega*dt;
  71     while (_phi>(2*pi)) _phi-=2*pi;
  72     while (_phi<(0   )) _phi+=2*pi;
  73     float a=Math::dot3(rot,_normal);
  74     if(a>0)
  75         _alphaalt=_alpha*Math::cos(a)
  76         +_nextrp->getrealAlpha()*Math::sin(a);
  77     else
  78         _alphaalt=_alpha*Math::cos(a)
  79         +_lastrp->getrealAlpha()*Math::sin(-a);
  80     //calculate the rotation of the fuselage, determine
  81     //the part in the same direction as alpha
  82     //and add it ro _alphaalt
  83     //alpha is rotation about "normal cross dirofzentf"
  84
  85     float dir[3];
  86     Math::cross3(_directionofzentipetalforce,_normal,dir);
  87     a=Math::dot3(rot,dir);
  88     _alphaalt -= a;
  89     _alphaalt= Math::clamp(_alphaalt,_alphamin,_alphamax);
  90 }
  91
  92 void Rotorpart::setRotor(Rotor *rotor)
  93 {
  94     _rotor=rotor;
  95 }
  96
  97 void Rotorpart::setParameter(char *parametername, float value)
  98 {
  99 #define p(a) if (strcmp(parametername,#a)==0) _##a = value; else
 100
 101     p(twist)
 102         p(number_of_segments)
 103         p(rel_len_where_incidence_is_measured)
 104         p(rel_len_blade_start)
 105         cout << "internal error in parameter set up for rotorpart: '"
 106             << parametername <<"'" << endl;
 107 #undef p
 108 }
 109
 110 void Rotorpart::setTorque(float torque_max_force,float torque_no_force)
 111 {
 112     _torque_max_force=torque_max_force;
 113     _torque_no_force=torque_no_force;
 114 }
 115
 116 void Rotorpart::setTorqueOfInertia(float toi)
 117 {
 118     _torque_of_inertia=toi;
 119 }
 120
 121 void Rotorpart::setWeight(float value)
 122 {
 123     _mass=value;
 124 }
 125
 126 float Rotorpart::getWeight(void)
 127 {
 128     return(_mass/.453); //_mass is in kg, returns pounds
 129 }
 130
 131 void Rotorpart::setPosition(float* p)
 132 {
 133     int i;
 134     for(i=0; i<3; i++) _pos[i] = p[i];
 135 }
 136
 137 float Rotorpart::getIncidence()
 138 {
 139     return(_incidence);
 140 }
 141
 142 void Rotorpart::getPosition(float* out)
 143 {
 144     int i;
 145     for(i=0; i<3; i++) out[i] = _pos[i];
 146 }
 147
 148 void Rotorpart::setPositionForceAttac(float* p)
 149 {
 150     int i;
 151     for(i=0; i<3; i++) _posforceattac[i] = p[i];
 152 }
 153
 154 void Rotorpart::getPositionForceAttac(float* out)
 155 {
 156     int i;
 157     for(i=0; i<3; i++) out[i] = _posforceattac[i];
 158 }
 159
 160 void Rotorpart::setSpeed(float* p)
 161 {
 162     int i;
 163     for(i=0; i<3; i++) _speed[i] = p[i];
 164     Math::unit3(_speed,_direction_of_movement);
 165 }
 166
 167 void Rotorpart::setDirectionofZentipetalforce(float* p)
 168 {
 169     int i;
 170     for(i=0; i<3; i++) _directionofzentipetalforce[i] = p[i];
 171 }
 172
 173 void Rotorpart::setDirectionofRotorPart(float* p)
 174 {
 175     int i;
 176     for(i=0; i<3; i++) _directionofrotorpart[i] = p[i];
 177 }
 178
 179 void Rotorpart::setOmega(float value)
 180 {
 181     _omega=value;
 182 }
 183
 184 void Rotorpart::setOmegaN(float value)
 185 {
 186     _omegan=value;
 187 }
 188
 189 void Rotorpart::setDdtOmega(float value)
 190 {
 191     _ddt_omega=value;
 192 }
 193
 194 void Rotorpart::setZentipetalForce(float f)
 195 {
 196     _zentipetalforce=f;
 197 }
 198
 199 void Rotorpart::setMinpitch(float f)
 200 {
 201     _minpitch=f;
 202 }
 203
 204 void Rotorpart::setMaxpitch(float f)
 205 {
 206     _maxpitch=f;
 207 }
 208
 209 void Rotorpart::setMaxcyclic(float f)
 210 {
 211     _maxcyclic=f;
 212 }
 213
 214 void Rotorpart::setMincyclic(float f)
 215 {
 216     _mincyclic=f;
 217 }
 218
 219 void Rotorpart::setDelta3(float f)
 220 {
 221     _delta3=f;
 222 }
 223
 224 void Rotorpart::setRelamp(float f)
 225 {
 226     _relamp=f;
 227 }
 228
 229 void Rotorpart::setTranslift(float f)
 230 {
 231     _translift=f;
 232 }
 233
 234 void Rotorpart::setDynamic(float f)
 235 {
 236     _dynamic=f;
 237 }
 238
 239 void Rotorpart::setRelLenHinge(float f)
 240 {
 241     _rellenhinge=f;
 242 }
 243
 244 void Rotorpart::setC2(float f)
 245 {
 246     _c2=f;
 247 }
 248
 249 void Rotorpart::setAlpha0(float f)
 250 {
 251     _alpha0=f;
 252 }
 253
 254 void Rotorpart::setAlphamin(float f)
 255 {
 256     _alphamin=f;
 257 }
 258
 259 void Rotorpart::setAlphamax(float f)
 260 {
 261     _alphamax=f;
 262 }
 263
 264 void Rotorpart::setAlpha0factor(float f)
 265 {
 266     _alpha0factor=f;
 267 }
 268
 269 void Rotorpart::setDiameter(float f)
 270 {
 271     _diameter=f;
 272 }
 273
 274 float Rotorpart::getPhi()
 275 {
 276     return(_phi);
 277 }
 278
 279 float Rotorpart::getAlpha(int i)
 280 {
 281     i=i&1;
 282
 283     if (i==0)
 284         return _alpha*180/3.14;//in Grad = 1
 285     else
 286     {
 287         if (_alpha2type==1) //yaw or roll
 288             return (getAlpha(0)-_oppositerp->getAlpha(0))/2;
 289         else //collective
 290             return (getAlpha(0)+_oppositerp->getAlpha(0)+
 291             _nextrp->getAlpha(0)+_lastrp->getAlpha(0))/4;
 292     }
 293 }
 294 float Rotorpart::getrealAlpha(void)
 295 {
 296     return _alpha;
 297 }
 298
 299 void Rotorpart::setAlphaoutput(char *text,int i)
 300 {
 301     SG_LOG(SG_FLIGHT, SG_DEBUG, "setAlphaoutput rotorpart ["
 302         << text << "] typ" << i);
 303
 304     strncpy(_alphaoutputbuf[i>0],text,255);
 305
 306     if (i>0) _alpha2type=i;
 307 }
 308
 309 char* Rotorpart::getAlphaoutput(int i)
 310 {
 311     return _alphaoutputbuf[i&1];
 312 }
 313
 314 void Rotorpart::setLen(float value)
 315 {
 316     _len=value;
 317 }
 318
 319 void Rotorpart::setNormal(float* p)
 320 {
 321     int i;
 322     for(i=0; i<3; i++) _normal[i] = p[i];
 323 }
 324
 325 void Rotorpart::getNormal(float* out)
 326 {
 327     int i;
 328     for(i=0; i<3; i++) out[i] = _normal[i];
 329 }
 330
 331 void Rotorpart::setCollective(float pos)
 332 {
 333     _collective = pos;
 334 }
 335
 336 void Rotorpart::setCyclic(float pos)
 337 {
 338     _cyclic = pos;
 339 }
 340
 341 void Rotorpart::setlastnextrp(Rotorpart*lastrp,Rotorpart*nextrp,
 342     Rotorpart *oppositerp)
 343 {
 344     _lastrp=lastrp;
 345     _nextrp=nextrp;
 346     _oppositerp=oppositerp;
 347 }
 348
 349 void Rotorpart::strncpy(char *dest,const char *src,int maxlen)
 350 {
 351     int n=0;
 352     while(src[n]&&n<(maxlen-1))
 353     {
 354         dest[n]=src[n];
 355         n++;
 356     }
 357     dest[n]=0;
 358 }
 359
 360 // Calculate the flapping angle, where zentripetal force and
 361 //lift compensate each other
 362 float Rotorpart::calculateAlpha(float* v_rel_air, float rho,
 363     float incidence, float cyc, float alphaalt, float *torque,
 364     float *returnlift)
 365 {
 366     float moment[3],v_local[3],v_local_scalar,lift_moment,v_flap[3],v_help[3];
 367     float ias;//nur f. dgb
 368     int i,n;
 369     for (i=0;i<3;i++)
 370         moment[i]=0;
 371     lift_moment=0;
 372     *torque=0;//
 373     if((_nextrp==NULL)||(_lastrp==NULL)||(_rotor==NULL))
 374         return 0.0;//not initialized. Can happen during startupt of flightgear
 375     if (returnlift!=NULL) *returnlift=0;
 376     float flap_omega=(_nextrp->getrealAlpha()-_lastrp->getrealAlpha())
 377         *_omega / pi;
 378     float local_width=_diameter*(1-_rel_len_blade_start)/2.
 379         /(float (_number_of_segments));
 380     for (n=0;n<_number_of_segments;n++)
 381     {
 382         float rel = (n+.5)/(float (_number_of_segments));
 383         float r= _diameter *0.5 *(rel*(1-_rel_len_blade_start)
 384             +_rel_len_blade_start);
 385         float local_incidence=incidence+_twist *rel - _twist
 386             *_rel_len_where_incidence_is_measured;
 387         float local_chord = _rotor->getChord()*rel+_rotor->getChord()
 388             *_rotor->getTaper()*(1-rel);
 389         float A = local_chord * local_width;
 390         //calculate the local air speed and the incidence to this speed;
 391         Math::mul3(_omega * r , _direction_of_movement , v_local);
 392
 393         // add speed component due to flapping
 394         Math::mul3(flap_omega * r,_normal,v_flap);
 395         Math::add3(v_flap,v_local,v_local);
 396         Math::sub3(v_rel_air,v_local,v_local);
 397         //v_local is now the total airspeed at the blade
 398         //apparent missing: calculating the local_wind = v_rel_air at
 399         //every point of the rotor. It differs due to aircraft-rotation
 400         //it is considered in v_flap
 401
 402         //substract now the component of the air speed parallel to
 403         //the blade;
 404        Math::mul3(Math::dot3(v_local,_directionofrotorpart),
 405            _directionofrotorpart,v_help);
 406         Math::sub3(v_local,v_help,v_local);
 407
 408         //split into direction and magnitude
 409         v_local_scalar=Math::mag3(v_local);
 410         if (v_local_scalar!=0)
 411             Math::unit3(v_local,v_local);
 412         float incidence_of_airspeed = Math::asin(Math::clamp(
 413             Math::dot3(v_local,_normal),-1,1)) + local_incidence;
 414         ias = incidence_of_airspeed;
 415         float lift_wo_cyc =
 416             _rotor->getLiftCoef(incidence_of_airspeed-cyc,v_local_scalar)
 417             * v_local_scalar * v_local_scalar * A *rho *0.5;
 418         float lift_with_cyc =
 419             _rotor->getLiftCoef(incidence_of_airspeed,v_local_scalar)
 420             * v_local_scalar * v_local_scalar *A *rho*0.5;
 421         float lift=lift_wo_cyc+_relamp*(lift_with_cyc-lift_wo_cyc);
 422         //take into account that the rotor is a resonant system where
 423         //the cyclic input hase increased result
 424         float drag = -_rotor->getDragCoef(incidence_of_airspeed,v_local_scalar)
 425             * v_local_scalar * v_local_scalar * A *rho*0.5;
 426         float angle = incidence_of_airspeed - incidence;
 427         //angle between blade movement caused by rotor-rotation and the
 428         //total movement of the blade
 429
 430         lift_moment += r*(lift * Math::cos(angle)
 431             - drag * Math::sin(angle));
 432         *torque     += r*(drag * Math::cos(angle)
 433             + lift * Math::sin(angle));
 434
 435         if (returnlift!=NULL) *returnlift+=lift;
 436     }
 437     float alpha=Math::atan2(lift_moment,_zentipetalforce * _len);
 438
 439     return (alpha);
 440 }
 441
 442 // Calculate the aerodynamic force given a wind vector v (in the
 443 // aircraft's "local" coordinates) and an air density rho.  Returns a
 444 // torque about the Y axis, too.
 445 void Rotorpart::calcForce(float* v, float rho,  float* out, float* torque,
 446     float* torque_scalar)
 447 {
 448     if (_compiled!=1)
 449     {
 450         for (int i=0;i<3;i++)
 451             torque[i]=out[i]=0;
 452         *torque_scalar=0;
 453         return;
 454     }
 455     _zentipetalforce=_mass*_len*_omega*_omega;
 456     float vrel[3],vreldir[3];
 457     Math::sub3(_speed,v,vrel);
 458     float scalar_torque=0,alpha_alteberechnung=0;
 459     Math::unit3(vrel,vreldir);//direction of blade-movement rel. to air
 460     float delta=Math::asin(Math::dot3(_normal,vreldir));
 461     //Angle of blade which would produce no vertical force (where the
 462     //effective incidence is zero)
 463
 464     float cyc=_mincyclic+(_cyclic+1)/2*(_maxcyclic-_mincyclic);
 465     float col=_minpitch+(_collective+1)/2*(_maxpitch-_minpitch);
 466     _incidence=(col+cyc)-_delta3*_alphaalt;
 467     //the incidence of the rotorblade due to control input reduced by the
 468     //delta3 effect, see README.YASIM
 469     float beta=_relamp*cyc+col;
 470     //the incidence of the rotorblade which is used for the calculation
 471
 472     float alpha,factor; //alpha is the flapping angle
 473     //the new flapping angle will be the old flapping angle
 474     //+ factor *(alpha - "old flapping angle")
 475     if((_omega*10)>_omegan)
 476     //the rotor is rotaing quite fast.
 477     //(at least 10% of the nominal rotational speed)
 478     {
 479         alpha=calculateAlpha(v,rho,_incidence,cyc,0,&scalar_torque);
 480         //the incidence is a function of alpha (if _delta* != 0)
 481         //Therefore missing: wrap this function in an integrator
 482         //(runge kutta e. g.)
 483
 484         factor=_dt*_dynamic;
 485         if (factor>1) factor=1;
 486     }
 487     else //the rotor is not rotating or rotating very slowly
 488     {
 489         alpha=calculateAlpha(v,rho,_incidence,cyc,alpha_alteberechnung,
 490             &scalar_torque);
 491         //calculate drag etc., e. g. for deccelrating the rotor if engine
 492         //is off and omega <10%
 493
 494         float rel =_omega*10 / _omegan;
 495         alpha=rel * alpha + (1-rel)* _alpha0;
 496         factor=_dt*_dynamic/10;
 497         if (factor>1) factor=1;
 498     }
 499
 500     float vz=Math::dot3(_normal,v); //the s
 501     float dirblade[3];
 502     Math::cross3(_normal,_directionofzentipetalforce,dirblade);
 503     float vblade=Math::abs(Math::dot3(dirblade,v));
 504     float tliftfactor=Math::sqrt(1+vblade*_translift);
 505
 506     alpha=_alphaalt+(alpha-_alphaalt)*factor;
 507     _alpha=alpha;
 508     float meancosalpha=(1*Math::cos(_lastrp->getrealAlpha())
 509         +1*Math::cos(_nextrp->getrealAlpha())
 510         +1*Math::cos(_oppositerp->getrealAlpha())
 511         +1*Math::cos(alpha))/4;
 512     float schwenkfactor=1-(Math::cos(_lastrp->getrealAlpha())-meancosalpha);
 513
 514     //missing: consideration of rellenhinge
 515     float xforce = Math::cos(alpha)*_zentipetalforce;
 516     float zforce = schwenkfactor*Math::sin(alpha)*_zentipetalforce;
 517     *torque_scalar=scalar_torque;
 518     scalar_torque+= 0*_ddt_omega*_torque_of_inertia;
 519     float thetorque = scalar_torque;
 520     int i;
 521     float f=_rotor->getCcw()?1:-1;
 522     for(i=0; i<3; i++) {
 523         _last_torque[i]=torque[i] = f*_normal[i]*thetorque;
 524         out[i] = _normal[i]*zforce*_rotor->getLiftFactor()
 525             +_directionofzentipetalforce[i]*xforce;
 526     }
 527 }
 528
 529 void Rotorpart::getAccelTorque(float relaccel,float *t)
 530 {
 531     int i;
 532     float f=_rotor->getCcw()?1:-1;
 533     for(i=0; i<3; i++) {
 534         t[i]=f*-1* _normal[i]*relaccel*_omegan* _torque_of_inertia;
 535         _rotor->addTorque(-relaccel*_omegan* _torque_of_inertia);
 536     }
 537 }
 538 }; // namespace yasim