1 #include <simgear/debug/logstream.hxx>
4 #include <Main/fg_props.hxx>
6 #include "Rotorpart.hpp"
14 SG_USING_STD(setprecision);
27 const float pi=3.14159;
29 static inline float sqr(float a) { return a * a; }
46 _base[0] = _base[1] = _base[2] = 0;
55 _forward[1]=_forward[2]=0;
56 _max_pitch=13./180*pi;
57 _maxcyclicail=10./180*pi;
58 _maxcyclicele=10./180*pi;
60 _mincyclicail=-10./180*pi;
61 _mincyclicele=-10./180*pi;
62 _min_pitch=-.5/180*pi;
66 _normal[0] = _normal[1] = 0;
68 _normal_with_yaw_roll[0]= _normal_with_yaw_roll[1]=0;
69 _normal_with_yaw_roll[2]=1;
71 _omega=_omegan=_omegarel=_omegarelneu=0;
81 _shared_flap_hinge=false;
82 _rellenteeterhinge=0.01;
89 _translift_maxfactor=1.3;
90 _ground_effect_constant=0.1;
91 _vortex_state_lift_factor=0.4;
104 _number_of_segments=1;
106 _rel_len_where_incidence_is_measured=0.7;
107 _torque_of_inertia=1;
111 _airfoil_incidence_no_lift=0;
112 _rel_len_blade_start=0;
113 _airfoil_lift_coefficient=0;
114 _airfoil_drag_coefficient0=0;
115 _airfoil_drag_coefficient1=0;
117 _global_ground[i] = 0;
118 _global_ground[2] = 1;
119 _global_ground[3] = -1e3;
120 _incidence_stall_zero_speed=18*pi/180.;
121 _incidence_stall_half_sonic_speed=14*pi/180.;
122 _lift_factor_stall=0.28;
123 _stall_change_over=2*pi/180.;
124 _drag_factor_stall=8;
129 for (int k=0;k<4;k++)
131 _groundeffectpos[k][i]=0;
132 _ground_effect_altitude=1;
134 _lift_factor=_f_ge=_f_vs=_f_tl=1;
135 _rotor_correction_factor=.65;
139 _num_ground_contact_pos=0;
145 for(i=0; i<_rotorparts.size(); i++) {
146 Rotorpart* r = (Rotorpart*)_rotorparts.get(i);
149 //untie the properties
152 SGPropertyNode * node = fgGetNode("/rotors", true)->getNode(_name,true);
153 node->untie("balance-ext");
154 node->untie("balance-int");
159 void Rotor::inititeration(float dt,float omegarel,float ddt_omegarel,float *rot)
164 _omega=_omegan*_omegarel;
165 _ddt_omega=_omegan*ddt_omegarel;
167 for(i=0; i<_rotorparts.size(); i++) {
168 float s = Math::sin(float(2*pi*i/_number_of_parts+(_phi-pi/2.)*(_ccw?1:-1)));
169 float c = Math::cos(float(2*pi*i/_number_of_parts+(_phi-pi/2.)*(_ccw?1:-1)));
170 Rotorpart* r = (Rotorpart*)_rotorparts.get(i);
172 r->setDdtOmega(_ddt_omega);
173 r->inititeration(dt,rot);
174 r->setCyclic(_cyclicail*c+_cyclicele*s);
177 //calculate the normal of the rotor disc, for calcualtion of the downwash
178 float side[3],help[3];
179 Math::cross3(_normal,_forward,side);
180 Math::mul3(Math::cos(_yaw)*Math::cos(_roll),_normal,_normal_with_yaw_roll);
182 Math::mul3(Math::sin(_yaw),_forward,help);
183 Math::add3(_normal_with_yaw_roll,help,_normal_with_yaw_roll);
185 Math::mul3(Math::sin(_roll),side,help);
186 Math::add3(_normal_with_yaw_roll,help,_normal_with_yaw_roll);
189 float Rotor::calcStall(float incidence,float speed)
191 float stall_incidence=_incidence_stall_zero_speed
192 +(_incidence_stall_half_sonic_speed
193 -_incidence_stall_zero_speed)*speed/(343./2);
194 //missing: Temeperature dependency of sonic speed
195 incidence = Math::abs(incidence);
196 if (incidence > (90./180.*pi))
197 incidence = pi-incidence;
198 float stall = (incidence-stall_incidence)/_stall_change_over;
199 stall = Math::clamp(stall,0,1);
201 _stall_sum+=stall*speed*speed;
202 _stall_v2sum+=speed*speed;
207 float Rotor::getLiftCoef(float incidence,float speed)
209 float stall=calcStall(incidence,speed);
210 /* the next shold look like this, but this is the inner loop of
211 the rotor simulation. For small angles (and we hav only small
212 angles) the first order approximation works well
213 float c1= Math::sin(incidence-_airfoil_incidence_no_lift)*_liftcoef;
214 for c2 we would need higher order, because in stall the angle can be large
217 if (incidence > (pi/2))
219 else if (incidence <-(pi/2))
223 float c1= (i2-_airfoil_incidence_no_lift)*_liftcoef;
226 float c2= Math::sin(2*(incidence-_airfoil_incidence_no_lift))
227 *_liftcoef*_lift_factor_stall;
228 return (1-stall)*c1 + stall *c2;
234 float Rotor::getDragCoef(float incidence,float speed)
236 float stall=calcStall(incidence,speed);
237 float c1= (Math::abs(Math::sin(incidence-_airfoil_incidence_no_lift))
238 *_dragcoef1+_dragcoef0);
239 float c2= c1*_drag_factor_stall;
240 return (1-stall)*c1 + stall *c2;
243 int Rotor::getValueforFGSet(int j,char *text,float *f)
245 if (_name[0]==0) return 0;
246 if (4>numRotorparts()) return 0; //compile first!
249 sprintf(text,"/rotors/%s/cone-deg", _name);
250 *f=( ((Rotorpart*)getRotorpart(0))->getrealAlpha()
251 +((Rotorpart*)getRotorpart(1*(_number_of_parts>>2)))->getrealAlpha()
252 +((Rotorpart*)getRotorpart(2*(_number_of_parts>>2)))->getrealAlpha()
253 +((Rotorpart*)getRotorpart(3*(_number_of_parts>>2)))->getrealAlpha()
259 sprintf(text,"/rotors/%s/roll-deg", _name);
260 _roll = ( ((Rotorpart*)getRotorpart(0))->getrealAlpha()
261 -((Rotorpart*)getRotorpart(2*(_number_of_parts>>2)))->getrealAlpha()
268 sprintf(text,"/rotors/%s/yaw-deg", _name);
269 _yaw=( ((Rotorpart*)getRotorpart(1*(_number_of_parts>>2)))->getrealAlpha()
270 -((Rotorpart*)getRotorpart(3*(_number_of_parts>>2)))->getrealAlpha()
277 sprintf(text,"/rotors/%s/rpm", _name);
278 *f=(_balance1>-1)?_omega/2/pi*60:0;
283 sprintf(text,"/rotors/%s/debug/debugfge",_name);
288 sprintf(text,"/rotors/%s/debug/debugfvs",_name);
293 sprintf(text,"/rotors/%s/debug/debugftl",_name);
298 sprintf(text,"/rotors/%s/balance", _name);
303 sprintf(text,"/rotors/%s/stall",_name);
304 *f=getOverallStall();
308 sprintf(text,"/rotors/%s/torque",_name);
314 if (b>=_number_of_blades)
319 sprintf(text,"/rotors/%s/blade[%i]/%s",
321 w==0?"position-deg":(w==1?"flap-deg":"incidence-deg"));
322 *f=((Rotorpart*)getRotorpart(0))->getPhi()*180/pi
323 +360*b/_number_of_blades*(_ccw?1:-1);
326 if (_balance1<=-1) *f=0;
333 rk=Math::clamp(rk,0,1);//Delete this
335 if(w==2) {k+=2;l+=2;}
337 if(w==1) {k+=1;l+=1;}
340 if (w==1) *f=rk*((Rotorpart*) getRotorpart(k*(_number_of_parts>>2)))->getrealAlpha()*180/pi
341 +rl*((Rotorpart*) getRotorpart(l*(_number_of_parts>>2)))->getrealAlpha()*180/pi;
342 else if(w==2) *f=rk*((Rotorpart*)getRotorpart(k*(_number_of_parts>>2)))->getIncidence()*180/pi
343 +rl*((Rotorpart*)getRotorpart(l*(_number_of_parts>>2)))->getIncidence()*180/pi;
348 void Rotorgear::setEngineOn(int value)
353 void Rotor::setAlpha0(float f)
358 void Rotor::setAlphamin(float f)
363 void Rotor::setAlphamax(float f)
368 void Rotor::setAlpha0factor(float f)
373 int Rotor::numRotorparts()
375 return _rotorparts.size();
378 Rotorpart* Rotor::getRotorpart(int n)
380 return ((Rotorpart*)_rotorparts.get(n));
383 int Rotorgear::getEngineon()
388 float Rotor::getTorqueOfInertia()
390 return _torque_of_inertia;
393 void Rotor::setTorque(float f)
398 void Rotor::addTorque(float f)
403 void Rotor::strncpy(char *dest,const char *src,int maxlen)
406 while(src[n]&&n<(maxlen-1))
414 void Rotor::setNormal(float* normal)
417 float invsum,sqrsum=0;
418 for(i=0; i<3; i++) { sqrsum+=normal[i]*normal[i];}
420 invsum=1/Math::sqrt(sqrsum);
425 _normal_with_yaw_roll[i]=_normal[i] = normal[i]*invsum;
429 void Rotor::setForward(float* forward)
432 float invsum,sqrsum=0;
433 for(i=0; i<3; i++) { sqrsum+=forward[i]*forward[i];}
435 invsum=1/Math::sqrt(sqrsum);
438 for(i=0; i<3; i++) { _forward[i] = forward[i]*invsum; }
441 void Rotor::setForceAtPitchA(float force)
443 _force_at_pitch_a=force;
446 void Rotor::setPowerAtPitch0(float value)
448 _power_at_pitch_0=value;
451 void Rotor::setPowerAtPitchB(float value)
453 _power_at_pitch_b=value;
456 void Rotor::setPitchA(float value)
458 _pitch_a=value/180*pi;
461 void Rotor::setPitchB(float value)
463 _pitch_b=value/180*pi;
466 void Rotor::setBase(float* base)
469 for(i=0; i<3; i++) _base[i] = base[i];
472 void Rotor::setMaxCyclicail(float value)
474 _maxcyclicail=value/180*pi;
477 void Rotor::setMaxCyclicele(float value)
479 _maxcyclicele=value/180*pi;
482 void Rotor::setMinCyclicail(float value)
484 _mincyclicail=value/180*pi;
487 void Rotor::setMinCyclicele(float value)
489 _mincyclicele=value/180*pi;
492 void Rotor::setMinCollective(float value)
494 _min_pitch=value/180*pi;
497 void Rotor::setMaxCollective(float value)
499 _max_pitch=value/180*pi;
502 void Rotor::setDiameter(float value)
507 void Rotor::setWeightPerBlade(float value)
509 _weight_per_blade=value;
512 void Rotor::setNumberOfBlades(float value)
514 _number_of_blades=int(value+.5);
517 void Rotor::setRelBladeCenter(float value)
519 _rel_blade_center=value;
522 void Rotor::setDynamic(float value)
527 void Rotor::setDelta3(float value)
532 void Rotor::setDelta(float value)
537 void Rotor::setTranslift(float value)
542 void Rotor::setSharedFlapHinge(bool s)
544 _shared_flap_hinge=s;
547 void Rotor::setBalance(float b)
552 void Rotor::setC2(float value)
557 void Rotor::setStepspersecond(float steps)
559 _stepspersecond=steps;
562 void Rotor::setRPM(float value)
567 void Rotor::setPhiNull(float value)
572 void Rotor::setRelLenHinge(float value)
574 _rel_len_hinge=value;
577 void Rotor::setAlphaoutput(int i, const char *text)
579 strncpy(_alphaoutput[i],text,255);
582 void Rotor::setName(const char *text)
584 strncpy(_name,text,256);//256: some space needed for settings
587 void Rotor::setCcw(int ccw)
592 void Rotor::setNotorque(int value)
597 void Rotor::setRelLenTeeterHinge(float f)
599 _rellenteeterhinge=f;
602 void Rotor::setTeeterdamp(float f)
607 void Rotor::setMaxteeterdamp(float f)
612 void Rotor::setGlobalGround(double *global_ground, float* global_vel)
615 for(i=0; i<4; i++) _global_ground[i] = global_ground[i];
618 void Rotor::setParameter(char *parametername, float value)
620 #define p(a,b) if (strcmp(parametername,#a)==0) _##a = (value * (b)); else
622 p(translift_maxfactor,1)
623 p(ground_effect_constant,1)
624 p(vortex_state_lift_factor,1)
632 p(number_of_segments,1)
634 p(rel_len_where_incidence_is_measured,1)
637 p(airfoil_incidence_no_lift,pi/180.)
638 p(rel_len_blade_start,1)
639 p(incidence_stall_zero_speed,pi/180.)
640 p(incidence_stall_half_sonic_speed,pi/180.)
641 p(lift_factor_stall,1)
642 p(stall_change_over,pi/180.)
643 p(drag_factor_stall,1)
644 p(airfoil_lift_coefficient,1)
645 p(airfoil_drag_coefficient0,1)
646 p(airfoil_drag_coefficient1,1)
648 p(rotor_correction_factor,1)
649 SG_LOG(SG_INPUT, SG_ALERT,
650 "internal error in parameter set up for rotor: '" <<
651 parametername <<"'" << endl);
655 float Rotor::getLiftFactor()
660 void Rotorgear::setRotorBrake(float lval)
662 lval = Math::clamp(lval, 0, 1);
666 void Rotor::setCollective(float lval)
668 lval = Math::clamp(lval, -1, 1);
670 _collective=_min_pitch+(lval+1)/2*(_max_pitch-_min_pitch);
671 for(i=0; i<_rotorparts.size(); i++) {
672 ((Rotorpart*)_rotorparts.get(i))->setCollective(_collective);
676 void Rotor::setCyclicele(float lval,float rval)
678 lval = Math::clamp(lval, -1, 1);
679 _cyclicele=_mincyclicele+(lval+1)/2*(_maxcyclicele-_mincyclicele);
682 void Rotor::setCyclicail(float lval,float rval)
684 lval = Math::clamp(lval, -1, 1);
686 _cyclicail=-(_mincyclicail+(lval+1)/2*(_maxcyclicail-_mincyclicail));
689 void Rotor::getPosition(float* out)
692 for(i=0; i<3; i++) out[i] = _base[i];
695 void Rotor::calcLiftFactor(float* v, float rho, State *s)
697 //calculates _lift_factor, which is a foactor for the lift of the rotor
699 //- ground effect (_f_ge)
700 //- vortex state (_f_vs)
701 //- translational lift (_f_tl)
706 // Calculate ground effect
707 _f_ge=1+_diameter/_ground_effect_altitude*_ground_effect_constant;
709 // Now calculate translational lift
710 float v_vert = Math::dot3(v,_normal);
712 Math::cross3(v,_normal,help);
713 float v_horiz = Math::mag3(help);
714 _f_tl = ((1-Math::pow(2.7183,-v_horiz/_translift_ve))
715 *(_translift_maxfactor-1)+1)/_translift_maxfactor;
717 _lift_factor = _f_ge*_f_tl*_f_vs;
719 //store the gravity direction
720 Glue::geodUp(s->pos, _grav_direction);
723 void Rotor::findGroundEffectAltitude(Ground * ground_cb,State *s)
725 _ground_effect_altitude=findGroundEffectAltitude(ground_cb,s,
726 _groundeffectpos[0],_groundeffectpos[1],
727 _groundeffectpos[2],_groundeffectpos[3]);
728 testForRotorGroundContact(ground_cb,s);
731 void Rotor::testForRotorGroundContact(Ground * ground_cb,State *s)
734 for (i=0;i<_num_ground_contact_pos;i++)
737 s->posLocalToGlobal(_ground_contact_pos[i], pt);
739 // Ask for the ground plane in the global coordinate system
740 double global_ground[4];
742 ground_cb->getGroundPlane(pt, global_ground, global_vel);
743 // find h, the distance to the ground
744 // The ground plane transformed to the local frame.
746 s->planeGlobalToLocal(global_ground, ground);
748 h = ground[3] - Math::dot3(_ground_contact_pos[i], ground);
749 // Now h is the distance from _ground_contact_pos[i] to ground
752 _balance1 -= (-h)/_diameter/_num_ground_contact_pos;
753 _balance1 = (_balance1<-1)?-1:_balance1;
757 float Rotor::findGroundEffectAltitude(Ground * ground_cb,State *s,
758 float *pos0,float *pos1,float *pos2,float *pos3,
759 int iteration,float a0,float a1,float a2,float a3)
773 Math::add3(p[0],p[2],p[4]);
774 Math::mul3(0.5,p[4],p[4]);//the center
776 float mina=100*_diameter;
778 for (int i=0;i<5;i++)
780 if (a[i]==-1)//in the first iteration,(iteration==0) no height is
781 //passed to this function, these missing values are
785 s->posLocalToGlobal(p[i], pt);
787 // Ask for the ground plane in the global coordinate system
788 double global_ground[4];
790 ground_cb->getGroundPlane(pt, global_ground, global_vel);
791 // find h, the distance to the ground
792 // The ground plane transformed to the local frame.
794 s->planeGlobalToLocal(global_ground, ground);
796 a[i] = ground[3] - Math::dot3(p[i], ground);
797 // Now a[i] is the distance from p[i] to ground
803 if (mina>=10*_diameter)
804 return mina; //the ground effect will be zero
806 //check if further recursion is neccessary
807 //if the height does not differ more than 20%, than
808 //we can return then mean height, if not split
809 //zhe square to four parts and calcualte the height
811 //suma * 0.2 is the mean
812 //0.15 is the maximum allowed difference from the mean
813 //to the height at the center
815 ||(Math::abs(suma*0.2-a[4])<(0.15*0.2*suma*(1<<iteration))))
818 float pc[4][3],ac[4]; //pc[i]=center of pos[i] and pos[(i+1)&3]
819 for (int i=0;i<4;i++)
821 Math::add3(p[i],p[(i+1)&3],pc[i]);
822 Math::mul3(0.5,pc[i],pc[i]);
824 s->posLocalToGlobal(pc[i], pt);
826 // Ask for the ground plane in the global coordinate system
827 double global_ground[4];
829 ground_cb->getGroundPlane(pt, global_ground, global_vel);
830 // find h, the distance to the ground
831 // The ground plane transformed to the local frame.
833 s->planeGlobalToLocal(global_ground, ground);
835 ac[i] = ground[3] - Math::dot3(p[i], ground);
836 // Now ac[i] is the distance from pc[i] to ground
839 (findGroundEffectAltitude(ground_cb,s,p[0],pc[1],p[4],pc[3],
840 iteration+1,a[0],ac[0],a[4],ac[3])
841 +findGroundEffectAltitude(ground_cb,s,p[1],pc[0],p[4],pc[1],
842 iteration+1,a[1],ac[0],a[4],ac[1])
843 +findGroundEffectAltitude(ground_cb,s,p[2],pc[1],p[4],pc[2],
844 iteration+1,a[2],ac[1],a[4],ac[2])
845 +findGroundEffectAltitude(ground_cb,s,p[3],pc[2],p[4],pc[3],
846 iteration+1,a[3],ac[2],a[4],ac[3])
850 void Rotor::getDownWash(float *pos, float *v_heli, float *downwash)
852 float pos2rotor[3],tmp[3];
853 Math::sub3(_base,pos,pos2rotor);
854 float dist=Math::dot3(pos2rotor,_normal_with_yaw_roll);
855 //calculate incidence at 0.7r;
856 float inc = _collective+_twist *0.7
857 - _twist*_rel_len_where_incidence_is_measured;
860 if (dist<0) // we are not in the downwash region
862 downwash[0]=downwash[1]=downwash[2]=0.;
866 //calculate the mean downwash speed directly beneath the rotor disk
867 float v1bar = Math::sin(inc) *_omega * 0.35 * _diameter * 0.8;
868 //0.35 * d = 0.7 *r, a good position to calcualte the mean downwashd
869 //0.8 the slip of the rotor.
871 //calculate the time the wind needed from thr rotor to here
872 if (v1bar< 1) v1bar = 1;
873 float time=dist/v1bar;
875 //calculate the pos2rotor, where the rotor was, "time" ago
876 Math::mul3(time,v_heli,tmp);
877 Math::sub3(pos2rotor,tmp,pos2rotor);
879 //and again calculate dist
880 dist=Math::dot3(pos2rotor,_normal_with_yaw_roll);
881 //missing the normal is offen not pointing to the normal of the rotor
882 //disk. Rotate the normal by yaw and tilt angle
886 if (dist<0) // we are not in the downwash region
888 downwash[0]=downwash[1]=downwash[2]=0.;
891 //of course this could be done in a runge kutta integrator, but it's such
892 //a approximation that I beleave, it would'nt be more realistic
894 //calculate the dist to the rotor-axis
895 Math::cross3(pos2rotor,_normal_with_yaw_roll,tmp);
896 float r= Math::mag3(tmp);
897 //calculate incidence at r;
898 float rel_r = r *2 /_diameter;
899 float inc_r = _collective+_twist * r /_diameter * 2
900 - _twist*_rel_len_where_incidence_is_measured;
902 //calculate the downwash speed directly beneath the rotor disk
905 v1 = Math::sin(inc_r) *_omega * r * 0.8;
907 //calcualte the downwash speed in a distance "dist" to the rotor disc,
908 //for large dist. The speed is assumed do follow a gausian distribution
909 //with sigma increasing with dist^2:
910 //sigma is assumed to be half of the rotor diameter directly beneath the
911 //disc and is assumed to the rotor diameter at dist = (diameter * sqrt(2))
913 float sigma=_diameter/2 + dist * dist / _diameter /4.;
914 float v2 = v1bar*_diameter/ (Math::sqrt(2 * pi) * sigma)
915 * Math::pow(2.7183,-.5*r*r/(sigma*sigma))*_diameter/2/sigma;
917 //calculate the weight of the two downwash velocities.
918 //Directly beneath the disc it is v1, far away it is v2
919 float g = Math::pow(2.7183,-2*dist/_diameter);
920 //at dist = rotor radius it is assumed to be 1/e * v1 + (1-1/e)* v2
922 float v = g * v1 + (1-g) * v2;
923 Math::mul3(-v,_normal_with_yaw_roll,downwash);
924 //the downwash is calculated in the opposite direction of the normal
927 void Rotor::compile()
929 // Have we already been compiled?
930 if(_rotorparts.size() != 0) return;
932 //rotor is divided into _number_of_parts parts
933 //each part is calcualted at _number_of_segments points
936 //and make it a factor of 4
937 _number_of_parts=(int(Math::clamp(_number_of_parts,4,256))>>2)<<2;
939 _dynamic=_dynamic*(1/ //inverse of the time
940 ( (60/_rotor_rpm)/4 //for rotating 90 deg
941 +(60/_rotor_rpm)/(2*_number_of_blades) //+ meantime a rotorblade
942 //will pass a given point
944 float directions[5][3];
945 //pointing forward, right, ... the 5th is ony for calculation
946 directions[0][0]=_forward[0];
947 directions[0][1]=_forward[1];
948 directions[0][2]=_forward[2];
953 Math::cross3(directions[i-1],_normal,directions[i]);
955 Math::cross3(_normal,directions[i-1],directions[i]);
956 Math::unit3(directions[i],directions[i]);
958 Math::set3(directions[4],directions[0]);
959 // now directions[0] is perpendicular to the _normal.and has a length
960 // of 1. if _forward is already normalized and perpendicular to the
961 // normal, directions[0] will be the same
962 _num_ground_contact_pos=(_number_of_parts<16)?_number_of_parts:16;
963 for (i=0;i<_num_ground_contact_pos;i++)
966 float s = Math::sin(pi*2*_num_ground_contact_pos/i);
967 float c = Math::cos(pi*2*_num_ground_contact_pos/i);
968 Math::mul3(c*_diameter*0.5,directions[0],_ground_contact_pos[i]);
969 Math::mul3(s*_diameter*0.5,directions[1],help);
970 Math::add3(help,_ground_contact_pos[i],_ground_contact_pos[i]);
971 Math::add3(_base,_ground_contact_pos[i],_ground_contact_pos[i]);
975 Math::mul3(_diameter*0.7,directions[i],_groundeffectpos[i]);
976 Math::add3(_base,_groundeffectpos[i],_groundeffectpos[i]);
978 float rotorpartmass = _weight_per_blade*_number_of_blades/_number_of_parts*.453;
979 //was pounds -> now kg
981 _torque_of_inertia = 1/12. * ( _number_of_parts * rotorpartmass) * _diameter
982 * _diameter * _rel_blade_center * _rel_blade_center /(0.5*0.5);
983 float speed=_rotor_rpm/60*_diameter*_rel_blade_center*pi;
984 float lentocenter=_diameter*_rel_blade_center*0.5;
985 float lentoforceattac=_diameter*_rel_len_hinge*0.5;
986 float zentforce=rotorpartmass*speed*speed/lentocenter;
987 float pitchaforce=_force_at_pitch_a/_number_of_parts*.453*9.81;
988 // was pounds of force, now N, devided by _number_of_parts
989 //(so its now per rotorpart)
991 float torque0=0,torquemax=0,torqueb=0;
992 float omega=_rotor_rpm/60*2*pi;
994 float omega0=omega*Math::sqrt(1/(1-_rel_len_hinge));
995 float delta_theoretical=pitchaforce/(_pitch_a*omega*lentocenter*2*rotorpartmass);
996 _delta*=delta_theoretical;
998 float relamp=(omega*omega/(2*_delta*Math::sqrt(sqr(omega0*omega0-omega*omega)
999 +4*_delta*_delta*omega*omega)))*_cyclic_factor;
1000 float relamp_theoretical=(omega*omega/(2*delta_theoretical*Math::sqrt(sqr(omega0*omega0-omega*omega)
1001 +4*delta_theoretical*delta_theoretical*omega*omega)))*_cyclic_factor;
1002 _phi=Math::acos(_rel_len_hinge);
1003 _phi-=Math::atan(_delta3);
1006 torque0=_power_at_pitch_0/_number_of_parts*1000/omega;
1007 // f*r=p/w ; p=f*s/t; r=s/t/w ; r*w*t = s
1008 torqueb=_power_at_pitch_b/_number_of_parts*1000/omega;
1009 torquemax=_power_at_pitch_b/_number_of_parts*1000/omega/_pitch_b*_max_pitch;
1019 Rotorpart* rps[256];
1020 for (i=0;i<_number_of_parts;i++)
1022 float lpos[3],lforceattac[3],lspeed[3],dirzentforce[3];
1023 float s = Math::sin(2*pi*i/_number_of_parts);
1024 float c = Math::cos(2*pi*i/_number_of_parts);
1025 float sp = Math::sin(float(2*pi*i/_number_of_parts-pi/2.+_phi));
1026 float cp = Math::cos(float(2*pi*i/_number_of_parts-pi/2.+_phi));
1027 float direction[3],nextdirection[3],help[3],direction90deg[3];
1028 Math::mul3(c ,directions[0],help);
1029 Math::mul3(s ,directions[1],direction);
1030 Math::add3(help,direction,direction);
1032 Math::mul3(c ,directions[1],help);
1033 Math::mul3(s ,directions[2],direction90deg);
1034 Math::add3(help,direction90deg,direction90deg);
1036 Math::mul3(cp ,directions[1],help);
1037 Math::mul3(sp ,directions[2],nextdirection);
1038 Math::add3(help,nextdirection,nextdirection);
1040 Math::mul3(lentocenter,direction,lpos);
1041 Math::add3(lpos,_base,lpos);
1042 Math::mul3(lentoforceattac,nextdirection,lforceattac);
1043 //nextdirection: +90deg (gyro)!!!
1045 Math::add3(lforceattac,_base,lforceattac);
1046 Math::mul3(speed,direction90deg,lspeed);
1047 Math::mul3(1,nextdirection,dirzentforce);
1049 Rotorpart* rp=rps[i]=newRotorpart(lpos, lforceattac, _normal,
1050 lspeed,dirzentforce,zentforce,pitchaforce,_delta3,rotorpartmass,
1051 _translift,_rel_len_hinge,lentocenter);
1052 int k = i*4/_number_of_parts;
1053 rp->setAlphaoutput(_alphaoutput[k&1?k:(_ccw?k^2:k)],0);
1054 rp->setAlphaoutput(_alphaoutput[4+(k&1?k:(_ccw?k^2:k))],1+(k>1));
1055 _rotorparts.add(rp);
1056 rp->setTorque(torquemax,torque0);
1057 rp->setRelamp(relamp);
1058 rp->setDirectionofRotorPart(direction);
1059 rp->setTorqueOfInertia(_torque_of_inertia/_number_of_parts);
1060 rp->setDirection(2*pi*i/_number_of_parts);
1062 for (i=0;i<_number_of_parts;i++)
1064 rps[i]->setlastnextrp(rps[(i-1+_number_of_parts)%_number_of_parts],
1065 rps[(i+1)%_number_of_parts],
1066 rps[(i+_number_of_parts/2)%_number_of_parts],
1067 rps[(i-_number_of_parts/4+_number_of_parts)%_number_of_parts],
1068 rps[(i+_number_of_parts/4)%_number_of_parts]);
1070 for (i=0;i<_number_of_parts;i++)
1072 rps[i]->setCompiled();
1074 float lift[4],torque[4], v_wind[3];
1075 v_wind[0]=v_wind[1]=v_wind[2]=0;
1076 rps[0]->setOmega(_omegan);
1078 if (_airfoil_lift_coefficient==0)
1080 //calculate the lift and drag coefficients now
1084 rps[0]->calculateAlpha(v_wind,rho_null,_pitch_a,0,0,
1085 &(torque[0]),&(lift[0])); //max_pitch a
1086 _liftcoef = pitchaforce/lift[0];
1089 rps[0]->calculateAlpha(v_wind,rho_null,0,0,0,&(torque[0]),&(lift[0]));
1094 rps[0]->calculateAlpha(v_wind,rho_null,0,0,0,&(torque[1]),&(lift[1]));
1099 rps[0]->calculateAlpha(v_wind,rho_null,_pitch_b,0,0,&(torque[2]),&(lift[2]));
1104 rps[0]->calculateAlpha(v_wind,rho_null,_pitch_b,0,0,&(torque[3]),&(lift[3]));
1109 _dragcoef1=torque0/torque[1];
1110 _dragcoef0=(torqueb-_dragcoef1*torque[3])/torque[2];
1114 _dragcoef1=(torque0/torque[0]-torqueb/torque[2])
1115 /(torque[1]/torque[0]-torque[3]/torque[2]);
1116 _dragcoef0=(torqueb-_dragcoef1*torque[3])/torque[2];
1121 _liftcoef=_airfoil_lift_coefficient/_number_of_parts*_number_of_blades;
1122 _dragcoef0=_airfoil_drag_coefficient0/_number_of_parts*_number_of_blades*_c2;
1123 _dragcoef1=_airfoil_drag_coefficient1/_number_of_parts*_number_of_blades*_c2;
1127 rps[0]->calculateAlpha(v_wind,rho_null,_pitch_a,0,0,
1128 &(torque[0]),&(lift[0])); //pitch a
1129 rps[0]->calculateAlpha(v_wind,rho_null,_pitch_b,0,0,
1130 &(torque[1]),&(lift[1])); //pitch b
1131 rps[0]->calculateAlpha(v_wind,rho_null,0,0,0,
1132 &(torque[3]),&(lift[3])); //pitch 0
1133 SG_LOG(SG_GENERAL, SG_INFO,
1134 "Rotor: coefficients for airfoil:" << endl << setprecision(6)
1135 << " drag0: " << _dragcoef0*_number_of_parts/_number_of_blades/_c2
1136 << " drag1: " << _dragcoef1*_number_of_parts/_number_of_blades/_c2
1137 << " lift: " << _liftcoef*_number_of_parts/_number_of_blades
1139 << "at 10 deg:" << endl
1140 << "drag: " << (Math::sin(10./180*pi)*_dragcoef1+_dragcoef0)
1141 *_number_of_parts/_number_of_blades/_c2
1142 << " lift: " << Math::sin(10./180*pi)*_liftcoef*_number_of_parts/_number_of_blades
1144 << "Some results (Pitch [degree], Power [kW], Lift [N])" << endl
1145 << 0.0f << "deg " << Math::abs(torque[3]*_number_of_parts*_omegan/1000) << "kW "
1146 << lift[3]*_number_of_parts << endl
1147 << _pitch_a*180/pi << "deg " << Math::abs(torque[0]*_number_of_parts*_omegan/1000)
1148 << "kW " << lift[0]*_number_of_parts << endl
1149 << _pitch_b*180/pi << "deg " << Math::abs(torque[1]*_number_of_parts*_omegan/1000)
1150 << "kW " << lift[1]*_number_of_parts << endl << endl );
1152 //first calculation of relamp is wrong
1153 //it used pitchaforce, but this was unknown and
1154 //on the default value
1155 _delta*=lift[0]/pitchaforce;
1156 relamp=(omega*omega/(2*_delta*Math::sqrt(sqr(omega0*omega0-omega*omega)
1157 +4*_delta*_delta*omega*omega)))*_cyclic_factor;
1158 for (i=0;i<_number_of_parts;i++)
1160 rps[i]->setRelamp(relamp);
1162 rps[0]->setOmega(0);
1165 //tie the properties
1166 /* After reset these values are totally wrong. I have to find out why
1167 SGPropertyNode * node = fgGetNode("/rotors", true)->getNode(_name,true);
1168 node->tie("balance_ext",SGRawValuePointer<float>(&_balance2),false);
1169 node->tie("balance_int",SGRawValuePointer<float>(&_balance1));
1173 std::ostream & operator<<(std::ostream & out, Rotor& r)
1175 #define i(x) << #x << ":" << r.x << endl
1176 #define iv(x) << #x << ":" << r.x[0] << ";" << r.x[1] << ";" <<r.x[2] << ";" << endl
1177 out << "Writing Info on Rotor "
1180 i(_omega) i(_omegan) i(_omegarel) i(_ddt_omega) i(_omegarelneu)
1183 i( _airfoil_incidence_no_lift)
1185 i( _airfoil_lift_coefficient)
1186 i( _airfoil_drag_coefficient0)
1187 i( _airfoil_drag_coefficient1)
1189 i( _number_of_segments)
1190 i( _number_of_parts)
1192 iv( _groundeffectpos[0])iv( _groundeffectpos[1])iv( _groundeffectpos[2])iv( _groundeffectpos[3])
1193 i( _ground_effect_altitude)
1195 iv( _normal_with_yaw_roll)
1198 i( _number_of_blades)
1199 i( _weight_per_blade)
1200 i( _rel_blade_center)
1203 i( _force_at_pitch_a)
1205 i( _power_at_pitch_0)
1206 i( _power_at_pitch_b)
1223 i( _alphamin) i(_alphamax) i(_alpha0) i(_alpha0factor)
1224 i( _teeterdamp) i(_maxteeterdamp)
1225 i( _rellenteeterhinge)
1227 i( _translift_maxfactor)
1228 i( _ground_effect_constant)
1229 i( _vortex_state_lift_factor)
1230 i( _vortex_state_c1)
1231 i( _vortex_state_c2)
1232 i( _vortex_state_c3)
1233 i( _vortex_state_e1)
1234 i( _vortex_state_e2)
1235 i( _vortex_state_e3)
1236 i( _lift_factor) i(_f_ge) i(_f_vs) i(_f_tl)
1241 i( _twist) //outer incidence = inner inner incidence + _twist
1242 i( _rel_len_where_incidence_is_measured)
1243 i( _torque_of_inertia)
1244 i( _rel_len_blade_start)
1245 i( _incidence_stall_zero_speed)
1246 i( _incidence_stall_half_sonic_speed)
1247 i( _lift_factor_stall)
1248 i( _stall_change_over)
1249 i( _drag_factor_stall)
1256 i( _cyclic_factor) <<endl;
1258 for(j=0; j<r._rotorparts.size(); j++) {
1259 out << *((Rotorpart*)r._rotorparts.get(j));
1266 void Rotor:: writeInfo()
1269 std::ostringstream buffer;
1271 FILE*f=fopen("c:\\fgmsvc\\bat\\log.txt","at");
1272 if (!f) f=fopen("c:\\fgmsvc\\bat\\log.txt","wt");
1275 fprintf(f,"%s",(const char *)buffer.str().c_str());
1280 Rotorpart* Rotor::newRotorpart(float* pos, float *posforceattac, float *normal,
1281 float* speed,float *dirzentforce, float zentforce,float maxpitchforce,
1282 float delta3,float mass,float translift,float rellenhinge,float len)
1284 Rotorpart *r = new Rotorpart();
1285 r->setPosition(pos);
1286 r->setNormal(normal);
1287 r->setZentipetalForce(zentforce);
1288 r->setPositionForceAttac(posforceattac);
1290 r->setDirectionofZentipetalforce(dirzentforce);
1291 r->setDelta3(delta3);
1292 r->setDynamic(_dynamic);
1293 r->setTranslift(_translift);
1296 r->setRelLenHinge(rellenhinge);
1297 r->setSharedFlapHinge(_shared_flap_hinge);
1298 r->setOmegaN(_omegan);
1299 r->setPhi(_phi_null);
1300 r->setAlpha0(_alpha0);
1301 r->setAlphamin(_alphamin);
1302 r->setAlphamax(_alphamax);
1303 r->setAlpha0factor(_alpha0factor);
1305 r->setDiameter(_diameter);
1307 #define p(a) r->setParameter(#a,_##a);
1309 p(number_of_segments)
1310 p(rel_len_where_incidence_is_measured)
1311 p(rel_len_blade_start)
1312 p(rotor_correction_factor)
1317 void Rotor::interp(float* v1, float* v2, float frac, float* out)
1319 out[0] = v1[0] + frac*(v2[0]-v1[0]);
1320 out[1] = v1[1] + frac*(v2[1]-v1[1]);
1321 out[2] = v1[2] + frac*(v2[2]-v1[2]);
1324 void Rotorgear::initRotorIteration(float *lrot,float dt)
1328 if (!_rotors.size()) return;
1329 Rotor* r0 = (Rotor*)_rotors.get(0);
1330 omegarel= r0->getOmegaRelNeu();
1331 for(i=0; i<_rotors.size(); i++) {
1332 Rotor* r = (Rotor*)_rotors.get(i);
1333 r->inititeration(dt,omegarel,0,lrot);
1337 void Rotorgear::calcForces(float* torqueOut)
1340 torqueOut[0]=torqueOut[1]=torqueOut[2]=0;
1341 // check,<if the engine can handle the torque of the rotors.
1342 // If not reduce the torque to the fueselage and change rotational
1343 // speed of the rotors instead
1346 float omegarel,omegan;
1347 Rotor* r0 = (Rotor*)_rotors.get(0);
1348 omegarel= r0->getOmegaRel();
1350 float total_torque_of_inertia=0;
1351 float total_torque=0;
1352 for(i=0; i<_rotors.size(); i++) {
1353 Rotor* r = (Rotor*)_rotors.get(i);
1354 omegan=r->getOmegan();
1355 total_torque_of_inertia+=r->getTorqueOfInertia()*omegan*omegan;
1356 //FIXME: this is constant, so this can be done in compile
1358 total_torque+=r->getTorque()*omegan;
1360 float max_torque_of_engine=0;
1363 max_torque_of_engine=_max_power_engine;
1364 float df=1-omegarel;
1365 df/=_engine_prop_factor;
1366 df = Math::clamp(df, 0, 1);
1367 max_torque_of_engine = df * _max_power_engine;
1371 float rel_torque_engine=1;
1372 if (total_torque<=0)
1373 rel_torque_engine=0;
1375 if (max_torque_of_engine>0)
1376 rel_torque_engine=1/max_torque_of_engine*total_torque;
1378 rel_torque_engine=0;
1380 //add the rotor brake and the gear fritcion
1382 if (r0->_rotorparts.size()) dt=((Rotorpart*)r0->_rotorparts.get(0))->getDt();
1384 float rotor_brake_torque;
1385 rotor_brake_torque=_rotorbrake*_max_power_rotor_brake+_rotorgear_friction;
1386 //clamp it to the value you need to stop the rotor
1387 //to avod accelerate the rotor to neagtive rpm:
1388 rotor_brake_torque=Math::clamp(rotor_brake_torque,0,
1389 total_torque_of_inertia/dt*omegarel);
1390 max_torque_of_engine-=rotor_brake_torque;
1392 //change the rotation of the rotors
1393 if ((max_torque_of_engine<total_torque) //decreasing rotation
1394 ||((max_torque_of_engine>total_torque)&&(omegarel<1))
1395 //increasing rotation due to engine
1396 ||(total_torque<0) ) //increasing rotation due to autorotation
1398 _ddt_omegarel=(max_torque_of_engine-total_torque)/total_torque_of_inertia;
1399 if(max_torque_of_engine>total_torque)
1401 //check if the acceleration is due to the engine. If yes,
1402 //the engine self limits the accel.
1403 float lim1=-total_torque/total_torque_of_inertia;
1404 //accel. by autorotation
1406 if (lim1<_engine_accel_limit) lim1=_engine_accel_limit;
1407 //if the accel by autorotation greater than the max. engine
1408 //accel, then this is the limit, if not: the engine is the limit
1409 if (_ddt_omegarel>lim1) _ddt_omegarel=lim1;
1411 if (_ddt_omegarel>5.5)_ddt_omegarel=5.5;
1412 //clamp it to avoid overflow. Should never be reached
1413 if (_ddt_omegarel<-5.5)_ddt_omegarel=-5.5;
1415 if (_max_power_engine<0.001) {omegarel=1;_ddt_omegarel=0;}
1416 //for debug: negative or no maxpower will result
1417 //in permanet 100% rotation
1419 omegarel+=dt*_ddt_omegarel;
1421 if (omegarel>2.5) omegarel=2.5;
1422 //clamp it to avoid overflow. Should never be reached
1423 if (omegarel<-.5) omegarel=-.5;
1425 r0->setOmegaRelNeu(omegarel);
1426 //calculate the torque, which is needed to accelerate the rotors.
1427 //Add this additional torque to the body
1428 for(j=0; j<_rotors.size(); j++) {
1429 Rotor* r = (Rotor*)_rotors.get(j);
1430 for(i=0; i<r->_rotorparts.size(); i++) {
1431 float torque_scalar=0;
1432 Rotorpart* rp = (Rotorpart*)r->_rotorparts.get(i);
1434 rp->getAccelTorque(_ddt_omegarel,torque);
1435 Math::add3(torque,torqueOut,torqueOut);
1439 _total_torque_on_engine=total_torque+_ddt_omegarel*total_torque_of_inertia;
1443 void Rotorgear::addRotor(Rotor* rotor)
1449 void Rotorgear::compile()
1452 for(int j=0; j<_rotors.size(); j++) {
1453 Rotor* r = (Rotor*)_rotors.get(j);
1458 void Rotorgear::getDownWash(float *pos, float * v_heli, float *downwash)
1461 downwash[0]=downwash[1]=downwash[2]=0;
1462 for(int i=0; i<_rotors.size(); i++) {
1463 Rotor* ro = (Rotor*)_rotors.get(i);
1464 ro->getDownWash(pos,v_heli,tmp);
1465 Math::add3(downwash,tmp,downwash); // + downwash
1469 void Rotorgear::setParameter(char *parametername, float value)
1471 #define p(a,b) if (strcmp(parametername,#a)==0) _##a = (value * (b)); else
1472 p(max_power_engine,1000)
1473 p(engine_prop_factor,1)
1474 p(yasimdragfactor,1)
1475 p(yasimliftfactor,1)
1476 p(max_power_rotor_brake,1000)
1477 p(rotorgear_friction,1000)
1478 p(engine_accel_limit,0.01)
1479 SG_LOG(SG_INPUT, SG_ALERT,
1480 "internal error in parameter set up for rotorgear: '"
1481 << parametername <<"'" << endl);
1484 int Rotorgear::getValueforFGSet(int j,char *text,float *f)
1488 sprintf(text,"/rotors/gear/total-torque");
1489 *f=_total_torque_on_engine;
1493 Rotorgear::Rotorgear()
1498 _max_power_rotor_brake=1;
1499 _rotorgear_friction=1;
1500 _max_power_engine=1000*450;
1501 _engine_prop_factor=0.05f;
1505 _engine_accel_limit=0.05f;
1506 _total_torque_on_engine=0;
1509 Rotorgear::~Rotorgear()
1511 for(int i=0; i<_rotors.size(); i++)
1512 delete (Rotor*)_rotors.get(i);
1515 }; // namespace yasim