fly/flightgear
1
0
Fork 0
Code Activity

Giant helicopter code update from Maik Justus.

Browse source
This commit is contained in:
andy 2006-08-14 21:59:44 +00:00
parent ad0921f3e0
commit 0838ca6d35
15 changed files with 1514 additions and 1632 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

72
src/FDM/YASim/Airplane.cpp
View file

@ -6,7 +6,6 @@
#include "RigidBody.hpp"
#include "Surface.hpp"
#include "Rotorpart.hpp"
#include "Rotorblade.hpp"
#include "Thruster.hpp"
#include "Airplane.hpp"
@ -87,8 +86,6 @@ Airplane::~Airplane()
delete (Wing*)_vstabs.get(i);
for(i=0; i<_weights.size(); i++)
delete (WeightRec*)_weights.get(i);
for(i=0; i<_rotors.size(); i++)
delete (Rotor*)_rotors.get(i);
}
void Airplane::iterate(float dt)
@ -168,6 +165,11 @@ Launchbar* Airplane::getLaunchbar()
return _model.getLaunchbar();
}
Rotorgear* Airplane::getRotorgear()
{
return _model.getRotorgear();
}
void Airplane::updateGearState()
{
for(int i=0; i<_gears.size(); i++) {
@ -276,11 +278,6 @@ void Airplane::addVStab(Wing* vstab)
_vstabs.add(vstab);
}
void Airplane::addRotor(Rotor* rotor)
{
_rotors.add(rotor);
}
void Airplane::addFuselage(float* front, float* back, float width,
float taper, float mid)
{
@ -480,41 +477,9 @@ float Airplane::compileWing(Wing* w)
return wgt;
}
float Airplane::compileRotor(Rotor* r)
float Airplane::compileRotorgear()
{
// Todo: add rotor to model!!!
// Todo: calc and add mass!!!
r->compile();
_model.addRotor(r);
float wgt = 0;
int i;
for(i=0; i<r->numRotorparts(); i++) {
Rotorpart* s = (Rotorpart*)r->getRotorpart(i);
_model.addRotorpart(s);
float mass = s->getWeight();
mass = mass * Math::sqrt(mass);
float pos[3];
s->getPosition(pos);
_model.getBody()->addMass(mass, pos);
wgt += mass;
}
for(i=0; i<r->numRotorblades(); i++) {
Rotorblade* b = (Rotorblade*)r->getRotorblade(i);
_model.addRotorblade(b);
float mass = b->getWeight();
mass = mass * Math::sqrt(mass);
float pos[3];
b->getPosition(pos);
_model.getBody()->addMass(mass, pos);
wgt += mass;
}
return wgt;
return getRotorgear()->compile(_model.getBody());
}
float Airplane::compileFuselage(Fuselage* f)
@ -654,8 +619,9 @@ void Airplane::compile()
int i;
for(i=0; i<_vstabs.size(); i++)
aeroWgt += compileWing((Wing*)_vstabs.get(i));
for(i=0; i<_rotors.size(); i++)
aeroWgt += compileRotor((Rotor*)_rotors.get(i));
// The rotor(s)
aeroWgt += compileRotorgear();
// The fuselage(s)
for(i=0; i<_fuselages.size(); i++)
@ -1077,13 +1043,29 @@ void Airplane::solveHelicopter()
{
_solutionIterations = 0;
_failureMsg = 0;
if (getRotorgear()!=0)
{
Rotorgear* rg = getRotorgear();
applyDragFactor(Math::pow(rg->getYasimDragFactor()/1000,
1/SOLVE_TWEAK));
applyLiftRatio(Math::pow(rg->getYasimLiftFactor(),
1/SOLVE_TWEAK));
}
else
//huh, no wing and no rotor? (_rotorgear is constructed,
//if a rotor is defined
{
applyDragFactor(Math::pow(15.7/1000, 1/SOLVE_TWEAK));
applyLiftRatio(Math::pow(104, 1/SOLVE_TWEAK));
}
setupState(0,0, &_cruiseState);
_model.setState(&_cruiseState);
setupWeights(true);
_controls.reset();
_model.getBody()->reset();
_model.setAir(_cruiseP, _cruiseT,
Atmosphere::calcStdDensity(_cruiseP, _cruiseT));
}
}; // namespace yasim

9
src/FDM/YASim/Airplane.hpp
View file

@ -36,10 +36,6 @@ public:
void setTail(Wing* tail);
void addVStab(Wing* vstab);
void addRotor(Rotor* Rotor);
int getNumRotors() {return _rotors.size();}
Rotor* getRotor(int i) {return (Rotor*)_rotors.get(i);}
void addFuselage(float* front, float* back, float width,
float taper=1, float mid=0.5);
int addTank(float* pos, float cap, float fuelDensity);
@ -64,6 +60,7 @@ public:
int numGear();
Gear* getGear(int g);
Hook* getHook();
Rotorgear* getRotorgear();
Launchbar* getLaunchbar();
int numThrusters() { return _thrusters.size(); }
@ -110,7 +107,7 @@ private:
void solve();
void solveHelicopter();
float compileWing(Wing* w);
float compileRotor(Rotor* w);
float compileRotorgear();
float compileFuselage(Fuselage* f);
void compileGear(GearRec* gr);
void applyDragFactor(float factor);
@ -142,8 +139,6 @@ private:
Vector _weights;
Vector _surfs; // NON-wing Surfaces
Vector _rotors;
Vector _solveWeights;
Vector _cruiseControls;

3
src/FDM/YASim/ControlMap.cpp
View file

@ -210,7 +210,8 @@ void ControlMap::applyControls(float dt)
case COLLECTIVE: ((Rotor*)obj)->setCollective(lval); break;
case CYCLICAIL: ((Rotor*)obj)->setCyclicail(lval,rval); break;
case CYCLICELE: ((Rotor*)obj)->setCyclicele(lval,rval); break;
case ROTORENGINEON: ((Rotor*)obj)->setEngineOn((int)lval); break;
case ROTORBRAKE: ((Rotorgear*)obj)->setRotorBrake(lval); break;
case ROTORENGINEON: ((Rotorgear*)obj)->setEngineOn((int)lval); break;
case REVERSE_THRUST: ((Jet*)obj)->setReverse(lval != 0); break;
case BOOST:
((PistonEngine*)((Thruster*)obj)->getEngine())->setBoost(lval);

