src/FDM/YASim/Rotorpart.cpp

   1 #include <simgear/debug/logstream.hxx>
   2
   3 #include "Math.hpp"
   4 #include "Rotorpart.hpp"
   5 #include <stdio.h>
   6 //#include <string.h>
   7 //#include <Main/fg_props.hxx>
   8 namespace yasim {
   9 const float pi=3.14159;
  10
  11 Rotorpart::Rotorpart()
  12 {
  13     _cyclic=0;
  14     _collective=0;
  15     _rellenhinge=0;
  16     _dt=0;
  17     #define set3(x,a,b,c) x[0]=a;x[1]=b;x[2]=c;
  18     set3 (_speed,1,0,0);
  19     set3 (_directionofzentipetalforce,1,0,0);
  20     #undef set3
  21     _zentipetalforce=1;
  22     _maxpitch=.02;
  23     _minpitch=0;
  24     _maxpitchforce=10;
  25     _maxcyclic=0.02;
  26     _mincyclic=-0.02;
  27     _delta3=0.5;
  28     _cyclic=0;
  29     _collective=0;
  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     _translift=0;
  46     _dynamic=100;
  47     _c2=0;
  48     _torque_max_force=0;
  49     _torque_no_force=0;
  50     _omega=0;
  51     _omegan=1;
  52     _phi=0;
  53     _len=1;
  54
  55
  56
  57
  58 }
  59
  60
  61 void Rotorpart::inititeration(float dt,float *rot)
  62 {
  63      //printf("init %5.3f",dt*1000);
  64      _dt=dt;
  65      _phi+=_omega*dt;
  66      while (_phi>(2*pi)) _phi-=2*pi;
  67      while (_phi<(0   )) _phi+=2*pi;
  68
  69      //_alphaalt=_alpha;
  70      //a=skalarprdukt _normal mit rot ergibt drehung um Normale
  71      //alphaalt=Math::cos(a)*alpha+.5*(Math::sin(a)*alphanachbarnlast-Math::sin(a)*alphanachbanext)
  72      float a=Math::dot3(rot,_normal);
  73      if(a>0)
  74         _alphaalt=_alpha*Math::cos(a)
  75                   +_nextrp->getrealAlpha()*Math::sin(a);
  76       else
  77         _alphaalt=_alpha*Math::cos(a)
  78                   +_lastrp->getrealAlpha()*Math::sin(-a);
  79      //jetzt noch Drehung des Rumpfes in gleiche Richtung wie alpha bestimmen
  80      //und zu _alphaalt hinzuf\81gen
  81      //alpha gibt drehung um normal cross dirofzentf an
  82
  83      float dir[3];
  84      Math::cross3(_directionofzentipetalforce,_normal,dir);
  85
  86
  87
  88      a=Math::dot3(rot,dir);
  89      _alphaalt -= a;
  90
  91      _alphaalt= Math::clamp(_alphaalt,_alphamin,_alphamax);
  92
  93
  94 }
  95 void Rotorpart::setTorque(float torque_max_force,float torque_no_force)
  96 {
  97     _torque_max_force=torque_max_force;
  98     _torque_no_force=torque_no_force;
  99 }
 100
 101
 102
 103 void Rotorpart::setWeight(float value)
 104 {
 105      _mass=value;
 106 }
 107 float Rotorpart::getWeight(void)
 108 {
 109      return(_mass/.453); //_mass is in kg, returns pounds
 110 }
 111
 112 void Rotorpart::setPosition(float* p)
 113 {
 114     int i;
 115     for(i=0; i<3; i++) _pos[i] = p[i];
 116 }
 117 float Rotorpart::getIncidence()
 118 {
 119     return(_incidence);
 120 }
 121
 122 void Rotorpart::getPosition(float* out)
 123 {
 124     int i;
 125     for(i=0; i<3; i++) out[i] = _pos[i];
 126 }
 127
 128 void Rotorpart::setPositionForceAttac(float* p)
 129 {
 130     int i;
 131     for(i=0; i<3; i++) _posforceattac[i] = p[i];
 132 }
 133
 134 void Rotorpart::getPositionForceAttac(float* out)
 135 {
 136     int i;
 137     for(i=0; i<3; i++) out[i] = _posforceattac[i];
 138     //printf("posforce: %5.3f %5.3f %5.3f ",out[0],out[1],out[2]);
 139 }
 140 void Rotorpart::setSpeed(float* p)
 141 {
 142     int i;
 143     for(i=0; i<3; i++) _speed[i] = p[i];
 144 }
 145 void Rotorpart::setDirectionofZentipetalforce(float* p)
 146 {
 147     int i;
 148     for(i=0; i<3; i++) _directionofzentipetalforce[i] = p[i];
 149 }
 150
 151 void Rotorpart::setOmega(float value)
 152 {
 153   _omega=value;
 154 }
 155 void Rotorpart::setOmegaN(float value)
 156 {
 157   _omegan=value;
 158 }
 159
 160
 161 void Rotorpart::setZentipetalForce(float f)
 162 {
 163     _zentipetalforce=f;
 164 }
 165 void Rotorpart::setMinpitch(float f)
 166 {
 167     _minpitch=f;
 168 }
 169 void Rotorpart::setMaxpitch(float f)
 170 {
 171     _maxpitch=f;
 172 }
 173 void Rotorpart::setMaxPitchForce(float f)
 174 {
 175     _maxpitchforce=f;
 176 }
 177 void Rotorpart::setMaxcyclic(float f)
 178 {
 179     _maxcyclic=f;
 180 }
 181 void Rotorpart::setMincyclic(float f)
 182 {
 183     _mincyclic=f;
 184 }
 185 void Rotorpart::setDelta3(float f)
 186 {
 187     _delta3=f;
 188 }
 189 void Rotorpart::setRelamp(float f)
 190 {
 191     _relamp=f;
 192 }
 193 void Rotorpart::setTranslift(float f)
 194 {
 195     _translift=f;
 196 }
 197 void Rotorpart::setDynamic(float f)
 198 {
 199     _dynamic=f;
 200 }
 201 void Rotorpart::setRelLenHinge(float f)
 202 {
 203     _rellenhinge=f;
 204 }
 205 void Rotorpart::setC2(float f)
 206 {
 207     _c2=f;
 208 }
 209 void Rotorpart::setAlpha0(float f)
 210 {
 211     _alpha0=f;
 212 }
 213 void Rotorpart::setAlphamin(float f)
 214 {
 215     _alphamin=f;
 216 }
 217 void Rotorpart::setAlphamax(float f)
 218 {
 219     _alphamax=f;
 220 }
 221 void Rotorpart::setAlpha0factor(float f)
 222 {
 223     _alpha0factor=f;
 224 }
 225
 226
 227 float Rotorpart::getPhi()
 228 {
 229   return(_phi);
 230 }
 231
 232 float Rotorpart::getAlpha(int i)
 233 {
 234   i=i&1;
 235   if (i==0)
 236     return _alpha*180/3.14;//in Grad = 1
 237   else
 238     if (_alpha2type==1) //yaw or roll
 239       return (getAlpha(0)-_oppositerp->getAlpha(0))/2;
 240      else //pitch
 241       return (getAlpha(0)+_oppositerp->getAlpha(0)+
 242               _nextrp->getAlpha(0)+_lastrp->getAlpha(0))/4;
 243
 244 }
 245 float Rotorpart::getrealAlpha(void)
 246 {
 247     return _alpha;
 248 }
 249 void Rotorpart::setAlphaoutput(char *text,int i)
 250 {
 251    SG_LOG(SG_FLIGHT, SG_DEBUG, "setAlphaoutput rotorpart ["
 252           << text << "] typ" << i);
 253
 254    strncpy(_alphaoutputbuf[i>0],text,255);
 255
 256    if (i>0) _alpha2type=i;
 257
 258
 259 }
 260 char* Rotorpart::getAlphaoutput(int i)
 261 {
 262     return _alphaoutputbuf[i&1];
 263 }
 264
 265 void Rotorpart::setLen(float value)
 266 {
 267   _len=value;
 268 }
 269
 270
 271 void Rotorpart::setNormal(float* p)
 272 {
 273     int i;
 274     for(i=0; i<3; i++) _normal[i] = p[i];
 275 }
 276
 277 void Rotorpart::getNormal(float* out)
 278 {
 279     int i;
 280     for(i=0; i<3; i++) out[i] = _normal[i];
 281 }
 282
 283
 284 void Rotorpart::setCollective(float pos)
 285 {
 286     _collective = pos;
 287 }
 288
 289 void Rotorpart::setCyclic(float pos)
 290 {
 291     _cyclic = pos;
 292 }
 293
 294 void Rotorpart::setlastnextrp(Rotorpart*lastrp,Rotorpart*nextrp,Rotorpart *oppositerp)
 295 {
 296   _lastrp=lastrp;
 297   _nextrp=nextrp;
 298   _oppositerp=oppositerp;
 299 }
 300
 301 void Rotorpart::strncpy(char *dest,const char *src,int maxlen)
 302 {
 303   int n=0;
 304   while(src[n]&&n<(maxlen-1))
 305   {
 306     dest[n]=src[n];
 307     n++;
 308   }
 309   dest[n]=0;
 310 }
 311
 312 // Calculate the aerodynamic force given a wind vector v (in the
 313 // aircraft's "local" coordinates) and an air density rho.  Returns a
 314 // torque about the Y axis, too.
 315 void Rotorpart::calcForce(float* v, float rho, float* out, float* torque)
 316 {
 317     {
 318         _zentipetalforce=_mass*_len*_omega*_omega;
 319         float vrel[3],vreldir[3];
 320         Math::sub3(_speed,v,vrel);
 321         Math::unit3(vrel,vreldir);//direction of blade-movement rel. to air
 322         float delta=Math::asin(Math::dot3(_normal,vreldir));//Angle of blade which would produce no vertical force
 323
 324         float cyc=_mincyclic+(_cyclic+1)/2*(_maxcyclic-_mincyclic);
 325         float col=_minpitch+(_collective+1)/2*(_maxpitch-_minpitch);
 326         //printf("[%5.3f]",col);
 327         _incidence=(col+cyc)-_delta3*_alphaalt;
 328         cyc*=_relamp;
 329         float beta=cyc+col;
 330
 331         //float c=_maxpitchforce/(_maxpitch*_zentipetalforce);
 332         float c,alpha,factor;
 333         if((_omega*10)>_omegan)
 334         {
 335           c=_maxpitchforce/_omegan/(_maxpitch*_mass*_len*_omega);
 336           alpha = c*(beta-delta)/(1+_delta3*c);
 337
 338           factor=_dt*_dynamic;
 339           if (factor>1) factor=1;
 340         }
 341         else
 342         {
 343           alpha=_alpha0;
 344           factor=_dt*_dynamic/10;
 345           if (factor>1) factor=1;
 346         }
 347
 348         float vz=Math::dot3(_normal,v);
 349         //alpha+=_c2*vz;
 350
 351         float fcw;
 352         if(_c2==0)
 353           fcw=0;
 354         else
 355           //fcw=vz/_c2*_maxpitchforce*_omega/_omegan;
 356           fcw=vz*(_c2-1)*_maxpitchforce*_omega/(_omegan*_omegan*_len*_maxpitch);
 357
 358         float dirblade[3];
 359         Math::cross3(_normal,_directionofzentipetalforce,dirblade);
 360         float vblade=Math::abs(Math::dot3(dirblade,v));
 361         float tliftfactor=Math::sqrt(1+vblade*_translift);
 362
 363
 364         alpha=_alphaalt+(alpha-_alphaalt)*factor;
 365         //alpha=_alphaalt+(alpha-_lastrp->getrealAlpha())*factor;
 366
 367
 368         _alpha=alpha;
 369
 370
 371         //float schwenkfactor=1;//  1/Math::cos(_lastrp->getrealAlpha());
 372
 373         float meancosalpha=(1*Math::cos(_lastrp->getrealAlpha())
 374                         +1*Math::cos(_nextrp->getrealAlpha())
 375                         +1*Math::cos(_oppositerp->getrealAlpha())
 376                         +1*Math::cos(alpha))/4;
 377         float schwenkfactor=1-(Math::cos(_lastrp->getrealAlpha())-meancosalpha);
 378
 379
 380         //fehlt: verringerung um rellenhinge
 381         float xforce = /*schwenkfactor*/ Math::cos(alpha)*_zentipetalforce;// /9.81*.453; //N->poundsofforce
 382         float zforce = fcw+tliftfactor*schwenkfactor*Math::sin(alpha)*_zentipetalforce;// /9.81*.453;
 383
 384         float thetorque = _torque_no_force+_torque_max_force*Math::abs(zforce/_maxpitchforce);
 385         /*
 386         printf("speed: %5.3f %5.3f %5.3f vwind: %5.3f %5.3f %5.3f sin %5.3f\n",
 387           _speed[0],_speed[1],_speed[2],
 388           v[0],v[1],v[2],Math::sin(alpha));
 389         */
 390
 391         int i;
 392         for(i=0; i<3; i++) {
 393           torque[i] = _normal[i]*thetorque;
 394           out[i] = _normal[i]*zforce+_directionofzentipetalforce[i]*xforce;
 395         }
 396         //printf("alpha: %5.3f force: %5.3f %5.3f %5.3f %5.3f %5.3f\n",alpha*180/3.14,xforce,zforce,out[0],out[1],out[2]);
 397         return;
 398     }
 399 }
 400
 401
 402 }; // namespace yasim