From 3b2d97289c7457fb693a974e8635a4b6ee3292ca Mon Sep 17 00:00:00 2001 From: andy Date: Mon, 1 Dec 2003 01:22:27 +0000 Subject: [PATCH] Cleanup and refactoring to better integrate the helicopter code into the core YASim stuff. Mostly cosmetic: whitespace adjustment, dead code & meaningless comment removal, a little code motion to better partition the helicopter handling from the original code (no more giant if() { ... } around the solver). Added a warning to the parser to try to eliminate the string booleans that crept in. There should be NO behavioral changes with this checkin. --- src/FDM/YASim/Airplane.cpp | 109 +++++++++++-------------------------- src/FDM/YASim/Airplane.hpp | 1 + src/FDM/YASim/FGFDM.cpp | 78 +++++++++++++------------- src/FDM/YASim/FGFDM.hpp | 2 +- src/FDM/YASim/Math.cpp | 12 +--- src/FDM/YASim/Math.hpp | 1 - src/FDM/YASim/Model.cpp | 44 ++++++--------- src/FDM/YASim/Model.hpp | 6 +- src/FDM/YASim/Rotor.cpp | 10 ++-- src/FDM/YASim/YASim.cxx | 2 +- 10 files changed, 99 insertions(+), 166 deletions(-) diff --git a/src/FDM/YASim/Airplane.cpp b/src/FDM/YASim/Airplane.cpp index 10bb49055..bc06500c4 100644 --- a/src/FDM/YASim/Airplane.cpp +++ b/src/FDM/YASim/Airplane.cpp @@ -73,7 +73,7 @@ void Airplane::iterate(float dt) // The gear might have moved. Change their aerodynamics. updateGearState(); - _model.iterate(dt); + _model.iterate(); } void Airplane::consumeFuel(float dt) @@ -449,38 +449,19 @@ float Airplane::compileWing(Wing* w) return wgt; } -float Airplane::compileRotor(Rotor* w) +float Airplane::compileRotor(Rotor* r) { - /* todo contact points - // The tip of the wing is a contact point - float tip[3]; - w->getTip(tip); - addContactPoint(tip); - if(w->isMirrored()) { - tip[1] *= -1; - addContactPoint(tip); - } - */ - - // Make sure it's initialized. The surfaces will pop out with - // total drag coefficients equal to their areas, which is what we - // want. - w->compile(); - _model.addRotor(w); + // Todo: add rotor to model!!! + // Todo: calc and add mass!!! + r->compile(); + _model.addRotor(r); float wgt = 0; int i; - /* Todo: add rotor to model!!! - Todo: calc and add mass!!! - */ - for(i=0; inumRotorparts(); i++) { - Rotorpart* s = (Rotorpart*)w->getRotorpart(i); - - //float td = s->getTotalDrag(); - //s->setTotalDrag(td); + for(i=0; inumRotorparts(); i++) { + Rotorpart* s = (Rotorpart*)r->getRotorpart(i); _model.addRotorpart(s); - float mass = s->getWeight(); mass = mass * Math::sqrt(mass); @@ -488,27 +469,20 @@ float Airplane::compileRotor(Rotor* w) s->getPosition(pos); _model.getBody()->addMass(mass, pos); wgt += mass; - } - for(i=0; inumRotorblades(); i++) { - Rotorblade* s = (Rotorblade*)w->getRotorblade(i); - - //float td = s->getTotalDrag(); - //s->setTotalDrag(td); - - _model.addRotorblade(s); + for(i=0; inumRotorblades(); i++) { + Rotorblade* b = (Rotorblade*)r->getRotorblade(i); + _model.addRotorblade(b); - float mass = s->getWeight(); + float mass = b->getWeight(); mass = mass * Math::sqrt(mass); float pos[3]; - s->getPosition(pos); + b->getPosition(pos); _model.getBody()->addMass(mass, pos); wgt += mass; - } - return wgt; } @@ -650,17 +624,14 @@ void Airplane::compile() if (_tail) aeroWgt += compileWing(_tail); int i; - for(i=0; i<_vstabs.size(); i++) { + for(i=0; i<_vstabs.size(); i++) aeroWgt += compileWing((Wing*)_vstabs.get(i)); - } - for(i=0; i<_rotors.size(); i++) { + for(i=0; i<_rotors.size(); i++) aeroWgt += compileRotor((Rotor*)_rotors.get(i)); - } // The fuselage(s) - for(i=0; i<_fuselages.size(); i++) { + for(i=0; i<_fuselages.size(); i++) aeroWgt += compileFuselage((Fuselage*)_fuselages.get(i)); - } // Count up the absolute weight we have float nonAeroWgt = _ballast; @@ -702,16 +673,14 @@ void Airplane::compile() // Ground effect float gepos[3]; - float gespan; + float gespan = 0; if(_wing) gespan = _wing->getGroundEffect(gepos); - else - gespan=0; _model.setGroundEffect(gepos, gespan, 0.15f); solveGear(); - - solve(); + if(_wing && _tail) solve(); + else solveHelicopter(); // Do this after solveGear, because it creates "gear" objects that // we don't want to affect. @@ -829,7 +798,7 @@ void Airplane::runCruise() // Precompute thrust in the model, and calculate aerodynamic forces _model.getBody()->recalc(); _model.getBody()->reset(); - _model.initIteration(.01);//hier parameter egal + _model.initIteration(); _model.calcForces(&_cruiseState); } @@ -872,7 +841,7 @@ void Airplane::runApproach() // Precompute thrust in the model, and calculate aerodynamic forces _model.getBody()->recalc(); _model.getBody()->reset(); - _model.initIteration(.01);//hier parameter egal + _model.initIteration(); _model.calcForces(&_approachState); } @@ -931,19 +900,8 @@ void Airplane::solve() float tmp[3]; _solutionIterations = 0; _failureMsg = 0; - if((_wing)&&(_tail)) - { - while(1) { -#if 0 - printf("%d %f %f %f %f %f\n", //DEBUG - _solutionIterations, - 1000*_dragFactor, - _liftRatio, - _cruiseAoA, - _tailIncidence, - _approachElevator.val); -#endif + while(1) { if(_solutionIterations++ > 10000) { _failureMsg = "Solution failed to converge after 10000 iterations"; return; @@ -1071,19 +1029,16 @@ void Airplane::solve() _failureMsg = "Tail incidence > 10 degrees"; return; } - } - else - { - applyDragFactor(Math::pow(15.7/1000, 1/SOLVE_TWEAK)); - applyLiftRatio(Math::pow(104, 1/SOLVE_TWEAK)); - setupState(0,0, &_cruiseState); - _model.setState(&_cruiseState); - _controls.reset(); - _model.getBody()->reset(); - - - } +} - return; +void Airplane::solveHelicopter() +{ + applyDragFactor(Math::pow(15.7/1000, 1/SOLVE_TWEAK)); + applyLiftRatio(Math::pow(104, 1/SOLVE_TWEAK)); + setupState(0,0, &_cruiseState); + _model.setState(&_cruiseState); + _controls.reset(); + _model.getBody()->reset(); } + }; // namespace yasim diff --git a/src/FDM/YASim/Airplane.hpp b/src/FDM/YASim/Airplane.hpp index 4bed725ea..16f0ca714 100644 --- a/src/FDM/YASim/Airplane.hpp +++ b/src/FDM/YASim/Airplane.hpp @@ -93,6 +93,7 @@ private: void setupState(float aoa, float speed, State* s); void solveGear(); void solve(); + void solveHelicopter(); float compileWing(Wing* w); float compileRotor(Rotor* w); float compileFuselage(Fuselage* f); diff --git a/src/FDM/YASim/FGFDM.cpp b/src/FDM/YASim/FGFDM.cpp index abe884351..c3a2c638c 100644 --- a/src/FDM/YASim/FGFDM.cpp +++ b/src/FDM/YASim/FGFDM.cpp @@ -130,9 +130,7 @@ void FGFDM::startElement(const char* name, const XMLAttributes &atts) _airplane.setWing(parseWing(a, name)); } else if(eq(name, "hstab")) { _airplane.setTail(parseWing(a, name)); - } else if(eq(name, "vstab")) { - _airplane.addVStab(parseWing(a, name)); - } else if(eq(name, "mstab")) { + } else if(eq(name, "vstab") || eq(name, "mstab")) { _airplane.addVStab(parseWing(a, name)); } else if(eq(name, "propeller")) { parsePropeller(a); @@ -380,46 +378,31 @@ void FGFDM::setOutputProperties() for(i=0; i<_airplane.getNumRotors(); i++) { Rotor*r=(Rotor*)_airplane.getRotor(i); - int j=0; + int j = 0; float f; char b[256]; - while(j=r->getValueforFGSet(j,b,&f)) - { - if (b[0]) - { - fgSetFloat(b,f); - } - } + while(j = r->getValueforFGSet(j, b, &f)) + if(b[0]) fgSetFloat(b,f); - for(j=0; jnumRotorparts(); j++) { + for(j=0; j < r->numRotorparts(); j++) { Rotorpart* s = (Rotorpart*)r->getRotorpart(j); char *b; int k; - for (k=0;k<2;k++) - { - b=s->getAlphaoutput(k); - if (b[0]) - { - fgSetFloat(b,s->getAlpha(k)); - //printf("setting [%s]\n",b); - } + for(k=0; k<2; k++) { + b=s->getAlphaoutput(k); + if(b[0]) fgSetFloat(b, s->getAlpha(k)); } } - for(j=0; jnumRotorblades(); j++) { + 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)); - } + for (k=0; k<2; k++) { + b = s->getAlphaoutput(k); + if(b[0]) fgSetFloat(b, s->getAlpha(k)); } } - } - + } for(i=0; i<_thrusters.size(); i++) { EngRec* er = (EngRec*)_thrusters.get(i); @@ -498,6 +481,7 @@ Wing* FGFDM::parseWing(XMLAttributes* a, const char* type) _currObj = w; return w; } + Rotor* FGFDM::parseRotor(XMLAttributes* a, const char* type) { Rotor* w = new Rotor(); @@ -522,8 +506,6 @@ Rotor* FGFDM::parseRotor(XMLAttributes* a, const char* type) forward[2] = attrf(a, "fz"); w->setForward(forward); - - w->setMaxCyclicail(attrf(a, "maxcyclicail", 7.6)); w->setMaxCyclicele(attrf(a, "maxcyclicele", 4.94)); w->setMinCyclicail(attrf(a, "mincyclicail", -7.6)); @@ -553,7 +535,7 @@ Rotor* FGFDM::parseRotor(XMLAttributes* a, const char* type) void setAlphamax(float f); void setAlpha0factor(float f); - if(attristrue(a,"ccw")) + if(attrb(a,"ccw")) w->setCcw(1); if(a->hasAttribute("name")) @@ -572,15 +554,11 @@ Rotor* FGFDM::parseRotor(XMLAttributes* a, const char* type) w->setForceAtPitchA(attrf(a, "forceatpitch_a", 3000)); w->setPowerAtPitch0(attrf(a, "poweratpitch_0", 300)); w->setPowerAtPitchB(attrf(a, "poweratpitch_b", 3000)); - if(attristrue(a,"notorque")) + if(attrb(a,"notorque")) w->setNotorque(1); - if(attristrue(a,"simblades")) + if(attrb(a,"simblades")) w->setSimBlades(1); - - - - _currObj = w; return w; } @@ -765,11 +743,29 @@ float FGFDM::attrf(XMLAttributes* atts, char* attr, float def) else return (float)atof(val); } -bool FGFDM::attristrue(XMLAttributes* atts, char* attr) +// ACK: the dreaded ambiguous string boolean. Remind me to shoot Maik +// when I have a chance. :). Unless you have a parser that can check +// symbol constants (we don't), this kind of coding is just a Bad +// Idea. This implementation, for example, silently returns a boolean +// falsehood for values of "1", "yes", "True", and "TRUE". Which is +// especially annoying preexisting boolean attributes in the same +// parser want to see "1" and will choke on a "true"... +// +// Unfortunately, this usage creeped into existing configuration files +// while I wasn't active, and it's going to be hard to remove. Issue +// a warning to nag people into changing their ways for now... +bool FGFDM::attrb(XMLAttributes* atts, char* attr) { const char* val = atts->getValue(attr); if(val == 0) return false; - else return eq(val,"true"); + + if(eq(val,"true")) { + SG_LOG(SG_FLIGHT, SG_ALERT, "Warning: " << + "deprecated 'true' boolean in YASim configuration file. " << + "Use numeric booleans (attribute=\"1\") instead"); + return true; + } + return attri(atts, attr, 0) ? true : false; } }; // namespace yasim diff --git a/src/FDM/YASim/FGFDM.hpp b/src/FDM/YASim/FGFDM.hpp index ee701b02b..c15eae391 100644 --- a/src/FDM/YASim/FGFDM.hpp +++ b/src/FDM/YASim/FGFDM.hpp @@ -48,7 +48,7 @@ private: int attri(XMLAttributes* atts, char* attr, int def); float attrf(XMLAttributes* atts, char* attr); float attrf(XMLAttributes* atts, char* attr, float def); - bool attristrue(XMLAttributes* atts, char* attr); + bool attrb(XMLAttributes* atts, char* attr); // The core Airplane object we manage. Airplane _airplane; diff --git a/src/FDM/YASim/Math.cpp b/src/FDM/YASim/Math.cpp index 00c5d09e4..3f3643c30 100644 --- a/src/FDM/YASim/Math.cpp +++ b/src/FDM/YASim/Math.cpp @@ -19,10 +19,6 @@ float Math::sqrt(float f) { return (float)::sqrt(f); } -float Math::sqr(float f) -{ - return f*f; -} float Math::ceil(float f) { @@ -142,15 +138,9 @@ float Math::mag3(float* v) return sqrt(dot3(v, v)); } - void Math::unit3(float* v, float* out) { - float mag=mag3(v); - float imag; - if (mag!=0) - imag= 1/mag; - else - imag=1; + float imag = 1/mag3(v); mul3(imag, v, out); } diff --git a/src/FDM/YASim/Math.hpp b/src/FDM/YASim/Math.hpp index 4189cd685..9acfeb4b6 100644 --- a/src/FDM/YASim/Math.hpp +++ b/src/FDM/YASim/Math.hpp @@ -12,7 +12,6 @@ public: // Simple wrappers around library routines static float abs(float f); static float sqrt(float f); - static float sqr(float f); static float ceil(float f); static float sin(float f); static float cos(float f); diff --git a/src/FDM/YASim/Model.cpp b/src/FDM/YASim/Model.cpp index 1a44230cf..ce1d8ecd7 100644 --- a/src/FDM/YASim/Model.cpp +++ b/src/FDM/YASim/Model.cpp @@ -1,5 +1,3 @@ -#include - #include "Atmosphere.hpp" #include "Thruster.hpp" #include "Math.hpp" @@ -17,6 +15,7 @@ #include "Model.hpp" namespace yasim { +#if 0 void printState(State* s) { State tmp = *s; @@ -39,6 +38,7 @@ void printState(State* s) printf("rot: %6.2f %6.2f %6.2f\n", tmp.rot[0], tmp.rot[1], tmp.rot[2]); printf("rac: %6.2f %6.2f %6.2f\n", tmp.racc[0], tmp.racc[1], tmp.racc[2]); } +#endif Model::Model() { @@ -72,7 +72,7 @@ void Model::getThrust(float* out) } } -void Model::initIteration(float dt) +void Model::initIteration() { // Precompute torque and angular momentum for the thrusters int i; @@ -96,10 +96,18 @@ void Model::initIteration(float dt) t->getGyro(v); Math::add3(v, _gyro, _gyro); } +} - - - +// 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 +void Model::initRotorIteration() +{ + int i; + float dt = _integrator.getInterval(); float lrot[3]; Math::vmul33(_s->orient, _s->rot, lrot); Math::mul3(dt,lrot,lrot); @@ -115,12 +123,12 @@ void Model::initIteration(float dt) Rotorblade* rp = (Rotorblade*)_rotorblades.get(i); rp->inititeration(dt,lrot); } - } -void Model::iterate(float dt) +void Model::iterate() { - initIteration(dt); + initIteration(); + initRotorIteration(); _body.recalc(); // FIXME: amortize this, somehow _integrator.calcNewInterval(); } @@ -145,12 +153,6 @@ State* Model::getState() return _s; } -void Model::resetState() -{ - //_s->resetState(); -} - - void Model::setState(State* s) { _integrator.setState(s); @@ -265,7 +267,6 @@ void Model::calcForces(State* s) // velocity), and are therefore constant across the four calls to // calcForces. They get computed before we begin the integration // step. - //printf("f"); _body.setGyro(_gyro); _body.addTorque(_torque); int i; @@ -300,8 +301,6 @@ void Model::calcForces(State* s) sf->calcForce(vs, _rho, force, torque); Math::add3(faero, force, faero); - - _body.addForce(pos, force); _body.addTorque(torque); } @@ -339,13 +338,6 @@ void Model::calcForces(State* s) _body.addForce(pos, force); _body.addTorque(torque); } - /* - { - float cg[3]; - _body.getCG(cg); - //printf("cg: %5.3lf %5.3lf %5.3lf ",cg[0],cg[1],cg[2]); - } - */ // Get a ground plane in local coordinates. The first three // elements are the normal vector, the final one is the distance @@ -385,8 +377,6 @@ void Model::newState(State* s) { _s = s; - //printState(s); - // Some simple collision detection float min = 1e8; float ground[4], pos[3], cmpr[3]; diff --git a/src/FDM/YASim/Model.hpp b/src/FDM/YASim/Model.hpp index 65002cad6..29425a23d 100644 --- a/src/FDM/YASim/Model.hpp +++ b/src/FDM/YASim/Model.hpp @@ -28,12 +28,11 @@ public: State* getState(); void setState(State* s); - void resetState(); bool isCrashed(); void setCrashed(bool crashed); float getAGL(); - void iterate(float dt); + void iterate(); // Externally-managed subcomponents int addThruster(Thruster* t); @@ -52,7 +51,7 @@ public: int numThrusters(); Thruster* getThruster(int handle); void setThruster(int handle, Thruster* t); - void initIteration(float dt); + void initIteration(); void getThrust(float* out); // @@ -68,6 +67,7 @@ public: virtual void newState(State* s); private: + void initRotorIteration(); void calcGearForce(Gear* g, float* v, float* rot, float* ground); float gearFriction(float wgt, float v, Gear* g); float localGround(State* s, float* out); diff --git a/src/FDM/YASim/Rotor.cpp b/src/FDM/YASim/Rotor.cpp index 9c7ccd762..910c41970 100644 --- a/src/FDM/YASim/Rotor.cpp +++ b/src/FDM/YASim/Rotor.cpp @@ -18,6 +18,8 @@ namespace yasim { const float pi=3.14159; +static inline float sqr(float a) { return a * a; } + Rotor::Rotor() { _alpha0=-.05; @@ -218,8 +220,8 @@ int Rotor::getValueforFGSet(int j,char *text,float *f) rk=l-p; rl=1-rk; /* - rl=Math::sqr(Math::sin(rl*pi/2)); - rk=Math::sqr(Math::sin(rk*pi/2)); + rl=sqr(Math::sin(rl*pi/2)); + rk=sqr(Math::sin(rk*pi/2)); */ if(w==2) {k+=2;l+=2;} else @@ -565,7 +567,7 @@ void Rotor::compile() float phi=Math::atan2(2*omega*_delta,omega0*omega0-omega*omega); //float relamp=omega*omega/(2*_delta*Math::sqrt(omega0*omega0-_delta*_delta)); - float relamp=omega*omega/(2*_delta*Math::sqrt(Math::sqr(omega0*omega0-omega*omega)+4*_delta*_delta*omega*omega)); + float relamp=omega*omega/(2*_delta*Math::sqrt(sqr(omega0*omega0-omega*omega)+4*_delta*_delta*omega*omega)); if (!_no_torque) { torque0=_power_at_pitch_0/4*1000/omega; @@ -664,7 +666,7 @@ void Rotor::compile() _delta*=maxpitchforce/(_max_pitch*omega*lentocenter*2*_weight_per_blade*.453); float phi=Math::atan2(2*omega*_delta,omega0*omega0-omega*omega); float phi2=Math::abs(omega0-omega)<.000000001?pi/2:Math::atan(2*omega*_delta/(omega0*omega0-omega*omega)); - float relamp=omega*omega/(2*_delta*Math::sqrt(Math::sqr(omega0*omega0-omega*omega)+4*_delta*_delta*omega*omega)); + float relamp=omega*omega/(2*_delta*Math::sqrt(sqr(omega0*omega0-omega*omega)+4*_delta*_delta*omega*omega)); if (!_no_torque) { torque0=_power_at_pitch_0/_number_of_blades*1000/omega; diff --git a/src/FDM/YASim/YASim.cxx b/src/FDM/YASim/YASim.cxx index 88e2ed605..98bf6e78f 100644 --- a/src/FDM/YASim/YASim.cxx +++ b/src/FDM/YASim/YASim.cxx @@ -118,7 +118,6 @@ void YASim::bind() sprintf(buf, "/engines/engine[%d]/mp-osi", i); fgUntie(buf); sprintf(buf, "/engines/engine[%d]/egt-degf", i); fgUntie(buf); } - } void YASim::init() @@ -128,6 +127,7 @@ void YASim::init() // Superclass hook common_init(); + m->setCrashed(false); // Figure out the initial speed type -- 2.39.5