1 #include <simgear/debug/logstream.hxx>
4 #include "Rotorpart.hpp"
9 const float pi=3.14159;
11 Rotorpart::Rotorpart()
18 #define set3(x,a,b,c) x[0]=a;x[1]=b;x[2]=c;
20 set3 (_directionofcentripetalforce,1,0,0);
21 set3 (_directionofrotorpart,0,1,0);
22 set3 (_direction_of_movement,1,0,0);
23 set3 (_last_torque,0,0,0);
41 _alphaoutputbuf[0][0]=0;
42 _alphaoutputbuf[1][0]=0;
62 _number_of_segments=1;
63 _rel_len_where_incidence_is_measured=0.7;
66 _rel_len_blade_start=0;
68 _rotor_correction_factor=0.6;
71 void Rotorpart::inititeration(float dt,float *rot)
75 while (_phi>(2*pi)) _phi-=2*pi;
76 while (_phi<(0 )) _phi+=2*pi;
77 float a=Math::dot3(rot,_normal);
79 _alphaalt=_alpha*Math::cos(a)
80 +_next90rp->getrealAlpha()*Math::sin(a);
82 _alphaalt=_alpha*Math::cos(a)
83 +_last90rp->getrealAlpha()*Math::sin(-a);
84 //calculate the rotation of the fuselage, determine
85 //the part in the same direction as alpha
86 //and add it ro _alphaalt
87 //alpha is rotation about "normal cross dirofzentf"
90 Math::cross3(_directionofcentripetalforce,_normal,dir);
91 a=Math::dot3(rot,dir);
93 _alphaalt= Math::clamp(_alphaalt,_alphamin,_alphamax);
96 void Rotorpart::setRotor(Rotor *rotor)
101 void Rotorpart::setParameter(char *parametername, float value)
103 #define p(a) if (strcmp(parametername,#a)==0) _##a = value; else
106 p(number_of_segments)
107 p(rel_len_where_incidence_is_measured)
108 p(rel_len_blade_start)
109 p(rotor_correction_factor)
110 SG_LOG(SG_INPUT, SG_ALERT,
111 "internal error in parameter set up for rotorpart: '"
112 << parametername <<"'" << endl);
116 void Rotorpart::setTorque(float torque_max_force,float torque_no_force)
118 _torque_max_force=torque_max_force;
119 _torque_no_force=torque_no_force;
122 void Rotorpart::setTorqueOfInertia(float toi)
124 _torque_of_inertia=toi;
127 void Rotorpart::setWeight(float value)
132 float Rotorpart::getWeight(void)
134 return(_mass/.453); //_mass is in kg, returns pounds
137 void Rotorpart::setPosition(float* p)
140 for(i=0; i<3; i++) _pos[i] = p[i];
143 float Rotorpart::getIncidence()
148 void Rotorpart::getPosition(float* out)
151 for(i=0; i<3; i++) out[i] = _pos[i];
154 void Rotorpart::setPositionForceAttac(float* p)
157 for(i=0; i<3; i++) _posforceattac[i] = p[i];
160 void Rotorpart::getPositionForceAttac(float* out)
163 for(i=0; i<3; i++) out[i] = _posforceattac[i];
166 void Rotorpart::setSpeed(float* p)
169 for(i=0; i<3; i++) _speed[i] = p[i];
170 Math::unit3(_speed,_direction_of_movement);
173 void Rotorpart::setDirectionofZentipetalforce(float* p)
176 for(i=0; i<3; i++) _directionofcentripetalforce[i] = p[i];
179 void Rotorpart::setDirectionofRotorPart(float* p)
182 for(i=0; i<3; i++) _directionofrotorpart[i] = p[i];
185 void Rotorpart::setOmega(float value)
190 void Rotorpart::setOmegaN(float value)
195 void Rotorpart::setDdtOmega(float value)
200 void Rotorpart::setZentipetalForce(float f)
205 void Rotorpart::setMinpitch(float f)
210 void Rotorpart::setMaxpitch(float f)
215 void Rotorpart::setMaxcyclic(float f)
220 void Rotorpart::setMincyclic(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::setC2(float f)
255 void Rotorpart::setAlpha0(float f)
257 if (f>-0.01) f=-0.01; //half a degree bending
261 void Rotorpart::setAlphamin(float f)
266 void Rotorpart::setAlphamax(float f)
271 void Rotorpart::setAlpha0factor(float f)
276 void Rotorpart::setDiameter(float f)
281 float Rotorpart::getPhi()
286 float Rotorpart::getAlpha(int i)
291 return _alpha*180/pi;//in Grad = 1
294 if (_alpha2type==1) //yaw or roll
295 return (getAlpha(0)-_oppositerp->getAlpha(0))/2;
297 return (getAlpha(0)+_oppositerp->getAlpha(0)+
298 _next90rp->getAlpha(0)+_last90rp->getAlpha(0))/4;
301 float Rotorpart::getrealAlpha(void)
306 void Rotorpart::setAlphaoutput(char *text,int i)
308 strncpy(_alphaoutputbuf[i>0],text,255);
309 if (i>0) _alpha2type=i;
312 char* Rotorpart::getAlphaoutput(int i)
314 return _alphaoutputbuf[i&1];
317 void Rotorpart::setLen(float value)
322 void Rotorpart::setNormal(float* p)
325 for(i=0; i<3; i++) _normal[i] = p[i];
328 void Rotorpart::getNormal(float* out)
331 for(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 moment[3],v_local[3],v_local_scalar,lift_moment,v_flap[3],v_help[3];
372 float ias;//nur f. dgb
376 lift_moment=-_mass*_len*9.81; //*cos yaw * cos roll
378 if((_nextrp==NULL)||(_lastrp==NULL)||(_rotor==NULL))
379 return 0.0;//not initialized. Can happen during startupt of flightgear
380 if (returnlift!=NULL) *returnlift=0;
381 /*float flap_omega=(_nextrp->getrealAlpha()-_lastrp->getrealAlpha())
382 *_omega / pi*_rotor->getNumberOfParts()/4; hier mal die alte version probieren?
384 float flap_omega=(_next90rp->getrealAlpha()-_last90rp->getrealAlpha())
386 float local_width=_diameter*(1-_rel_len_blade_start)/2.
387 /(float (_number_of_segments));
388 for (n=0;n<_number_of_segments;n++)
390 float rel = (n+.5)/(float (_number_of_segments));
391 float r= _diameter *0.5 *(rel*(1-_rel_len_blade_start)
392 +_rel_len_blade_start);
393 float local_incidence=incidence+_twist *rel -
394 _twist *_rel_len_where_incidence_is_measured;
395 float local_chord = _rotor->getChord()*rel+_rotor->getChord()
396 *_rotor->getTaper()*(1-rel);
397 float A = local_chord * local_width;
398 //calculate the local air speed and the incidence to this speed;
399 Math::mul3(_omega * r , _direction_of_movement , v_local);
401 // add speed component due to flapping
402 Math::mul3(flap_omega * r,_normal,v_flap);
403 Math::add3(v_flap,v_local,v_local);
404 Math::sub3(v_rel_air,v_local,v_local);
405 //v_local is now the total airspeed at the blade
406 //apparent missing: calculating the local_wind = v_rel_air at
407 //every point of the rotor. It differs due to aircraft-rotation
408 //it is considered in v_flap
410 //substract now the component of the air speed parallel to
412 Math::mul3(Math::dot3(v_local,_directionofrotorpart),
413 _directionofrotorpart,v_help);
414 Math::sub3(v_local,v_help,v_local);
416 //split into direction and magnitude
417 v_local_scalar=Math::mag3(v_local);
418 if (v_local_scalar!=0)
419 //Math::unit3(v_local,v_help);
420 Math::mul3(1/v_local_scalar,v_local,v_help);
421 float incidence_of_airspeed = Math::asin(Math::clamp(
422 Math::dot3(v_help,_normal),-1,1)) + local_incidence;
423 ias = incidence_of_airspeed;
425 //reduce the ias (Prantl factor)
426 float prantl_factor=2/pi*Math::acos(Math::exp(
427 -_rotor->getNumberOfBlades()/2.*(1-rel)
428 *Math::sqrt(1+1/Math::sqr(Math::tan(
429 pi/2-Math::abs(incidence_of_airspeed-local_incidence))))));
430 incidence_of_airspeed = (incidence_of_airspeed+
431 _rotor->getAirfoilIncidenceNoLift())*prantl_factor
432 *_rotor_correction_factor-_rotor->getAirfoilIncidenceNoLift();
433 ias = incidence_of_airspeed;
434 float lift_wo_cyc = _rotor->getLiftCoef(incidence_of_airspeed
435 -cyc*_rotor_correction_factor,v_local_scalar)
436 * v_local_scalar * v_local_scalar * A *rho *0.5;
437 float lift_with_cyc =
438 _rotor->getLiftCoef(incidence_of_airspeed,v_local_scalar)
439 * v_local_scalar * v_local_scalar *A *rho*0.5;
440 float lift=lift_wo_cyc+_relamp*(lift_with_cyc-lift_wo_cyc);
441 //take into account that the rotor is a resonant system where
442 //the cyclic input hase increased result
443 float drag = -_rotor->getDragCoef(incidence_of_airspeed,v_local_scalar)
444 * v_local_scalar * v_local_scalar * A *rho*0.5;
445 float angle = incidence_of_airspeed - incidence;
446 //angle between blade movement caused by rotor-rotation and the
447 //total movement of the blade
449 /* the next shold look like this, but this is the inner loop of
450 the rotor simulation. For small angles (and we hav only small
451 angles) the first order approximation works well
452 lift_moment += r*(lift * Math::cos(angle)
453 - drag * Math::sin(angle));
454 *torque += r*(drag * Math::cos(angle)
455 + lift * Math::sin(angle));
457 lift_moment += r*(lift * (1-angle*angle)
459 *torque += r*(drag * (1-angle*angle)
462 if (returnlift!=NULL) *returnlift+=lift;
464 //as above, use 1st order approximation
465 //float alpha=Math::atan2(lift_moment,_centripetalforce * _len);
467 alpha=lift_moment/(_centripetalforce * _len - _mass * _len * 9.81 /_alpha0);
468 //centripetalforce is >=0 and _alpha0<-0.01
472 // Calculate the aerodynamic force given a wind vector v (in the
473 // aircraft's "local" coordinates) and an air density rho. Returns a
474 // torque about the Y axis, too.
475 void Rotorpart::calcForce(float* v, float rho, float* out, float* torque,
476 float* torque_scalar)
480 for (int i=0;i<3;i++)
485 _centripetalforce=_mass*_len*_omega*_omega;
486 float vrel[3],vreldir[3];
487 Math::sub3(_speed,v,vrel);
488 float scalar_torque=0;
489 Math::unit3(vrel,vreldir);//direction of blade-movement rel. to air
490 //Angle of blade which would produce no vertical force (where the
491 //effective incidence is zero)
493 //float cyc=_mincyclic+(_cyclic+1)/2*(_maxcyclic-_mincyclic);
495 float col=_minpitch+(_collective+1)/2*(_maxpitch-_minpitch);
496 _incidence=(col+cyc)-_delta3*_alphaalt;
497 //the incidence of the rotorblade due to control input reduced by the
498 //delta3 effect, see README.YASIM
499 //float beta=_relamp*cyc+col;
500 //the incidence of the rotorblade which is used for the calculation
502 float alpha,factor; //alpha is the flapping angle
503 //the new flapping angle will be the old flapping angle
504 //+ factor *(alpha - "old flapping angle")
505 alpha=calculateAlpha(v,rho,_incidence,cyc,0,&scalar_torque);
506 //the incidence is a function of alpha (if _delta* != 0)
507 //Therefore missing: wrap this function in an integrator
508 //(runge kutta e. g.)
511 if (factor>1) factor=1;
514 Math::cross3(_normal,_directionofcentripetalforce,dirblade);
515 float vblade=Math::abs(Math::dot3(dirblade,v));
517 alpha=_alphaalt+(alpha-_alphaalt)*factor;
519 float meancosalpha=(1*Math::cos(_last90rp->getrealAlpha())
520 +1*Math::cos(_next90rp->getrealAlpha())
521 +1*Math::cos(_oppositerp->getrealAlpha())
522 +1*Math::cos(alpha))/4;
523 float schwenkfactor=1-(Math::cos(_lastrp->getrealAlpha())-meancosalpha)*_rotor->getNumberOfParts()/4;
525 //missing: consideration of rellenhinge
526 float xforce = Math::cos(alpha)*_centripetalforce;
527 float zforce = schwenkfactor*Math::sin(alpha)*_centripetalforce;
528 *torque_scalar=scalar_torque;
529 scalar_torque+= 0*_ddt_omega*_torque_of_inertia;
530 float thetorque = scalar_torque;
532 float f=_rotor->getCcw()?1:-1;
534 _last_torque[i]=torque[i] = f*_normal[i]*thetorque;
535 out[i] = _normal[i]*zforce*_rotor->getLiftFactor()
536 +_directionofcentripetalforce[i]*xforce;
540 void Rotorpart::getAccelTorque(float relaccel,float *t)
543 float f=_rotor->getCcw()?1:-1;
545 t[i]=f*-1* _normal[i]*relaccel*_omegan* _torque_of_inertia;// *_omeagan ?
546 _rotor->addTorque(-relaccel*_omegan* _torque_of_inertia);
549 std::ostream & operator<<(std::ostream & out, const Rotorpart& rp)
551 #define i(x) << #x << ":" << rp.x << endl
552 #define iv(x) << #x << ":" << rp.x[0] << ";" << rp.x[1] << ";" <<rp.x[2] << ";" << endl
553 out << "Writing Info on Rotorpart " << endl
557 iv( _pos) // position in local coords
558 iv( _posforceattac) // position in local coords
559 iv( _normal) //direcetion of the rotation axis
560 i( _torque_max_force)
563 iv( _direction_of_movement)
564 iv( _directionofcentripetalforce)
565 iv( _directionofrotorpart)
566 i( _centripetalforce)
580 i( _alphamin) i(_alphamax) i(_alpha0) i(_alpha0factor)
583 i( _omega) i(_omegan) i(_ddt_omega)
587 i( _twist) //outer incidence = inner inner incidence + _twist
588 i( _number_of_segments)
589 i( _rel_len_where_incidence_is_measured)
590 i( _rel_len_blade_start)
592 i( _torque_of_inertia)
594 i (_alphaoutputbuf[0])
595 i (_alphaoutputbuf[1])
597 i(_rotor_correction_factor)
603 }; // namespace yasim