3 #include "Rotorpart.hpp"
4 #include "Rotorblade.hpp"
10 const float pi=3.14159;
26 _base[0] = _base[1] = _base[2] = 0;
33 _force_at_max_pitch=0;
36 _forward[1]=_forward[2]=0;
37 _max_pitch=13./180*pi;
38 _maxcyclicail=10./180*pi;
39 _maxcyclicele=10./180*pi;
41 _mincyclicail=-10./180*pi;
42 _mincyclicele=-10./180*pi;
43 _min_pitch=-.5/180*pi;
45 _normal[0] = _normal[1] = 0;
48 _omega=_omegan=_omegarel=0;
55 _rellenteeterhinge=0.01;
69 for(i=0; i<_rotorparts.size(); i++) {
70 Rotorpart* r = (Rotorpart*)_rotorparts.get(i);
73 for(i=0; i<_rotorblades.size(); i++) {
74 Rotorblade* r = (Rotorblade*)_rotorblades.get(i);
80 void Rotor::inititeration(float dt)
82 if ((_engineon)&&(_omegarel>=1)) return;
83 if ((!_engineon)&&(_omegarel<=0)) return;
84 _omegarel+=dt*1/5.*(_engineon?1:-1); //hier 30
85 _omegarel=Math::clamp(_omegarel,0,1);
86 _omega=_omegan*_omegarel;
88 for(i=0; i<_rotorparts.size(); i++) {
89 Rotorpart* r = (Rotorpart*)_rotorparts.get(i);
92 for(i=0; i<_rotorblades.size(); i++) {
93 Rotorblade* r = (Rotorblade*)_rotorblades.get(i);
98 int Rotor::getValueforFGSet(int j,char *text,float *f)
100 if (_name[0]==0) return 0;
105 if (!numRotorblades()) return 0;
108 sprintf(text,"/rotors/%s/cone", _name);
110 *f=( ((Rotorblade*)getRotorblade(0))->getFlapatPos(0)
111 +((Rotorblade*)getRotorblade(0))->getFlapatPos(1)
112 +((Rotorblade*)getRotorblade(0))->getFlapatPos(2)
113 +((Rotorblade*)getRotorblade(0))->getFlapatPos(3)
120 sprintf(text,"/rotors/%s/roll", _name);
122 *f=( ((Rotorblade*)getRotorblade(0))->getFlapatPos(1)
123 -((Rotorblade*)getRotorblade(0))->getFlapatPos(3)
129 sprintf(text,"/rotors/%s/yaw", _name);
131 *f=( ((Rotorblade*)getRotorblade(0))->getFlapatPos(2)
132 -((Rotorblade*)getRotorblade(0))->getFlapatPos(0)
138 sprintf(text,"/rotors/%s/rpm", _name);
147 if (b>=numRotorblades()) return 0;
149 sprintf(text,"/rotors/%s/blade%i_%s",
151 w==0?"pos":(w==1?"flap":"incidence"));
152 if (w==0) *f=((Rotorblade*)getRotorblade(b))->getPhi()*180/pi;
153 else if (w==1) *f=((Rotorblade*) getRotorblade(b))->getrealAlpha()*180/pi;
154 else *f=((Rotorblade*)getRotorblade(b))->getIncidence()*180/pi;
160 if (4!=numRotorparts()) return 0; //compile first!
163 sprintf(text,"/rotors/%s/cone", _name);
164 *f=( ((Rotorpart*)getRotorpart(0))->getrealAlpha()
165 +((Rotorpart*)getRotorpart(1))->getrealAlpha()
166 +((Rotorpart*)getRotorpart(2))->getrealAlpha()
167 +((Rotorpart*)getRotorpart(3))->getrealAlpha()
173 sprintf(text,"/rotors/%s/roll", _name);
174 *f=( ((Rotorpart*)getRotorpart(0))->getrealAlpha()
175 -((Rotorpart*)getRotorpart(2))->getrealAlpha()
176 )/2*180/pi*(_ccw?-1:1);
181 sprintf(text,"/rotors/%s/yaw", _name);
182 *f=( ((Rotorpart*)getRotorpart(1))->getrealAlpha()
183 -((Rotorpart*)getRotorpart(3))->getrealAlpha()
189 sprintf(text,"/rotors/%s/rpm", _name);
196 if (b>=_number_of_blades) return 0;
198 sprintf(text,"/rotors/%s/blade%i_%s",
200 w==0?"pos":(w==1?"flap":"incidence"));
201 *f=((Rotorpart*)getRotorpart(0))->getPhi()*180/pi+360*b/_number_of_blades*(_ccw?1:-1);
212 rl=Math::sqr(Math::sin(rl*pi/2));
213 rk=Math::sqr(Math::sin(rk*pi/2));
215 if(w==2) {k+=2;l+=2;}
217 if(w==1) {k+=1;l+=1;}
220 if (w==1) *f=rk*((Rotorpart*) getRotorpart(k))->getrealAlpha()*180/pi
221 +rl*((Rotorpart*) getRotorpart(l))->getrealAlpha()*180/pi;
222 else if(w==2) *f=rk*((Rotorpart*)getRotorpart(k))->getIncidence()*180/pi
223 +rl*((Rotorpart*)getRotorpart(l))->getIncidence()*180/pi;
229 void Rotor::setEngineOn(int value)
234 void Rotor::setAlpha0(float f)
238 void Rotor::setAlphamin(float f)
242 void Rotor::setAlphamax(float f)
246 void Rotor::setAlpha0factor(float f)
252 int Rotor::numRotorparts()
254 return _rotorparts.size();
257 Rotorpart* Rotor::getRotorpart(int n)
259 return ((Rotorpart*)_rotorparts.get(n));
261 int Rotor::numRotorblades()
263 return _rotorblades.size();
266 Rotorblade* Rotor::getRotorblade(int n)
268 return ((Rotorblade*)_rotorblades.get(n));
271 void Rotor::strncpy(char *dest,const char *src,int maxlen)
274 while(src[n]&&n<(maxlen-1))
284 void Rotor::setNormal(float* normal)
287 float invsum,sqrsum=0;
288 for(i=0; i<3; i++) { sqrsum+=normal[i]*normal[i];}
291 invsum=1/Math::sqrt(sqrsum);
294 for(i=0; i<3; i++) { _normal[i] = normal[i]*invsum; }
297 void Rotor::setForward(float* forward)
300 float invsum,sqrsum=0;
301 for(i=0; i<3; i++) { sqrsum+=forward[i]*forward[i];}
304 invsum=1/Math::sqrt(sqrsum);
307 for(i=0; i<3; i++) { _forward[i] = forward[i]*invsum; }
311 void Rotor::setForceAtPitchA(float force)
313 _force_at_pitch_a=force;
315 void Rotor::setPowerAtPitch0(float value)
317 _power_at_pitch_0=value;
319 void Rotor::setPowerAtPitchB(float value)
321 _power_at_pitch_b=value;
323 void Rotor::setPitchA(float value)
325 _pitch_a=value/180*pi;
327 void Rotor::setPitchB(float value)
329 _pitch_b=value/180*pi;
331 void Rotor::setBase(float* base)
334 for(i=0; i<3; i++) _base[i] = base[i];
338 void Rotor::setMaxCyclicail(float value)
340 _maxcyclicail=value/180*pi;
342 void Rotor::setMaxCyclicele(float value)
344 _maxcyclicele=value/180*pi;
346 void Rotor::setMinCyclicail(float value)
348 _mincyclicail=value/180*pi;
350 void Rotor::setMinCyclicele(float value)
352 _mincyclicele=value/180*pi;
354 void Rotor::setMinCollective(float value)
356 _min_pitch=value/180*pi;
358 void Rotor::setMaxCollective(float value)
360 _max_pitch=value/180*pi;
362 void Rotor::setDiameter(float value)
366 void Rotor::setWeightPerBlade(float value)
368 _weight_per_blade=value;
370 void Rotor::setNumberOfBlades(float value)
372 _number_of_blades=int(value+.5);
374 void Rotor::setRelBladeCenter(float value)
376 _rel_blade_center=value;
378 void Rotor::setDynamic(float value)
382 void Rotor::setDelta3(float value)
386 void Rotor::setDelta(float value)
390 void Rotor::setTranslift(float value)
394 void Rotor::setC2(float value)
398 void Rotor::setStepspersecond(float steps)
400 _stepspersecond=steps;
402 void Rotor::setRPM(float value)
406 void Rotor::setRelLenHinge(float value)
408 _rel_len_hinge=value;
411 void Rotor::setAlphaoutput(int i, const char *text)
413 //printf("SetAlphaoutput %i [%s]\n",i,text);
414 strncpy(_alphaoutput[i],text,255);
416 void Rotor::setName(const char *text)
418 strncpy(_name,text,128);//128: some space needed for settings
421 void Rotor::setCcw(int ccw)
425 void Rotor::setNotorque(int value)
429 void Rotor::setSimBlades(int value)
434 void Rotor::setRelLenTeeterHinge(float f)
436 _rellenteeterhinge=f;
438 void Rotor::setTeeterdamp(float f)
442 void Rotor::setMaxteeterdamp(float f)
450 void Rotor::setCollective(float lval)
452 lval = Math::clamp(lval, -1, 1);
454 //printf("col: %5.3f\n",lval);
455 for(i=0; i<_rotorparts.size(); i++) {
456 ((Rotorpart*)_rotorparts.get(i))->setCollective(lval);
459 float col=_min_pitch+(lval+1)/2*(_max_pitch-_min_pitch);
460 for(i=0; i<_rotorblades.size(); i++) {
461 ((Rotorblade*)_rotorblades.get(i))->setCollective(col);
465 void Rotor::setCyclicele(float lval,float rval)
467 rval = Math::clamp(rval, -1, 1);
468 lval = Math::clamp(lval, -1, 1);
469 float col=_mincyclicele+(lval+1)/2*(_maxcyclicele-_mincyclicele);
471 for(i=0; i<_rotorblades.size(); i++) {
472 //((Rotorblade*)_rotorblades.get(i))->setCyclicele(col*Math::sin(((Rotorblade*)_rotorblades.get(i))->getPhi()));
473 ((Rotorblade*)_rotorblades.get(i))->setCyclicele(col);
475 if (_rotorparts.size()!=4) return;
476 //printf(" ele: %5.3f %5.3f\n",lval,rval);
477 ((Rotorpart*)_rotorparts.get(1))->setCyclic(lval);
478 ((Rotorpart*)_rotorparts.get(3))->setCyclic(-lval);
480 void Rotor::setCyclicail(float lval,float rval)
482 lval = Math::clamp(lval, -1, 1);
483 rval = Math::clamp(rval, -1, 1);
484 float col=_mincyclicail+(lval+1)/2*(_maxcyclicail-_mincyclicail);
486 for(i=0; i<_rotorblades.size(); i++) {
487 ((Rotorblade*)_rotorblades.get(i))->setCyclicail(col);
489 if (_rotorparts.size()!=4) return;
490 //printf("ail: %5.3f %5.3f\n",lval,rval);
492 ((Rotorpart*)_rotorparts.get(0))->setCyclic(-lval);
493 ((Rotorpart*)_rotorparts.get(2))->setCyclic( lval);
497 float Rotor::getGroundEffect(float* posOut)
501 for(i=0; i<3; i++) posOut[i] = _base[i];
502 float span = _length * Math::cos(_sweep) * Math::cos(_dihedral);
503 span = 2*(span + Math::abs(_base[2]));
508 void Rotor::compile()
510 // Have we already been compiled?
511 if(_rotorparts.size() != 0) return;
513 //rotor is divided into 4 pointlike parts
515 printf("debug: e %f...%f a%f...%f %f...%f\n",
516 _mincyclicele,_maxcyclicele,
517 _mincyclicail,_maxcyclicail,
518 _min_pitch,_max_pitch);
522 _dynamic=_dynamic*(1/ //inverse of the time
523 ( (60/_rotor_rpm)/4 //for rotating 90 deg
524 +(60/_rotor_rpm)/(2*_number_of_blades) //+ meantime a rotorblade will pass a given point
526 float directions[5][3];//pointing forward, right, ... the 5th is ony for calculation
527 directions[0][0]=_forward[0];
528 directions[0][1]=_forward[1];
529 directions[0][2]=_forward[2];
531 printf("Rotor rotating ccw? %i\n",_ccw);
536 Math::cross3(directions[i-1],_normal,directions[i]);
538 Math::cross3(_normal,directions[i-1],directions[i]);
539 Math::unit3(directions[i],directions[i]);
541 Math::set3(directions[4],directions[0]);
542 float rotorpartmass = _weight_per_blade*_number_of_blades/4*.453;//was pounds -> now kg
543 float speed=_rotor_rpm/60*_diameter*_rel_blade_center*pi;
544 float lentocenter=_diameter*_rel_blade_center*0.5;
545 float lentoforceattac=_diameter*_rel_len_hinge*0.5;
546 float zentforce=rotorpartmass*speed*speed/lentocenter;
547 _force_at_max_pitch=_force_at_pitch_a/_pitch_a*_max_pitch;
548 float maxpitchforce=_force_at_max_pitch/4*.453*9.81;//was pounds of force, now N
549 float torque0=0,torquemax=0;
550 float omega=_rotor_rpm/60*2*pi;
552 float omega0=omega*Math::sqrt(1/(1-_rel_len_hinge));
553 //float omega0=omega*Math::sqrt((1-_rel_len_hinge));
554 //_delta=omega*_delta;
555 _delta*=maxpitchforce/(_max_pitch*omega*lentocenter*2*rotorpartmass);
557 float phi=Math::atan2(2*omega*_delta,omega0*omega0-omega*omega);
558 //float relamp=omega*omega/(2*_delta*Math::sqrt(omega0*omega0-_delta*_delta));
559 float relamp=omega*omega/(2*_delta*Math::sqrt(Math::sqr(omega0*omega0-omega*omega)+4*_delta*_delta*omega*omega));
562 torque0=_power_at_pitch_0/4*1000/omega;
563 torquemax=_power_at_pitch_b/4*1000/omega/_pitch_b*_max_pitch;
573 printf("spd: %5.3f lentoc: %5.3f dia: %5.3f rbl: %5.3f hing: %5.3f lfa:%5.3f\n"
574 "zf: %5.3f mpf: %5.3f\n"
575 "tq: %5.3f..%5.3f d3:%5.3f\n"
576 "o/o0: %5.3f phi: %5.3f relamp: %5.3f delta:%5.3f\n"
577 ,speed,lentocenter,_diameter,_rel_blade_center,_rel_len_hinge,
578 lentoforceattac,zentforce,maxpitchforce,
579 torque0,torquemax,_delta3,
580 omega/omega0,phi*180/pi,relamp,_delta);
585 float lpos[3],lforceattac[3],lspeed[3],dirzentforce[3];
587 Math::mul3(lentocenter,directions[i],lpos);
588 Math::add3(lpos,_base,lpos);
589 Math::mul3(lentoforceattac,directions[i+1],lforceattac);//i+1: +90deg (gyro)!!!
590 Math::add3(lforceattac,_base,lforceattac);
591 Math::mul3(speed,directions[i+1],lspeed);
592 Math::mul3(1,directions[i+1],dirzentforce);
594 float maxcyclic=(i&1)?_maxcyclicele:_maxcyclicail;
595 float mincyclic=(i&1)?_mincyclicele:_mincyclicail;
598 Rotorpart* rp=rps[i]=newRotorpart(lpos, lforceattac, _normal,
599 lspeed,dirzentforce,zentforce,maxpitchforce, _max_pitch,_min_pitch,mincyclic,maxcyclic,
600 _delta3,rotorpartmass,_translift,_rel_len_hinge,lentocenter);
601 rp->setAlphaoutput(_alphaoutput[i&1?i:(_ccw?i^2:i)],0);
602 rp->setAlphaoutput(_alphaoutput[4+(i&1?i:(_ccw?i^2:i))],1+(i>1));
604 rp->setTorque(torquemax,torque0);
605 rp->setRelamp(relamp);
612 rps[i]->setlastnextrp(rps[(i-1)%4],rps[(i+1)%4],rps[(i+2)%4]);
617 float directions[5][3];//pointing forward, right, ... the 5th is ony for calculation
618 directions[0][0]=_forward[0];
619 directions[0][1]=_forward[1];
620 directions[0][2]=_forward[2];
622 printf("Rotor rotating ccw? %i\n",_ccw);
627 Math::cross3(directions[i-1],_normal,directions[i]);
629 // Math::cross3(_normal,directions[i-1],directions[i]);
630 Math::unit3(directions[i],directions[i]);
632 Math::set3(directions[4],directions[0]);
633 float speed=_rotor_rpm/60*_diameter*_rel_blade_center*pi;
634 float lentocenter=_diameter*_rel_blade_center*0.5;
635 float lentoforceattac=_diameter*_rel_len_hinge*0.5;
636 float zentforce=_weight_per_blade*.453*speed*speed/lentocenter;
637 _force_at_max_pitch=_force_at_pitch_a/_pitch_a*_max_pitch;
638 float maxpitchforce=_force_at_max_pitch/_number_of_blades*.453*9.81;//was pounds of force, now N
639 float torque0=0,torquemax=0;
640 float omega=_rotor_rpm/60*2*pi;
642 float omega0=omega*Math::sqrt(1/(1-_rel_len_hinge));
643 //float omega0=omega*Math::sqrt(1-_rel_len_hinge);
644 //_delta=omega*_delta;
645 _delta*=maxpitchforce/(_max_pitch*omega*lentocenter*2*_weight_per_blade*.453);
646 float phi=Math::atan2(2*omega*_delta,omega0*omega0-omega*omega);
647 float phi2=Math::abs(omega0-omega)<.000000001?pi/2:Math::atan(2*omega*_delta/(omega0*omega0-omega*omega));
648 float relamp=omega*omega/(2*_delta*Math::sqrt(Math::sqr(omega0*omega0-omega*omega)+4*_delta*_delta*omega*omega));
651 torque0=_power_at_pitch_0/_number_of_blades*1000/omega;
652 torquemax=_power_at_pitch_b/_number_of_blades*1000/omega/_pitch_b*_max_pitch;
662 printf("spd: %5.3f lentoc: %5.3f dia: %5.3f rbl: %5.3f hing: %5.3f lfa:%5.3f\n"
663 "zf: %5.3f mpf: %5.3f\n"
664 "tq: %5.3f..%5.3f d3:%5.3f\n"
665 "o/o0: %5.3f phi: %5.3f relamp:%5.3f delta:%5.3f\n"
666 ,speed,lentocenter,_diameter,_rel_blade_center,_rel_len_hinge,
667 lentoforceattac,zentforce,maxpitchforce,
668 torque0,torquemax,_delta3,
669 omega/omega0,float(phi*180/pi),relamp,_delta);
671 float lspeed[3],dirzentforce[3];
673 float f=(!_ccw)?1:-1;
674 //Math::mul3(f*speed,directions[1],lspeed);
675 Math::mul3(f,directions[1],dirzentforce);
676 for (i=0;i<_number_of_blades;i++)
682 Rotorblade* rb=newRotorblade(_base,_normal,directions[0],directions[1],
683 lentoforceattac,_rel_len_hinge,
684 dirzentforce,zentforce,maxpitchforce, _max_pitch,
685 _delta3,_weight_per_blade*.453,_translift,2*pi/_number_of_blades*i,
686 omega,lentocenter,/*f* */speed);
687 //rp->setAlphaoutput(_alphaoutput[i&1?i:(_ccw?i^2:i)],0);
688 //rp->setAlphaoutput(_alphaoutput[4+(i&1?i:(_ccw?i^2:i))],1+(i>1));
689 _rotorblades.add(rb);
690 rb->setTorque(torquemax,torque0);
691 rb->setDeltaPhi(pi/2.-phi);
692 rb->setDelta(_delta);
694 rb->calcFrontRight();
701 rps[i]->setlastnextrp(rps[(i-1)%4],rps[(i+1)%4],rps[(i+2)%4]);
709 Rotorblade* Rotor::newRotorblade(float* pos, float *normal, float *front, float *right,
710 float lforceattac,float rellenhinge,
711 float *dirzentforce, float zentforce,float maxpitchforce,float maxpitch,
712 float delta3,float mass,float translift,float phi,float omega,float len,float speed)
714 Rotorblade *r = new Rotorblade();
716 r->setNormal(normal);
719 r->setMaxPitchForce(maxpitchforce);
720 r->setZentipetalForce(zentforce);
721 r->setAlpha0(_alpha0);
722 r->setAlphamin(_alphamin);
723 r->setAlphamax(_alphamax);
724 r->setAlpha0factor(_alpha0factor);
729 r->setDirectionofZentipetalforce(dirzentforce);
730 r->setMaxpitch(maxpitch);
731 r->setDelta3(delta3);
732 r->setDynamic(_dynamic);
733 r->setTranslift(_translift);
735 r->setStepspersecond(_stepspersecond);
739 r->setLforceattac(lforceattac);
741 r->setLenHinge(rellenhinge);
742 r->setRelLenTeeterHinge(_rellenteeterhinge);
743 r->setTeeterdamp(_teeterdamp);
744 r->setMaxteeterdamp(_maxteeterdamp);
747 #define a(x) x[0],x[1],x[2]
748 printf("newrp: pos(%5.3f %5.3f %5.3f) pfa (%5.3f %5.3f %5.3f)\n"
749 " nor(%5.3f %5.3f %5.3f) spd (%5.3f %5.3f %5.3f)\n"
750 " dzf(%5.3f %5.3f %5.3f) zf (%5.3f) mpf (%5.3f)\n"
751 " pit (%5.3f..%5.3f) mcy (%5.3f..%5.3f) d3 (%5.3f)\n"
752 ,a(pos),a(posforceattac),a(normal),
753 a(speed),a(dirzentforce),zentforce,maxpitchforce,minpitch,maxpitch,mincyclic,maxcyclic,
760 Rotorpart* Rotor::newRotorpart(float* pos, float *posforceattac, float *normal,
761 float* speed,float *dirzentforce, float zentforce,float maxpitchforce,
762 float maxpitch, float minpitch, float mincyclic,float maxcyclic,
763 float delta3,float mass,float translift,float rellenhinge,float len)
765 Rotorpart *r = new Rotorpart();
767 r->setNormal(normal);
768 r->setMaxPitchForce(maxpitchforce);
769 r->setZentipetalForce(zentforce);
771 r->setPositionForceAttac(posforceattac);
774 r->setDirectionofZentipetalforce(dirzentforce);
775 r->setMaxpitch(maxpitch);
776 r->setMinpitch(minpitch);
777 r->setMaxcyclic(maxcyclic);
778 r->setMincyclic(mincyclic);
779 r->setDelta3(delta3);
780 r->setDynamic(_dynamic);
781 r->setTranslift(_translift);
784 r->setRelLenHinge(rellenhinge);
785 r->setOmegaN(_omegan);
786 r->setAlpha0(_alpha0);
787 r->setAlphamin(_alphamin);
788 r->setAlphamax(_alphamax);
789 r->setAlpha0factor(_alpha0factor);
794 #define a(x) x[0],x[1],x[2]
795 printf("newrp: pos(%5.3f %5.3f %5.3f) pfa (%5.3f %5.3f %5.3f)\n"
796 " nor(%5.3f %5.3f %5.3f) spd (%5.3f %5.3f %5.3f)\n"
797 " dzf(%5.3f %5.3f %5.3f) zf (%5.3f) mpf (%5.3f)\n"
798 " pit (%5.3f..%5.3f) mcy (%5.3f..%5.3f) d3 (%5.3f)\n"
799 ,a(pos),a(posforceattac),a(normal),
800 a(speed),a(dirzentforce),zentforce,maxpitchforce,minpitch,maxpitch,mincyclic,maxcyclic,
806 void Rotor::interp(float* v1, float* v2, float frac, float* out)
808 out[0] = v1[0] + frac*(v2[0]-v1[0]);
809 out[1] = v1[1] + frac*(v2[1]-v1[1]);
810 out[2] = v1[2] + frac*(v2[2]-v1[2]);
813 }; // namespace yasim