3 #include <simgear/debug/logstream.hxx>
6 #include "Rotorpart.hpp"
13 const float pi=3.14159;
15 Rotorpart::Rotorpart()
21 _shared_flap_hinge=false;
23 #define set3(x,a,b,c) x[0]=a;x[1]=b;x[2]=c;
25 set3 (_directionofcentripetalforce,1,0,0);
26 set3 (_directionofrotorpart,0,1,0);
27 set3 (_direction_of_movement,1,0,0);
28 set3 (_last_torque,0,0,0);
42 _alphaoutputbuf[0][0]=0;
43 _alphaoutputbuf[1][0]=0;
63 _number_of_segments=1;
64 _rel_len_where_incidence_is_measured=0.7;
67 _rel_len_blade_start=0;
69 _rotor_correction_factor=0.6;
74 void Rotorpart::inititeration(float dt,float *rot)
78 while (_phi>(2*pi)) _phi-=2*pi;
79 while (_phi<(0 )) _phi+=2*pi;
80 float a=Math::dot3(rot,_normal);
82 _alphaalt=_alpha*Math::cos(a)
83 +_next90rp->getrealAlpha()*Math::sin(a);
85 _alphaalt=_alpha*Math::cos(a)
86 +_last90rp->getrealAlpha()*Math::sin(-a);
87 //calculate the rotation of the fuselage, determine
88 //the part in the same direction as alpha
89 //and add it ro _alphaalt
90 //alpha is rotation about "normal cross dirofzentf"
93 Math::cross3(_directionofcentripetalforce,_normal,dir);
94 a=Math::dot3(rot,dir);
96 _alphaalt= Math::clamp(_alphaalt,_alphamin,_alphamax);
100 b=_rotor->getBalance();
101 float s =Math::sin(_phi+_direction);
102 //float c =Math::cos(_phi+_direction);
104 _balance=(b>0)?(1.-s*(1.-b)):(1.-s)*(1.+b);
106 _balance=(b>0)?1.:1.+b;
109 void Rotorpart::setRotor(Rotor *rotor)
114 void Rotorpart::setParameter(const char *parametername, float value)
116 #define p(a) if (strcmp(parametername,#a)==0) _##a = value; else
119 p(number_of_segments)
120 p(rel_len_where_incidence_is_measured)
121 p(rel_len_blade_start)
122 p(rotor_correction_factor)
123 SG_LOG(SG_INPUT, SG_ALERT,
124 "internal error in parameter set up for rotorpart: '"
125 << parametername <<"'" << endl);
129 void Rotorpart::setTorque(float torque_max_force,float torque_no_force)
131 _torque_max_force=torque_max_force;
132 _torque_no_force=torque_no_force;
135 void Rotorpart::setTorqueOfInertia(float toi)
137 _torque_of_inertia=toi;
140 void Rotorpart::setWeight(float value)
145 float Rotorpart::getWeight(void)
147 return(_mass/.453); //_mass is in kg, returns pounds
150 void Rotorpart::setPosition(float* p)
152 for(int i=0; i<3; i++) _pos[i] = p[i];
155 float Rotorpart::getIncidence()
160 void Rotorpart::getPosition(float* out)
162 for(int i=0; i<3; i++) out[i] = _pos[i];
165 void Rotorpart::setPositionForceAttac(float* p)
167 for(int i=0; i<3; i++) _posforceattac[i] = p[i];
170 void Rotorpart::getPositionForceAttac(float* out)
172 for(int i=0; i<3; i++) out[i] = _posforceattac[i];
175 void Rotorpart::setSpeed(float* p)
177 for(int i=0; i<3; i++) _speed[i] = p[i];
178 Math::unit3(_speed,_direction_of_movement);
181 void Rotorpart::setDirectionofZentipetalforce(float* p)
183 for(int i=0; i<3; i++) _directionofcentripetalforce[i] = p[i];
186 void Rotorpart::setDirectionofRotorPart(float* p)
188 for(int i=0; i<3; i++) _directionofrotorpart[i] = p[i];
191 void Rotorpart::setDirection(float direction)
193 _direction=direction;
196 void Rotorpart::setOmega(float value)
201 void Rotorpart::setPhi(float value)
206 void Rotorpart::setOmegaN(float value)
211 void Rotorpart::setDdtOmega(float value)
216 void Rotorpart::setZentipetalForce(float f)
222 void Rotorpart::setDelta3(float f)
227 void Rotorpart::setRelamp(float f)
232 void Rotorpart::setTranslift(float f)
237 void Rotorpart::setDynamic(float f)
242 void Rotorpart::setRelLenHinge(float f)
247 void Rotorpart::setSharedFlapHinge(bool s)
249 _shared_flap_hinge=s;
252 void Rotorpart::setC2(float f)
257 void Rotorpart::setAlpha0(float f)
259 if (f>-0.01) f=-0.01; //half a degree bending
263 void Rotorpart::setAlphamin(float f)
268 void Rotorpart::setAlphamax(float f)
273 void Rotorpart::setAlpha0factor(float f)
278 void Rotorpart::setDiameter(float f)
283 float Rotorpart::getPhi()
288 float Rotorpart::getAlpha(int i)
293 return _alpha*180/pi;//in Grad = 1
296 if (_alpha2type==1) //yaw or roll
297 return (getAlpha(0)-_oppositerp->getAlpha(0))/2;
299 return (getAlpha(0)+_oppositerp->getAlpha(0)+
300 _next90rp->getAlpha(0)+_last90rp->getAlpha(0))/4;
303 float Rotorpart::getrealAlpha(void)
308 void Rotorpart::setAlphaoutput(char *text,int i)
310 strncpy(_alphaoutputbuf[i>0],text,255);
311 if (i>0) _alpha2type=i;
314 char* Rotorpart::getAlphaoutput(int i)
316 return _alphaoutputbuf[i&1];
319 void Rotorpart::setLen(float value)
324 void Rotorpart::setNormal(float* p)
326 for(int i=0; i<3; i++) _normal[i] = p[i];
329 void Rotorpart::getNormal(float* out)
331 for(int i=0; i<3; i++) out[i] = _normal[i];
334 void Rotorpart::setCollective(float pos)
339 void Rotorpart::setCyclic(float pos)
344 void Rotorpart::setlastnextrp(Rotorpart*lastrp,Rotorpart*nextrp,
345 Rotorpart *oppositerp,Rotorpart*last90rp,Rotorpart*next90rp)
349 _oppositerp=oppositerp;
354 void Rotorpart::strncpy(char *dest,const char *src,int maxlen)
357 while(src[n]&&n<(maxlen-1))
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,
371 float v_local[3],v_local_scalar,lift_moment,v_flap[3],v_help[3];
372 float relgrav = Math::dot3(_normal,_rotor->getGravDirection());
373 lift_moment=-_mass*_len*9.81*relgrav;
375 if((_nextrp==NULL)||(_lastrp==NULL)||(_rotor==NULL))
376 return 0.0;//not initialized. Can happen during startup of flightgear
377 if (returnlift!=NULL) *returnlift=0;
378 float flap_omega=(_next90rp->getrealAlpha()-_last90rp->getrealAlpha())
380 float local_width=_diameter*(1-_rel_len_blade_start)/2.
381 /(float (_number_of_segments));
382 for (int n=0;n<_number_of_segments;n++)
384 float rel = (n+.5)/(float (_number_of_segments));
385 float r= _diameter *0.5 *(rel*(1-_rel_len_blade_start)
386 +_rel_len_blade_start);
387 float local_incidence=incidence+_twist *rel -
388 _twist *_rel_len_where_incidence_is_measured;
389 float local_chord = _rotor->getChord()*rel+_rotor->getChord()
390 *_rotor->getTaper()*(1-rel);
391 float A = local_chord * local_width;
392 //calculate the local air speed and the incidence to this speed;
393 Math::mul3(_omega * r , _direction_of_movement , v_local);
395 // add speed component due to flapping
396 Math::mul3(flap_omega * r,_normal,v_flap);
397 Math::add3(v_flap,v_local,v_local);
398 Math::sub3(v_rel_air,v_local,v_local);
399 //v_local is now the total airspeed at the blade
400 //apparent missing: calculating the local_wind = v_rel_air at
401 //every point of the rotor. It differs due to aircraft-rotation
402 //it is considered in v_flap
404 //substract now the component of the air speed parallel to
406 Math::mul3(Math::dot3(v_local,_directionofrotorpart),
407 _directionofrotorpart,v_help);
408 Math::sub3(v_local,v_help,v_local);
410 //split into direction and magnitude
411 v_local_scalar=Math::mag3(v_local);
412 if (v_local_scalar!=0)
413 //Math::unit3(v_local,v_help);
414 Math::mul3(1/v_local_scalar,v_local,v_help);
415 float incidence_of_airspeed = Math::asin(Math::clamp(
416 Math::dot3(v_help,_normal),-1,1)) + local_incidence;
417 //ias = incidence_of_airspeed;
419 //reduce the ias (Prantl factor)
420 float prantl_factor=2/pi*Math::acos(Math::exp(
421 -_rotor->getNumberOfBlades()/2.*(1-rel)
422 *Math::sqrt(1+1/Math::sqr(Math::tan(
423 pi/2-Math::abs(incidence_of_airspeed-local_incidence))))));
424 incidence_of_airspeed = (incidence_of_airspeed+
425 _rotor->getAirfoilIncidenceNoLift())*prantl_factor
426 *_rotor_correction_factor-_rotor->getAirfoilIncidenceNoLift();
427 //ias = incidence_of_airspeed;
428 float lift_wo_cyc = _rotor->getLiftCoef(incidence_of_airspeed
429 -cyc*_rotor_correction_factor*prantl_factor,v_local_scalar)
430 * v_local_scalar * v_local_scalar * A *rho *0.5;
431 float lift_with_cyc =
432 _rotor->getLiftCoef(incidence_of_airspeed,v_local_scalar)
433 * v_local_scalar * v_local_scalar *A *rho*0.5;
434 float lift=lift_wo_cyc+_relamp*(lift_with_cyc-lift_wo_cyc);
435 //take into account that the rotor is a resonant system where
436 //the cyclic input hase increased result
437 float drag = -_rotor->getDragCoef(incidence_of_airspeed,v_local_scalar)
438 * v_local_scalar * v_local_scalar * A *rho*0.5;
439 float angle = incidence_of_airspeed - incidence;
440 //angle between blade movement caused by rotor-rotation and the
441 //total movement of the blade
443 lift_moment += r*(lift * Math::cos(angle)
444 - drag * Math::sin(angle));
445 *torque += r*(drag * Math::cos(angle)
446 + lift * Math::sin(angle));
447 if (returnlift!=NULL) *returnlift+=lift;
449 //use 1st order approximation for alpha
450 //float alpha=Math::atan2(lift_moment,_centripetalforce * _len);
452 if (_shared_flap_hinge)
455 if (Math::abs(_alphaalt) >1e-6)
456 div=(_centripetalforce * _len - _mass * _len * 9.81 * relgrav /_alpha0*(_alphaalt+_oppositerp->getAlphaAlt())/(2.0*_alphaalt));
457 if (Math::abs(div)>1e-6)
459 alpha=lift_moment/div;
461 else if(Math::abs(_alphaalt+_oppositerp->getAlphaAlt())>1e-6)
463 float div=(_centripetalforce * _len - _mass * _len * 9.81 *0.5 * relgrav)*(_alphaalt+_oppositerp->getAlphaAlt());
464 if (Math::abs(div)>1e-6)
466 alpha=_oppositerp->getAlphaAlt()+lift_moment/div*_alphaalt;
473 if (_omega/_omegan<0.2)
475 float frac = 0.001+_omega/_omegan*4.995;
476 alpha=Math::clamp(alpha,_alphamin,_alphamax);
477 alpha=_alphaalt*(1-frac)+frac*alpha;
482 float div=(_centripetalforce * _len - _mass * _len * 9.81 /_alpha0);
483 if (Math::abs(div)>1e-6)
484 alpha=lift_moment/div;
492 // Calculate the aerodynamic force given a wind vector v (in the
493 // aircraft's "local" coordinates) and an air density rho. Returns a
494 // torque about the Y axis, too.
495 void Rotorpart::calcForce(float* v, float rho, float* out, float* torque,
496 float* torque_scalar)
500 for (int i=0;i<3;i++)
505 _centripetalforce=_mass*_len*_omega*_omega;
506 float vrel[3],vreldir[3];
507 Math::sub3(_speed,v,vrel);
508 float scalar_torque=0;
509 Math::unit3(vrel,vreldir);//direction of blade-movement rel. to air
510 //Angle of blade which would produce no vertical force (where the
511 //effective incidence is zero)
514 float col=_collective;
515 if (_shared_flap_hinge)
516 _incidence=(col+cyc)-_delta3*0.5*(_alphaalt-_oppositerp->getAlphaAlt());
518 _incidence=(col+cyc)-_delta3*_alphaalt;
519 //the incidence of the rotorblade due to control input reduced by the
520 //delta3 effect, see README.YASIM
521 //float beta=_relamp*cyc+col;
522 //the incidence of the rotorblade which is used for the calculation
524 float alpha,factor; //alpha is the flapping angle
525 //the new flapping angle will be the old flapping angle
526 //+ factor *(alpha - "old flapping angle")
527 alpha=calculateAlpha(v,rho,_incidence,cyc,0,&scalar_torque);
528 alpha=Math::clamp(alpha,_alphamin,_alphamax);
529 //the incidence is a function of alpha (if _delta* != 0)
530 //Therefore missing: wrap this function in an integrator
531 //(runge kutta e. g.)
534 if (factor>1) factor=1;
537 Math::cross3(_normal,_directionofcentripetalforce,dirblade);
538 //float vblade=Math::abs(Math::dot3(dirblade,v));
540 alpha=_alphaalt+(alpha-_alphaalt)*factor;
542 float meancosalpha=(1*Math::cos(_last90rp->getrealAlpha())
543 +1*Math::cos(_next90rp->getrealAlpha())
544 +1*Math::cos(_oppositerp->getrealAlpha())
545 +1*Math::cos(alpha))/4;
546 float schwenkfactor=1-(Math::cos(_lastrp->getrealAlpha())-meancosalpha)*_rotor->getNumberOfParts()/4;
548 //missing: consideration of rellenhinge
551 _centripetalforce*=_balance;
552 scalar_torque*=_balance;
554 float xforce = Math::cos(alpha)*_centripetalforce;
555 float zforce = schwenkfactor*Math::sin(alpha)*_centripetalforce;
556 *torque_scalar=scalar_torque;
557 scalar_torque+= 0*_ddt_omega*_torque_of_inertia;
558 float thetorque = scalar_torque;
559 float f=_rotor->getCcw()?1:-1;
560 for(int i=0; i<3; i++) {
561 _last_torque[i]=torque[i] = f*_normal[i]*thetorque;
562 out[i] = _normal[i]*zforce*_rotor->getLiftFactor()
563 +_directionofcentripetalforce[i]*xforce;
567 void Rotorpart::getAccelTorque(float relaccel,float *t)
569 float f=_rotor->getCcw()?1:-1;
570 for(int i=0; i<3; i++) {
571 t[i]=f*-1* _normal[i]*relaccel*_omegan* _torque_of_inertia;// *_omeagan ?
572 _rotor->addTorque(-relaccel*_omegan* _torque_of_inertia);
576 std::ostream & operator<<(std::ostream & out, const Rotorpart& rp)
578 #define i(x) << #x << ":" << rp.x << endl
579 #define iv(x) << #x << ":" << rp.x[0] << ";" << rp.x[1] << ";" <<rp.x[2] << ";" << endl
580 out << "Writing Info on Rotorpart " << endl
584 iv( _pos) // position in local coords
585 iv( _posforceattac) // position in local coords
586 iv( _normal) //direcetion of the rotation axis
587 i( _torque_max_force)
590 iv( _direction_of_movement)
591 iv( _directionofcentripetalforce)
592 iv( _directionofrotorpart)
593 i( _centripetalforce)
603 i( _alphamin) i(_alphamax) i(_alpha0) i(_alpha0factor)
606 i( _omega) i(_omegan) i(_ddt_omega)
610 i( _twist) //outer incidence = inner inner incidence + _twist
611 i( _number_of_segments)
612 i( _rel_len_where_incidence_is_measured)
613 i( _rel_len_blade_start)
615 i( _torque_of_inertia)
617 i (_alphaoutputbuf[0])
618 i (_alphaoutputbuf[1])
620 i(_rotor_correction_factor)
626 }; // namespace yasim