1
src/FDM/YASim/ControlMap.hpp
View file

@ -15,6 +15,7 @@ public:
INCIDENCE, FLAP0, FLAP1, SLAT, SPOILER, VECTOR,
BOOST, CASTERING, PROPPITCH, PROPFEATHER,
COLLECTIVE, CYCLICAIL, CYCLICELE, ROTORENGINEON,
ROTORBRAKE,
REVERSE_THRUST, WASTEGATE };
enum { OPT_SPLIT = 0x01,

60
src/FDM/YASim/FGFDM.cpp
View file

@ -16,7 +16,6 @@
#include "TurbineEngine.hpp"
#include "Rotor.hpp"
#include "Rotorpart.hpp"
#include "Rotorblade.hpp"
#include "FGFDM.hpp"
@ -176,7 +175,19 @@ void FGFDM::startElement(const char* name, const XMLAttributes &atts)
v[2] = attrf(a, "z");
_airplane.setPilotPos(v);
} else if(eq(name, "rotor")) {
_airplane.addRotor(parseRotor(a, name));
_airplane.getModel()->getRotorgear()->addRotor(parseRotor(a, name));
} else if(eq(name, "rotorgear")) {
Rotorgear* r = _airplane.getModel()->getRotorgear();
_currObj = r;
#define p(x) if (a->hasAttribute(#x)) r->setParameter((char *)#x,attrf(a,#x) );
p(max_power_engine)
p(engine_prop_factor)
p(yasimdragfactor)
p(yasimliftfactor)
p(max_power_rotor_brake)
p(engine_accell_limit)
#undef p
r->setInUse();
} else if(eq(name, "wing")) {
_airplane.setWing(parseWing(a, name));
} else if(eq(name, "hstab")) {
@ -475,8 +486,8 @@ void FGFDM::setOutputProperties(float dt)
p->prop->setFloatValue(val);
}
for(i=0; i<_airplane.getNumRotors(); i++) {
Rotor*r=(Rotor*)_airplane.getRotor(i);
for(i=0; i<_airplane.getRotorgear()->getNumRotors(); i++) {
Rotor*r=(Rotor*)_airplane.getRotorgear()->getRotor(i);
int j = 0;
float f;
char b[256];
@ -492,15 +503,6 @@ void FGFDM::setOutputProperties(float dt)
if(b[0]) fgSetFloat(b, s->getAlpha(k));
}
}
for(j=0; j < r->numRotorblades(); j++) {
Rotorblade* s = (Rotorblade*)r->getRotorblade(j);
char *b;
int k;
for (k=0; k<2; k++) {
b = s->getAlphaoutput(k);
if(b[0]) fgSetFloat(b, s->getAlpha(k));
}
}
}
float fuelDensity = _airplane.getFuelDensity(0); // HACK
@ -677,9 +679,34 @@ Rotor* FGFDM::parseRotor(XMLAttributes* a, const char* type)
w->setPowerAtPitchB(attrf(a, "poweratpitch_b", 3000));
if(attrb(a,"notorque"))
w->setNotorque(1);
if(attrb(a,"simblades"))
w->setSimBlades(1);
#define p(x) if (a->hasAttribute(#x)) w->setParameter((char *)#x,attrf(a,#x) );
p(translift_ve)
p(translift_maxfactor)
p(ground_effect_constant)
p(vortex_state_lift_factor)
p(vortex_state_c1)
p(vortex_state_c2)
p(vortex_state_c3)
p(vortex_state_e1)
p(vortex_state_e2)
p(twist)
p(number_of_segments)
p(rel_len_where_incidence_is_measured)
p(chord)
p(taper)
p(airfoil_incidence_no_lift)
p(rel_len_blade_start)
p(incidence_stall_zero_speed)
p(incidence_stall_half_sonic_speed)
p(lift_factor_stall)
p(stall_change_over)
p(drag_factor_stall)
p(airfoil_lift_coefficient)
p(airfoil_drag_coefficient0)
p(airfoil_drag_coefficient1)
#undef p
_currObj = w;
return w;
}
@ -853,7 +880,8 @@ int FGFDM::parseOutput(const char* name)
if(eq(name, "COLLECTIVE")) return ControlMap::COLLECTIVE;
if(eq(name, "CYCLICAIL")) return ControlMap::CYCLICAIL;
if(eq(name, "CYCLICELE")) return ControlMap::CYCLICELE;
if(eq(name, "ROTORENGINEON")) return ControlMap::ROTORENGINEON;
if(eq(name, "ROTORGEARENGINEON")) return ControlMap::ROTORENGINEON;
if(eq(name, "ROTORBRAKE")) return ControlMap::ROTORBRAKE;
if(eq(name, "REVERSE_THRUST")) return ControlMap::REVERSE_THRUST;
if(eq(name, "WASTEGATE")) return ControlMap::WASTEGATE;
SG_LOG(SG_FLIGHT,SG_ALERT,"Unrecognized control type '"

136
src/FDM/YASim/Model.cpp
View file

@ -11,7 +11,6 @@
#include "Surface.hpp"
#include "Rotor.hpp"
#include "Rotorpart.hpp"
#include "Rotorblade.hpp"
#include "Glue.hpp"
#include "Ground.hpp"
@ -131,31 +130,22 @@ void Model::initIteration()
}
// FIXME: This method looks to me like it's doing *integration*, not
// initialization. Integration code should ideally go into
// calcForces. Only very "unstiff" problems can be solved well like
// this (see the engine code for an example); I don't know if rotor
// dynamics qualify or not.
// -Andy
// This function initializes some variables for the rotor calculation
// Furthermore it integrates in "void Rotorpart::inititeration
// (float dt,float *rot)" the "rotor orientation" by omega*dt for the
// 3D-visualization of the heli only. and it compensates the rotation
// of the fuselage. The rotor does not follow the rotation of the fuselage.
// Therefore its rotation must be subtracted from the orientation of the
// rotor.
// Maik
void Model::initRotorIteration()
{
int i;
float dt = _integrator.getInterval();
float lrot[3];
if (!_rotorgear.isInUse()) return;
Math::vmul33(_s->orient, _s->rot, lrot);
Math::mul3(dt,lrot,lrot);
for(i=0; i<_rotors.size(); i++) {
Rotor* r = (Rotor*)_rotors.get(i);
r->inititeration(dt);
}
for(i=0; i<_rotorparts.size(); i++) {
Rotorpart* rp = (Rotorpart*)_rotorparts.get(i);
rp->inititeration(dt,lrot);
}
for(i=0; i<_rotorblades.size(); i++) {
Rotorblade* rp = (Rotorblade*)_rotorblades.get(i);
rp->inititeration(dt,lrot);
}
_rotorgear.initRotorIteration(lrot,dt);
}
void Model::iterate()
@ -207,17 +197,9 @@ Surface* Model::getSurface(int handle)
return (Surface*)_surfaces.get(handle);
}
Rotorpart* Model::getRotorpart(int handle)
Rotorgear* Model::getRotorgear(void)
{
return (Rotorpart*)_rotorparts.get(handle);
}
Rotorblade* Model::getRotorblade(int handle)
{
return (Rotorblade*)_rotorblades.get(handle);
}
Rotor* Model::getRotor(int handle)
{
return (Rotor*)_rotors.get(handle);
return &_rotorgear;
}
int Model::addThruster(Thruster* t)
@ -255,19 +237,6 @@ int Model::addSurface(Surface* surf)
return _surfaces.add(surf);
}
int Model::addRotorpart(Rotorpart* rpart)
{
return _rotorparts.add(rpart);
}
int Model::addRotorblade(Rotorblade* rblade)
{
return _rotorblades.add(rblade);
}
int Model::addRotor(Rotor* r)
{
return _rotors.add(r);
}
int Model::addGear(Gear* gear)
{
return _gears.add(gear);
@ -341,6 +310,24 @@ void Model::updateGround(State* s)
_ground_cb->getGroundPlane(pt, global_ground, global_vel);
g->setGlobalGround(global_ground, global_vel);
}
for(i=0; i<_rotorgear.getRotors()->size(); i++) {
Rotor* r = (Rotor*)_rotorgear.getRotors()->get(i);
// Get the point of the rotor center
float pos[3];
r->getPosition(pos);
// Transform the local coordinates to
// global coordinates.
double pt[3];
s->posLocalToGlobal(pos, pt);
// Ask for the ground plane in the global coordinate system
double global_ground[4];
float global_vel[3];
_ground_cb->getGroundPlane(pt, global_ground, global_vel);
r->setGlobalGround(global_ground, global_vel);
}
// The arrester hook
if(_hook) {
@ -370,7 +357,7 @@ void Model::calcForces(State* s)
// step.
_body.setGyro(_gyro);
_body.addTorque(_torque);
int i;
int i,j;
for(i=0; i<_thrusters.size(); i++) {
Thruster* t = (Thruster*)_thrusters.get(i);
float thrust[3], pos[3];
@ -413,44 +400,47 @@ void Model::calcForces(State* s)
_body.addForce(pos, force);
_body.addTorque(torque);
}
for(i=0; i<_rotorparts.size(); i++) {
Rotorpart* sf = (Rotorpart*)_rotorparts.get(i);
for (j=0; j<_rotorgear.getRotors()->size();j++)
{
Rotor* r = (Rotor *)_rotorgear.getRotors()->get(j);
float vs[3], pos[3];
r->getPosition(pos);
localWind(pos, s, vs, alt);
r->calcLiftFactor(vs, _rho,s);
float tq=0;
// total torque of rotor (scalar) for calculating new rotor rpm
for(i=0; i<r->_rotorparts.size(); i++) {
float torque_scalar=0;
Rotorpart* rp = (Rotorpart*)r->_rotorparts.get(i);
// Vsurf = wind - velocity + (rot cross (cg - pos))
float vs[3], pos[3];
sf->getPosition(pos);
localWind(pos, s, vs, alt);
rp->getPosition(pos);
localWind(pos, s, vs, alt,true);
float force[3], torque[3];
sf->calcForce(vs, _rho, force, torque);
//Math::add3(faero, force, faero);
sf->getPositionForceAttac(pos);
rp->calcForce(vs, _rho, force, torque, &torque_scalar);
tq+=torque_scalar;
rp->getPositionForceAttac(pos);
_body.addForce(pos, force);
_body.addTorque(torque);
}
for(i=0; i<_rotorblades.size(); i++) {
Rotorblade* sf = (Rotorblade*)_rotorblades.get(i);
// Vsurf = wind - velocity + (rot cross (cg - pos))
float vs[3], pos[3];
sf->getPosition(pos);
localWind(pos, s, vs, alt);
float force[3], torque[3];
sf->calcForce(vs, _rho, force, torque);
//Math::add3(faero, force, faero);
sf->getPositionForceAttac(pos);
_body.addForce(pos, force);
r->setTorque(tq);
}
if (_rotorgear.isInUse())
{
float torque[3];
_rotorgear.calcForces(torque);
_body.addTorque(torque);
}
// Account for ground effect by multiplying the vertical force
// component by an amount linear with the fraction of the wingspan
// above the ground.
if ((_groundEffectSpan != 0) && (_groundEffect != 0 ))
{
float dist = ground[3] - Math::dot3(ground, _wingCenter);
if(dist > 0 && dist < _groundEffectSpan) {
float fz = Math::dot3(faero, ground);
@ -459,6 +449,7 @@ void Model::calcForces(State* s)
Math::mul3(fz, ground, faero);
_body.addForce(faero);
}
}
// Convert the velocity and rotation vectors to local coordinates
float lrot[3], lv[3];
@ -528,7 +519,7 @@ void Model::newState(State* s)
// Calculates the airflow direction at the given point and for the
// specified aircraft velocity.
void Model::localWind(float* pos, State* s, float* out, float alt)
void Model::localWind(float* pos, State* s, float* out, float alt, bool is_rotor)
{
float tmp[3], lwind[3], lrot[3], lv[3];
@ -556,6 +547,15 @@ void Model::localWind(float* pos, State* s, float* out, float alt)
Math::mul3(-1, out, out); // (negated)
Math::add3(lwind, out, out); // + wind
Math::sub3(out, lv, out); // - velocity
//add the downwash of the rotors if it is not self a rotor
if (_rotorgear.isInUse()&&!is_rotor)
{
_rotorgear.getDownWash(pos,lv,tmp);
Math::add3(out,tmp, out); // + downwash
}
}
}; // namespace yasim

17
src/FDM/YASim/Model.hpp
View file

@ -6,6 +6,7 @@
#include "BodyEnvironment.hpp"
#include "Vector.hpp"
#include "Turbulence.hpp"
#include "Rotor.hpp"
namespace yasim {
@ -14,8 +15,6 @@ class Integrator;
class Thruster;
class Surface;
class Rotorpart;
class Rotorblade;
class Rotor;
class Gear;
class Ground;
class Hook;
@ -43,16 +42,11 @@ public:
// Externally-managed subcomponents
int addThruster(Thruster* t);
int addSurface(Surface* surf);
int addRotorpart(Rotorpart* rpart);
int addRotorblade(Rotorblade* rblade);
int addRotor(Rotor* rotor);
int addGear(Gear* gear);
void addHook(Hook* hook);
void addLaunchbar(Launchbar* launchbar);
Surface* getSurface(int handle);
Rotorpart* getRotorpart(int handle);
Rotorblade* getRotorblade(int handle);
Rotor* getRotor(int handle);
Rotorgear* getRotorgear(void);
Gear* getGear(int handle);
Hook* getHook(void);
Launchbar* getLaunchbar(void);
@ -84,7 +78,8 @@ private:
void initRotorIteration();
void calcGearForce(Gear* g, float* v, float* rot, float* ground);
float gearFriction(float wgt, float v, Gear* g);
void localWind(float* pos, State* s, float* out, float alt);
void localWind(float* pos, State* s, float* out, float alt,
bool is_rotor = false);
Integrator _integrator;
RigidBody _body;
@ -93,9 +88,7 @@ private:
Vector _thrusters;
Vector _surfaces;
Vector _rotorparts;
Vector _rotorblades;
Vector _rotors;
Rotorgear _rotorgear;
Vector _gears;
Hook* _hook;
Launchbar* _launchbar;

3
src/FDM/YASim/RigidBody.cpp
View file

@ -172,8 +172,9 @@ void RigidBody::getAccel(float* pos, float* accelOut)
float a[3];
float rate = Math::mag3(_spin);
Math::set3(_spin, a);
if (rate !=0 )
Math::mul3(1/rate, a, a);
//an else branch is not neccesary. a, which is a=(0,0,0) in the else case, is only used in a dot product
float v[3];
Math::sub3(_cg, pos, v); // v = cg - pos
Math::mul3(Math::dot3(v, a), a, a); // a = a * (v dot a)

977
src/FDM/YASim/Rotor.cpp
View file

File diff suppressed because it is too large Load diff

154
src/FDM/YASim/Rotor.hpp
View file

@ -3,22 +3,39 @@
#include "Vector.hpp"
#include "Rotorpart.hpp"
#include "Rotorblade.hpp"
#include "Integrator.hpp"
#include "RigidBody.hpp"
#include "BodyEnvironment.hpp"
namespace yasim {
class Surface;
class Rotorpart;
const float rho_null=1.184f; //25DegC, 101325Pa
class Rotor {
private:
float _torque;
float _omega,_omegan,_omegarel,_ddt_omega,_omegarelneu;
float _chord;
float _taper;
float _airfoil_incidence_no_lift;
float _collective;
float _airfoil_lift_coefficient;
float _airfoil_drag_coefficient0;
float _airfoil_drag_coefficient1;
int _ccw;
public:
Rotor();
~Rotor();
// Rotor geometry:
void setNormal(float* normal); //the normal vector (direction of rotormast, pointing up)
void setForward(float* forward); //the normal vector pointing forward (for ele and ail)
//void setMaxPitchForce(float force);
void setNormal(float* normal);
//the normal vector (direction of rotormast, pointing up)
void setForward(float* forward);
//the normal vector pointing forward (for ele and ail)
void setForceAtPitchA(float force);
void setPowerAtPitch0(float value);
void setPowerAtPitchB(float value);
@ -44,32 +61,28 @@ public:
void setRPM(float value);
void setRelLenHinge(float value);
void setBase(float* base); // in local coordinates
void getPosition(float* out);
void setCyclicail(float lval,float rval);
void setCyclicele(float lval,float rval);
void setCollective(float lval);
void setAlphaoutput(int i, const char *text);
void setCcw(int ccw);
void setSimBlades(int value);
void setEngineOn(int value);
int getCcw() {return _ccw;};
void setParameter(char *parametername, float value);
void setGlobalGround(double* global_ground, float* global_vel);
float getTorqueOfInertia();
int getValueforFGSet(int j,char *b,float *f);
void setName(const char *text);
void inititeration(float dt);
void inititeration(float dt,float omegarel,float ddt_omegarel,float *rot);
void compile();
void getTip(float* tip);
// Ground effect information, stil missing
void calcLiftFactor(float* v, float rho, State *s);
void getDownWash(float *pos, float * v_heli, float *downwash);
float getGroundEffect(float* posOut);
// Query the list of Rotorpart objects
int numRotorparts();
Rotorpart* getRotorpart(int n);
// Query the list of Rotorblade objects
int numRotorblades();
Rotorblade* getRotorblade(int n);
void setAlpha0(float f);
void setAlphamin(float f);
void setAlphamax(float f);
@ -77,28 +90,33 @@ public:
void setMaxteeterdamp(float f);
void setRelLenTeeterHinge(float value);
void setAlpha0factor(float f);
void setTorque(float f);
void addTorque(float f);
float getTorque() {return _torque;}
float getLiftFactor();
float getLiftCoef(float incidence,float speed);
float getDragCoef(float incidence,float speed);
float getOmegaRel() {return _omegarel;}
float getOmegaRelNeu() {return _omegarelneu;}
void setOmegaRelNeu(float orn) {_omegarelneu=orn;}
float getOmegan() {return _omegan;}
float getTaper() { return _taper;}
float getChord() { return _chord;}
float getOverallStall()
{if (_stall_v2sum !=0 ) return _stall_sum/_stall_v2sum; else return 0;}
Vector _rotorparts;
private:
void strncpy(char *dest,const char *src,int maxlen);
void interp(float* v1, float* v2, float frac, float* out);
float calcStall(float incidence,float speed);
Rotorpart* newRotorpart(float* pos, float *posforceattac, float *normal,
float* speed,float *dirzentforce, float zentforce,float maxpitchforce,float maxpitch, float minpitch, float mincyclic,float maxcyclic,
float* speed,float *dirzentforce, float zentforce,float maxpitchforce,
float maxpitch, float minpitch, float mincyclic,float maxcyclic,
float delta3,float mass,float translift,float rellenhinge,float len);
Rotorblade* newRotorblade(
float* pos, float *normal,float *front, float *right,
float lforceattac,float relenhinge,
float *dirzentforce, float zentforce,float maxpitchforce,float maxpitch,
float delta3,float mass,float translift,float phi,float omega,float len,float speed);
Vector _rotorparts;
Vector _rotorblades;
float _base[3];
float _normal[3];//the normal vector (direction of rotormast, pointing up)
float _normal_with_yaw_roll[3];//the normal vector (perpendicular to rotordisc)
float _forward[3];
float _diameter;
int _number_of_blades;
@ -106,7 +124,6 @@ private:
float _rel_blade_center;
float _min_pitch;
float _max_pitch;
float _force_at_max_pitch;
float _force_at_pitch_a;
float _pitch_a;
float _power_at_pitch_0;
@ -128,18 +145,79 @@ private:
float _stepspersecond;
char _alphaoutput[8][256];
char _name[256];
int _ccw;
int _engineon;
float _omega,_omegan,_omegarel;
float _alphamin,_alphamax,_alpha0,_alpha0factor;
float _teeterdamp,_maxteeterdamp;
float _rellenteeterhinge;
float _translift_ve;
float _translift_maxfactor;
float _ground_effect_constant;
float _vortex_state_lift_factor;
float _vortex_state_c1;
float _vortex_state_c2;
float _vortex_state_c3;
float _vortex_state_e1;
float _vortex_state_e2;
float _vortex_state_e3;
float _lift_factor,_f_ge,_f_vs,_f_tl;
float _vortex_state;
double _global_ground[4];
float _liftcoef;
float _dragcoef0;
float _dragcoef1;
float _twist; //outer incidence = inner inner incidence + _twist
int _number_of_segments;
float _rel_len_where_incidence_is_measured;
float _torque_of_inertia;
float _rel_len_blade_start;
float _incidence_stall_zero_speed;
float _incidence_stall_half_sonic_speed;
float _lift_factor_stall;
float _stall_change_over;
float _drag_factor_stall;
float _stall_sum;
float _stall_v2sum;
float _yaw;
float _roll;
};
class Rotorgear {
private:
int _in_use;
int _engineon;
float _max_power_engine;
float _engine_prop_factor;
float _yasimdragfactor;
float _yasimliftfactor;
float _rotorbrake;
float _max_power_rotor_brake;
float _ddt_omegarel;
float _engine_accell_limit;
Vector _rotors;
public:
Rotorgear();
~Rotorgear();
int isInUse() {return _in_use;}
void setInUse() {_in_use = 1;}
float compile(RigidBody* body);
void addRotor(Rotor* rotor);
int getNumRotors() {return _rotors.size();}
Rotor* getRotor(int i) {return (Rotor*)_rotors.get(i);}
void calcForces(float* torqueOut);
void setParameter(char *parametername, float value);
void setEngineOn(int value);
int getEngineon();
void setRotorBrake(float lval);
float getYasimDragFactor() { return _yasimdragfactor;}
float getYasimLiftFactor() { return _yasimliftfactor;}
float getMaxPowerEngine() { return _max_power_engine;}
float getMaxPowerRotorBrake() { return _max_power_rotor_brake;}
float getRotorBrake() { return _rotorbrake;}
float getEnginePropFactor() {return _engine_prop_factor;}
Vector* getRotors() { return &_rotors;}
void initRotorIteration(float *lrot,float dt);
void getDownWash(float *pos, float * v_heli, float *downwash);
};
}; // namespace yasim

542
src/FDM/YASim/Rotorblade.cpp
View file

@ -1,542 +0,0 @@
#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

146
src/FDM/YASim/Rotorblade.hpp
View file

@ -1,146 +0,0 @@
#ifndef _ROTORBLADE_HPP
#define _ROTORBLADE_HPP
namespace yasim {
class Rotorblade
{
public:
Rotorblade();
// Position of this surface in local coords
void setPosition(float* p);
void getPosition(float* out);
float getPhi();
void setPhi(float value);
void setPositionForceAttac(float* p);
void getPositionForceAttac(float* out);
void setNormal(float* p);
void setFront(float* p);
void setRight(float* p);
void getNormal(float* out);
void setMaxPitchForce(float force);
void setCollective(float pos);
void setCyclicele(float pos);
void setCyclicail(float pos);
void setOmega(float value);
void setOmegaN(float value);
void setLen(float value);
void setLenHinge(float value);
void setLforceattac(float value);
void setSpeed(float p);
void setDirectionofZentipetalforce(float* p);
void setZentipetalForce(float f);
void setMaxpitch(float f);
void setDelta3(float f);
void setDelta(float f);
void setDeltaPhi(float f);
void setDynamic(float f);
void setTranslift(float f);
void setC2(float f);
void setStepspersecond(float steps);
void setZentForce(float f);
float getAlpha(int i);
float getrealAlpha(void);
char* getAlphaoutput(int i);
void setAlphaoutput(char *text,int i);
void inititeration(float dt,float *rot);
float getWeight(void);
void setWeight(float value);
float getFlapatPos(int k);
// local -> Rotorblade coords
//void setOrientation(float* o);
void calcForce(float* v, float rho, float* forceOut, float* torqueOut);
void setlastnextrp(Rotorblade*lastrp,Rotorblade*nextrp,Rotorblade *oppositerp);
void setTorque(float torque_max_force,float torque_no_force);
void calcFrontRight();
float getIncidence();
void setAlpha0(float f);
void setAlphamin(float f);
void setAlphamax(float f);
void setAlpha0factor(float f);
void setTeeterdamp(float f);
void setMaxteeterdamp(float f);
void setRelLenTeeterHinge(float value);
private:
void strncpy(char *dest,const char *src,int maxlen);
Rotorblade *_lastrp,*_nextrp,*_oppositerp;
float _dt;
float _phi,_omega,_omegan;
float _delta;
float _deltaphi;
int _first;
float _len;
float _lforceattac;
float _pos[3]; // position in local coords
float _posforceattac[3]; // position in local coords
float _normal[3]; //direcetion of the rotation axis
float _front[3],_right[3];
float _lright[3],_lfront[3];
float _torque_max_force;
float _torque_no_force;
float _speed;
float _directionofzentipetalforce[3];
float _zentipetalforce;
float _maxpitch;
float _maxpitchforce;
float _cyclicele;
float _cyclicail;
float _collective;
float _delta3;
float _dynamic;
float _flapatpos[4];//flapangle at 0, 90, 180 and 270 degree, for graphics
float _translift;
float _c2;
float _mass;
float _alpha;
float _alphaalt;
float _alphaomega;
float _rellenhinge;
float _incidence;
float _alphamin,_alphamax,_alpha0,_alpha0factor;
float _stepspersecond;
float _teeter,_ddtteeter;
float _teeterdamp,_maxteeterdamp;
float _rellenteeterhinge;
char _alphaoutputbuf[2][256];
int _alpha2type;
//float _orient[9]; // local->surface orthonormal matrix
bool _calcforcesdone;
float _oldt[3],_oldf[3];
float _showa,_showb;
};
}; // namespace yasim
#endif // _ROTORBLADE_HPP

308
src/FDM/YASim/Rotorpart.cpp
View file

@ -2,31 +2,34 @@
#include "Math.hpp"
#include "Rotorpart.hpp"
#include "Rotor.hpp"
#include <stdio.h>
//#include <string.h>
//#include <Main/fg_props.hxx>
#include <string.h>
namespace yasim {
const float pi=3.14159;
float _help = 0;
Rotorpart::Rotorpart()
{
_compiled=0;
_cyclic=0;
_collective=0;
_rellenhinge=0;
_dt=0;
#define set3(x,a,b,c) x[0]=a;x[1]=b;x[2]=c;
#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);
#undef set3
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;
_maxpitchforce=10;
_maxcyclic=0.02;
_mincyclic=-0.02;
_delta3=0.5;
_cyclic=0;
_collective=0;
_collective=-1;
_relamp=1;
_mass=10;
_incidence = 0;
@ -49,26 +52,24 @@ Rotorpart::Rotorpart()
_torque_no_force=0;
_omega=0;
_omegan=1;
_ddt_omega=0;
_phi=0;
_len=1;
_rotor=NULL;
_twist=0;
_number_of_segments=1;
_rel_len_where_incidence_is_measured=0.7;
_diameter=10;
_torque_of_inertia=0;
_rel_len_blade_start=0;
}
void Rotorpart::inititeration(float dt,float *rot)
{
//printf("init %5.3f",dt*1000);
_dt=dt;
_phi+=_omega*dt;
while (_phi>(2*pi)) _phi-=2*pi;
while (_phi<(0 )) _phi+=2*pi;
//_alphaalt=_alpha;
//a=skalarprdukt _normal mit rot ergibt drehung um Normale
//alphaalt=Math::cos(a)*alpha+.5*(Math::sin(a)*alphanachbarnlast-Math::sin(a)*alphanachbanext)
float a=Math::dot3(rot,_normal);
if(a>0)
_alphaalt=_alpha*Math::cos(a)
@ -76,34 +77,52 @@ void Rotorpart::inititeration(float dt,float *rot)
else
_alphaalt=_alpha*Math::cos(a)
+_lastrp->getrealAlpha()*Math::sin(-a);
//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
//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);
a=Math::dot3(rot,dir);
_alphaalt -= a;
_alphaalt= Math::clamp(_alphaalt,_alphamin,_alphamax);
}
void Rotorpart::setRotor(Rotor *rotor)
{
_rotor=rotor;
}
void Rotorpart::setParameter(char *parametername, float value)
{
#define p(a) if (strcmp(parametername,#a)==0) _##a = value; else
p(twist)
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;
#undef p
}
void Rotorpart::setTorque(float torque_max_force,float torque_no_force)
{
_torque_max_force=torque_max_force;
_torque_no_force=torque_no_force;
}
void Rotorpart::setTorqueOfInertia(float toi)
{
_torque_of_inertia=toi;
}
void Rotorpart::setWeight(float value)
{
_mass=value;
}
float Rotorpart::getWeight(void)
{
return(_mass/.453); //_mass is in kg, returns pounds
@ -114,6 +133,7 @@ void Rotorpart::setPosition(float* p)
int i;
for(i=0; i<3; i++) _pos[i] = p[i];
}
float Rotorpart::getIncidence()
{
return(_incidence);
@ -135,94 +155,121 @@ void Rotorpart::getPositionForceAttac(float* out)
{
int i;
for(i=0; i<3; i++) out[i] = _posforceattac[i];
//printf("posforce: %5.3f %5.3f %5.3f ",out[0],out[1],out[2]);
}
void Rotorpart::setSpeed(float* p)
{
int i;
for(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];
}
void Rotorpart::setDirectionofRotorPart(float* p)
{
int i;
for(i=0; i<3; i++) _directionofrotorpart[i] = p[i];
}
void Rotorpart::setOmega(float value)
{
_omega=value;
}
void Rotorpart::setOmegaN(float value)
{
_omegan=value;
}
void Rotorpart::setDdtOmega(float value)
{
_ddt_omega=value;
}
void Rotorpart::setZentipetalForce(float f)
{
_zentipetalforce=f;
}
void Rotorpart::setMinpitch(float f)
{
_minpitch=f;
}
void Rotorpart::setMaxpitch(float f)
{
_maxpitch=f;
}
void Rotorpart::setMaxPitchForce(float f)
{
_maxpitchforce=f;
}
void Rotorpart::setMaxcyclic(float f)
{
_maxcyclic=f;
}
void Rotorpart::setMincyclic(float f)
{
_mincyclic=f;
}
void Rotorpart::setDelta3(float f)
{
_delta3=f;
}
void Rotorpart::setRelamp(float f)
{
_relamp=f;
}
void Rotorpart::setTranslift(float f)
{
_translift=f;
}
void Rotorpart::setDynamic(float f)
{
_dynamic=f;
}
void Rotorpart::setRelLenHinge(float f)
{
_rellenhinge=f;
}
void Rotorpart::setC2(float f)
{
_c2=f;
}
void Rotorpart::setAlpha0(float f)
{
_alpha0=f;
}
void Rotorpart::setAlphamin(float f)
{
_alphamin=f;
}
void Rotorpart::setAlphamax(float f)
{
_alphamax=f;
}
void Rotorpart::setAlpha0factor(float f)
{
_alpha0factor=f;
}
void Rotorpart::setDiameter(float f)
{
_diameter=f;
}
float Rotorpart::getPhi()
{
@ -232,20 +279,23 @@ float Rotorpart::getPhi()
float Rotorpart::getAlpha(int i)
{
i=i&1;
if (i==0)
return _alpha*180/3.14;//in Grad = 1
else
{
if (_alpha2type==1) //yaw or roll
return (getAlpha(0)-_oppositerp->getAlpha(0))/2;
else //pitch
else //collective
return (getAlpha(0)+_oppositerp->getAlpha(0)+
_nextrp->getAlpha(0)+_lastrp->getAlpha(0))/4;
}
}
float Rotorpart::getrealAlpha(void)
{
return _alpha;
}
void Rotorpart::setAlphaoutput(char *text,int i)
{
SG_LOG(SG_FLIGHT, SG_DEBUG, "setAlphaoutput rotorpart ["
@ -254,9 +304,8 @@ void Rotorpart::setAlphaoutput(char *text,int i)
strncpy(_alphaoutputbuf[i>0],text,255);
if (i>0) _alpha2type=i;
}
char* Rotorpart::getAlphaoutput(int i)
{
return _alphaoutputbuf[i&1];
@ -267,7 +316,6 @@ void Rotorpart::setLen(float value)
_len=value;
}
void Rotorpart::setNormal(float* p)
{
int i;
@ -280,7 +328,6 @@ void Rotorpart::getNormal(float* out)
for(i=0; i<3; i++) out[i] = _normal[i];
}
void Rotorpart::setCollective(float pos)
{
_collective = pos;
@ -291,7 +338,8 @@ void Rotorpart::setCyclic(float pos)
_cyclic = pos;
}
void Rotorpart::setlastnextrp(Rotorpart*lastrp,Rotorpart*nextrp,Rotorpart *oppositerp)
void Rotorpart::setlastnextrp(Rotorpart*lastrp,Rotorpart*nextrp,
Rotorpart *oppositerp)
{
_lastrp=lastrp;
_nextrp=nextrp;
@ -309,94 +357,182 @@ void Rotorpart::strncpy(char *dest,const char *src,int maxlen)
dest[n]=0;
}
// Calculate the flapping angle, where zentripetal force and
//lift compensate each other
float Rotorpart::calculateAlpha(float* v_rel_air, float rho,
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;
*torque=0;//
if((_nextrp==NULL)||(_lastrp==NULL)||(_rotor==NULL))
return 0.0;//not initialized. Can happen during startupt of flightgear
if (returnlift!=NULL) *returnlift=0;
float flap_omega=(_nextrp->getrealAlpha()-_lastrp->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++)
{
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_chord = _rotor->getChord()*rel+_rotor->getChord()
*_rotor->getTaper()*(1-rel);
float A = local_chord * local_width;
//calculate the local air speed and the incidence to this speed;
Math::mul3(_omega * r , _direction_of_movement , v_local);
// add speed component due to flapping
Math::mul3(flap_omega * r,_normal,v_flap);
Math::add3(v_flap,v_local,v_local);
Math::sub3(v_rel_air,v_local,v_local);
//v_local is now the total airspeed at the blade
//apparent missing: calculating the local_wind = v_rel_air at
//every point of the rotor. It differs due to aircraft-rotation
//it is considered in v_flap
//substract now the component of the air speed parallel to
//the blade;
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);
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)
* v_local_scalar * v_local_scalar * A *rho *0.5;
float lift_with_cyc =
_rotor->getLiftCoef(incidence_of_airspeed,v_local_scalar)
* v_local_scalar * v_local_scalar *A *rho*0.5;
float lift=lift_wo_cyc+_relamp*(lift_with_cyc-lift_wo_cyc);
//take into account that the rotor is a resonant system where
//the cyclic input hase increased result
float drag = -_rotor->getDragCoef(incidence_of_airspeed,v_local_scalar)
* v_local_scalar * v_local_scalar * A *rho*0.5;
float angle = incidence_of_airspeed - incidence;
//angle between blade movement caused by rotor-rotation and the
//total movement of the blade
lift_moment += r*(lift * Math::cos(angle)
- 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);
return (alpha);
}
// 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 Rotorpart::calcForce(float* v, float rho, float* out, float* torque)
void Rotorpart::calcForce(float* v, float rho, float* out, float* torque,
float* torque_scalar)
{
if (_compiled!=1)
{
for (int i=0;i<3;i++)
torque[i]=out[i]=0;
*torque_scalar=0;
return;
}
_zentipetalforce=_mass*_len*_omega*_omega;
float vrel[3],vreldir[3];
Math::sub3(_speed,v,vrel);
float scalar_torque=0,alpha_alteberechnung=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
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);
//printf("[%5.3f]",col);
_incidence=(col+cyc)-_delta3*_alphaalt;
cyc*=_relamp;
float beta=cyc+col;
//the incidence of the rotorblade due to control input reduced by the
//delta3 effect, see README.YASIM
float beta=_relamp*cyc+col;
//the incidence of the rotorblade which is used for the calculation
//float c=_maxpitchforce/(_maxpitch*_zentipetalforce);
float c,alpha,factor;
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)
{
c=_maxpitchforce/_omegan/(_maxpitch*_mass*_len*_omega);
alpha = c*(beta-delta)/(1+_delta3*c);
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.)
factor=_dt*_dynamic;
if (factor>1) factor=1;
}
else
else //the rotor is not rotating or rotating very slowly
{
alpha=_alpha0;
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;
}
float vz=Math::dot3(_normal,v);
//alpha+=_c2*vz;
float fcw;
if(_c2==0)
fcw=0;
else
//fcw=vz/_c2*_maxpitchforce*_omega/_omegan;
fcw=vz*(_c2-1)*_maxpitchforce*_omega/(_omegan*_omegan*_len*_maxpitch);
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);
alpha=_alphaalt+(alpha-_alphaalt)*factor;
//alpha=_alphaalt+(alpha-_lastrp->getrealAlpha())*factor;
_alpha=alpha;
//float schwenkfactor=1;// 1/Math::cos(_lastrp->getrealAlpha());
float meancosalpha=(1*Math::cos(_lastrp->getrealAlpha())
+1*Math::cos(_nextrp->getrealAlpha())
+1*Math::cos(_oppositerp->getrealAlpha())
+1*Math::cos(alpha))/4;
float schwenkfactor=1-(Math::cos(_lastrp->getrealAlpha())-meancosalpha);
//fehlt: verringerung um rellenhinge
float xforce = /*schwenkfactor*/ Math::cos(alpha)*_zentipetalforce;// /9.81*.453; //N->poundsofforce
float zforce = fcw+tliftfactor*schwenkfactor*Math::sin(alpha)*_zentipetalforce;// /9.81*.453;
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));
*/
//missing: consideration of rellenhinge
float xforce = Math::cos(alpha)*_zentipetalforce;
float zforce = schwenkfactor*Math::sin(alpha)*_zentipetalforce;
*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++) {
torque[i] = _normal[i]*thetorque;
out[i] = _normal[i]*zforce+_directionofzentipetalforce[i]*xforce;
}
//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]);
return;
_last_torque[i]=torque[i] = f*_normal[i]*thetorque;
out[i] = _normal[i]*zforce*_rotor->getLiftFactor()
+_directionofzentipetalforce[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;
_rotor->addTorque(-relaccel*_omegan* _torque_of_inertia);
}
}
}; // namespace yasim

