1 #include <simgear/debug/logstream.hxx>
4 #include "Rotorpart.hpp"
9 const float pi=3.14159;
11 Rotorpart::Rotorpart()
17 _shared_flap_hinge=false;
19 #define set3(x,a,b,c) x[0]=a;x[1]=b;x[2]=c;
21 set3 (_directionofcentripetalforce,1,0,0);
22 set3 (_directionofrotorpart,0,1,0);
23 set3 (_direction_of_movement,1,0,0);
24 set3 (_last_torque,0,0,0);
38 _alphaoutputbuf[0][0]=0;
39 _alphaoutputbuf[1][0]=0;
59 _number_of_segments=1;
60 _rel_len_where_incidence_is_measured=0.7;
63 _rel_len_blade_start=0;
65 _rotor_correction_factor=0.6;
70 void Rotorpart::inititeration(float dt,float *rot)
74 while (_phi>(2*pi)) _phi-=2*pi;
75 while (_phi<(0 )) _phi+=2*pi;
76 float a=Math::dot3(rot,_normal);
78 _alphaalt=_alpha*Math::cos(a)
79 +_next90rp->getrealAlpha()*Math::sin(a);
81 _alphaalt=_alpha*Math::cos(a)
82 +_last90rp->getrealAlpha()*Math::sin(-a);
83 //calculate the rotation of the fuselage, determine
84 //the part in the same direction as alpha
85 //and add it ro _alphaalt
86 //alpha is rotation about "normal cross dirofzentf"
89 Math::cross3(_directionofcentripetalforce,_normal,dir);
90 a=Math::dot3(rot,dir);
92 _alphaalt= Math::clamp(_alphaalt,_alphamin,_alphamax);
96 b=_rotor->getBalance();
97 float s =Math::sin(_phi+_direction);
98 float c =Math::cos(_phi+_direction);
100 _balance=(b>0)?(1.-s*(1.-b)):(1.-s)*(1.+b);
102 _balance=(b>0)?1.:1.+b;
105 void Rotorpart::setRotor(Rotor *rotor)
110 void Rotorpart::setParameter(char *parametername, float value)
112 #define p(a) if (strcmp(parametername,#a)==0) _##a = value; else
115 p(number_of_segments)
116 p(rel_len_where_incidence_is_measured)
117 p(rel_len_blade_start)
118 p(rotor_correction_factor)
119 SG_LOG(SG_INPUT, SG_ALERT,
120 "internal error in parameter set up for rotorpart: '"
121 << parametername <<"'" << endl);
125 void Rotorpart::setTorque(float torque_max_force,float torque_no_force)
127 _torque_max_force=torque_max_force;
128 _torque_no_force=torque_no_force;
131 void Rotorpart::setTorqueOfInertia(float toi)
133 _torque_of_inertia=toi;
136 void Rotorpart::setWeight(float value)
141 float Rotorpart::getWeight(void)
143 return(_mass/.453); //_mass is in kg, returns pounds
146 void Rotorpart::setPosition(float* p)
149 for(i=0; i<3; i++) _pos[i] = p[i];
152 float Rotorpart::getIncidence()
157 void Rotorpart::getPosition(float* out)
160 for(i=0; i<3; i++) out[i] = _pos[i];
163 void Rotorpart::setPositionForceAttac(float* p)
166 for(i=0; i<3; i++) _posforceattac[i] = p[i];
169 void Rotorpart::getPositionForceAttac(float* out)
172 for(i=0; i<3; i++) out[i] = _posforceattac[i];
175 void Rotorpart::setSpeed(float* p)
178 for(i=0; i<3; i++) _speed[i] = p[i];
179 Math::unit3(_speed,_direction_of_movement);
182 void Rotorpart::setDirectionofZentipetalforce(float* p)
185 for(i=0; i<3; i++) _directionofcentripetalforce[i] = p[i];
188 void Rotorpart::setDirectionofRotorPart(float* p)
191 for(i=0; i<3; i++) _directionofrotorpart[i] = p[i];
194 void Rotorpart::setDirection(float direction)
196 _direction=direction;
199 void Rotorpart::setOmega(float value)
204 void Rotorpart::setPhi(float value)
209 void Rotorpart::setOmegaN(float value)
214 void Rotorpart::setDdtOmega(float value)
219 void Rotorpart::setZentipetalForce(float f)
225 void Rotorpart::setDelta3(float f)
230 void Rotorpart::setRelamp(float f)
235 void Rotorpart::setTranslift(float f)
240 void Rotorpart::setDynamic(float f)
245 void Rotorpart::setRelLenHinge(float f)
250 void Rotorpart::setSharedFlapHinge(bool s)
252 _shared_flap_hinge=s;
255 void Rotorpart::setC2(float f)
260 void Rotorpart::setAlpha0(float f)
262 if (f>-0.01) f=-0.01; //half a degree bending
266 void Rotorpart::setAlphamin(float f)
271 void Rotorpart::setAlphamax(float f)
276 void Rotorpart::setAlpha0factor(float f)
281 void Rotorpart::setDiameter(float f)
286 float Rotorpart::getPhi()
291 float Rotorpart::getAlpha(int i)
296 return _alpha*180/pi;//in Grad = 1
299 if (_alpha2type==1) //yaw or roll
300 return (getAlpha(0)-_oppositerp->getAlpha(0))/2;
302 return (getAlpha(0)+_oppositerp->getAlpha(0)+
303 _next90rp->getAlpha(0)+_last90rp->getAlpha(0))/4;
306 float Rotorpart::getrealAlpha(void)
311 void Rotorpart::setAlphaoutput(char *text,int i)
313 strncpy(_alphaoutputbuf[i>0],text,255);
314 if (i>0) _alpha2type=i;
317 char* Rotorpart::getAlphaoutput(int i)
319 return _alphaoutputbuf[i&1];
322 void Rotorpart::setLen(float value)
327 void Rotorpart::setNormal(float* p)
330 for(i=0; i<3; i++) _normal[i] = p[i];
333 void Rotorpart::getNormal(float* out)
336 for(i=0; i<3; i++) out[i] = _normal[i];
339 void Rotorpart::setCollective(float pos)
344 void Rotorpart::setCyclic(float pos)
349 void Rotorpart::setlastnextrp(Rotorpart*lastrp,Rotorpart*nextrp,
350 Rotorpart *oppositerp,Rotorpart*last90rp,Rotorpart*next90rp)
354 _oppositerp=oppositerp;
359 void Rotorpart::strncpy(char *dest,const char *src,int maxlen)
362 while(src[n]&&n<(maxlen-1))
370 // Calculate the flapping angle, where zentripetal force and
371 //lift compensate each other
372 float Rotorpart::calculateAlpha(float* v_rel_air, float rho,
373 float incidence, float cyc, float alphaalt, float *torque,
376 float moment[3],v_local[3],v_local_scalar,lift_moment,v_flap[3],v_help[3];
377 float ias;//nur f. dgb
381 float relgrav = Math::dot3(_normal,_rotor->getGravDirection());
382 lift_moment=-_mass*_len*9.81*relgrav;
384 if((_nextrp==NULL)||(_lastrp==NULL)||(_rotor==NULL))
385 return 0.0;//not initialized. Can happen during startupt of flightgear
386 if (returnlift!=NULL) *returnlift=0;
387 float flap_omega=(_next90rp->getrealAlpha()-_last90rp->getrealAlpha())
389 float local_width=_diameter*(1-_rel_len_blade_start)/2.
390 /(float (_number_of_segments));
391 for (n=0;n<_number_of_segments;n++)
393 float rel = (n+.5)/(float (_number_of_segments));
394 float r= _diameter *0.5 *(rel*(1-_rel_len_blade_start)
395 +_rel_len_blade_start);
396 float local_incidence=incidence+_twist *rel -
397 _twist *_rel_len_where_incidence_is_measured;
398 float local_chord = _rotor->getChord()*rel+_rotor->getChord()
399 *_rotor->getTaper()*(1-rel);
400 float A = local_chord * local_width;
401 //calculate the local air speed and the incidence to this speed;
402 Math::mul3(_omega * r , _direction_of_movement , v_local);
404 // add speed component due to flapping
405 Math::mul3(flap_omega * r,_normal,v_flap);
406 Math::add3(v_flap,v_local,v_local);
407 Math::sub3(v_rel_air,v_local,v_local);
408 //v_local is now the total airspeed at the blade
409 //apparent missing: calculating the local_wind = v_rel_air at
410 //every point of the rotor. It differs due to aircraft-rotation
411 //it is considered in v_flap
413 //substract now the component of the air speed parallel to
415 Math::mul3(Math::dot3(v_local,_directionofrotorpart),
416 _directionofrotorpart,v_help);
417 Math::sub3(v_local,v_help,v_local);
419 //split into direction and magnitude
420 v_local_scalar=Math::mag3(v_local);
421 if (v_local_scalar!=0)
422 //Math::unit3(v_local,v_help);
423 Math::mul3(1/v_local_scalar,v_local,v_help);
424 float incidence_of_airspeed = Math::asin(Math::clamp(
425 Math::dot3(v_help,_normal),-1,1)) + local_incidence;
426 ias = incidence_of_airspeed;
428 //reduce the ias (Prantl factor)
429 float prantl_factor=2/pi*Math::acos(Math::exp(
430 -_rotor->getNumberOfBlades()/2.*(1-rel)
431 *Math::sqrt(1+1/Math::sqr(Math::tan(
432 pi/2-Math::abs(incidence_of_airspeed-local_incidence))))));
433 incidence_of_airspeed = (incidence_of_airspeed+
434 _rotor->getAirfoilIncidenceNoLift())*prantl_factor
435 *_rotor_correction_factor-_rotor->getAirfoilIncidenceNoLift();
436 ias = incidence_of_airspeed;
437 float lift_wo_cyc = _rotor->getLiftCoef(incidence_of_airspeed
438 -cyc*_rotor_correction_factor*prantl_factor,v_local_scalar)
439 * v_local_scalar * v_local_scalar * A *rho *0.5;
440 float lift_with_cyc =
441 _rotor->getLiftCoef(incidence_of_airspeed,v_local_scalar)
442 * v_local_scalar * v_local_scalar *A *rho*0.5;
443 float lift=lift_wo_cyc+_relamp*(lift_with_cyc-lift_wo_cyc);
444 //take into account that the rotor is a resonant system where
445 //the cyclic input hase increased result
446 float drag = -_rotor->getDragCoef(incidence_of_airspeed,v_local_scalar)
447 * v_local_scalar * v_local_scalar * A *rho*0.5;
448 float angle = incidence_of_airspeed - incidence;
449 //angle between blade movement caused by rotor-rotation and the
450 //total movement of the blade
452 /* the next shold look like this, but this is the inner loop of
453 the rotor simulation. For small angles (and we hav only small
454 angles) the first order approximation works well
455 lift_moment += r*(lift * Math::cos(angle)
456 - drag * Math::sin(angle));
457 *torque += r*(drag * Math::cos(angle)
458 + lift * Math::sin(angle));
460 lift_moment += r*(lift * (1-angle*angle)
462 *torque += r*(drag * (1-angle*angle)
465 if (returnlift!=NULL) *returnlift+=lift;
467 //as above, use 1st order approximation
468 //float alpha=Math::atan2(lift_moment,_centripetalforce * _len);
470 if (_shared_flap_hinge)
473 if (Math::abs(_alphaalt) >1e-6)
474 div=(_centripetalforce * _len - _mass * _len * 9.81 * relgrav /_alpha0*(_alphaalt+_oppositerp->getAlphaAlt())/(2.0*_alphaalt));
475 if (Math::abs(div)>1e-6)
476 alpha=lift_moment/div;
477 else if(Math::abs(_alphaalt+_oppositerp->getAlphaAlt())>1e-6)
479 float div=(_centripetalforce * _len - _mass * _len * 9.81 *0.5 * relgrav)*(_alphaalt+_oppositerp->getAlphaAlt());
480 if (Math::abs(div)>1e-6)
481 alpha=_oppositerp->getAlphaAlt()+lift_moment/div*_alphaalt;
488 float div=(_centripetalforce * _len - _mass * _len * 9.81 /_alpha0);
489 if (Math::abs(div)>1e-6)
490 alpha=lift_moment/div;
498 // Calculate the aerodynamic force given a wind vector v (in the
499 // aircraft's "local" coordinates) and an air density rho. Returns a
500 // torque about the Y axis, too.
501 void Rotorpart::calcForce(float* v, float rho, float* out, float* torque,
502 float* torque_scalar)
506 for (int i=0;i<3;i++)
511 _centripetalforce=_mass*_len*_omega*_omega;
512 float vrel[3],vreldir[3];
513 Math::sub3(_speed,v,vrel);
514 float scalar_torque=0;
515 Math::unit3(vrel,vreldir);//direction of blade-movement rel. to air
516 //Angle of blade which would produce no vertical force (where the
517 //effective incidence is zero)
520 float col=_collective;
521 if (_shared_flap_hinge)
522 _incidence=(col+cyc)-_delta3*0.5*(_alphaalt-_oppositerp->getAlphaAlt());
524 _incidence=(col+cyc)-_delta3*_alphaalt;
525 //the incidence of the rotorblade due to control input reduced by the
526 //delta3 effect, see README.YASIM
527 //float beta=_relamp*cyc+col;
528 //the incidence of the rotorblade which is used for the calculation
530 float alpha,factor; //alpha is the flapping angle
531 //the new flapping angle will be the old flapping angle
532 //+ factor *(alpha - "old flapping angle")
533 alpha=calculateAlpha(v,rho,_incidence,cyc,0,&scalar_torque);
534 alpha=Math::clamp(alpha,_alphamin,_alphamax);
535 //the incidence is a function of alpha (if _delta* != 0)
536 //Therefore missing: wrap this function in an integrator
537 //(runge kutta e. g.)
540 if (factor>1) factor=1;
543 Math::cross3(_normal,_directionofcentripetalforce,dirblade);
544 float vblade=Math::abs(Math::dot3(dirblade,v));
546 alpha=_alphaalt+(alpha-_alphaalt)*factor;
548 float meancosalpha=(1*Math::cos(_last90rp->getrealAlpha())
549 +1*Math::cos(_next90rp->getrealAlpha())
550 +1*Math::cos(_oppositerp->getrealAlpha())
551 +1*Math::cos(alpha))/4;
552 float schwenkfactor=1-(Math::cos(_lastrp->getrealAlpha())-meancosalpha)*_rotor->getNumberOfParts()/4;
554 //missing: consideration of rellenhinge
557 _centripetalforce*=_balance;
558 scalar_torque*=_balance;
560 float xforce = Math::cos(alpha)*_centripetalforce;
561 float zforce = schwenkfactor*Math::sin(alpha)*_centripetalforce;
562 *torque_scalar=scalar_torque;
563 scalar_torque+= 0*_ddt_omega*_torque_of_inertia;
564 float thetorque = scalar_torque;
566 float f=_rotor->getCcw()?1:-1;
568 _last_torque[i]=torque[i] = f*_normal[i]*thetorque;
569 out[i] = _normal[i]*zforce*_rotor->getLiftFactor()
570 +_directionofcentripetalforce[i]*xforce;
574 void Rotorpart::getAccelTorque(float relaccel,float *t)
577 float f=_rotor->getCcw()?1:-1;
579 t[i]=f*-1* _normal[i]*relaccel*_omegan* _torque_of_inertia;// *_omeagan ?
580 _rotor->addTorque(-relaccel*_omegan* _torque_of_inertia);
583 std::ostream & operator<<(std::ostream & out, const Rotorpart& rp)
585 #define i(x) << #x << ":" << rp.x << endl
586 #define iv(x) << #x << ":" << rp.x[0] << ";" << rp.x[1] << ";" <<rp.x[2] << ";" << endl
587 out << "Writing Info on Rotorpart " << endl
591 iv( _pos) // position in local coords
592 iv( _posforceattac) // position in local coords
593 iv( _normal) //direcetion of the rotation axis
594 i( _torque_max_force)
597 iv( _direction_of_movement)
598 iv( _directionofcentripetalforce)
599 iv( _directionofrotorpart)
600 i( _centripetalforce)
610 i( _alphamin) i(_alphamax) i(_alpha0) i(_alpha0factor)
613 i( _omega) i(_omegan) i(_ddt_omega)
617 i( _twist) //outer incidence = inner inner incidence + _twist
618 i( _number_of_segments)
619 i( _rel_len_where_incidence_is_measured)
620 i( _rel_len_blade_start)
622 i( _torque_of_inertia)
624 i (_alphaoutputbuf[0])
625 i (_alphaoutputbuf[1])
627 i(_rotor_correction_factor)
633 }; // namespace yasim