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;
79 _shared_flap_hinge=false;
80 _rellenteeterhinge=0.01;
87 _translift_maxfactor=1.3;
88 _ground_effect_constant=0.1;
89 _vortex_state_lift_factor=0.4;
102 _number_of_segments=1;
104 _rel_len_where_incidence_is_measured=0.7;
105 _torque_of_inertia=1;
109 _airfoil_incidence_no_lift=0;
110 _rel_len_blade_start=0;
111 _airfoil_lift_coefficient=0;
112 _airfoil_drag_coefficient0=0;
113 _airfoil_drag_coefficient1=0;
115 _global_ground[i] = 0;
116 _global_ground[2] = 1;
117 _global_ground[3] = -1e3;
118 _incidence_stall_zero_speed=18*pi/180.;
119 _incidence_stall_half_sonic_speed=14*pi/180.;
120 _lift_factor_stall=0.28;
121 _stall_change_over=2*pi/180.;
122 _drag_factor_stall=8;
127 for (int k=0;k<4;k++)
129 _groundeffectpos[k][i]=0;
130 _ground_effect_altitude=1;
132 _lift_factor=_f_ge=_f_vs=_f_tl=1;
133 _rotor_correction_factor=.65;
139 for(i=0; i<_rotorparts.size(); i++) {
140 Rotorpart* r = (Rotorpart*)_rotorparts.get(i);
145 void Rotor::inititeration(float dt,float omegarel,float ddt_omegarel,float *rot)
150 _omega=_omegan*_omegarel;
151 _ddt_omega=_omegan*ddt_omegarel;
153 for(i=0; i<_rotorparts.size(); i++) {
154 float s = Math::sin(float(2*pi*i/_number_of_parts+(_phi-pi/2.)*(_ccw?1:-1)));
155 float c = Math::cos(float(2*pi*i/_number_of_parts+(_phi-pi/2.)*(_ccw?1:-1)));
156 Rotorpart* r = (Rotorpart*)_rotorparts.get(i);
158 r->setDdtOmega(_ddt_omega);
159 r->inititeration(dt,rot);
160 r->setCyclic(_cyclicail*c+_cyclicele*s);
163 //calculate the normal of the rotor disc, for calcualtion of the downwash
164 float side[3],help[3];
165 Math::cross3(_normal,_forward,side);
166 Math::mul3(Math::cos(_yaw)*Math::cos(_roll),_normal,_normal_with_yaw_roll);
168 Math::mul3(Math::sin(_yaw),_forward,help);
169 Math::add3(_normal_with_yaw_roll,help,_normal_with_yaw_roll);
171 Math::mul3(Math::sin(_roll),side,help);
172 Math::add3(_normal_with_yaw_roll,help,_normal_with_yaw_roll);
175 float Rotor::calcStall(float incidence,float speed)
177 float stall_incidence=_incidence_stall_zero_speed
178 +(_incidence_stall_half_sonic_speed
179 -_incidence_stall_zero_speed)*speed/(343./2);
180 //missing: Temeperature dependency of sonic speed
181 incidence = Math::abs(incidence);
182 if (incidence > (90./180.*pi))
183 incidence = pi-incidence;
184 float stall = (incidence-stall_incidence)/_stall_change_over;
185 stall = Math::clamp(stall,0,1);
187 _stall_sum+=stall*speed*speed;
188 _stall_v2sum+=speed*speed;
193 float Rotor::getLiftCoef(float incidence,float speed)
195 float stall=calcStall(incidence,speed);
196 /* the next shold look like this, but this is the inner loop of
197 the rotor simulation. For small angles (and we hav only small
198 angles) the first order approximation works well
199 float c1= Math::sin(incidence-_airfoil_incidence_no_lift)*_liftcoef;
200 for c2 we would need higher order, because in stall the angle can be large
203 if (incidence > (pi/2))
205 else if (incidence <-(pi/2))
209 float c1= (i2-_airfoil_incidence_no_lift)*_liftcoef;
212 float c2= Math::sin(2*(incidence-_airfoil_incidence_no_lift))
213 *_liftcoef*_lift_factor_stall;
214 return (1-stall)*c1 + stall *c2;
220 float Rotor::getDragCoef(float incidence,float speed)
222 float stall=calcStall(incidence,speed);
223 float c1= (Math::abs(Math::sin(incidence-_airfoil_incidence_no_lift))
224 *_dragcoef1+_dragcoef0);
225 float c2= c1*_drag_factor_stall;
226 return (1-stall)*c1 + stall *c2;
229 int Rotor::getValueforFGSet(int j,char *text,float *f)
231 if (_name[0]==0) return 0;
232 if (4>numRotorparts()) return 0; //compile first!
235 sprintf(text,"/rotors/%s/cone-deg", _name);
236 *f=( ((Rotorpart*)getRotorpart(0))->getrealAlpha()
237 +((Rotorpart*)getRotorpart(1*(_number_of_parts>>2)))->getrealAlpha()
238 +((Rotorpart*)getRotorpart(2*(_number_of_parts>>2)))->getrealAlpha()
239 +((Rotorpart*)getRotorpart(3*(_number_of_parts>>2)))->getrealAlpha()
245 sprintf(text,"/rotors/%s/roll-deg", _name);
246 _roll = ( ((Rotorpart*)getRotorpart(0))->getrealAlpha()
247 -((Rotorpart*)getRotorpart(2*(_number_of_parts>>2)))->getrealAlpha()
254 sprintf(text,"/rotors/%s/yaw-deg", _name);
255 _yaw=( ((Rotorpart*)getRotorpart(1*(_number_of_parts>>2)))->getrealAlpha()
256 -((Rotorpart*)getRotorpart(3*(_number_of_parts>>2)))->getrealAlpha()
263 sprintf(text,"/rotors/%s/rpm", _name);
269 sprintf(text,"/rotors/%s/debug/debugfge",_name);
274 sprintf(text,"/rotors/%s/debug/debugfvs",_name);
279 sprintf(text,"/rotors/%s/debug/debugftl",_name);
284 sprintf(text,"/rotors/%s/debug/vortexstate",_name);
289 sprintf(text,"/rotors/%s/stall",_name);
290 *f=getOverallStall();
294 sprintf(text,"/rotors/%s/torque",_name);
300 if (b>=_number_of_blades)
305 sprintf(text,"/rotors/%s/blade[%i]/%s",
307 w==0?"position-deg":(w==1?"flap-deg":"incidence-deg"));
308 *f=((Rotorpart*)getRotorpart(0))->getPhi()*180/pi
309 +360*b/_number_of_blades*(_ccw?1:-1);
318 rk=Math::clamp(rk,0,1);//Delete this
320 if(w==2) {k+=2;l+=2;}
322 if(w==1) {k+=1;l+=1;}
325 if (w==1) *f=rk*((Rotorpart*) getRotorpart(k*(_number_of_parts>>2)))->getrealAlpha()*180/pi
326 +rl*((Rotorpart*) getRotorpart(l*(_number_of_parts>>2)))->getrealAlpha()*180/pi;
327 else if(w==2) *f=rk*((Rotorpart*)getRotorpart(k*(_number_of_parts>>2)))->getIncidence()*180/pi
328 +rl*((Rotorpart*)getRotorpart(l*(_number_of_parts>>2)))->getIncidence()*180/pi;
333 void Rotorgear::setEngineOn(int value)
338 void Rotor::setAlpha0(float f)
343 void Rotor::setAlphamin(float f)
348 void Rotor::setAlphamax(float f)
353 void Rotor::setAlpha0factor(float f)
358 int Rotor::numRotorparts()
360 return _rotorparts.size();
363 Rotorpart* Rotor::getRotorpart(int n)
365 return ((Rotorpart*)_rotorparts.get(n));
368 int Rotorgear::getEngineon()
373 float Rotor::getTorqueOfInertia()
375 return _torque_of_inertia;
378 void Rotor::setTorque(float f)
383 void Rotor::addTorque(float f)
388 void Rotor::strncpy(char *dest,const char *src,int maxlen)
391 while(src[n]&&n<(maxlen-1))
399 void Rotor::setNormal(float* normal)
402 float invsum,sqrsum=0;
403 for(i=0; i<3; i++) { sqrsum+=normal[i]*normal[i];}
405 invsum=1/Math::sqrt(sqrsum);
410 _normal_with_yaw_roll[i]=_normal[i] = normal[i]*invsum;
414 void Rotor::setForward(float* forward)
417 float invsum,sqrsum=0;
418 for(i=0; i<3; i++) { sqrsum+=forward[i]*forward[i];}
420 invsum=1/Math::sqrt(sqrsum);
423 for(i=0; i<3; i++) { _forward[i] = forward[i]*invsum; }
426 void Rotor::setForceAtPitchA(float force)
428 _force_at_pitch_a=force;
431 void Rotor::setPowerAtPitch0(float value)
433 _power_at_pitch_0=value;
436 void Rotor::setPowerAtPitchB(float value)
438 _power_at_pitch_b=value;
441 void Rotor::setPitchA(float value)
443 _pitch_a=value/180*pi;
446 void Rotor::setPitchB(float value)
448 _pitch_b=value/180*pi;
451 void Rotor::setBase(float* base)
454 for(i=0; i<3; i++) _base[i] = base[i];
457 void Rotor::setMaxCyclicail(float value)
459 _maxcyclicail=value/180*pi;
462 void Rotor::setMaxCyclicele(float value)
464 _maxcyclicele=value/180*pi;
467 void Rotor::setMinCyclicail(float value)
469 _mincyclicail=value/180*pi;
472 void Rotor::setMinCyclicele(float value)
474 _mincyclicele=value/180*pi;
477 void Rotor::setMinCollective(float value)
479 _min_pitch=value/180*pi;
482 void Rotor::setMaxCollective(float value)
484 _max_pitch=value/180*pi;
487 void Rotor::setDiameter(float value)
492 void Rotor::setWeightPerBlade(float value)
494 _weight_per_blade=value;
497 void Rotor::setNumberOfBlades(float value)
499 _number_of_blades=int(value+.5);
502 void Rotor::setRelBladeCenter(float value)
504 _rel_blade_center=value;
507 void Rotor::setDynamic(float value)
512 void Rotor::setDelta3(float value)
517 void Rotor::setDelta(float value)
522 void Rotor::setTranslift(float value)
527 void Rotor::setSharedFlapHinge(bool s)
529 _shared_flap_hinge=s;
532 void Rotor::setC2(float value)
537 void Rotor::setStepspersecond(float steps)
539 _stepspersecond=steps;
542 void Rotor::setRPM(float value)
547 void Rotor::setPhiNull(float value)
552 void Rotor::setRelLenHinge(float value)
554 _rel_len_hinge=value;
557 void Rotor::setAlphaoutput(int i, const char *text)
559 strncpy(_alphaoutput[i],text,255);
562 void Rotor::setName(const char *text)
564 strncpy(_name,text,256);//256: some space needed for settings
567 void Rotor::setCcw(int ccw)
572 void Rotor::setNotorque(int value)
577 void Rotor::setRelLenTeeterHinge(float f)
579 _rellenteeterhinge=f;
582 void Rotor::setTeeterdamp(float f)
587 void Rotor::setMaxteeterdamp(float f)
592 void Rotor::setGlobalGround(double *global_ground, float* global_vel)
595 for(i=0; i<4; i++) _global_ground[i] = global_ground[i];
598 void Rotor::setParameter(char *parametername, float value)
600 #define p(a,b) if (strcmp(parametername,#a)==0) _##a = (value * (b)); else
602 p(translift_maxfactor,1)
603 p(ground_effect_constant,1)
604 p(vortex_state_lift_factor,1)
612 p(number_of_segments,1)
614 p(rel_len_where_incidence_is_measured,1)
617 p(airfoil_incidence_no_lift,pi/180.)
618 p(rel_len_blade_start,1)
619 p(incidence_stall_zero_speed,pi/180.)
620 p(incidence_stall_half_sonic_speed,pi/180.)
621 p(lift_factor_stall,1)
622 p(stall_change_over,pi/180.)
623 p(drag_factor_stall,1)
624 p(airfoil_lift_coefficient,1)
625 p(airfoil_drag_coefficient0,1)
626 p(airfoil_drag_coefficient1,1)
628 p(rotor_correction_factor,1)
629 SG_LOG(SG_INPUT, SG_ALERT,
630 "internal error in parameter set up for rotor: '" <<
631 parametername <<"'" << endl);
635 float Rotor::getLiftFactor()
640 void Rotorgear::setRotorBrake(float lval)
642 lval = Math::clamp(lval, 0, 1);
646 void Rotor::setCollective(float lval)
648 lval = Math::clamp(lval, -1, 1);
650 _collective=_min_pitch+(lval+1)/2*(_max_pitch-_min_pitch);
651 for(i=0; i<_rotorparts.size(); i++) {
652 ((Rotorpart*)_rotorparts.get(i))->setCollective(_collective);
656 void Rotor::setCyclicele(float lval,float rval)
658 lval = Math::clamp(lval, -1, 1);
659 _cyclicele=_mincyclicele+(lval+1)/2*(_maxcyclicele-_mincyclicele);
662 void Rotor::setCyclicail(float lval,float rval)
664 lval = Math::clamp(lval, -1, 1);
666 _cyclicail=-(_mincyclicail+(lval+1)/2*(_maxcyclicail-_mincyclicail));
669 void Rotor::getPosition(float* out)
672 for(i=0; i<3; i++) out[i] = _base[i];
675 void Rotor::calcLiftFactor(float* v, float rho, State *s)
677 //calculates _lift_factor, which is a foactor for the lift of the rotor
679 //- ground effect (_f_ge)
680 //- vortex state (_f_vs)
681 //- translational lift (_f_tl)
686 // Calculate ground effect
687 _f_ge=1+_diameter/_ground_effect_altitude*_ground_effect_constant;
689 // Now calculate translational lift
690 float v_vert = Math::dot3(v,_normal);
692 Math::cross3(v,_normal,help);
693 float v_horiz = Math::mag3(help);
694 _f_tl = ((1-Math::pow(2.7183,-v_horiz/_translift_ve))
695 *(_translift_maxfactor-1)+1)/_translift_maxfactor;
697 _lift_factor = _f_ge*_f_tl*_f_vs;
700 void Rotor::findGroundEffectAltitude(Ground * ground_cb,State *s)
702 _ground_effect_altitude=findGroundEffectAltitude(ground_cb,s,
703 _groundeffectpos[0],_groundeffectpos[1],
704 _groundeffectpos[2],_groundeffectpos[3]);
707 float Rotor::findGroundEffectAltitude(Ground * ground_cb,State *s,
708 float *pos0,float *pos1,float *pos2,float *pos3,
709 int iteration,float a0,float a1,float a2,float a3)
723 Math::add3(p[0],p[2],p[4]);
724 Math::mul3(0.5,p[4],p[4]);//the center
726 float mina=100*_diameter;
728 for (int i=0;i<5;i++)
730 if (a[i]==-1)//in the first iteration,(iteration==0) no height is
731 //passed to this function, these missing values are
735 s->posLocalToGlobal(p[i], pt);
737 // Ask for the ground plane in the global coordinate system
738 double global_ground[4];
740 ground_cb->getGroundPlane(pt, global_ground, global_vel);
741 // find h, the distance to the ground
742 // The ground plane transformed to the local frame.
744 s->planeGlobalToLocal(global_ground, ground);
746 a[i] = ground[3] - Math::dot3(p[i], ground);
747 // Now a[i] is the distance from p[i] to ground
753 if (mina>=10*_diameter)
754 return mina; //the ground effect will be zero
756 //check if further recursion is neccessary
757 //if the height does not differ more than 20%, than
758 //we can return then mean height, if not split
759 //zhe square to four parts and calcualte the height
761 //suma * 0.2 is the mean
762 //0.15 is the maximum allowed difference from the mean
763 //to the height at the center
765 ||(Math::abs(suma*0.2-a[4])<(0.15*0.2*suma*(1<<iteration))))
768 float pc[4][3],ac[4]; //pc[i]=center of pos[i] and pos[(i+1)&3]
769 for (int i=0;i<4;i++)
771 Math::add3(p[i],p[(i+1)&3],pc[i]);
772 Math::mul3(0.5,pc[i],pc[i]);
774 s->posLocalToGlobal(pc[i], pt);
776 // Ask for the ground plane in the global coordinate system
777 double global_ground[4];
779 ground_cb->getGroundPlane(pt, global_ground, global_vel);
780 // find h, the distance to the ground
781 // The ground plane transformed to the local frame.
783 s->planeGlobalToLocal(global_ground, ground);
785 ac[i] = ground[3] - Math::dot3(p[i], ground);
786 // Now ac[i] is the distance from pc[i] to ground
789 (findGroundEffectAltitude(ground_cb,s,p[0],pc[1],p[4],pc[3],
790 iteration+1,a[0],ac[0],a[4],ac[3])
791 +findGroundEffectAltitude(ground_cb,s,p[1],pc[0],p[4],pc[1],
792 iteration+1,a[1],ac[0],a[4],ac[1])
793 +findGroundEffectAltitude(ground_cb,s,p[2],pc[1],p[4],pc[2],
794 iteration+1,a[2],ac[1],a[4],ac[2])
795 +findGroundEffectAltitude(ground_cb,s,p[3],pc[2],p[4],pc[3],
796 iteration+1,a[3],ac[2],a[4],ac[3])
800 void Rotor::getDownWash(float *pos, float *v_heli, float *downwash)
802 float pos2rotor[3],tmp[3];
803 Math::sub3(_base,pos,pos2rotor);
804 float dist=Math::dot3(pos2rotor,_normal_with_yaw_roll);
805 //calculate incidence at 0.7r;
806 float inc = _collective+_twist *0.7
807 - _twist*_rel_len_where_incidence_is_measured;
810 if (dist<0) // we are not in the downwash region
812 downwash[0]=downwash[1]=downwash[2]=0.;
816 //calculate the mean downwash speed directly beneath the rotor disk
817 float v1bar = Math::sin(inc) *_omega * 0.35 * _diameter * 0.8;
818 //0.35 * d = 0.7 *r, a good position to calcualte the mean downwashd
819 //0.8 the slip of the rotor.
821 //calculate the time the wind needed from thr rotor to here
822 if (v1bar< 1) v1bar = 1;
823 float time=dist/v1bar;
825 //calculate the pos2rotor, where the rotor was, "time" ago
826 Math::mul3(time,v_heli,tmp);
827 Math::sub3(pos2rotor,tmp,pos2rotor);
829 //and again calculate dist
830 dist=Math::dot3(pos2rotor,_normal_with_yaw_roll);
831 //missing the normal is offen not pointing to the normal of the rotor
832 //disk. Rotate the normal by yaw and tilt angle
836 if (dist<0) // we are not in the downwash region
838 downwash[0]=downwash[1]=downwash[2]=0.;
841 //of course this could be done in a runge kutta integrator, but it's such
842 //a approximation that I beleave, it would'nt be more realistic
844 //calculate the dist to the rotor-axis
845 Math::cross3(pos2rotor,_normal_with_yaw_roll,tmp);
846 float r= Math::mag3(tmp);
847 //calculate incidence at r;
848 float rel_r = r *2 /_diameter;
849 float inc_r = _collective+_twist * r /_diameter * 2
850 - _twist*_rel_len_where_incidence_is_measured;
852 //calculate the downwash speed directly beneath the rotor disk
855 v1 = Math::sin(inc_r) *_omega * r * 0.8;
857 //calcualte the downwash speed in a distance "dist" to the rotor disc,
858 //for large dist. The speed is assumed do follow a gausian distribution
859 //with sigma increasing with dist^2:
860 //sigma is assumed to be half of the rotor diameter directly beneath the
861 //disc and is assumed to the rotor diameter at dist = (diameter * sqrt(2))
863 float sigma=_diameter/2 + dist * dist / _diameter /4.;
864 float v2 = v1bar*_diameter/ (Math::sqrt(2 * pi) * sigma)
865 * Math::pow(2.7183,-.5*r*r/(sigma*sigma))*_diameter/2/sigma;
867 //calculate the weight of the two downwash velocities.
868 //Directly beneath the disc it is v1, far away it is v2
869 float g = Math::pow(2.7183,-2*dist/_diameter);
870 //at dist = rotor radius it is assumed to be 1/e * v1 + (1-1/e)* v2
872 float v = g * v1 + (1-g) * v2;
873 Math::mul3(-v,_normal_with_yaw_roll,downwash);
874 //the downwash is calculated in the opposite direction of the normal
877 void Rotor::compile()
879 // Have we already been compiled?
880 if(_rotorparts.size() != 0) return;
882 //rotor is divided into _number_of_parts parts
883 //each part is calcualted at _number_of_segments points
886 //and make it a factor of 4
887 _number_of_parts=(int(Math::clamp(_number_of_parts,4,256))>>2)<<2;
889 _dynamic=_dynamic*(1/ //inverse of the time
890 ( (60/_rotor_rpm)/4 //for rotating 90 deg
891 +(60/_rotor_rpm)/(2*_number_of_blades) //+ meantime a rotorblade
892 //will pass a given point
894 float directions[5][3];
895 //pointing forward, right, ... the 5th is ony for calculation
896 directions[0][0]=_forward[0];
897 directions[0][1]=_forward[1];
898 directions[0][2]=_forward[2];
903 Math::cross3(directions[i-1],_normal,directions[i]);
905 Math::cross3(_normal,directions[i-1],directions[i]);
906 Math::unit3(directions[i],directions[i]);
908 Math::set3(directions[4],directions[0]);
909 // now directions[0] is perpendicular to the _normal.and has a length
910 // of 1. if _forward is already normalized and perpendicular to the
911 // normal, directions[0] will be the same
914 Math::mul3(_diameter*0.7,directions[i],_groundeffectpos[i]);
915 Math::add3(_base,_groundeffectpos[i],_groundeffectpos[i]);
917 float rotorpartmass = _weight_per_blade*_number_of_blades/_number_of_parts*.453;
918 //was pounds -> now kg
920 _torque_of_inertia = 1/12. * ( _number_of_parts * rotorpartmass) * _diameter
921 * _diameter * _rel_blade_center * _rel_blade_center /(0.5*0.5);
922 float speed=_rotor_rpm/60*_diameter*_rel_blade_center*pi;
923 float lentocenter=_diameter*_rel_blade_center*0.5;
924 float lentoforceattac=_diameter*_rel_len_hinge*0.5;
925 float zentforce=rotorpartmass*speed*speed/lentocenter;
926 float pitchaforce=_force_at_pitch_a/_number_of_parts*.453*9.81;
927 // was pounds of force, now N, devided by _number_of_parts
928 //(so its now per rotorpart)
930 float torque0=0,torquemax=0,torqueb=0;
931 float omega=_rotor_rpm/60*2*pi;
933 float omega0=omega*Math::sqrt(1/(1-_rel_len_hinge));
934 float delta_theoretical=pitchaforce/(_pitch_a*omega*lentocenter*2*rotorpartmass);
935 _delta*=delta_theoretical;
937 float relamp=(omega*omega/(2*_delta*Math::sqrt(sqr(omega0*omega0-omega*omega)
938 +4*_delta*_delta*omega*omega)))*_cyclic_factor;
939 float relamp_theoretical=(omega*omega/(2*delta_theoretical*Math::sqrt(sqr(omega0*omega0-omega*omega)
940 +4*delta_theoretical*delta_theoretical*omega*omega)))*_cyclic_factor;
941 _phi=Math::acos(_rel_len_hinge);
942 SG_LOG(SG_GENERAL, SG_ALERT,
943 "phi: " << _phi*180/3.14 << " delta3: " << _delta3 << "(" << Math::atan(_delta3)*180/3.14 << ")" <<endl);
944 _phi-=Math::atan(_delta3);
947 torque0=_power_at_pitch_0/_number_of_parts*1000/omega;
948 // f*r=p/w ; p=f*s/t; r=s/t/w ; r*w*t = s
949 torqueb=_power_at_pitch_b/_number_of_parts*1000/omega;
950 torquemax=_power_at_pitch_b/_number_of_parts*1000/omega/_pitch_b*_max_pitch;
961 for (i=0;i<_number_of_parts;i++)
963 float lpos[3],lforceattac[3],lspeed[3],dirzentforce[3];
964 float s = Math::sin(2*pi*i/_number_of_parts);
965 float c = Math::cos(2*pi*i/_number_of_parts);
966 float sp = Math::sin(float(2*pi*i/_number_of_parts-pi/2.+_phi));
967 float cp = Math::cos(float(2*pi*i/_number_of_parts-pi/2.+_phi));
968 float direction[3],nextdirection[3],help[3],direction90deg[3];
969 Math::mul3(c ,directions[0],help);
970 Math::mul3(s ,directions[1],direction);
971 Math::add3(help,direction,direction);
973 Math::mul3(c ,directions[1],help);
974 Math::mul3(s ,directions[2],direction90deg);
975 Math::add3(help,direction90deg,direction90deg);
977 Math::mul3(cp ,directions[1],help);
978 Math::mul3(sp ,directions[2],nextdirection);
979 Math::add3(help,nextdirection,nextdirection);
981 Math::mul3(lentocenter,direction,lpos);
982 Math::add3(lpos,_base,lpos);
983 Math::mul3(lentoforceattac,nextdirection,lforceattac);
984 //nextdirection: +90deg (gyro)!!!
986 Math::add3(lforceattac,_base,lforceattac);
987 Math::mul3(speed,direction90deg,lspeed);
988 Math::mul3(1,nextdirection,dirzentforce);
990 Rotorpart* rp=rps[i]=newRotorpart(lpos, lforceattac, _normal,
991 lspeed,dirzentforce,zentforce,pitchaforce,_delta3,rotorpartmass,
992 _translift,_rel_len_hinge,lentocenter);
993 int k = i*4/_number_of_parts;
994 rp->setAlphaoutput(_alphaoutput[k&1?k:(_ccw?k^2:k)],0);
995 rp->setAlphaoutput(_alphaoutput[4+(k&1?k:(_ccw?k^2:k))],1+(k>1));
997 rp->setTorque(torquemax,torque0);
998 rp->setRelamp(relamp);
999 rp->setDirectionofRotorPart(direction);
1000 rp->setTorqueOfInertia(_torque_of_inertia/_number_of_parts);
1002 for (i=0;i<_number_of_parts;i++)
1004 rps[i]->setlastnextrp(rps[(i-1+_number_of_parts)%_number_of_parts],
1005 rps[(i+1)%_number_of_parts],
1006 rps[(i+_number_of_parts/2)%_number_of_parts],
1007 rps[(i-_number_of_parts/4+_number_of_parts)%_number_of_parts],
1008 rps[(i+_number_of_parts/4)%_number_of_parts]);
1010 for (i=0;i<_number_of_parts;i++)
1012 rps[i]->setCompiled();
1014 float lift[4],torque[4], v_wind[3];
1015 v_wind[0]=v_wind[1]=v_wind[2]=0;
1016 rps[0]->setOmega(_omegan);
1018 if (_airfoil_lift_coefficient==0)
1020 //calculate the lift and drag coefficients now
1024 rps[0]->calculateAlpha(v_wind,rho_null,_pitch_a,0,0,
1025 &(torque[0]),&(lift[0])); //max_pitch a
1026 _liftcoef = pitchaforce/lift[0];
1029 rps[0]->calculateAlpha(v_wind,rho_null,0,0,0,&(torque[0]),&(lift[0]));
1034 rps[0]->calculateAlpha(v_wind,rho_null,0,0,0,&(torque[1]),&(lift[1]));
1039 rps[0]->calculateAlpha(v_wind,rho_null,_pitch_b,0,0,&(torque[2]),&(lift[2]));
1044 rps[0]->calculateAlpha(v_wind,rho_null,_pitch_b,0,0,&(torque[3]),&(lift[3]));
1049 _dragcoef1=torque0/torque[1];
1050 _dragcoef0=(torqueb-_dragcoef1*torque[3])/torque[2];
1054 _dragcoef1=(torque0/torque[0]-torqueb/torque[2])
1055 /(torque[1]/torque[0]-torque[3]/torque[2]);
1056 _dragcoef0=(torqueb-_dragcoef1*torque[3])/torque[2];
1061 _liftcoef=_airfoil_lift_coefficient/_number_of_parts*_number_of_blades;
1062 _dragcoef0=_airfoil_drag_coefficient0/_number_of_parts*_number_of_blades*_c2;
1063 _dragcoef1=_airfoil_drag_coefficient1/_number_of_parts*_number_of_blades*_c2;
1067 rps[0]->calculateAlpha(v_wind,rho_null,_pitch_a,0,0,
1068 &(torque[0]),&(lift[0])); //pitch a
1069 rps[0]->calculateAlpha(v_wind,rho_null,_pitch_b,0,0,
1070 &(torque[1]),&(lift[1])); //pitch b
1071 rps[0]->calculateAlpha(v_wind,rho_null,0,0,0,
1072 &(torque[3]),&(lift[3])); //pitch 0
1073 SG_LOG(SG_GENERAL, SG_INFO,
1074 "Rotor: coefficients for airfoil:" << endl << setprecision(6)
1075 << " drag0: " << _dragcoef0*_number_of_parts/_number_of_blades/_c2
1076 << " drag1: " << _dragcoef1*_number_of_parts/_number_of_blades/_c2
1077 << " lift: " << _liftcoef*_number_of_parts/_number_of_blades
1079 << "at 10 deg:" << endl
1080 << "drag: " << (Math::sin(10./180*pi)*_dragcoef1+_dragcoef0)
1081 *_number_of_parts/_number_of_blades/_c2
1082 << " lift: " << Math::sin(10./180*pi)*_liftcoef*_number_of_parts/_number_of_blades
1084 << "Some results (Pitch [degree], Power [kW], Lift [N])" << endl
1085 << 0.0f << "deg " << Math::abs(torque[3]*_number_of_parts*_omegan/1000) << "kW "
1086 << lift[3]*_number_of_parts << endl
1087 << _pitch_a*180/pi << "deg " << Math::abs(torque[0]*_number_of_parts*_omegan/1000)
1088 << "kW " << lift[0]*_number_of_parts << endl
1089 << _pitch_b*180/pi << "deg " << Math::abs(torque[1]*_number_of_parts*_omegan/1000)
1090 << "kW " << lift[1]*_number_of_parts << endl << endl );
1092 //first calculation of relamp is wrong
1093 //it used pitchaforce, but this was unknown and
1094 //on the default value
1095 _delta*=lift[0]/pitchaforce;
1096 relamp=(omega*omega/(2*_delta*Math::sqrt(sqr(omega0*omega0-omega*omega)
1097 +4*_delta*_delta*omega*omega)))*_cyclic_factor;
1098 for (i=0;i<_number_of_parts;i++)
1100 rps[i]->setRelamp(relamp);
1102 rps[0]->setOmega(0);
1105 std::ostream & operator<<(std::ostream & out, Rotor& r)
1107 #define i(x) << #x << ":" << r.x << endl
1108 #define iv(x) << #x << ":" << r.x[0] << ";" << r.x[1] << ";" <<r.x[2] << ";" << endl
1109 out << "Writing Info on Rotor "
1112 i(_omega) i(_omegan) i(_omegarel) i(_ddt_omega) i(_omegarelneu)
1115 i( _airfoil_incidence_no_lift)
1117 i( _airfoil_lift_coefficient)
1118 i( _airfoil_drag_coefficient0)
1119 i( _airfoil_drag_coefficient1)
1121 i( _number_of_segments)
1122 i( _number_of_parts)
1124 iv( _groundeffectpos[0])iv( _groundeffectpos[1])iv( _groundeffectpos[2])iv( _groundeffectpos[3])
1125 i( _ground_effect_altitude)
1127 iv( _normal_with_yaw_roll)
1130 i( _number_of_blades)
1131 i( _weight_per_blade)
1132 i( _rel_blade_center)
1135 i( _force_at_pitch_a)
1137 i( _power_at_pitch_0)
1138 i( _power_at_pitch_b)
1155 i( _alphamin) i(_alphamax) i(_alpha0) i(_alpha0factor)
1156 i( _teeterdamp) i(_maxteeterdamp)
1157 i( _rellenteeterhinge)
1159 i( _translift_maxfactor)
1160 i( _ground_effect_constant)
1161 i( _vortex_state_lift_factor)
1162 i( _vortex_state_c1)
1163 i( _vortex_state_c2)
1164 i( _vortex_state_c3)
1165 i( _vortex_state_e1)
1166 i( _vortex_state_e2)
1167 i( _vortex_state_e3)
1168 i( _lift_factor) i(_f_ge) i(_f_vs) i(_f_tl)
1173 i( _twist) //outer incidence = inner inner incidence + _twist
1174 i( _rel_len_where_incidence_is_measured)
1175 i( _torque_of_inertia)
1176 i( _rel_len_blade_start)
1177 i( _incidence_stall_zero_speed)
1178 i( _incidence_stall_half_sonic_speed)
1179 i( _lift_factor_stall)
1180 i( _stall_change_over)
1181 i( _drag_factor_stall)
1188 i( _cyclic_factor) <<endl;
1190 for(j=0; j<r._rotorparts.size(); j++) {
1191 out << *((Rotorpart*)r._rotorparts.get(j));
1198 void Rotor:: writeInfo()
1201 std::ostringstream buffer;
1203 FILE*f=fopen("c:\\fgmsvc\\bat\\log.txt","at");
1204 if (!f) f=fopen("c:\\fgmsvc\\bat\\log.txt","wt");
1207 fprintf(f,"%s",(const char *)buffer.str().c_str());
1212 Rotorpart* Rotor::newRotorpart(float* pos, float *posforceattac, float *normal,
1213 float* speed,float *dirzentforce, float zentforce,float maxpitchforce,
1214 float delta3,float mass,float translift,float rellenhinge,float len)
1216 Rotorpart *r = new Rotorpart();
1217 r->setPosition(pos);
1218 r->setNormal(normal);
1219 r->setZentipetalForce(zentforce);
1220 r->setPositionForceAttac(posforceattac);
1222 r->setDirectionofZentipetalforce(dirzentforce);
1223 r->setDelta3(delta3);
1224 r->setDynamic(_dynamic);
1225 r->setTranslift(_translift);
1228 r->setRelLenHinge(rellenhinge);
1229 r->setSharedFlapHinge(_shared_flap_hinge);
1230 r->setOmegaN(_omegan);
1231 r->setPhi(_phi_null);
1232 r->setAlpha0(_alpha0);
1233 r->setAlphamin(_alphamin);
1234 r->setAlphamax(_alphamax);
1235 r->setAlpha0factor(_alpha0factor);
1237 r->setDiameter(_diameter);
1239 #define p(a) r->setParameter(#a,_##a);
1241 p(number_of_segments)
1242 p(rel_len_where_incidence_is_measured)
1243 p(rel_len_blade_start)
1244 p(rotor_correction_factor)
1249 void Rotor::interp(float* v1, float* v2, float frac, float* out)
1251 out[0] = v1[0] + frac*(v2[0]-v1[0]);
1252 out[1] = v1[1] + frac*(v2[1]-v1[1]);
1253 out[2] = v1[2] + frac*(v2[2]-v1[2]);
1256 void Rotorgear::initRotorIteration(float *lrot,float dt)
1260 if (!_rotors.size()) return;
1261 Rotor* r0 = (Rotor*)_rotors.get(0);
1262 omegarel= r0->getOmegaRelNeu();
1263 for(i=0; i<_rotors.size(); i++) {
1264 Rotor* r = (Rotor*)_rotors.get(i);
1265 r->inititeration(dt,omegarel,0,lrot);
1269 void Rotorgear::calcForces(float* torqueOut)
1272 torqueOut[0]=torqueOut[1]=torqueOut[2]=0;
1273 // check,<if the engine can handle the torque of the rotors.
1274 // If not reduce the torque to the fueselage and change rotational
1275 // speed of the rotors instead
1278 float omegarel,omegan;
1279 Rotor* r0 = (Rotor*)_rotors.get(0);
1280 omegarel= r0->getOmegaRel();
1282 float total_torque_of_inertia=0;
1283 float total_torque=0;
1284 for(i=0; i<_rotors.size(); i++) {
1285 Rotor* r = (Rotor*)_rotors.get(i);
1286 omegan=r->getOmegan();
1287 total_torque_of_inertia+=r->getTorqueOfInertia()*omegan*omegan;
1288 //FIXME: this is constant, so this can be done in compile
1290 total_torque+=r->getTorque()*omegan;
1292 float max_torque_of_engine=0;
1295 max_torque_of_engine=_max_power_engine;
1296 float df=1-omegarel;
1297 df/=_engine_prop_factor;
1298 df = Math::clamp(df, 0, 1);
1299 max_torque_of_engine = df * _max_power_engine;
1303 float rel_torque_engine=1;
1304 if (total_torque<=0)
1305 rel_torque_engine=0;
1307 if (max_torque_of_engine>0)
1308 rel_torque_engine=1/max_torque_of_engine*total_torque;
1310 rel_torque_engine=0;
1312 //add the rotor brake and the gear fritcion
1314 if (r0->_rotorparts.size()) dt=((Rotorpart*)r0->_rotorparts.get(0))->getDt();
1316 float rotor_brake_torque;
1317 rotor_brake_torque=_rotorbrake*_max_power_rotor_brake+_rotorgear_friction;
1318 //clamp it to the value you need to stop the rotor
1319 //to avod accelerate the rotor to neagtive rpm:
1320 rotor_brake_torque=Math::clamp(rotor_brake_torque,0,
1321 total_torque_of_inertia/dt*omegarel);
1322 max_torque_of_engine-=rotor_brake_torque;
1324 //change the rotation of the rotors
1325 if ((max_torque_of_engine<total_torque) //decreasing rotation
1326 ||((max_torque_of_engine>total_torque)&&(omegarel<1))
1327 //increasing rotation due to engine
1328 ||(total_torque<0) ) //increasing rotation due to autorotation
1330 _ddt_omegarel=(max_torque_of_engine-total_torque)/total_torque_of_inertia;
1331 if(max_torque_of_engine>total_torque)
1333 //check if the acceleration is due to the engine. If yes,
1334 //the engine self limits the accel.
1335 float lim1=-total_torque/total_torque_of_inertia;
1336 //accel. by autorotation
1338 if (lim1<_engine_accel_limit) lim1=_engine_accel_limit;
1339 //if the accel by autorotation greater than the max. engine
1340 //accel, then this is the limit, if not: the engine is the limit
1341 if (_ddt_omegarel>lim1) _ddt_omegarel=lim1;
1343 if (_ddt_omegarel>5.5)_ddt_omegarel=5.5;
1344 //clamp it to avoid overflow. Should never be reached
1345 if (_ddt_omegarel<-5.5)_ddt_omegarel=-5.5;
1347 if (_max_power_engine<0.001) {omegarel=1;_ddt_omegarel=0;}
1348 //for debug: negative or no maxpower will result
1349 //in permanet 100% rotation
1351 omegarel+=dt*_ddt_omegarel;
1353 if (omegarel>2.5) omegarel=2.5;
1354 //clamp it to avoid overflow. Should never be reached
1355 if (omegarel<-.5) omegarel=-.5;
1357 r0->setOmegaRelNeu(omegarel);
1358 //calculate the torque, which is needed to accelerate the rotors.
1359 //Add this additional torque to the body
1360 for(j=0; j<_rotors.size(); j++) {
1361 Rotor* r = (Rotor*)_rotors.get(j);
1362 for(i=0; i<r->_rotorparts.size(); i++) {
1363 float torque_scalar=0;
1364 Rotorpart* rp = (Rotorpart*)r->_rotorparts.get(i);
1366 rp->getAccelTorque(_ddt_omegarel,torque);
1367 Math::add3(torque,torqueOut,torqueOut);
1371 _total_torque_on_engine=total_torque+_ddt_omegarel*total_torque_of_inertia;
1375 void Rotorgear::addRotor(Rotor* rotor)
1381 void Rotorgear::compile()
1384 for(int j=0; j<_rotors.size(); j++) {
1385 Rotor* r = (Rotor*)_rotors.get(j);
1390 void Rotorgear::getDownWash(float *pos, float * v_heli, float *downwash)
1393 downwash[0]=downwash[1]=downwash[2]=0;
1394 for(int i=0; i<_rotors.size(); i++) {
1395 Rotor* ro = (Rotor*)_rotors.get(i);
1396 ro->getDownWash(pos,v_heli,tmp);
1397 Math::add3(downwash,tmp,downwash); // + downwash
1401 void Rotorgear::setParameter(char *parametername, float value)
1403 #define p(a,b) if (strcmp(parametername,#a)==0) _##a = (value * (b)); else
1404 p(max_power_engine,1000)
1405 p(engine_prop_factor,1)
1406 p(yasimdragfactor,1)
1407 p(yasimliftfactor,1)
1408 p(max_power_rotor_brake,1000)
1409 p(rotorgear_friction,1000)
1410 p(engine_accel_limit,0.01)
1411 SG_LOG(SG_INPUT, SG_ALERT,
1412 "internal error in parameter set up for rotorgear: '"
1413 << parametername <<"'" << endl);
1416 int Rotorgear::getValueforFGSet(int j,char *text,float *f)
1420 sprintf(text,"/rotors/gear/total-torque");
1421 *f=_total_torque_on_engine;
1425 Rotorgear::Rotorgear()
1430 _max_power_rotor_brake=1;
1431 _rotorgear_friction=1;
1432 _max_power_engine=1000*450;
1433 _engine_prop_factor=0.05f;
1437 _engine_accel_limit=0.05f;
1438 _total_torque_on_engine=0;
1441 Rotorgear::~Rotorgear()
1443 for(int i=0; i<_rotors.size(); i++)
1444 delete (Rotor*)_rotors.get(i);
1447 }; // namespace yasim