74
src/FDM/YASim/Rotorpart.hpp
View file

@ -2,31 +2,33 @@
#define _ROTORPART_HPP
namespace yasim {
class Rotorpart
{
public:
class Rotor;
class Rotorpart
{
private:
float _dt;
float _last_torque[3];
int _compiled;
public:
Rotorpart();
// Position of this surface in local coords
void setPosition(float* p);
void getPosition(float* out);
void setCompiled() {_compiled=1;}
float getDt() {return _dt;}
void setPositionForceAttac(float* p);
void getPositionForceAttac(float* out);
void setNormal(float* p);
void getNormal(float* out);
void setMaxPitchForce(float force);
void setCollective(float pos);
void setCyclic(float pos);
void getLastTorque(float *t)
{for (int i=0;i<3;i++) t[i]=_last_torque[i];}
void getAccelTorque(float relaccel,float *t);
void setSpeed(float* p);
void setDirectionofZentipetalforce(float* p);
void setDirectionofRotorPart(float* p);
void setZentipetalForce(float f);
void setMaxpitch(float f);
void setMinpitch(float f);
@ -39,27 +41,24 @@ public:
void setZentForce(float f);
void setRelLenHinge(float f);
void setRelamp(float f);
void setDiameter(float f);
float getAlpha(int i);
float getrealAlpha(void);
char* getAlphaoutput(int i);
void setAlphaoutput(char *text,int i);
void inititeration(float dt,float *rot);
float getWeight(void);
void setWeight(float value);
void calcForce(float* v, float rho, float* forceOut, float* torqueOut);
void setlastnextrp(Rotorpart*lastrp,Rotorpart*nextrp,Rotorpart *oppositerp);
void calcForce(float* v, float rho, float* forceOut, float* torqueOut,
float* torque_scalar);
float calculateAlpha(float* v, float rho, float incidence, float cyc,
float alphaalt, float *torque,float *returnlift=0);
void setlastnextrp(Rotorpart*lastrp,Rotorpart*nextrp,
Rotorpart *oppositerp);
void setTorque(float torque_max_force,float torque_no_force);
void setOmega(float value);
void setOmegaN(float value);
void setDdtOmega(float value);
float getIncidence();
float getPhi();
void setAlphamin(float f);
@ -67,24 +66,27 @@ public:
void setAlpha0(float f);
void setAlpha0factor(float f);
void setLen(float value);
void setParameter(char *parametername, float value);
void setRotor(Rotor *rotor);
void setTorqueOfInertia(float toi);
private:
private:
void strncpy(char *dest,const char *src,int maxlen);
Rotorpart *_lastrp,*_nextrp,*_oppositerp;
Rotor *_rotor;
float _dt;
float _pos[3]; // position in local coords
float _posforceattac[3]; // position in local coords
float _normal[3]; //direcetion of the rotation axis
float _torque_max_force;
float _torque_no_force;
float _speed[3];
float _direction_of_movement[3];
float _directionofzentipetalforce[3];
float _directionofrotorpart[3];
float _zentipetalforce;
float _maxpitch;
float _minpitch;
float _maxpitchforce;
float _maxcyclic;
float _mincyclic;
float _cyclic;
@ -99,18 +101,22 @@ private:
float _alphamin,_alphamax,_alpha0,_alpha0factor;
float _rellenhinge;
float _relamp;
float _omega,_omegan;
float _omega,_omegan,_ddt_omega;
float _phi;
float _len;
float _incidence;
float _twist; //outer incidence = inner inner incidence + _twist
int _number_of_segments;
float _rel_len_where_incidence_is_measured;
float _rel_len_blade_start;
float _rel_len_blade_measured;
float _diameter;
float _torque_of_inertia;
float _torque;
// total torque of rotor (scalar) for calculating new rotor rpm
char _alphaoutputbuf[2][256];
int _alpha2type;
};
};
}; // namespace yasim
#endif // _ROTORPART_HPP

2
src/FDM/YASim/Surface.cpp
View file

@ -295,6 +295,8 @@ float Surface::stallFunc(float* v)
float Surface::flapLift(float alpha)
{
float flapLift = _cz * _flapPos * (_flapLift-1);
if(_stalls[0] == 0)
return 0;
if(alpha < 0) alpha = -alpha;
if(alpha < _stalls[0])