src/FDM/YASim/Rotorpart.cpp

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