543 lines
13 KiB
C++
543 lines
13 KiB
C++
|
#include "Math.hpp"
|
|||
|
|
#include "Rotorblade.hpp"
|
|||
|
|
#include <stdio.h>
|
|||
|
|
//#include <string.h>
|
|||
|
|
//#include <Main/fg_props.hxx>
|
|||
|
|
namespace yasim {
|
|||
|
|
const float pi=3.14159;
|
|||
|
|
|
|||
|
|
Rotorblade::Rotorblade()
|
|||
|
|
{
|
|||
|
|
/*
|
|||
|
|
_orient[0] = 1; _orient[1] = 0; _orient[2] = 0;
|
|||
|
|
_orient[3] = 0; _orient[4] = 1; _orient[5] = 0;
|
|||
|
|
_orient[6] = 0; _orient[7] = 0; _orient[8] = 1;
|
|||
|
|
*/
|
|||
|
|
_collective=0;
|
|||
|
|
_dt=0;
|
|||
|
|
_speed=0;
|
|||
|
|
#define set3(x,a,b,c) x[0]=a;x[1]=b;x[2]=c;
|
|||
|
|
set3 (_directionofzentipetalforce,1,0,0);
|
|||
|
|
#undef set3
|
|||
|
|
_zentipetalforce=1;
|
|||
|
|
_maxpitch=.02;
|
|||
|
|
_maxpitchforce=10;
|
|||
|
|
_delta3=0.5;
|
|||
|
|
_cyclicail=0;
|
|||
|
|
_cyclicele=0;
|
|||
|
|
_collective=0;
|
|||
|
|
_flapatpos[0]=_flapatpos[1]=_flapatpos[2]=_flapatpos[3]=0;
|
|||
|
|
_flapatpos[0]=.1;
|
|||
|
|
_flapatpos[1]=.2;
|
|||
|
|
_flapatpos[2]=.3;
|
|||
|
|
_flapatpos[3]=.4;
|
|||
|
|
_len=1;
|
|||
|
|
_lforceattac=1;
|
|||
|
|
_calcforcesdone=false;
|
|||
|
|
_phi=0;
|
|||
|
|
_omega=0;
|
|||
|
|
_omegan=1;
|
|||
|
|
_mass=10;
|
|||
|
|
_alpha=0;
|
|||
|
|
_alphaoutputbuf[0][0]=0;
|
|||
|
|
_alphaoutputbuf[1][0]=0;
|
|||
|
|
_alpha2type=0;
|
|||
|
|
_alphaalt=0;
|
|||
|
|
_alphaomega=0;
|
|||
|
|
_lastrp=0;
|
|||
|
|
_nextrp=0;
|
|||
|
|
_oppositerp=0;
|
|||
|
|
_translift=0;
|
|||
|
|
_dynamic=100;
|
|||
|
|
_c2=1;
|
|||
|
|
_stepspersecond=240;
|
|||
|
|
_torque_max_force=0;
|
|||
|
|
_torque_no_force=0;
|
|||
|
|
_deltaphi=0;
|
|||
|
|
_alphamin=-.1;
|
|||
|
|
_alphamax= .1;
|
|||
|
|
_alpha0=-.05;
|
|||
|
|
_alpha0factor=1;
|
|||
|
|
_rellenhinge=0;
|
|||
|
|
_teeter=0;
|
|||
|
|
_ddtteeter=0;
|
|||
|
|
_teeterdamp=0.00001;
|
|||
|
|
_maxteeterdamp=0;
|
|||
|
|
_rellenteeterhinge=0.01;
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
void Rotorblade::inititeration(float dt,float *rot)
|
|||
|
|
{
|
|||
|
|
//printf("init %5.3f",dt*1000);
|
|||
|
|
_dt=dt;
|
|||
|
|
_calcforcesdone=false;
|
|||
|
|
float a=Math::dot3(rot,_normal);
|
|||
|
|
_phi+=a;
|
|||
|
|
_phi+=_omega*dt;
|
|||
|
|
while (_phi>(2*pi)) _phi-=2*pi;
|
|||
|
|
while (_phi<(0 )) _phi+=2*pi;
|
|||
|
|
//jetzt noch Drehung des Rumpfes in gleiche Richtung wie alpha bestimmen
|
|||
|
|
//und zu _alphaalt hinzuf<75>gen
|
|||
|
|
//alpha gibt drehung um normal cross dirofzentf an
|
|||
|
|
|
|||
|
|
float dir[3];
|
|||
|
|
Math::cross3(_lright,_normal,dir);
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
a=-Math::dot3(rot,dir);
|
|||
|
|
float alphaneu= _alpha+a;
|
|||
|
|
// alphaneu= Math::clamp(alphaneu,-.5,.5);
|
|||
|
|
//_alphaomega=(alphaneu-_alphaalt)/_dt;//now calculated in calcforces
|
|||
|
|
|
|||
|
|
_alphaalt = alphaneu;
|
|||
|
|
|
|||
|
|
|
|||
|
|
calcFrontRight();
|
|||
|
|
}
|
|||
|
|
void Rotorblade::setTorque(float torque_max_force,float torque_no_force)
|
|||
|
|
{
|
|||
|
|
_torque_max_force=torque_max_force;
|
|||
|
|
_torque_no_force=torque_no_force;
|
|||
|
|
}
|
|||
|
|
void Rotorblade::setAlpha0(float f)
|
|||
|
|
{
|
|||
|
|
_alpha0=f;
|
|||
|
|
}
|
|||
|
|
void Rotorblade::setAlphamin(float f)
|
|||
|
|
{
|
|||
|
|
_alphamin=f;
|
|||
|
|
}
|
|||
|
|
void Rotorblade::setAlphamax(float f)
|
|||
|
|
{
|
|||
|
|
_alphamax=f;
|
|||
|
|
}
|
|||
|
|
void Rotorblade::setAlpha0factor(float f)
|
|||
|
|
{
|
|||
|
|
_alpha0factor=f;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
void Rotorblade::setWeight(float value)
|
|||
|
|
{
|
|||
|
|
_mass=value;
|
|||
|
|
}
|
|||
|
|
float Rotorblade::getWeight(void)
|
|||
|
|
{
|
|||
|
|
return(_mass/.453); //_mass is in kg, returns pounds
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
void Rotorblade::setPosition(float* p)
|
|||
|
|
{
|
|||
|
|
int i;
|
|||
|
|
for(i=0; i<3; i++) _pos[i] = p[i];
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
void Rotorblade::calcFrontRight()
|
|||
|
|
{
|
|||
|
|
float tmpcf[3],tmpsr[3],tmpsf[3],tmpcr[3];
|
|||
|
|
Math::mul3(Math::cos(_phi),_right,tmpcr);
|
|||
|
|
Math::mul3(Math::cos(_phi),_front,tmpcf);
|
|||
|
|
Math::mul3(Math::sin(_phi),_right,tmpsr);
|
|||
|
|
Math::mul3(Math::sin(_phi),_front,tmpsf);
|
|||
|
|
|
|||
|
|
Math::add3(tmpcf,tmpsr,_lfront);
|
|||
|
|
Math::sub3(tmpcr,tmpsf,_lright);
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
void Rotorblade::getPosition(float* out)
|
|||
|
|
{
|
|||
|
|
float dir[3];
|
|||
|
|
Math::mul3(_len,_lfront,dir);
|
|||
|
|
Math::add3(_pos,dir,out);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
void Rotorblade::setPositionForceAttac(float* p)
|
|||
|
|
{
|
|||
|
|
int i;
|
|||
|
|
for(i=0; i<3; i++) _posforceattac[i] = p[i];
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
void Rotorblade::getPositionForceAttac(float* out)
|
|||
|
|
{
|
|||
|
|
float dir[3];
|
|||
|
|
Math::mul3(_len*_rellenhinge*2,_lfront,dir);
|
|||
|
|
Math::add3(_pos,dir,out);
|
|||
|
|
}
|
|||
|
|
void Rotorblade::setSpeed(float p)
|
|||
|
|
{
|
|||
|
|
_speed = p;
|
|||
|
|
}
|
|||
|
|
void Rotorblade::setDirectionofZentipetalforce(float* p)
|
|||
|
|
{
|
|||
|
|
int i;
|
|||
|
|
for(i=0; i<3; i++) _directionofzentipetalforce[i] = p[i];
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
void Rotorblade::setZentipetalForce(float f)
|
|||
|
|
{
|
|||
|
|
_zentipetalforce=f;
|
|||
|
|
}
|
|||
|
|
void Rotorblade::setMaxpitch(float f)
|
|||
|
|
{
|
|||
|
|
_maxpitch=f;
|
|||
|
|
}
|
|||
|
|
void Rotorblade::setMaxPitchForce(float f)
|
|||
|
|
{
|
|||
|
|
_maxpitchforce=f;
|
|||
|
|
}
|
|||
|
|
void Rotorblade::setDelta(float f)
|
|||
|
|
{
|
|||
|
|
_delta=f;
|
|||
|
|
}
|
|||
|
|
void Rotorblade::setDeltaPhi(float f)
|
|||
|
|
{
|
|||
|
|
_deltaphi=f;
|
|||
|
|
}
|
|||
|
|
void Rotorblade::setDelta3(float f)
|
|||
|
|
{
|
|||
|
|
_delta3=f;
|
|||
|
|
}
|
|||
|
|
void Rotorblade::setTranslift(float f)
|
|||
|
|
{
|
|||
|
|
_translift=f;
|
|||
|
|
}
|
|||
|
|
void Rotorblade::setDynamic(float f)
|
|||
|
|
{
|
|||
|
|
_dynamic=f;
|
|||
|
|
}
|
|||
|
|
void Rotorblade::setC2(float f)
|
|||
|
|
{
|
|||
|
|
_c2=f;
|
|||
|
|
}
|
|||
|
|
void Rotorblade::setStepspersecond(float steps)
|
|||
|
|
{
|
|||
|
|
_stepspersecond=steps;
|
|||
|
|
}
|
|||
|
|
void Rotorblade::setRelLenTeeterHinge(float f)
|
|||
|
|
{
|
|||
|
|
_rellenteeterhinge=f;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
void Rotorblade::setTeeterdamp(float f)
|
|||
|
|
{
|
|||
|
|
_teeterdamp=f;
|
|||
|
|
}
|
|||
|
|
void Rotorblade::setMaxteeterdamp(float f)
|
|||
|
|
{
|
|||
|
|
_maxteeterdamp=f;
|
|||
|
|
}
|
|||
|
|
float Rotorblade::getAlpha(int i)
|
|||
|
|
{
|
|||
|
|
i=i&1;
|
|||
|
|
if ((i==0)&&(_first))
|
|||
|
|
return _alpha*180/3.14;//in Grad = 1
|
|||
|
|
else
|
|||
|
|
if(i==0)
|
|||
|
|
return _showa;
|
|||
|
|
else
|
|||
|
|
return _showb;
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
float Rotorblade::getrealAlpha(void)
|
|||
|
|
{
|
|||
|
|
return _alpha;
|
|||
|
|
}
|
|||
|
|
void Rotorblade::setAlphaoutput(char *text,int i)
|
|||
|
|
{
|
|||
|
|
printf("setAlphaoutput Rotorblade [%s] typ %i\n",text,i);
|
|||
|
|
|
|||
|
|
strncpy(_alphaoutputbuf[i>0],text,255);
|
|||
|
|
|
|||
|
|
if (i>0) _alpha2type=i;
|
|||
|
|
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
char* Rotorblade::getAlphaoutput(int i)
|
|||
|
|
{
|
|||
|
|
#define wstep 30
|
|||
|
|
if ((i==0)&&(_first))
|
|||
|
|
{
|
|||
|
|
int winkel=(int)(.5+_phi/pi*180/wstep);
|
|||
|
|
winkel%=(360/wstep);
|
|||
|
|
sprintf(_alphaoutputbuf[0],"/blades/pos%03i",winkel*wstep);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
|
|||
|
|
int winkel=(int)(.5+_phi/pi*180/wstep);
|
|||
|
|
winkel%=(360/wstep);
|
|||
|
|
if (i==0)
|
|||
|
|
sprintf(_alphaoutputbuf[i&1],"/blades/showa_%i_%03i",i,winkel*wstep);
|
|||
|
|
else
|
|||
|
|
if (_first)
|
|||
|
|
sprintf(_alphaoutputbuf[i&1],"/blades/damp_%03i",winkel*wstep);
|
|||
|
|
else
|
|||
|
|
sprintf(_alphaoutputbuf[i&1],"/blades/showb_%i_%03i",i,winkel*wstep);
|
|||
|
|
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
return _alphaoutputbuf[i&1];
|
|||
|
|
#undef wstep
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
void Rotorblade::setNormal(float* p)
|
|||
|
|
{
|
|||
|
|
int i;
|
|||
|
|
for(i=0; i<3; i++) _normal[i] = p[i];
|
|||
|
|
}
|
|||
|
|
void Rotorblade::setFront(float* p)
|
|||
|
|
{
|
|||
|
|
int i;
|
|||
|
|
for(i=0; i<3; i++) _lfront[i]=_front[i] = p[i];
|
|||
|
|
printf("front: %5.3f %5.3f %5.3f\n",p[0],p[1],p[2]);
|
|||
|
|
}
|
|||
|
|
void Rotorblade::setRight(float* p)
|
|||
|
|
{
|
|||
|
|
int i;
|
|||
|
|
for(i=0; i<3; i++) _lright[i]=_right[i] = p[i];
|
|||
|
|
printf("right: %5.3f %5.3f %5.3f\n",p[0],p[1],p[2]);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
void Rotorblade::getNormal(float* out)
|
|||
|
|
{
|
|||
|
|
int i;
|
|||
|
|
for(i=0; i<3; i++) out[i] = _normal[i];
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
void Rotorblade::setCollective(float pos)
|
|||
|
|
{
|
|||
|
|
_collective = pos;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
void Rotorblade::setCyclicele(float pos)
|
|||
|
|
{
|
|||
|
|
_cyclicele = -pos;
|
|||
|
|
}
|
|||
|
|
void Rotorblade::setCyclicail(float pos)
|
|||
|
|
{
|
|||
|
|
_cyclicail = -pos;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
void Rotorblade::setPhi(float value)
|
|||
|
|
{
|
|||
|
|
_phi=value;
|
|||
|
|
if(value==0) _first=1; else _first =0;
|
|||
|
|
}
|
|||
|
|
float Rotorblade::getPhi()
|
|||
|
|
{
|
|||
|
|
return(_phi);
|
|||
|
|
}
|
|||
|
|
void Rotorblade::setOmega(float value)
|
|||
|
|
{
|
|||
|
|
_omega=value;
|
|||
|
|
}
|
|||
|
|
void Rotorblade::setOmegaN(float value)
|
|||
|
|
{
|
|||
|
|
_omegan=value;
|
|||
|
|
}
|
|||
|
|
void Rotorblade::setLen(float value)
|
|||
|
|
{
|
|||
|
|
_len=value;
|
|||
|
|
}
|
|||
|
|
void Rotorblade::setLenHinge(float value)
|
|||
|
|
{
|
|||
|
|
_rellenhinge=value;
|
|||
|
|
}
|
|||
|
|
void Rotorblade::setLforceattac(float value)
|
|||
|
|
{
|
|||
|
|
_lforceattac=value;
|
|||
|
|
}
|
|||
|
|
float Rotorblade::getIncidence()
|
|||
|
|
{
|
|||
|
|
return(_incidence);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
float Rotorblade::getFlapatPos(int k)
|
|||
|
|
{
|
|||
|
|
return _flapatpos[k%4];
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
/*
|
|||
|
|
void Rotorblade::setlastnextrp(Rotorblade*lastrp,Rotorblade*nextrp,Rotorblade *oppositerp)
|
|||
|
|
{
|
|||
|
|
_lastrp=lastrp;
|
|||
|
|
_nextrp=nextrp;
|
|||
|
|
_oppositerp=oppositerp;
|
|||
|
|
}
|
|||
|
|
*/
|
|||
|
|
|
|||
|
|
void Rotorblade::strncpy(char *dest,const char *src,int maxlen)
|
|||
|
|
{
|
|||
|
|
int n=0;
|
|||
|
|
while(src[n]&&n<(maxlen-1))
|
|||
|
|
{
|
|||
|
|
dest[n]=src[n];
|
|||
|
|
n++;
|
|||
|
|
}
|
|||
|
|
dest[n]=0;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
// Calculate the aerodynamic force given a wind vector v (in the
|
|||
|
|
// aircraft's "local" coordinates) and an air density rho. Returns a
|
|||
|
|
// torque about the Y axis, too.
|
|||
|
|
void Rotorblade::calcForce(float* v, float rho, float* out, float* torque)
|
|||
|
|
{
|
|||
|
|
|
|||
|
|
//printf("cf: alt:%g aw:%g ",_alphaalt,_alphaomega);
|
|||
|
|
//if (_first) printf("p: %5.3f e:%5.3f a:%5.3f p:%5.3f",_collective,_cyclicele,_cyclicail,_phi);
|
|||
|
|
if (_calcforcesdone)
|
|||
|
|
{
|
|||
|
|
int i;
|
|||
|
|
for(i=0; i<3; i++) {
|
|||
|
|
torque[i] = _oldt[i];
|
|||
|
|
out[i] = _oldf[i];
|
|||
|
|
}
|
|||
|
|
return;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
{
|
|||
|
|
int k;
|
|||
|
|
if (_omega>0)
|
|||
|
|
for (k=1;k<=4;k++)
|
|||
|
|
{
|
|||
|
|
if ((_phi<=(float(k)*pi/2))&&((_phi+_omega*_dt)>=(float(k)*pi/2)))
|
|||
|
|
{
|
|||
|
|
_flapatpos[k%4]=_alphaalt;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
for (k=0;k<4;k++)
|
|||
|
|
{
|
|||
|
|
if ((_phi>=(float(k)*pi/2))&&((_phi+_omega*_dt)<=(float(k)*pi/2)))
|
|||
|
|
{
|
|||
|
|
_flapatpos[k%4]=_alphaalt;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
float ldt;
|
|||
|
|
int steps=int(_dt*_stepspersecond);
|
|||
|
|
if (steps<=0) steps=1;
|
|||
|
|
ldt=_dt/steps;
|
|||
|
|
float lphi;
|
|||
|
|
float f[3];
|
|||
|
|
f[0]=f[1]=f[2]=0;
|
|||
|
|
float t[3];
|
|||
|
|
t[0]=t[1]=t[2]=0;
|
|||
|
|
|
|||
|
|
//_zentipetalforce=_mass*_omega*_omega*_len*(_rellenhinge+(1-_rellenhinge)*Math::cos(_alphalt));
|
|||
|
|
//_zentipetalforce=_mass*_omega*_omega*_len/(_rellenhinge+(1-_rellenhinge)*Math::cos(_alphalt)); //incl teeter
|
|||
|
|
_speed=_omega*_len*(1-_rellenhinge+_rellenhinge*Math::cos(_alphaalt));
|
|||
|
|
|
|||
|
|
float vrel[3],vreldir[3],speed[3];
|
|||
|
|
Math::mul3(_speed,_lright,speed);
|
|||
|
|
Math::sub3(speed,v,vrel);
|
|||
|
|
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
|
|||
|
|
float lalphaalt=_alphaalt;
|
|||
|
|
float lalpha=_alpha;
|
|||
|
|
float lalphaomega=_alphaomega;
|
|||
|
|
if((_phi>0.01)&&(_first)&&(_phi<0.02))
|
|||
|
|
{
|
|||
|
|
printf("mass:%5.3f delta: %5.3f _dt: %5.7f ldt: %5.7f st:%i w: %5.3f w0: %5.3f\n",
|
|||
|
|
_mass,_delta,_dt,ldt,steps,_omega,Math::sqrt(_zentipetalforce*(1-_rellenhinge)/_len/_mass));
|
|||
|
|
}
|
|||
|
|
for (int step=0;step<steps;step++)
|
|||
|
|
{
|
|||
|
|
lphi=_phi+(step-steps/2.)*ldt*_omega;
|
|||
|
|
//_zentipetalforce=_mass*_omega*_omega*_len/(_rellenhinge+(1-_rellenhinge)*Math::cos(lalphaalt)); //incl teeter
|
|||
|
|
_zentipetalforce=_mass*_omega*_omega*_len;
|
|||
|
|
//printf("[%5.3f]",col);
|
|||
|
|
float beta=-_cyclicele*Math::sin(lphi-0*_deltaphi)+_cyclicail*Math::cos(lphi-0*_deltaphi)+_collective-_delta3*lalphaalt;
|
|||
|
|
if (step==(steps/2)) _incidence=beta;
|
|||
|
|
//printf("be:%5.3f de:%5.3f ",beta,delta);
|
|||
|
|
//printf("\nvd: %5.3f %5.3f %5.3f ",vreldir[0],vreldir[1],vreldir[2]);
|
|||
|
|
//printf("lr: %5.3f %5.3f %5.3f\n",_lright[0],_lright[1],_lright[2]);
|
|||
|
|
//printf("no: %5.3f %5.3f %5.3f ",_normal[0],_normal[1],_normal[2]);
|
|||
|
|
//printf("sp: %5.3f %5.3f %5.3f\n ",speed[0],speed[1],speed[2]);
|
|||
|
|
//printf("vr: %5.3f %5.3f %5.3f ",vrel[0],vrel[1],vrel[2]);
|
|||
|
|
//printf("v : %5.3f %5.3f %5.3f ",v[0],v[1],v[2]);
|
|||
|
|
|
|||
|
|
//float c=_maxpitchforce/(_maxpitch*_zentipetalforce);
|
|||
|
|
|
|||
|
|
float zforcealph=(beta-delta)/_maxpitch*_maxpitchforce*_omega/_omegan;
|
|||
|
|
float zforcezent=(1-_rellenhinge)*Math::sin(lalphaalt)*_zentipetalforce;
|
|||
|
|
float zforcelowspeed=(_omegan-_omega)/_omegan*(lalpha-_alpha0)*_mass*_alpha0factor;
|
|||
|
|
float zf=zforcealph-zforcezent-zforcelowspeed;
|
|||
|
|
_showa=zforcealph;
|
|||
|
|
_showb=-zforcezent;
|
|||
|
|
float vv=Math::sin(lalphaomega)*_len;
|
|||
|
|
zf-=vv*_delta*2*_mass;
|
|||
|
|
vv+=zf/_mass*ldt;
|
|||
|
|
if ((_omega*10)<_omegan)
|
|||
|
|
vv*=.5+5*(_omega/_omegan);//reduce if omega is low
|
|||
|
|
//if (_first) _showb=vv*_delta*2*_mass;//for debugging output
|
|||
|
|
lalpha=Math::asin(Math::sin(lalphaalt)+(vv/_len)*ldt);
|
|||
|
|
lalpha=Math::clamp(lalpha,_alphamin,_alphamax);
|
|||
|
|
float vblade=Math::abs(Math::dot3(_lfront,v));
|
|||
|
|
float tliftfactor=Math::sqrt(1+vblade*_translift);
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
float xforce = Math::cos(lalpha)*_zentipetalforce;//
|
|||
|
|
float zforce = tliftfactor*Math::sin(lalpha)*_zentipetalforce;//
|
|||
|
|
float thetorque = _torque_no_force+_torque_max_force*Math::abs(zforce/_maxpitchforce);
|
|||
|
|
/*
|
|||
|
|
printf("speed: %5.3f %5.3f %5.3f vwind: %5.3f %5.3f %5.3f sin %5.3f\n",
|
|||
|
|
_speed[0],_speed[1],_speed[2],
|
|||
|
|
v[0],v[1],v[2],Math::sin(alpha));
|
|||
|
|
*/
|
|||
|
|
int i;
|
|||
|
|
for(i=0; i<3; i++) {
|
|||
|
|
t[i] += _normal[i]*thetorque;
|
|||
|
|
f[i] += _normal[i]*zforce+_lfront[i]*xforce;
|
|||
|
|
}
|
|||
|
|
lalphaomega=(lalpha-lalphaalt)/ldt;
|
|||
|
|
lalphaalt=lalpha;
|
|||
|
|
|
|||
|
|
/*
|
|||
|
|
_ddtteeter+=_len*_omega/(1-_rellenhinge)*lalphaomega*ldt;
|
|||
|
|
|
|||
|
|
float teeterforce=-_zentipetalforce*Math::sin(_teeter)*_c2;
|
|||
|
|
teeterforce-=Math::clamp(_ddtteeter*_teeterdamp,-_maxteeterdamp,_maxteeterdamp);
|
|||
|
|
_ddtteeter+=teeterforce/_mass;
|
|||
|
|
|
|||
|
|
_teeter+=_ddtteeter*ldt;
|
|||
|
|
if (_first) _showb=_teeter*180/pi;
|
|||
|
|
*/
|
|||
|
|
}
|
|||
|
|
_alpha=lalpha;
|
|||
|
|
_alphaomega=lalphaomega;
|
|||
|
|
/*
|
|||
|
|
if (_first) printf("aneu: %5.3f zfa:%5.3f vv:%g ao:%.3g xf:%.3g zf:%.3g \r",_alpha,zforcealph,vv,_alpha
|
|||
|
|
,xforce,zforce);
|
|||
|
|
*/
|
|||
|
|
int i;
|
|||
|
|
for(i=0; i<3; i++) {
|
|||
|
|
torque[i] = _oldt[i]=t[i]/steps;
|
|||
|
|
out[i] = _oldf[i]=f[i]/steps;
|
|||
|
|
}
|
|||
|
|
_calcforcesdone=true;
|
|||
|
|
//printf("alpha: %5.3f force: %5.3f %5.3f %5.3f %5.3f %5.3f\n",alpha*180/3.14,xforce,zforce,out[0],out[1],out[2]);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
}; // namespace yasim
|