1 #include <simgear/debug/logstream.hxx>
5 #include "Rotorpart.hpp"
12 SG_USING_STD(setprecision);
25 const float pi=3.14159;
27 static inline float sqr(float a) { return a * a; }
44 _base[0] = _base[1] = _base[2] = 0;
53 _forward[1]=_forward[2]=0;
54 _max_pitch=13./180*pi;
55 _maxcyclicail=10./180*pi;
56 _maxcyclicele=10./180*pi;
58 _mincyclicail=-10./180*pi;
59 _mincyclicele=-10./180*pi;
60 _min_pitch=-.5/180*pi;
64 _normal[0] = _normal[1] = 0;
66 _normal_with_yaw_roll[0]= _normal_with_yaw_roll[1]=0;
67 _normal_with_yaw_roll[2]=1;
69 _omega=_omegan=_omegarel=_omegarelneu=0;
78 _rellenteeterhinge=0.01;
85 _translift_maxfactor=1.3;
86 _ground_effect_constant=0.1;
87 _vortex_state_lift_factor=0.4;
100 _number_of_segments=1;
102 _rel_len_where_incidence_is_measured=0.7;
103 _torque_of_inertia=1;
107 _airfoil_incidence_no_lift=0;
108 _rel_len_blade_start=0;
109 _airfoil_lift_coefficient=0;
110 _airfoil_drag_coefficient0=0;
111 _airfoil_drag_coefficient1=0;
113 _global_ground[i] = 0;
114 _global_ground[2] = 1;
115 _global_ground[3] = -1e3;
116 _incidence_stall_zero_speed=18*pi/180.;
117 _incidence_stall_half_sonic_speed=14*pi/180.;
118 _lift_factor_stall=0.28;
119 _stall_change_over=2*pi/180.;
120 _drag_factor_stall=8;
125 for (int k=0;k<4;k++)
127 _groundeffectpos[k][i]=0;
128 _ground_effect_altitude=1;
130 _lift_factor=_f_ge=_f_vs=_f_tl=1;
131 _rotor_correction_factor=.65;
137 for(i=0; i<_rotorparts.size(); i++) {
138 Rotorpart* r = (Rotorpart*)_rotorparts.get(i);
143 void Rotor::inititeration(float dt,float omegarel,float ddt_omegarel,float *rot)
148 _omega=_omegan*_omegarel;
149 _ddt_omega=_omegan*ddt_omegarel;
151 for(i=0; i<_rotorparts.size(); i++) {
152 float s = Math::sin(2*pi*i/_number_of_parts);
153 float c = Math::cos(2*pi*i/_number_of_parts);
154 Rotorpart* r = (Rotorpart*)_rotorparts.get(i);
156 r->setDdtOmega(_ddt_omega);
157 r->inititeration(dt,rot);
158 r->setCyclic(_cyclicail*c+_cyclicele*s);
161 //calculate the normal of the rotor disc, for calcualtion of the downwash
162 float side[3],help[3];
163 Math::cross3(_normal,_forward,side);
164 Math::mul3(Math::cos(_yaw)*Math::cos(_roll),_normal,_normal_with_yaw_roll);
166 Math::mul3(Math::sin(_yaw),_forward,help);
167 Math::add3(_normal_with_yaw_roll,help,_normal_with_yaw_roll);
169 Math::mul3(Math::sin(_roll),side,help);
170 Math::add3(_normal_with_yaw_roll,help,_normal_with_yaw_roll);
173 float Rotor::calcStall(float incidence,float speed)
175 float stall_incidence=_incidence_stall_zero_speed
176 +(_incidence_stall_half_sonic_speed
177 -_incidence_stall_zero_speed)*speed/(343./2);
178 //missing: Temeperature dependency of sonic speed
179 incidence = Math::abs(incidence);
180 if (incidence > (90./180.*pi))
181 incidence = pi-incidence;
182 float stall = (incidence-stall_incidence)/_stall_change_over;
183 stall = Math::clamp(stall,0,1);
185 _stall_sum+=stall*speed*speed;
186 _stall_v2sum+=speed*speed;
191 float Rotor::getLiftCoef(float incidence,float speed)
193 float stall=calcStall(incidence,speed);
194 /* the next shold look like this, but this is the inner loop of
195 the rotor simulation. For small angles (and we hav only small
196 angles) the first order approximation works well
197 float c1= Math::sin(incidence-_airfoil_incidence_no_lift)*_liftcoef;
198 for c2 we would need higher order, because in stall the angle can be large
201 if (incidence > (pi/2))
203 else if (incidence <-(pi/2))
207 float c1= (i2-_airfoil_incidence_no_lift)*_liftcoef;
210 float c2= Math::sin(2*(incidence-_airfoil_incidence_no_lift))
211 *_liftcoef*_lift_factor_stall;
212 return (1-stall)*c1 + stall *c2;
218 float Rotor::getDragCoef(float incidence,float speed)
220 float stall=calcStall(incidence,speed);
221 float c1= (Math::abs(Math::sin(incidence-_airfoil_incidence_no_lift))
222 *_dragcoef1+_dragcoef0);
223 float c2= c1*_drag_factor_stall;
224 return (1-stall)*c1 + stall *c2;
227 int Rotor::getValueforFGSet(int j,char *text,float *f)
229 if (_name[0]==0) return 0;
230 if (4>numRotorparts()) return 0; //compile first!
233 sprintf(text,"/rotors/%s/cone", _name);
234 *f=( ((Rotorpart*)getRotorpart(0))->getrealAlpha()
235 +((Rotorpart*)getRotorpart(1*(_number_of_parts>>2)))->getrealAlpha()
236 +((Rotorpart*)getRotorpart(2*(_number_of_parts>>2)))->getrealAlpha()
237 +((Rotorpart*)getRotorpart(3*(_number_of_parts>>2)))->getrealAlpha()
243 sprintf(text,"/rotors/%s/roll", _name);
244 _roll = ( ((Rotorpart*)getRotorpart(0))->getrealAlpha()
245 -((Rotorpart*)getRotorpart(2*(_number_of_parts>>2)))->getrealAlpha()
252 sprintf(text,"/rotors/%s/yaw", _name);
253 _yaw=( ((Rotorpart*)getRotorpart(1*(_number_of_parts>>2)))->getrealAlpha()
254 -((Rotorpart*)getRotorpart(3*(_number_of_parts>>2)))->getrealAlpha()
261 sprintf(text,"/rotors/%s/rpm", _name);
267 sprintf(text,"/rotors/%s/debug/debugfge",_name);
272 sprintf(text,"/rotors/%s/debug/debugfvs",_name);
277 sprintf(text,"/rotors/%s/debug/debugftl",_name);
282 sprintf(text,"/rotors/%s/debug/vortexstate",_name);
287 sprintf(text,"/rotors/%s/stall",_name);
288 *f=getOverallStall();
292 sprintf(text,"/rotors/%s/torque",_name);
298 if (b>=_number_of_blades)
303 sprintf(text,"/rotors/%s/blade%i_%s",
305 w==0?"pos":(w==1?"flap":"incidence"));
306 *f=((Rotorpart*)getRotorpart(0))->getPhi()*180/pi
307 +360*b/_number_of_blades*(_ccw?1:-1);
316 rk=Math::clamp(rk,0,1);//Delete this
318 if(w==2) {k+=2;l+=2;}
320 if(w==1) {k+=1;l+=1;}
323 if (w==1) *f=rk*((Rotorpart*) getRotorpart(k*(_number_of_parts>>2)))->getrealAlpha()*180/pi
324 +rl*((Rotorpart*) getRotorpart(l*(_number_of_parts>>2)))->getrealAlpha()*180/pi;
325 else if(w==2) *f=rk*((Rotorpart*)getRotorpart(k*(_number_of_parts>>2)))->getIncidence()*180/pi
326 +rl*((Rotorpart*)getRotorpart(l*(_number_of_parts>>2)))->getIncidence()*180/pi;
331 void Rotorgear::setEngineOn(int value)
336 void Rotor::setAlpha0(float f)
341 void Rotor::setAlphamin(float f)
346 void Rotor::setAlphamax(float f)
351 void Rotor::setAlpha0factor(float f)
356 int Rotor::numRotorparts()
358 return _rotorparts.size();
361 Rotorpart* Rotor::getRotorpart(int n)
363 return ((Rotorpart*)_rotorparts.get(n));
366 int Rotorgear::getEngineon()
371 float Rotor::getTorqueOfInertia()
373 return _torque_of_inertia;
376 void Rotor::setTorque(float f)
381 void Rotor::addTorque(float f)
386 void Rotor::strncpy(char *dest,const char *src,int maxlen)
389 while(src[n]&&n<(maxlen-1))
397 void Rotor::setNormal(float* normal)
400 float invsum,sqrsum=0;
401 for(i=0; i<3; i++) { sqrsum+=normal[i]*normal[i];}
403 invsum=1/Math::sqrt(sqrsum);
408 _normal_with_yaw_roll[i]=_normal[i] = normal[i]*invsum;
412 void Rotor::setForward(float* forward)
415 float invsum,sqrsum=0;
416 for(i=0; i<3; i++) { sqrsum+=forward[i]*forward[i];}
418 invsum=1/Math::sqrt(sqrsum);
421 for(i=0; i<3; i++) { _forward[i] = forward[i]*invsum; }
424 void Rotor::setForceAtPitchA(float force)
426 _force_at_pitch_a=force;
429 void Rotor::setPowerAtPitch0(float value)
431 _power_at_pitch_0=value;
434 void Rotor::setPowerAtPitchB(float value)
436 _power_at_pitch_b=value;
439 void Rotor::setPitchA(float value)
441 _pitch_a=value/180*pi;
444 void Rotor::setPitchB(float value)
446 _pitch_b=value/180*pi;
449 void Rotor::setBase(float* base)
452 for(i=0; i<3; i++) _base[i] = base[i];
455 void Rotor::setMaxCyclicail(float value)
457 _maxcyclicail=value/180*pi;
460 void Rotor::setMaxCyclicele(float value)
462 _maxcyclicele=value/180*pi;
465 void Rotor::setMinCyclicail(float value)
467 _mincyclicail=value/180*pi;
470 void Rotor::setMinCyclicele(float value)
472 _mincyclicele=value/180*pi;
475 void Rotor::setMinCollective(float value)
477 _min_pitch=value/180*pi;
480 void Rotor::setMaxCollective(float value)
482 _max_pitch=value/180*pi;
485 void Rotor::setDiameter(float value)
490 void Rotor::setWeightPerBlade(float value)
492 _weight_per_blade=value;
495 void Rotor::setNumberOfBlades(float value)
497 _number_of_blades=int(value+.5);
500 void Rotor::setRelBladeCenter(float value)
502 _rel_blade_center=value;
505 void Rotor::setDynamic(float value)
510 void Rotor::setDelta3(float value)
515 void Rotor::setDelta(float value)
520 void Rotor::setTranslift(float value)
525 void Rotor::setC2(float value)
530 void Rotor::setStepspersecond(float steps)
532 _stepspersecond=steps;
535 void Rotor::setRPM(float value)
540 void Rotor::setRelLenHinge(float value)
542 _rel_len_hinge=value;
545 void Rotor::setAlphaoutput(int i, const char *text)
547 strncpy(_alphaoutput[i],text,255);
550 void Rotor::setName(const char *text)
552 strncpy(_name,text,256);//256: some space needed for settings
555 void Rotor::setCcw(int ccw)
560 void Rotor::setNotorque(int value)
565 void Rotor::setRelLenTeeterHinge(float f)
567 _rellenteeterhinge=f;
570 void Rotor::setTeeterdamp(float f)
575 void Rotor::setMaxteeterdamp(float f)
580 void Rotor::setGlobalGround(double *global_ground, float* global_vel)
583 for(i=0; i<4; i++) _global_ground[i] = global_ground[i];
586 void Rotor::setParameter(char *parametername, float value)
588 #define p(a,b) if (strcmp(parametername,#a)==0) _##a = (value * (b)); else
590 p(translift_maxfactor,1)
591 p(ground_effect_constant,1)
592 p(vortex_state_lift_factor,1)
600 p(number_of_segments,1)
602 p(rel_len_where_incidence_is_measured,1)
605 p(airfoil_incidence_no_lift,pi/180.)
606 p(rel_len_blade_start,1)
607 p(incidence_stall_zero_speed,pi/180.)
608 p(incidence_stall_half_sonic_speed,pi/180.)
609 p(lift_factor_stall,1)
610 p(stall_change_over,pi/180.)
611 p(drag_factor_stall,1)
612 p(airfoil_lift_coefficient,1)
613 p(airfoil_drag_coefficient0,1)
614 p(airfoil_drag_coefficient1,1)
616 p(rotor_correction_factor,1)
617 SG_LOG(SG_INPUT, SG_ALERT,
618 "internal error in parameter set up for rotor: '" <<
619 parametername <<"'" << endl);
623 float Rotor::getLiftFactor()
628 void Rotorgear::setRotorBrake(float lval)
630 lval = Math::clamp(lval, 0, 1);
634 void Rotor::setCollective(float lval)
636 lval = Math::clamp(lval, -1, 1);
638 for(i=0; i<_rotorparts.size(); i++) {
639 ((Rotorpart*)_rotorparts.get(i))->setCollective(lval);
641 _collective=_min_pitch+(lval+1)/2*(_max_pitch-_min_pitch);
644 void Rotor::setCyclicele(float lval,float rval)
646 lval = Math::clamp(lval, -1, 1);
647 _cyclicele=_mincyclicele+(lval+1)/2*(_maxcyclicele-_mincyclicele);
650 void Rotor::setCyclicail(float lval,float rval)
652 lval = Math::clamp(lval, -1, 1);
654 _cyclicail=-(_mincyclicail+(lval+1)/2*(_maxcyclicail-_mincyclicail));
657 void Rotor::getPosition(float* out)
660 for(i=0; i<3; i++) out[i] = _base[i];
663 void Rotor::calcLiftFactor(float* v, float rho, State *s)
665 //calculates _lift_factor, which is a foactor for the lift of the rotor
667 //- ground effect (_f_ge)
668 //- vortex state (_f_vs)
669 //- translational lift (_f_tl)
674 // Calculate ground effect
675 _f_ge=1+_diameter/_ground_effect_altitude*_ground_effect_constant;
677 // Now calculate translational lift
678 float v_vert = Math::dot3(v,_normal);
680 Math::cross3(v,_normal,help);
681 float v_horiz = Math::mag3(help);
682 _f_tl = ((1-Math::pow(2.7183,-v_horiz/_translift_ve))
683 *(_translift_maxfactor-1)+1)/_translift_maxfactor;
685 _lift_factor = _f_ge*_f_tl*_f_vs;
688 void Rotor::findGroundEffectAltitude(Ground * ground_cb,State *s)
690 _ground_effect_altitude=findGroundEffectAltitude(ground_cb,s,
691 _groundeffectpos[0],_groundeffectpos[1],
692 _groundeffectpos[2],_groundeffectpos[3]);
695 float Rotor::findGroundEffectAltitude(Ground * ground_cb,State *s,
696 float *pos0,float *pos1,float *pos2,float *pos3,
697 int iteration,float a0,float a1,float a2,float a3)
711 Math::add3(p[0],p[2],p[4]);
712 Math::mul3(0.5,p[4],p[4]);//the center
714 float mina=100*_diameter;
716 for (int i=0;i<5;i++)
718 if (a[i]==-1)//in the first iteration,(iteration==0) no height is
719 //passed to this function, these missing values are
723 s->posLocalToGlobal(p[i], pt);
725 // Ask for the ground plane in the global coordinate system
726 double global_ground[4];
728 ground_cb->getGroundPlane(pt, global_ground, global_vel);
729 // find h, the distance to the ground
730 // The ground plane transformed to the local frame.
732 s->planeGlobalToLocal(global_ground, ground);
734 a[i] = ground[3] - Math::dot3(p[i], ground);
735 // Now a[i] is the distance from p[i] to ground
741 if (mina>=10*_diameter)
742 return mina; //the ground effect will be zero
744 //check if further recursion is neccessary
745 //if the height does not differ more than 20%, than
746 //we can return then mean height, if not split
747 //zhe square to four parts and calcualte the height
749 //suma * 0.2 is the mean
750 //0.15 is the maximum allowed difference from the mean
751 //to the height at the center
753 ||(Math::abs(suma*0.2-a[4])<(0.15*0.2*suma*(1<<iteration))))
756 float pc[4][3],ac[4]; //pc[i]=center of pos[i] and pos[(i+1)&3]
757 for (int i=0;i<4;i++)
759 Math::add3(p[i],p[(i+1)&3],pc[i]);
760 Math::mul3(0.5,pc[i],pc[i]);
762 s->posLocalToGlobal(pc[i], pt);
764 // Ask for the ground plane in the global coordinate system
765 double global_ground[4];
767 ground_cb->getGroundPlane(pt, global_ground, global_vel);
768 // find h, the distance to the ground
769 // The ground plane transformed to the local frame.
771 s->planeGlobalToLocal(global_ground, ground);
773 ac[i] = ground[3] - Math::dot3(p[i], ground);
774 // Now ac[i] is the distance from pc[i] to ground
777 (findGroundEffectAltitude(ground_cb,s,p[0],pc[1],p[4],pc[3],
778 iteration+1,a[0],ac[0],a[4],ac[3])
779 +findGroundEffectAltitude(ground_cb,s,p[1],pc[0],p[4],pc[1],
780 iteration+1,a[1],ac[0],a[4],ac[1])
781 +findGroundEffectAltitude(ground_cb,s,p[2],pc[1],p[4],pc[2],
782 iteration+1,a[2],ac[1],a[4],ac[2])
783 +findGroundEffectAltitude(ground_cb,s,p[3],pc[2],p[4],pc[3],
784 iteration+1,a[3],ac[2],a[4],ac[3])
788 void Rotor::getDownWash(float *pos, float *v_heli, float *downwash)
790 float pos2rotor[3],tmp[3];
791 Math::sub3(_base,pos,pos2rotor);
792 float dist=Math::dot3(pos2rotor,_normal_with_yaw_roll);
793 //calculate incidence at 0.7r;
794 float inc = _collective+_twist *0.7
795 - _twist*_rel_len_where_incidence_is_measured;
798 if (dist<0) // we are not in the downwash region
800 downwash[0]=downwash[1]=downwash[2]=0.;
804 //calculate the mean downwash speed directly beneath the rotor disk
805 float v1bar = Math::sin(inc) *_omega * 0.35 * _diameter * 0.8;
806 //0.35 * d = 0.7 *r, a good position to calcualte the mean downwashd
807 //0.8 the slip of the rotor.
809 //calculate the time the wind needed from thr rotor to here
810 if (v1bar< 1) v1bar = 1;
811 float time=dist/v1bar;
813 //calculate the pos2rotor, where the rotor was, "time" ago
814 Math::mul3(time,v_heli,tmp);
815 Math::sub3(pos2rotor,tmp,pos2rotor);
817 //and again calculate dist
818 dist=Math::dot3(pos2rotor,_normal_with_yaw_roll);
819 //missing the normal is offen not pointing to the normal of the rotor
820 //disk. Rotate the normal by yaw and tilt angle
824 if (dist<0) // we are not in the downwash region
826 downwash[0]=downwash[1]=downwash[2]=0.;
829 //of course this could be done in a runge kutta integrator, but it's such
830 //a approximation that I beleave, it would'nt be more realistic
832 //calculate the dist to the rotor-axis
833 Math::cross3(pos2rotor,_normal_with_yaw_roll,tmp);
834 float r= Math::mag3(tmp);
835 //calculate incidence at r;
836 float rel_r = r *2 /_diameter;
837 float inc_r = _collective+_twist * r /_diameter * 2
838 - _twist*_rel_len_where_incidence_is_measured;
840 //calculate the downwash speed directly beneath the rotor disk
843 v1 = Math::sin(inc_r) *_omega * r * 0.8;
845 //calcualte the downwash speed in a distance "dist" to the rotor disc,
846 //for large dist. The speed is assumed do follow a gausian distribution
847 //with sigma increasing with dist^2:
848 //sigma is assumed to be half of the rotor diameter directly beneath the
849 //disc and is assumed to the rotor diameter at dist = (diameter * sqrt(2))
851 float sigma=_diameter/2 + dist * dist / _diameter /4.;
852 float v2 = v1bar*_diameter/ (Math::sqrt(2 * pi) * sigma)
853 * Math::pow(2.7183,-.5*r*r/(sigma*sigma))*_diameter/2/sigma;
855 //calculate the weight of the two downwash velocities.
856 //Directly beneath the disc it is v1, far away it is v2
857 float g = Math::pow(2.7183,-2*dist/_diameter);
858 //at dist = rotor radius it is assumed to be 1/e * v1 + (1-1/e)* v2
860 float v = g * v1 + (1-g) * v2;
861 Math::mul3(-v,_normal_with_yaw_roll,downwash);
862 //the downwash is calculated in the opposite direction of the normal
865 void Rotor::compile()
867 // Have we already been compiled?
868 if(_rotorparts.size() != 0) return;
870 //rotor is divided into _number_of_parts parts
871 //each part is calcualted at _number_of_segments points
874 //and make it a factor of 4
875 _number_of_parts=(int(Math::clamp(_number_of_parts,4,256))>>2)<<2;
877 _dynamic=_dynamic*(1/ //inverse of the time
878 ( (60/_rotor_rpm)/4 //for rotating 90 deg
879 +(60/_rotor_rpm)/(2*_number_of_blades) //+ meantime a rotorblade
880 //will pass a given point
882 float directions[5][3];
883 //pointing forward, right, ... the 5th is ony for calculation
884 directions[0][0]=_forward[0];
885 directions[0][1]=_forward[1];
886 directions[0][2]=_forward[2];
891 Math::cross3(directions[i-1],_normal,directions[i]);
893 Math::cross3(_normal,directions[i-1],directions[i]);
894 Math::unit3(directions[i],directions[i]);
896 Math::set3(directions[4],directions[0]);
899 Math::mul3(_diameter*0.7,directions[i],_groundeffectpos[i]);
900 Math::add3(_base,_groundeffectpos[i],_groundeffectpos[i]);
902 float rotorpartmass = _weight_per_blade*_number_of_blades/_number_of_parts*.453;
903 //was pounds -> now kg
905 _torque_of_inertia = 1/12. * ( _number_of_parts * rotorpartmass) * _diameter
906 * _diameter * _rel_blade_center * _rel_blade_center /(0.5*0.5);
907 float speed=_rotor_rpm/60*_diameter*_rel_blade_center*pi;
908 float lentocenter=_diameter*_rel_blade_center*0.5;
909 float lentoforceattac=_diameter*_rel_len_hinge*0.5;
910 float zentforce=rotorpartmass*speed*speed/lentocenter;
911 float pitchaforce=_force_at_pitch_a/_number_of_parts*.453*9.81;
912 // was pounds of force, now N, devided by _number_of_parts
913 //(so its now per rotorpart)
915 float torque0=0,torquemax=0,torqueb=0;
916 float omega=_rotor_rpm/60*2*pi;
918 float omega0=omega*Math::sqrt(1/(1-_rel_len_hinge));
919 _delta*=pitchaforce/(_pitch_a*omega*lentocenter*2*rotorpartmass);
921 float phi=Math::atan2(2*omega*_delta,omega0*omega0-omega*omega);
922 float relamp=(omega*omega/(2*_delta*Math::sqrt(sqr(omega0*omega0-omega*omega)
923 +4*_delta*_delta*omega*omega)))*_cyclic_factor;
926 torque0=_power_at_pitch_0/_number_of_parts*1000/omega;
927 // f*r=p/w ; p=f*s/t; r=s/t/w ; r*w*t = s
928 torqueb=_power_at_pitch_b/_number_of_parts*1000/omega;
929 torquemax=_power_at_pitch_b/_number_of_parts*1000/omega/_pitch_b*_max_pitch;
940 for (i=0;i<_number_of_parts;i++)
942 float lpos[3],lforceattac[3],lspeed[3],dirzentforce[3];
943 float s = Math::sin(2*pi*i/_number_of_parts);
944 float c = Math::cos(2*pi*i/_number_of_parts);
945 float direction[3],nextdirection[3],help[3];
946 Math::mul3(c ,directions[0],help);
947 Math::mul3(s ,directions[1],direction);
948 Math::add3(help,direction,direction);
950 Math::mul3(c ,directions[1],help);
951 Math::mul3(s ,directions[2],nextdirection);
952 Math::add3(help,nextdirection,nextdirection);
954 Math::mul3(lentocenter,direction,lpos);
955 Math::add3(lpos,_base,lpos);
956 Math::mul3(lentoforceattac,nextdirection,lforceattac);
957 //nextdirection: +90deg (gyro)!!!
959 Math::add3(lforceattac,_base,lforceattac);
960 Math::mul3(speed,nextdirection,lspeed);
961 Math::mul3(1,nextdirection,dirzentforce);
964 float maxcyclic=(i&1)?_maxcyclicele:_maxcyclicail;
965 float mincyclic=(i&1)?_mincyclicele:_mincyclicail;
967 Rotorpart* rp=rps[i]=newRotorpart(lpos, lforceattac, _normal,
968 lspeed,dirzentforce,zentforce,pitchaforce, _max_pitch,_min_pitch,
969 mincyclic,maxcyclic,_delta3,rotorpartmass,_translift,
970 _rel_len_hinge,lentocenter);
971 int k = i*4/_number_of_parts;
972 rp->setAlphaoutput(_alphaoutput[k&1?k:(_ccw?k^2:k)],0);
973 rp->setAlphaoutput(_alphaoutput[4+(k&1?k:(_ccw?k^2:k))],1+(k>1));
975 rp->setTorque(torquemax,torque0);
976 rp->setRelamp(relamp);
977 rp->setDirectionofRotorPart(direction);
978 rp->setTorqueOfInertia(_torque_of_inertia/_number_of_parts);
980 for (i=0;i<_number_of_parts;i++)
982 rps[i]->setlastnextrp(rps[(i-1+_number_of_parts)%_number_of_parts],
983 rps[(i+1)%_number_of_parts],
984 rps[(i+_number_of_parts/2)%_number_of_parts],
985 rps[(i-_number_of_parts/4+_number_of_parts)%_number_of_parts],
986 rps[(i+_number_of_parts/4)%_number_of_parts]);
988 for (i=0;i<_number_of_parts;i++)
990 rps[i]->setCompiled();
992 float lift[4],torque[4], v_wind[3];
993 v_wind[0]=v_wind[1]=v_wind[2]=0;
994 rps[0]->setOmega(_omegan);
996 if (_airfoil_lift_coefficient==0)
998 //calculate the lift and drag coefficients now
1002 rps[0]->calculateAlpha(v_wind,rho_null,_pitch_a,0,0,
1003 &(torque[0]),&(lift[0])); //max_pitch a
1004 _liftcoef = pitchaforce/lift[0];
1007 rps[0]->calculateAlpha(v_wind,rho_null,0,0,0,&(torque[0]),&(lift[0]));
1012 rps[0]->calculateAlpha(v_wind,rho_null,0,0,0,&(torque[1]),&(lift[1]));
1017 rps[0]->calculateAlpha(v_wind,rho_null,_pitch_b,0,0,&(torque[2]),&(lift[2]));
1022 rps[0]->calculateAlpha(v_wind,rho_null,_pitch_b,0,0,&(torque[3]),&(lift[3]));
1027 _dragcoef1=torque0/torque[1];
1028 _dragcoef0=(torqueb-_dragcoef1*torque[3])/torque[2];
1032 _dragcoef1=(torque0/torque[0]-torqueb/torque[2])
1033 /(torque[1]/torque[0]-torque[3]/torque[2]);
1034 _dragcoef0=(torqueb-_dragcoef1*torque[3])/torque[2];
1039 _liftcoef=_airfoil_lift_coefficient/_number_of_parts*_number_of_blades;
1040 _dragcoef0=_airfoil_drag_coefficient0/_number_of_parts*_number_of_blades*_c2;
1041 _dragcoef1=_airfoil_drag_coefficient1/_number_of_parts*_number_of_blades*_c2;
1045 rps[0]->calculateAlpha(v_wind,rho_null,_pitch_a,0,0,
1046 &(torque[0]),&(lift[0])); //pitch a
1047 rps[0]->calculateAlpha(v_wind,rho_null,_pitch_b,0,0,
1048 &(torque[1]),&(lift[1])); //pitch b
1049 rps[0]->calculateAlpha(v_wind,rho_null,0,0,0,
1050 &(torque[3]),&(lift[3])); //pitch 0
1051 SG_LOG(SG_GENERAL, SG_DEBUG,
1052 "Rotor: coefficients for airfoil:" << endl << setprecision(6)
1053 << " drag0: " << _dragcoef0*_number_of_parts/_number_of_blades/_c2
1054 << " drag1: " << _dragcoef1*_number_of_parts/_number_of_blades/_c2
1055 << " lift: " << _liftcoef*_number_of_parts/_number_of_blades
1057 << "at 10 deg:" << endl
1058 << "drag: " << (Math::sin(10./180*pi)*_dragcoef1+_dragcoef0)
1059 *_number_of_parts/_number_of_blades/_c2
1060 << " lift: " << Math::sin(10./180*pi)*_liftcoef*_number_of_parts/_number_of_blades
1062 << "Some results (Pitch [degree], Power [kW], Lift [N])" << endl
1063 << 0.0f << "deg " << Math::abs(torque[3]*_number_of_parts*_omegan/1000) << "kW "
1064 << lift[3]*_number_of_parts << endl
1065 << _pitch_a*180/pi << "deg " << Math::abs(torque[0]*_number_of_parts*_omegan/1000)
1066 << "kW " << lift[0]*_number_of_parts << endl
1067 << _pitch_b*180/pi << "deg " << Math::abs(torque[1]*_number_of_parts*_omegan/1000)
1068 << "kW " << lift[1]*_number_of_parts << endl << endl );
1070 //first calculation of relamp is wrong
1071 //it used pitchaforce, but this was unknown and
1072 //on the default value
1073 _delta*=lift[0]/pitchaforce;
1074 relamp=(omega*omega/(2*_delta*Math::sqrt(sqr(omega0*omega0-omega*omega)
1075 +4*_delta*_delta*omega*omega)))*_cyclic_factor;
1076 for (i=0;i<_number_of_parts;i++)
1078 rps[i]->setRelamp(relamp);
1080 rps[0]->setOmega(0);
1083 std::ostream & operator<<(std::ostream & out, /*const*/ Rotor& r)
1085 #define i(x) << #x << ":" << r.x << endl
1086 #define iv(x) << #x << ":" << r.x[0] << ";" << r.x[1] << ";" <<r.x[2] << ";" << endl
1087 out << "Writing Info on Rotor "
1090 i(_omega) i(_omegan) i(_omegarel) i(_ddt_omega) i(_omegarelneu)
1093 i( _airfoil_incidence_no_lift)
1095 i( _airfoil_lift_coefficient)
1096 i( _airfoil_drag_coefficient0)
1097 i( _airfoil_drag_coefficient1)
1099 i( _number_of_segments)
1100 i( _number_of_parts)
1102 iv( _groundeffectpos[0])iv( _groundeffectpos[1])iv( _groundeffectpos[2])iv( _groundeffectpos[3])
1103 i( _ground_effect_altitude)
1105 iv( _normal_with_yaw_roll)
1108 i( _number_of_blades)
1109 i( _weight_per_blade)
1110 i( _rel_blade_center)
1113 i( _force_at_pitch_a)
1115 i( _power_at_pitch_0)
1116 i( _power_at_pitch_b)
1133 i( _alphamin) i(_alphamax) i(_alpha0) i(_alpha0factor)
1134 i( _teeterdamp) i(_maxteeterdamp)
1135 i( _rellenteeterhinge)
1137 i( _translift_maxfactor)
1138 i( _ground_effect_constant)
1139 i( _vortex_state_lift_factor)
1140 i( _vortex_state_c1)
1141 i( _vortex_state_c2)
1142 i( _vortex_state_c3)
1143 i( _vortex_state_e1)
1144 i( _vortex_state_e2)
1145 i( _vortex_state_e3)
1146 i( _lift_factor) i(_f_ge) i(_f_vs) i(_f_tl)
1151 i( _twist) //outer incidence = inner inner incidence + _twist
1152 i( _rel_len_where_incidence_is_measured)
1153 i( _torque_of_inertia)
1154 i( _rel_len_blade_start)
1155 i( _incidence_stall_zero_speed)
1156 i( _incidence_stall_half_sonic_speed)
1157 i( _lift_factor_stall)
1158 i( _stall_change_over)
1159 i( _drag_factor_stall)
1166 i( _cyclic_factor) <<endl;
1168 for(j=0; j<r._rotorparts.size(); j++) {
1169 out << *((Rotorpart*)r._rotorparts.get(j));
1176 void Rotor:: writeInfo()
1179 std::ostringstream buffer;
1181 FILE*f=fopen("c:\\fgmsvc\\bat\\log.txt","at");
1182 if (!f) f=fopen("c:\\fgmsvc\\bat\\log.txt","wt");
1185 fprintf(f,"%s",(const char *)buffer.str().c_str());
1190 Rotorpart* Rotor::newRotorpart(float* pos, float *posforceattac, float *normal,
1191 float* speed,float *dirzentforce, float zentforce,float maxpitchforce,
1192 float maxpitch, float minpitch, float mincyclic,float maxcyclic,
1193 float delta3,float mass,float translift,float rellenhinge,float len)
1195 Rotorpart *r = new Rotorpart();
1196 r->setPosition(pos);
1197 r->setNormal(normal);
1198 r->setZentipetalForce(zentforce);
1199 r->setPositionForceAttac(posforceattac);
1201 r->setDirectionofZentipetalforce(dirzentforce);
1202 r->setMaxpitch(maxpitch);
1203 r->setMinpitch(minpitch);
1204 r->setMaxcyclic(maxcyclic);
1205 r->setMincyclic(mincyclic);
1206 r->setDelta3(delta3);
1207 r->setDynamic(_dynamic);
1208 r->setTranslift(_translift);
1211 r->setRelLenHinge(rellenhinge);
1212 r->setOmegaN(_omegan);
1213 r->setAlpha0(_alpha0);
1214 r->setAlphamin(_alphamin);
1215 r->setAlphamax(_alphamax);
1216 r->setAlpha0factor(_alpha0factor);
1218 r->setDiameter(_diameter);
1220 #define p(a) r->setParameter(#a,_##a);
1222 p(number_of_segments)
1223 p(rel_len_where_incidence_is_measured)
1224 p(rel_len_blade_start)
1225 p(rotor_correction_factor)
1230 void Rotor::interp(float* v1, float* v2, float frac, float* out)
1232 out[0] = v1[0] + frac*(v2[0]-v1[0]);
1233 out[1] = v1[1] + frac*(v2[1]-v1[1]);
1234 out[2] = v1[2] + frac*(v2[2]-v1[2]);
1237 void Rotorgear::initRotorIteration(float *lrot,float dt)
1241 if (!_rotors.size()) return;
1242 Rotor* r0 = (Rotor*)_rotors.get(0);
1243 omegarel= r0->getOmegaRelNeu();
1244 for(i=0; i<_rotors.size(); i++) {
1245 Rotor* r = (Rotor*)_rotors.get(i);
1246 r->inititeration(dt,omegarel,0,lrot);
1250 void Rotorgear::calcForces(float* torqueOut)
1253 torqueOut[0]=torqueOut[1]=torqueOut[2]=0;
1254 // check,<if the engine can handle the torque of the rotors.
1255 // If not reduce the torque to the fueselage and change rotational
1256 // speed of the rotors instead
1259 float omegarel,omegan;
1260 Rotor* r0 = (Rotor*)_rotors.get(0);
1261 omegarel= r0->getOmegaRel();
1263 float total_torque_of_inertia=0;
1264 float total_torque=0;
1265 for(i=0; i<_rotors.size(); i++) {
1266 Rotor* r = (Rotor*)_rotors.get(i);
1267 omegan=r->getOmegan();
1268 total_torque_of_inertia+=r->getTorqueOfInertia()*omegan*omegan;
1269 //FIXME: this is constant, so this can be done in compile
1271 total_torque+=r->getTorque()*omegan;
1273 float max_torque_of_engine=0;
1276 max_torque_of_engine=_max_power_engine;
1277 float df=1-omegarel;
1278 df/=_engine_prop_factor;
1279 df = Math::clamp(df, 0, 1);
1280 max_torque_of_engine = df * _max_power_engine;
1284 float rel_torque_engine=1;
1285 if (total_torque<=0)
1286 rel_torque_engine=0;
1288 if (max_torque_of_engine>0)
1289 rel_torque_engine=1/max_torque_of_engine*total_torque;
1291 rel_torque_engine=0;
1293 //add the rotor brake and the gear fritcion
1295 if (r0->_rotorparts.size()) dt=((Rotorpart*)r0->_rotorparts.get(0))->getDt();
1297 float rotor_brake_torque;
1298 rotor_brake_torque=_rotorbrake*_max_power_rotor_brake+_rotorgear_friction;
1299 //clamp it to the value you need to stop the rotor
1300 //to avod accelerate the rotor to neagtive rpm:
1301 rotor_brake_torque=Math::clamp(rotor_brake_torque,0,
1302 total_torque_of_inertia/dt*omegarel);
1303 max_torque_of_engine-=rotor_brake_torque;
1305 //change the rotation of the rotors
1306 if ((max_torque_of_engine<total_torque) //decreasing rotation
1307 ||((max_torque_of_engine>total_torque)&&(omegarel<1))
1308 //increasing rotation due to engine
1309 ||(total_torque<0) ) //increasing rotation due to autorotation
1311 _ddt_omegarel=(max_torque_of_engine-total_torque)/total_torque_of_inertia;
1312 if(max_torque_of_engine>total_torque)
1314 //check if the acceleration is due to the engine. If yes,
1315 //the engine self limits the accel.
1316 float lim1=-total_torque/total_torque_of_inertia;
1317 //accel. by autorotation
1319 if (lim1<_engine_accel_limit) lim1=_engine_accel_limit;
1320 //if the accel by autorotation greater than the max. engine
1321 //accel, then this is the limit, if not: the engine is the limit
1322 if (_ddt_omegarel>lim1) _ddt_omegarel=lim1;
1324 if (_ddt_omegarel>5.5)_ddt_omegarel=5.5;
1325 //clamp it to avoid overflow. Should never be reached
1326 if (_ddt_omegarel<-5.5)_ddt_omegarel=-5.5;
1328 if (_max_power_engine<0.001) {omegarel=1;_ddt_omegarel=0;}
1329 //for debug: negative or no maxpower will result
1330 //in permanet 100% rotation
1332 omegarel+=dt*_ddt_omegarel;
1334 if (omegarel>2.5) omegarel=2.5;
1335 //clamp it to avoid overflow. Should never be reached
1336 if (omegarel<-.5) omegarel=-.5;
1338 r0->setOmegaRelNeu(omegarel);
1339 //calculate the torque, which is needed to accelerate the rotors.
1340 //Add this additional torque to the body
1341 for(j=0; j<_rotors.size(); j++) {
1342 Rotor* r = (Rotor*)_rotors.get(j);
1343 for(i=0; i<r->_rotorparts.size(); i++) {
1344 float torque_scalar=0;
1345 Rotorpart* rp = (Rotorpart*)r->_rotorparts.get(i);
1347 rp->getAccelTorque(_ddt_omegarel,torque);
1348 Math::add3(torque,torqueOut,torqueOut);
1352 _total_torque_on_engine=total_torque+_ddt_omegarel*total_torque_of_inertia;
1356 void Rotorgear::addRotor(Rotor* rotor)
1362 float Rotorgear::compile(RigidBody* body)
1365 for(int j=0; j<_rotors.size(); j++) {
1366 Rotor* r = (Rotor*)_rotors.get(j);
1369 for(i=0; i<r->numRotorparts(); i++) {
1370 Rotorpart* rp = (Rotorpart*)r->getRotorpart(i);
1371 float mass = rp->getWeight();
1372 mass = mass * Math::sqrt(mass);
1374 rp->getPosition(pos);
1375 body->addMass(mass, pos);
1382 void Rotorgear::getDownWash(float *pos, float * v_heli, float *downwash)
1385 downwash[0]=downwash[1]=downwash[2]=0;
1386 for(int i=0; i<_rotors.size(); i++) {
1387 Rotor* ro = (Rotor*)_rotors.get(i);
1388 ro->getDownWash(pos,v_heli,tmp);
1389 Math::add3(downwash,tmp,downwash); // + downwash
1393 void Rotorgear::setParameter(char *parametername, float value)
1395 #define p(a,b) if (strcmp(parametername,#a)==0) _##a = (value * (b)); else
1396 p(max_power_engine,1000)
1397 p(engine_prop_factor,1)
1398 p(yasimdragfactor,1)
1399 p(yasimliftfactor,1)
1400 p(max_power_rotor_brake,1000)
1401 p(rotorgear_friction,1000)
1402 p(engine_accel_limit,0.01)
1403 SG_LOG(SG_INPUT, SG_ALERT,
1404 "internal error in parameter set up for rotorgear: '"
1405 << parametername <<"'" << endl);
1408 int Rotorgear::getValueforFGSet(int j,char *text,float *f)
1412 sprintf(text,"/rotors/gear/total_torque");
1413 *f=_total_torque_on_engine;
1417 Rotorgear::Rotorgear()
1422 _max_power_rotor_brake=1;
1423 _rotorgear_friction=1;
1424 _max_power_engine=1000*450;
1425 _engine_prop_factor=0.05f;
1429 _engine_accel_limit=0.05f;
1430 _total_torque_on_engine=0;
1433 Rotorgear::~Rotorgear()
1435 for(int i=0; i<_rotors.size(); i++)
1436 delete (Rotor*)_rotors.get(i);
1439 }; // namespace yasim