From 5333f82eb1c98a9dac366a36e78f47f903f2681d Mon Sep 17 00:00:00 2001 From: curt Date: Thu, 16 Oct 2003 14:56:13 +0000 Subject: [PATCH] Maik Justus: modifications to add helicopter modeling to YASim. --- src/FDM/YASim/Airplane.cpp | 125 +++++++++++++++++++++++++--- src/FDM/YASim/Airplane.hpp | 8 ++ src/FDM/YASim/ControlMap.cpp | 8 ++ src/FDM/YASim/ControlMap.hpp | 3 +- src/FDM/YASim/FGFDM.cpp | 154 +++++++++++++++++++++++++++++++++++ src/FDM/YASim/FGFDM.hpp | 2 + src/FDM/YASim/Makefile.am | 3 + src/FDM/YASim/Math.cpp | 26 +++++- src/FDM/YASim/Math.hpp | 4 + src/FDM/YASim/Model.cpp | 106 +++++++++++++++++++++++- src/FDM/YASim/Model.hpp | 18 +++- src/FDM/YASim/YASim.cxx | 6 +- 12 files changed, 440 insertions(+), 23 deletions(-) diff --git a/src/FDM/YASim/Airplane.cpp b/src/FDM/YASim/Airplane.cpp index 6f3b68a07..10bb49055 100644 --- a/src/FDM/YASim/Airplane.cpp +++ b/src/FDM/YASim/Airplane.cpp @@ -5,8 +5,9 @@ #include "Glue.hpp" #include "RigidBody.hpp" #include "Surface.hpp" +#include "Rotorpart.hpp" +#include "Rotorblade.hpp" #include "Thruster.hpp" - #include "Airplane.hpp" namespace yasim { @@ -72,7 +73,7 @@ void Airplane::iterate(float dt) // The gear might have moved. Change their aerodynamics. updateGearState(); - _model.iterate(); + _model.iterate(dt); } void Airplane::consumeFuel(float dt) @@ -255,6 +256,11 @@ 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) { @@ -443,6 +449,69 @@ float Airplane::compileWing(Wing* w) return wgt; } +float Airplane::compileRotor(Rotor* w) +{ + /* 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); + + 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); + + _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; inumRotorblades(); i++) { + Rotorblade* s = (Rotorblade*)w->getRotorblade(i); + + //float td = s->getTotalDrag(); + //s->setTotalDrag(td); + + _model.addRotorblade(s); + + + float mass = s->getWeight(); + mass = mass * Math::sqrt(mass); + float pos[3]; + s->getPosition(pos); + _model.getBody()->addMass(mass, pos); + wgt += mass; + + } + + return wgt; +} + float Airplane::compileFuselage(Fuselage* f) { // The front and back are contact points @@ -576,12 +645,17 @@ void Airplane::compile() float aeroWgt = 0; // The Wing objects - aeroWgt += compileWing(_wing); - aeroWgt += compileWing(_tail); + if (_wing) + aeroWgt += compileWing(_wing); + if (_tail) + aeroWgt += compileWing(_tail); 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 fuselage(s) for(i=0; i<_fuselages.size(); i++) { @@ -628,10 +702,15 @@ void Airplane::compile() // Ground effect float gepos[3]; - float gespan = _wing->getGroundEffect(gepos); + float gespan; + if(_wing) + gespan = _wing->getGroundEffect(gepos); + else + gespan=0; _model.setGroundEffect(gepos, gespan, 0.15f); solveGear(); + solve(); // Do this after solveGear, because it creates "gear" objects that @@ -750,7 +829,7 @@ void Airplane::runCruise() // Precompute thrust in the model, and calculate aerodynamic forces _model.getBody()->recalc(); _model.getBody()->reset(); - _model.initIteration(); + _model.initIteration(.01);//hier parameter egal _model.calcForces(&_cruiseState); } @@ -793,7 +872,7 @@ void Airplane::runApproach() // Precompute thrust in the model, and calculate aerodynamic forces _model.getBody()->recalc(); _model.getBody()->reset(); - _model.initIteration(); + _model.initIteration(.01);//hier parameter egal _model.calcForces(&_approachState); } @@ -801,8 +880,10 @@ void Airplane::applyDragFactor(float factor) { float applied = Math::pow(factor, SOLVE_TWEAK); _dragFactor *= applied; - _wing->setDragScale(_wing->getDragScale() * applied); - _tail->setDragScale(_tail->getDragScale() * applied); + if(_wing) + _wing->setDragScale(_wing->getDragScale() * applied); + if(_tail) + _tail->setDragScale(_tail->getDragScale() * applied); int i; for(i=0; i<_vstabs.size(); i++) { Wing* w = (Wing*)_vstabs.get(i); @@ -818,8 +899,10 @@ void Airplane::applyLiftRatio(float factor) { float applied = Math::pow(factor, SOLVE_TWEAK); _liftRatio *= applied; - _wing->setLiftRatio(_wing->getLiftRatio() * applied); - _tail->setLiftRatio(_tail->getLiftRatio() * applied); + if(_wing) + _wing->setLiftRatio(_wing->getLiftRatio() * applied); + if(_tail) + _tail->setLiftRatio(_tail->getLiftRatio() * applied); int i; for(i=0; i<_vstabs.size(); i++) { Wing* w = (Wing*)_vstabs.get(i); @@ -848,6 +931,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 @@ -859,9 +944,9 @@ void Airplane::solve() _approachElevator.val); #endif - if(_solutionIterations++ > 10000) { + if(_solutionIterations++ > 10000) { _failureMsg = "Solution failed to converge after 10000 iterations"; - return; + return; } // Run an iteration at cruise, and extract the needed numbers: @@ -986,5 +1071,19 @@ 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; } }; // namespace yasim diff --git a/src/FDM/YASim/Airplane.hpp b/src/FDM/YASim/Airplane.hpp index 8fffa4ba6..4bed725ea 100644 --- a/src/FDM/YASim/Airplane.hpp +++ b/src/FDM/YASim/Airplane.hpp @@ -4,6 +4,7 @@ #include "ControlMap.hpp" #include "Model.hpp" #include "Wing.hpp" +#include "Rotor.hpp" #include "Vector.hpp" namespace yasim { @@ -33,6 +34,10 @@ 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); @@ -89,6 +94,7 @@ private: void solveGear(); void solve(); float compileWing(Wing* w); + float compileRotor(Rotor* w); float compileFuselage(Fuselage* f); void compileGear(GearRec* gr); void applyDragFactor(float factor); @@ -119,6 +125,8 @@ private: Vector _weights; Vector _surfs; // NON-wing Surfaces + Vector _rotors; + Vector _cruiseControls; State _cruiseState; float _cruiseP; diff --git a/src/FDM/YASim/ControlMap.cpp b/src/FDM/YASim/ControlMap.cpp index 17a6e6921..fd4f99b7c 100644 --- a/src/FDM/YASim/ControlMap.cpp +++ b/src/FDM/YASim/ControlMap.cpp @@ -4,6 +4,7 @@ #include "PistonEngine.hpp" #include "Gear.hpp" #include "Wing.hpp" +#include "Rotor.hpp" #include "Math.hpp" #include "Propeller.hpp" @@ -199,6 +200,10 @@ void ControlMap::applyControls(float dt) case FLAP0: ((Wing*)obj)->setFlap0(lval, rval); break; case FLAP1: ((Wing*)obj)->setFlap1(lval, rval); break; case SPOILER: ((Wing*)obj)->setSpoiler(lval, rval); break; + 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 BOOST: ((Thruster*)obj)->getPistonEngine()->setBoost(lval); break; @@ -213,6 +218,9 @@ float ControlMap::rangeMin(int type) case FLAP0: return -1; // [-1:1] case FLAP1: return -1; case STEER: return -1; + case CYCLICELE: return -1; + case CYCLICAIL: return -1; + case COLLECTIVE: return -1; case MAGNETOS: return 0; // [0:3] default: return 0; // [0:1] } diff --git a/src/FDM/YASim/ControlMap.hpp b/src/FDM/YASim/ControlMap.hpp index c1a3cb6f7..c3dbda2ee 100644 --- a/src/FDM/YASim/ControlMap.hpp +++ b/src/FDM/YASim/ControlMap.hpp @@ -13,7 +13,8 @@ public: ADVANCE, REHEAT, PROP, BRAKE, STEER, EXTEND, INCIDENCE, FLAP0, FLAP1, SLAT, SPOILER, VECTOR, - BOOST, CASTERING, PROPPITCH }; + BOOST, CASTERING, PROPPITCH, + COLLECTIVE, CYCLICAIL, CYCLICELE, ROTORENGINEON}; enum { OPT_SPLIT = 0x01, OPT_INVERT = 0x02, diff --git a/src/FDM/YASim/FGFDM.cpp b/src/FDM/YASim/FGFDM.cpp index 09dcad718..44ee1e8b3 100644 --- a/src/FDM/YASim/FGFDM.cpp +++ b/src/FDM/YASim/FGFDM.cpp @@ -10,8 +10,12 @@ #include "PropEngine.hpp" #include "Propeller.hpp" #include "PistonEngine.hpp" +#include "Rotor.hpp" +#include "Rotorpart.hpp" +#include "Rotorblade.hpp" #include "FGFDM.hpp" + namespace yasim { // Some conversion factors @@ -28,6 +32,7 @@ static const float INHG2PA = 3386.389; static const float K2DEGF = 1.8; static const float K2DEGFOFFSET = -459.4; static const float CIN2CM = 1.6387064e-5; +static const float YASIM_PI = 3.14159265358979323846; // Stubs, so that this can be compiled without the FlightGear // binary. What's the best way to handle this? @@ -119,12 +124,16 @@ void FGFDM::startElement(const char* name, const XMLAttributes &atts) v[1] = attrf(a, "y"); v[2] = attrf(a, "z"); _airplane.setPilotPos(v); + } else if(eq(name, "rotor")) { + _airplane.addRotor(parseRotor(a, name)); } else if(eq(name, "wing")) { _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")) { + _airplane.addVStab(parseWing(a, name)); } else if(eq(name, "propeller")) { parsePropeller(a); } else if(eq(name, "thruster")) { @@ -226,6 +235,11 @@ void FGFDM::startElement(const char* name, const XMLAttributes &atts) } else if(eq(name, "spoiler")) { ((Wing*)_currObj)->setSpoiler(attrf(a, "start"), attrf(a, "end"), attrf(a, "lift"), attrf(a, "drag")); + /* } else if(eq(name, "collective")) { + ((Rotor*)_currObj)->setcollective(attrf(a, "min"), attrf(a, "max")); + } else if(eq(name, "cyclic")) { + ((Rotor*)_currObj)->setcyclic(attrf(a, "ail"), attrf(a, "ele")); + */ } else if(eq(name, "actionpt")) { v[0] = attrf(a, "x"); v[1] = attrf(a, "y"); @@ -351,6 +365,49 @@ void FGFDM::setOutputProperties() fgSetFloat("/consumables/fuel/total-fuel-norm", totalFuel/totalCap); } + for(i=0; i<_airplane.getNumRotors(); i++) { + Rotor*r=(Rotor*)_airplane.getRotor(i); + int j=0; + float f; + char b[256]; + while(j=r->getValueforFGSet(j,b,&f)) + { + if (b[0]) + { + fgSetFloat(b,f); + } + } + + for(j=0; jnumRotorparts(); 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(j=0; jnumRotorblades(); 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(i=0; i<_thrusters.size(); i++) { EngRec* er = (EngRec*)_thrusters.get(i); Thruster* t = er->eng; @@ -425,6 +482,92 @@ Wing* FGFDM::parseWing(XMLAttributes* a, const char* type) float effect = attrf(a, "effectiveness", 1); w->setDragScale(w->getDragScale()*effect); + _currObj = w; + return w; +} +Rotor* FGFDM::parseRotor(XMLAttributes* a, const char* type) +{ + Rotor* w = new Rotor(); + + float defDihed = 0; + + float pos[3]; + pos[0] = attrf(a, "x"); + pos[1] = attrf(a, "y"); + pos[2] = attrf(a, "z"); + w->setBase(pos); + + float normal[3]; + normal[0] = attrf(a, "nx"); + normal[1] = attrf(a, "ny"); + normal[2] = attrf(a, "nz"); + w->setNormal(normal); + + float forward[3]; + forward[0] = attrf(a, "fx"); + forward[1] = attrf(a, "fy"); + 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)); + w->setMinCyclicele(attrf(a, "mincyclicele", -4.94)); + w->setMaxCollective(attrf(a, "maxcollective", 15.8)); + w->setMinCollective(attrf(a, "mincollective", -0.2)); + w->setDiameter(attrf(a, "diameter", 10.2)); + w->setWeightPerBlade(attrf(a, "weightperblade", 44)); + w->setNumberOfBlades(attrf(a, "numblades", 4)); + w->setRelBladeCenter(attrf(a, "relbladecenter", 0.7)); + w->setDynamic(attrf(a, "dynamic", 0.7)); + w->setDelta3(attrf(a, "delta3", 0)); + w->setDelta(attrf(a, "delta", 0)); + w->setTranslift(attrf(a, "translift", 0.05)); + w->setC2(attrf(a, "dragfactor", 1)); + w->setStepspersecond(attrf(a, "stepspersecond", 120)); + w->setRPM(attrf(a, "rpm", 424)); + w->setRelLenHinge(attrf(a, "rellenflaphinge", 0.07)); + w->setAlpha0((attrf(a, "flap0", -5))*YASIM_PI/180); + w->setAlphamin((attrf(a, "flapmin", -15))/180*YASIM_PI); + w->setAlphamax((attrf(a, "flapmax", 15))*YASIM_PI/180); + w->setAlpha0factor(attrf(a, "flap0factor", 1)); + w->setTeeterdamp(attrf(a,"teeterdamp",.0001)); + w->setMaxteeterdamp(attrf(a,"maxteeterdamp",1000)); + w->setRelLenTeeterHinge(attrf(a,"rellenteeterhinge",0.01)); + void setAlphamin(float f); + void setAlphamax(float f); + void setAlpha0factor(float f); + + if(attristrue(a,"ccw")) + w->setCcw(1); + + if(a->hasAttribute("name")) + w->setName(a->getValue("name") ); + if(a->hasAttribute("alphaout0")) + w->setAlphaoutput(0,a->getValue("alphaout0") ); + if(a->hasAttribute("alphaout1")) w->setAlphaoutput(1,a->getValue("alphaout1") ); + if(a->hasAttribute("alphaout2")) w->setAlphaoutput(2,a->getValue("alphaout2") ); + if(a->hasAttribute("alphaout3")) w->setAlphaoutput(3,a->getValue("alphaout3") ); + if(a->hasAttribute("coneout")) w->setAlphaoutput(4,a->getValue("coneout") ); + if(a->hasAttribute("yawout")) w->setAlphaoutput(5,a->getValue("yawout") ); + if(a->hasAttribute("rollout")) w->setAlphaoutput(6,a->getValue("rollout") ); + + w->setPitchA(attrf(a, "pitch_a", 10)); + w->setPitchB(attrf(a, "pitch_b", 10)); + 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")) + w->setNotorque(1); + if(attristrue(a,"simblades")) + w->setSimBlades(1); + + + + + _currObj = w; return w; } @@ -531,6 +674,10 @@ int FGFDM::parseOutput(const char* name) if(eq(name, "SPOILER")) return ControlMap::SPOILER; if(eq(name, "CASTERING")) return ControlMap::CASTERING; if(eq(name, "PROPPITCH")) return ControlMap::PROPPITCH; + 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; SG_LOG(SG_FLIGHT,SG_ALERT,"Unrecognized control type '" << name << "' in YASim aircraft description."); exit(1); @@ -605,4 +752,11 @@ float FGFDM::attrf(XMLAttributes* atts, char* attr, float def) else return (float)atof(val); } +bool FGFDM::attristrue(XMLAttributes* atts, char* attr) +{ + const char* val = atts->getValue(attr); + if(val == 0) return false; + else return eq(val,"true"); +} + }; // namespace yasim diff --git a/src/FDM/YASim/FGFDM.hpp b/src/FDM/YASim/FGFDM.hpp index cd91e50d4..ee701b02b 100644 --- a/src/FDM/YASim/FGFDM.hpp +++ b/src/FDM/YASim/FGFDM.hpp @@ -36,6 +36,7 @@ private: void setOutputProperties(); + Rotor* parseRotor(XMLAttributes* a, const char* name); Wing* parseWing(XMLAttributes* a, const char* name); int parseAxis(const char* name); int parseOutput(const char* name); @@ -47,6 +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); // The core Airplane object we manage. Airplane _airplane; diff --git a/src/FDM/YASim/Makefile.am b/src/FDM/YASim/Makefile.am index 8619c58a4..8cd1ab7b9 100644 --- a/src/FDM/YASim/Makefile.am +++ b/src/FDM/YASim/Makefile.am @@ -22,6 +22,9 @@ SHARED_SOURCES = \ PropEngine.cpp PropEngine.hpp \ Propeller.cpp Propeller.hpp \ RigidBody.cpp RigidBody.hpp \ + Rotor.cpp Rotor.hpp \ + Rotorblade.cpp Rotorblade.hpp \ + Rotorpart.cpp Rotorpart.hpp \ SimpleJet.cpp SimpleJet.hpp \ Surface.cpp Surface.hpp \ Thruster.cpp Thruster.hpp \ diff --git a/src/FDM/YASim/Math.cpp b/src/FDM/YASim/Math.cpp index 4662d70b2..00c5d09e4 100644 --- a/src/FDM/YASim/Math.cpp +++ b/src/FDM/YASim/Math.cpp @@ -19,12 +19,26 @@ float Math::sqrt(float f) { return (float)::sqrt(f); } +float Math::sqr(float f) +{ + return f*f; +} float Math::ceil(float f) { return (float)::ceil(f); } +float Math::acos(float f) +{ + return (float)::acos(f); +} + +float Math::asin(float f) +{ + return (float)::asin(f); +} + float Math::cos(float f) { return (float)::cos(f); @@ -40,6 +54,11 @@ float Math::tan(float f) return (float)::tan(f); } +float Math::atan(float f) +{ + return (float)::atan(f); +} + float Math::atan2(float y, float x) { return (float)::atan2(y, x); @@ -126,7 +145,12 @@ float Math::mag3(float* v) void Math::unit3(float* v, float* out) { - float imag = 1/mag3(v); + float mag=mag3(v); + float imag; + if (mag!=0) + imag= 1/mag; + else + imag=1; mul3(imag, v, out); } diff --git a/src/FDM/YASim/Math.hpp b/src/FDM/YASim/Math.hpp index 071f35cfc..4189cd685 100644 --- a/src/FDM/YASim/Math.hpp +++ b/src/FDM/YASim/Math.hpp @@ -12,11 +12,15 @@ 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); static float tan(float f); + static float atan(float f); static float atan2(float y, float x); + static float asin(float f); + static float acos(float f); // Takes two args and runs afoul of the Koenig rules. static float pow(double base, double exp); diff --git a/src/FDM/YASim/Model.cpp b/src/FDM/YASim/Model.cpp index 8cc50c9d0..1a44230cf 100644 --- a/src/FDM/YASim/Model.cpp +++ b/src/FDM/YASim/Model.cpp @@ -9,6 +9,9 @@ #include "PistonEngine.hpp" #include "Gear.hpp" #include "Surface.hpp" +#include "Rotor.hpp" +#include "Rotorpart.hpp" +#include "Rotorblade.hpp" #include "Glue.hpp" #include "Model.hpp" @@ -69,7 +72,7 @@ void Model::getThrust(float* out) } } -void Model::initIteration() +void Model::initIteration(float dt) { // Precompute torque and angular momentum for the thrusters int i; @@ -93,11 +96,31 @@ void Model::initIteration() t->getGyro(v); Math::add3(v, _gyro, _gyro); } + + + + + float lrot[3]; + 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); + } + } -void Model::iterate() +void Model::iterate(float dt) { - initIteration(); + initIteration(dt); _body.recalc(); // FIXME: amortize this, somehow _integrator.calcNewInterval(); } @@ -122,6 +145,12 @@ State* Model::getState() return _s; } +void Model::resetState() +{ + //_s->resetState(); +} + + void Model::setState(State* s) { _integrator.setState(s); @@ -143,6 +172,19 @@ Surface* Model::getSurface(int handle) return (Surface*)_surfaces.get(handle); } +Rotorpart* Model::getRotorpart(int handle) +{ + 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); +} + int Model::addThruster(Thruster* t) { return _thrusters.add(t); @@ -168,6 +210,19 @@ 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); @@ -210,6 +265,7 @@ 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; @@ -243,9 +299,53 @@ void Model::calcForces(State* s) float force[3], torque[3]; sf->calcForce(vs, _rho, force, torque); Math::add3(faero, force, faero); + + + _body.addForce(pos, force); _body.addTorque(torque); } + for(i=0; i<_rotorparts.size(); i++) { + Rotorpart* sf = (Rotorpart*)_rotorparts.get(i); + + // Vsurf = wind - velocity + (rot cross (cg - pos)) + float vs[3], pos[3]; + sf->getPosition(pos); + localWind(pos, s, vs); + + float force[3], torque[3]; + sf->calcForce(vs, _rho, force, torque); + //Math::add3(faero, force, faero); + + sf->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); + + float force[3], torque[3]; + sf->calcForce(vs, _rho, force, torque); + //Math::add3(faero, force, faero); + + sf->getPositionForceAttac(pos); + + _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 diff --git a/src/FDM/YASim/Model.hpp b/src/FDM/YASim/Model.hpp index e3ef98fd8..65002cad6 100644 --- a/src/FDM/YASim/Model.hpp +++ b/src/FDM/YASim/Model.hpp @@ -12,6 +12,9 @@ namespace yasim { class Integrator; class Thruster; class Surface; +class Rotorpart; +class Rotorblade; +class Rotor; class Gear; class Model : public BodyEnvironment { @@ -24,24 +27,32 @@ public: State* getState(); void setState(State* s); + + void resetState(); bool isCrashed(); void setCrashed(bool crashed); float getAGL(); - void iterate(); + void iterate(float dt); // 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); Surface* getSurface(int handle); + Rotorpart* getRotorpart(int handle); + Rotorblade* getRotorblade(int handle); + Rotor* getRotor(int handle); Gear* getGear(int handle); // Semi-private methods for use by the Airplane solver. int numThrusters(); Thruster* getThruster(int handle); void setThruster(int handle, Thruster* t); - void initIteration(); + void initIteration(float dt); void getThrust(float* out); // @@ -67,6 +78,9 @@ private: Vector _thrusters; Vector _surfaces; + Vector _rotorparts; + Vector _rotorblades; + Vector _rotors; Vector _gears; float _groundEffectSpan; diff --git a/src/FDM/YASim/YASim.cxx b/src/FDM/YASim/YASim.cxx index 294ddfe35..88e2ed605 100644 --- a/src/FDM/YASim/YASim.cxx +++ b/src/FDM/YASim/YASim.cxx @@ -23,8 +23,9 @@ using namespace yasim; -static const float RAD2DEG = 180/3.14159265358979323846; -static const float PI2 = 3.14159265358979323846*2; +static const float YASIM_PI = 3.14159265358979323846; +static const float RAD2DEG = 180/YASIM_PI; +static const float PI2 = YASIM_PI*2; static const float RAD2RPM = 9.54929658551; static const float M2FT = 3.2808399; static const float FT2M = 0.3048; @@ -127,7 +128,6 @@ void YASim::init() // Superclass hook common_init(); - m->setCrashed(false); // Figure out the initial speed type -- 2.39.5