+#include <ostream>
+
#include <simgear/debug/logstream.hxx>
#include "Math.hpp"
#include <stdio.h>
#include <string.h>
namespace yasim {
+using std::endl;
+
const float pi=3.14159;
float _help = 0;
Rotorpart::Rotorpart()
_cyclic=0;
_collective=0;
_rellenhinge=0;
+ _shared_flap_hinge=false;
_dt=0;
#define set3(x,a,b,c) x[0]=a;x[1]=b;x[2]=c;
set3 (_speed,1,0,0);
- set3 (_directionofzentipetalforce,1,0,0);
+ set3 (_directionofcentripetalforce,1,0,0);
set3 (_directionofrotorpart,0,1,0);
set3 (_direction_of_movement,1,0,0);
set3 (_last_torque,0,0,0);
#undef set3
- _zentipetalforce=1;
- _maxpitch=.02;
- _minpitch=0;
- _maxcyclic=0.02;
- _mincyclic=-0.02;
+ _centripetalforce=1;
_delta3=0.5;
_cyclic=0;
_collective=-1;
_lastrp=0;
_nextrp=0;
_oppositerp=0;
+ _last90rp=0;
+ _next90rp=0;
_translift=0;
_dynamic=100;
_c2=0;
_diameter=10;
_torque_of_inertia=0;
_rel_len_blade_start=0;
+ _torque=0;
+ _rotor_correction_factor=0.6;
+ _direction=0;
+ _balance=1;
}
void Rotorpart::inititeration(float dt,float *rot)
float a=Math::dot3(rot,_normal);
if(a>0)
_alphaalt=_alpha*Math::cos(a)
- +_nextrp->getrealAlpha()*Math::sin(a);
+ +_next90rp->getrealAlpha()*Math::sin(a);
else
_alphaalt=_alpha*Math::cos(a)
- +_lastrp->getrealAlpha()*Math::sin(-a);
+ +_last90rp->getrealAlpha()*Math::sin(-a);
//calculate the rotation of the fuselage, determine
//the part in the same direction as alpha
//and add it ro _alphaalt
//alpha is rotation about "normal cross dirofzentf"
float dir[3];
- Math::cross3(_directionofzentipetalforce,_normal,dir);
+ Math::cross3(_directionofcentripetalforce,_normal,dir);
a=Math::dot3(rot,dir);
_alphaalt -= a;
_alphaalt= Math::clamp(_alphaalt,_alphamin,_alphamax);
+
+ //unbalance
+ float b;
+ b=_rotor->getBalance();
+ float s =Math::sin(_phi+_direction);
+ //float c =Math::cos(_phi+_direction);
+ if (s>0)
+ _balance=(b>0)?(1.-s*(1.-b)):(1.-s)*(1.+b);
+ else
+ _balance=(b>0)?1.:1.+b;
}
void Rotorpart::setRotor(Rotor *rotor)
_rotor=rotor;
}
-void Rotorpart::setParameter(char *parametername, float value)
+void Rotorpart::setParameter(const char *parametername, float value)
{
#define p(a) if (strcmp(parametername,#a)==0) _##a = value; else
p(number_of_segments)
p(rel_len_where_incidence_is_measured)
p(rel_len_blade_start)
- cout << "internal error in parameter set up for rotorpart: '"
- << parametername <<"'" << endl;
+ p(rotor_correction_factor)
+ SG_LOG(SG_INPUT, SG_ALERT,
+ "internal error in parameter set up for rotorpart: '"
+ << parametername <<"'" << endl);
#undef p
}
void Rotorpart::setPosition(float* p)
{
- int i;
- for(i=0; i<3; i++) _pos[i] = p[i];
+ for(int i=0; i<3; i++) _pos[i] = p[i];
}
float Rotorpart::getIncidence()
void Rotorpart::getPosition(float* out)
{
- int i;
- for(i=0; i<3; i++) out[i] = _pos[i];
+ for(int i=0; i<3; i++) out[i] = _pos[i];
}
void Rotorpart::setPositionForceAttac(float* p)
{
- int i;
- for(i=0; i<3; i++) _posforceattac[i] = p[i];
+ for(int i=0; i<3; i++) _posforceattac[i] = p[i];
}
void Rotorpart::getPositionForceAttac(float* out)
{
- int i;
- for(i=0; i<3; i++) out[i] = _posforceattac[i];
+ for(int i=0; i<3; i++) out[i] = _posforceattac[i];
}
void Rotorpart::setSpeed(float* p)
{
- int i;
- for(i=0; i<3; i++) _speed[i] = p[i];
+ for(int i=0; i<3; i++) _speed[i] = p[i];
Math::unit3(_speed,_direction_of_movement);
}
void Rotorpart::setDirectionofZentipetalforce(float* p)
{
- int i;
- for(i=0; i<3; i++) _directionofzentipetalforce[i] = p[i];
+ for(int i=0; i<3; i++) _directionofcentripetalforce[i] = p[i];
}
void Rotorpart::setDirectionofRotorPart(float* p)
{
- int i;
- for(i=0; i<3; i++) _directionofrotorpart[i] = p[i];
+ for(int i=0; i<3; i++) _directionofrotorpart[i] = p[i];
+}
+
+void Rotorpart::setDirection(float direction)
+{
+ _direction=direction;
}
void Rotorpart::setOmega(float value)
_omega=value;
}
+void Rotorpart::setPhi(float value)
+{
+ _phi=value;
+}
+
void Rotorpart::setOmegaN(float value)
{
_omegan=value;
void Rotorpart::setZentipetalForce(float f)
{
- _zentipetalforce=f;
+ _centripetalforce=f;
}
-void Rotorpart::setMinpitch(float f)
-{
- _minpitch=f;
-}
-
-void Rotorpart::setMaxpitch(float f)
-{
- _maxpitch=f;
-}
-
-void Rotorpart::setMaxcyclic(float f)
-{
- _maxcyclic=f;
-}
-
-void Rotorpart::setMincyclic(float f)
-{
- _mincyclic=f;
-}
void Rotorpart::setDelta3(float f)
{
_rellenhinge=f;
}
+void Rotorpart::setSharedFlapHinge(bool s)
+{
+ _shared_flap_hinge=s;
+}
+
void Rotorpart::setC2(float f)
{
_c2=f;
void Rotorpart::setAlpha0(float f)
{
+ if (f>-0.01) f=-0.01; //half a degree bending
_alpha0=f;
}
i=i&1;
if (i==0)
- return _alpha*180/3.14;//in Grad = 1
+ return _alpha*180/pi;//in Grad = 1
else
{
if (_alpha2type==1) //yaw or roll
return (getAlpha(0)-_oppositerp->getAlpha(0))/2;
else //collective
return (getAlpha(0)+_oppositerp->getAlpha(0)+
- _nextrp->getAlpha(0)+_lastrp->getAlpha(0))/4;
+ _next90rp->getAlpha(0)+_last90rp->getAlpha(0))/4;
}
}
float Rotorpart::getrealAlpha(void)
void Rotorpart::setAlphaoutput(char *text,int i)
{
- SG_LOG(SG_FLIGHT, SG_DEBUG, "setAlphaoutput rotorpart ["
- << text << "] typ" << i);
-
strncpy(_alphaoutputbuf[i>0],text,255);
-
if (i>0) _alpha2type=i;
}
void Rotorpart::setNormal(float* p)
{
- int i;
- for(i=0; i<3; i++) _normal[i] = p[i];
+ for(int i=0; i<3; i++) _normal[i] = p[i];
}
void Rotorpart::getNormal(float* out)
{
- int i;
- for(i=0; i<3; i++) out[i] = _normal[i];
+ for(int i=0; i<3; i++) out[i] = _normal[i];
}
void Rotorpart::setCollective(float pos)
}
void Rotorpart::setlastnextrp(Rotorpart*lastrp,Rotorpart*nextrp,
- Rotorpart *oppositerp)
+ Rotorpart *oppositerp,Rotorpart*last90rp,Rotorpart*next90rp)
{
_lastrp=lastrp;
_nextrp=nextrp;
_oppositerp=oppositerp;
+ _last90rp=last90rp;
+ _next90rp=next90rp;
}
void Rotorpart::strncpy(char *dest,const char *src,int maxlen)
float incidence, float cyc, float alphaalt, float *torque,
float *returnlift)
{
- float moment[3],v_local[3],v_local_scalar,lift_moment,v_flap[3],v_help[3];
- float ias;//nur f. dgb
- int i,n;
- for (i=0;i<3;i++)
- moment[i]=0;
- lift_moment=0;
+ float v_local[3],v_local_scalar,lift_moment,v_flap[3],v_help[3];
+ float relgrav = Math::dot3(_normal,_rotor->getGravDirection());
+ lift_moment=-_mass*_len*9.81*relgrav;
*torque=0;//
if((_nextrp==NULL)||(_lastrp==NULL)||(_rotor==NULL))
- return 0.0;//not initialized. Can happen during startupt of flightgear
+ return 0.0;//not initialized. Can happen during startup of flightgear
if (returnlift!=NULL) *returnlift=0;
- float flap_omega=(_nextrp->getrealAlpha()-_lastrp->getrealAlpha())
+ float flap_omega=(_next90rp->getrealAlpha()-_last90rp->getrealAlpha())
*_omega / pi;
float local_width=_diameter*(1-_rel_len_blade_start)/2.
/(float (_number_of_segments));
- for (n=0;n<_number_of_segments;n++)
+ for (int n=0;n<_number_of_segments;n++)
{
float rel = (n+.5)/(float (_number_of_segments));
float r= _diameter *0.5 *(rel*(1-_rel_len_blade_start)
+_rel_len_blade_start);
- float local_incidence=incidence+_twist *rel - _twist
- *_rel_len_where_incidence_is_measured;
+ float local_incidence=incidence+_twist *rel -
+ _twist *_rel_len_where_incidence_is_measured;
float local_chord = _rotor->getChord()*rel+_rotor->getChord()
*_rotor->getTaper()*(1-rel);
float A = local_chord * local_width;
//substract now the component of the air speed parallel to
//the blade;
- Math::mul3(Math::dot3(v_local,_directionofrotorpart),
+ Math::mul3(Math::dot3(v_local,_directionofrotorpart),
_directionofrotorpart,v_help);
Math::sub3(v_local,v_help,v_local);
//split into direction and magnitude
v_local_scalar=Math::mag3(v_local);
if (v_local_scalar!=0)
- Math::unit3(v_local,v_local);
+ //Math::unit3(v_local,v_help);
+ Math::mul3(1/v_local_scalar,v_local,v_help);
float incidence_of_airspeed = Math::asin(Math::clamp(
- Math::dot3(v_local,_normal),-1,1)) + local_incidence;
- ias = incidence_of_airspeed;
- float lift_wo_cyc =
- _rotor->getLiftCoef(incidence_of_airspeed-cyc,v_local_scalar)
+ Math::dot3(v_help,_normal),-1,1)) + local_incidence;
+ //ias = incidence_of_airspeed;
+
+ //reduce the ias (Prantl factor)
+ float prantl_factor=2/pi*Math::acos(Math::exp(
+ -_rotor->getNumberOfBlades()/2.*(1-rel)
+ *Math::sqrt(1+1/Math::sqr(Math::tan(
+ pi/2-Math::abs(incidence_of_airspeed-local_incidence))))));
+ incidence_of_airspeed = (incidence_of_airspeed+
+ _rotor->getAirfoilIncidenceNoLift())*prantl_factor
+ *_rotor_correction_factor-_rotor->getAirfoilIncidenceNoLift();
+ //ias = incidence_of_airspeed;
+ float lift_wo_cyc = _rotor->getLiftCoef(incidence_of_airspeed
+ -cyc*_rotor_correction_factor*prantl_factor,v_local_scalar)
* v_local_scalar * v_local_scalar * A *rho *0.5;
float lift_with_cyc =
_rotor->getLiftCoef(incidence_of_airspeed,v_local_scalar)
- drag * Math::sin(angle));
*torque += r*(drag * Math::cos(angle)
+ lift * Math::sin(angle));
-
if (returnlift!=NULL) *returnlift+=lift;
}
- float alpha=Math::atan2(lift_moment,_zentipetalforce * _len);
-
+ //use 1st order approximation for alpha
+ //float alpha=Math::atan2(lift_moment,_centripetalforce * _len);
+ float alpha;
+ if (_shared_flap_hinge)
+ {
+ float div=0;
+ if (Math::abs(_alphaalt) >1e-6)
+ div=(_centripetalforce * _len - _mass * _len * 9.81 * relgrav /_alpha0*(_alphaalt+_oppositerp->getAlphaAlt())/(2.0*_alphaalt));
+ if (Math::abs(div)>1e-6)
+ {
+ alpha=lift_moment/div;
+ }
+ else if(Math::abs(_alphaalt+_oppositerp->getAlphaAlt())>1e-6)
+ {
+ float div=(_centripetalforce * _len - _mass * _len * 9.81 *0.5 * relgrav)*(_alphaalt+_oppositerp->getAlphaAlt());
+ if (Math::abs(div)>1e-6)
+ {
+ alpha=_oppositerp->getAlphaAlt()+lift_moment/div*_alphaalt;
+ }
+ else
+ alpha=_alphaalt;
+ }
+ else
+ alpha=_alphaalt;
+ if (_omega/_omegan<0.2)
+ {
+ float frac = 0.001+_omega/_omegan*4.995;
+ alpha=Math::clamp(alpha,_alphamin,_alphamax);
+ alpha=_alphaalt*(1-frac)+frac*alpha;
+ }
+ }
+ else
+ {
+ float div=(_centripetalforce * _len - _mass * _len * 9.81 /_alpha0);
+ if (Math::abs(div)>1e-6)
+ alpha=lift_moment/div;
+ else
+ alpha=_alphaalt;
+ }
+
return (alpha);
}
*torque_scalar=0;
return;
}
- _zentipetalforce=_mass*_len*_omega*_omega;
+ _centripetalforce=_mass*_len*_omega*_omega;
float vrel[3],vreldir[3];
Math::sub3(_speed,v,vrel);
- float scalar_torque=0,alpha_alteberechnung=0;
+ float scalar_torque=0;
Math::unit3(vrel,vreldir);//direction of blade-movement rel. to air
- float delta=Math::asin(Math::dot3(_normal,vreldir));
//Angle of blade which would produce no vertical force (where the
//effective incidence is zero)
- float cyc=_mincyclic+(_cyclic+1)/2*(_maxcyclic-_mincyclic);
- float col=_minpitch+(_collective+1)/2*(_maxpitch-_minpitch);
- _incidence=(col+cyc)-_delta3*_alphaalt;
+ float cyc=_cyclic;
+ float col=_collective;
+ if (_shared_flap_hinge)
+ _incidence=(col+cyc)-_delta3*0.5*(_alphaalt-_oppositerp->getAlphaAlt());
+ else
+ _incidence=(col+cyc)-_delta3*_alphaalt;
//the incidence of the rotorblade due to control input reduced by the
//delta3 effect, see README.YASIM
- float beta=_relamp*cyc+col;
+ //float beta=_relamp*cyc+col;
//the incidence of the rotorblade which is used for the calculation
float alpha,factor; //alpha is the flapping angle
//the new flapping angle will be the old flapping angle
//+ factor *(alpha - "old flapping angle")
- if((_omega*10)>_omegan)
- //the rotor is rotaing quite fast.
- //(at least 10% of the nominal rotational speed)
- {
- alpha=calculateAlpha(v,rho,_incidence,cyc,0,&scalar_torque);
- //the incidence is a function of alpha (if _delta* != 0)
- //Therefore missing: wrap this function in an integrator
- //(runge kutta e. g.)
+ alpha=calculateAlpha(v,rho,_incidence,cyc,0,&scalar_torque);
+ alpha=Math::clamp(alpha,_alphamin,_alphamax);
+ //the incidence is a function of alpha (if _delta* != 0)
+ //Therefore missing: wrap this function in an integrator
+ //(runge kutta e. g.)
- factor=_dt*_dynamic;
- if (factor>1) factor=1;
- }
- else //the rotor is not rotating or rotating very slowly
- {
- alpha=calculateAlpha(v,rho,_incidence,cyc,alpha_alteberechnung,
- &scalar_torque);
- //calculate drag etc., e. g. for deccelrating the rotor if engine
- //is off and omega <10%
-
- float rel =_omega*10 / _omegan;
- alpha=rel * alpha + (1-rel)* _alpha0;
- factor=_dt*_dynamic/10;
- if (factor>1) factor=1;
- }
+ factor=_dt*_dynamic;
+ if (factor>1) factor=1;
- float vz=Math::dot3(_normal,v); //the s
float dirblade[3];
- Math::cross3(_normal,_directionofzentipetalforce,dirblade);
- float vblade=Math::abs(Math::dot3(dirblade,v));
- float tliftfactor=Math::sqrt(1+vblade*_translift);
+ Math::cross3(_normal,_directionofcentripetalforce,dirblade);
+ //float vblade=Math::abs(Math::dot3(dirblade,v));
alpha=_alphaalt+(alpha-_alphaalt)*factor;
_alpha=alpha;
- float meancosalpha=(1*Math::cos(_lastrp->getrealAlpha())
- +1*Math::cos(_nextrp->getrealAlpha())
+ float meancosalpha=(1*Math::cos(_last90rp->getrealAlpha())
+ +1*Math::cos(_next90rp->getrealAlpha())
+1*Math::cos(_oppositerp->getrealAlpha())
+1*Math::cos(alpha))/4;
- float schwenkfactor=1-(Math::cos(_lastrp->getrealAlpha())-meancosalpha);
+ float schwenkfactor=1-(Math::cos(_lastrp->getrealAlpha())-meancosalpha)*_rotor->getNumberOfParts()/4;
//missing: consideration of rellenhinge
- float xforce = Math::cos(alpha)*_zentipetalforce;
- float zforce = schwenkfactor*Math::sin(alpha)*_zentipetalforce;
+
+ //add the unbalance
+ _centripetalforce*=_balance;
+ scalar_torque*=_balance;
+
+ float xforce = Math::cos(alpha)*_centripetalforce;
+ float zforce = schwenkfactor*Math::sin(alpha)*_centripetalforce;
*torque_scalar=scalar_torque;
scalar_torque+= 0*_ddt_omega*_torque_of_inertia;
float thetorque = scalar_torque;
- int i;
float f=_rotor->getCcw()?1:-1;
- for(i=0; i<3; i++) {
+ for(int i=0; i<3; i++) {
_last_torque[i]=torque[i] = f*_normal[i]*thetorque;
out[i] = _normal[i]*zforce*_rotor->getLiftFactor()
- +_directionofzentipetalforce[i]*xforce;
+ +_directionofcentripetalforce[i]*xforce;
}
}
void Rotorpart::getAccelTorque(float relaccel,float *t)
{
- int i;
float f=_rotor->getCcw()?1:-1;
- for(i=0; i<3; i++) {
- t[i]=f*-1* _normal[i]*relaccel*_omegan* _torque_of_inertia;
+ for(int i=0; i<3; i++) {
+ t[i]=f*-1* _normal[i]*relaccel*_omegan* _torque_of_inertia;// *_omeagan ?
_rotor->addTorque(-relaccel*_omegan* _torque_of_inertia);
}
}
+
+std::ostream & operator<<(std::ostream & out, const Rotorpart& rp)
+{
+#define i(x) << #x << ":" << rp.x << endl
+#define iv(x) << #x << ":" << rp.x[0] << ";" << rp.x[1] << ";" <<rp.x[2] << ";" << endl
+ out << "Writing Info on Rotorpart " << endl
+ i( _dt)
+ iv( _last_torque)
+ i( _compiled)
+ iv( _pos) // position in local coords
+ iv( _posforceattac) // position in local coords
+ iv( _normal) //direcetion of the rotation axis
+ i( _torque_max_force)
+ i( _torque_no_force)
+ iv( _speed)
+ iv( _direction_of_movement)
+ iv( _directionofcentripetalforce)
+ iv( _directionofrotorpart)
+ i( _centripetalforce)
+ i( _cyclic)
+ i( _collective)
+ i( _delta3)
+ i( _dynamic)
+ i( _translift)
+ i( _c2)
+ i( _mass)
+ i( _alpha)
+ i( _alphaalt)
+ i( _alphamin) i(_alphamax) i(_alpha0) i(_alpha0factor)
+ i( _rellenhinge)
+ i( _relamp)
+ i( _omega) i(_omegan) i(_ddt_omega)
+ i( _phi)
+ i( _len)
+ i( _incidence)
+ i( _twist) //outer incidence = inner inner incidence + _twist
+ i( _number_of_segments)
+ i( _rel_len_where_incidence_is_measured)
+ i( _rel_len_blade_start)
+ i( _diameter)
+ i( _torque_of_inertia)
+ i( _torque)
+ i (_alphaoutputbuf[0])
+ i (_alphaoutputbuf[1])
+ i( _alpha2type)
+ i(_rotor_correction_factor)
+ << endl;
+#undef i
+#undef iv
+ return out;
+}
}; // namespace yasim