fly/flightgear
1
0
Fork 0
Code Activity

YASim CLI tool refactoring

Browse source
This commit is contained in:
Henning Stahlke 2018-02-06 00:10:03 +01:00
parent da961b97f2
commit 1b21f85c52
8 changed files with 312 additions and 304 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

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

@ -13,18 +13,17 @@
#include "Thruster.hpp" #include "Thruster.hpp"
#include "Hitch.hpp" #include "Hitch.hpp"
#include "Airplane.hpp" #include "Airplane.hpp"
#include "yasim-common.hpp"
namespace yasim { namespace yasim {
//default prop names //default prop names
static const char* DEF_PROP_ELEVATOR_TRIM = "/controls/flight/elevator-trim";
// gadgets // gadgets
inline float abs(float f) { return f<0 ? -f : f; } inline float abs(float f) { return f<0 ? -f : f; }
Airplane::Airplane() Airplane::Airplane()
{ {
_approachConfig.isApproach = true;
} }
Airplane::~Airplane() Airplane::~Airplane()
@ -50,12 +49,12 @@ Airplane::~Airplane()
} }
for(i=0; i<_solveWeights.size(); i++) for(i=0; i<_solveWeights.size(); i++)
delete (SolveWeight*)_solveWeights.get(i); delete (SolveWeight*)_solveWeights.get(i);
for(i=0; i<_cruiseConfig.controls.size(); i++) { for(i=0; i<_config[CRUISE].controls.size(); i++) {
ControlSetting* c = (ControlSetting*)_cruiseConfig.controls.get(i); ControlSetting* c = (ControlSetting*)_config[CRUISE].controls.get(i);
delete c; delete c;
} }
for(i=0; i<_approachConfig.controls.size(); i++) { for(i=0; i<_config[APPROACH].controls.size(); i++) {
ControlSetting* c = (ControlSetting*)_approachConfig.controls.get(i); ControlSetting* c = (ControlSetting*)_config[APPROACH].controls.get(i);
delete c; delete c;
} }
delete _wing; delete _wing;
@ -130,23 +129,18 @@ void Airplane::updateGearState()
} }
} }
void Airplane::setApproach(float speed, float altitude, float aoa, float fuel, float gla) /// setup a config record
void Airplane::setConfig(Configuration cfg, float speed, float altitude, float fuel, float gla, float aoa)
{ {
_approachConfig.speed = speed; _config[cfg].id = cfg;
_approachConfig.altitude = altitude; _config[cfg].speed = speed;
// solver assumes fixed (given) AoA for approach, so setup once _config[cfg].altitude = altitude;
_approachConfig.state.setupOrientationFromAoa(aoa); // solver assumes fixed (given) AoA for approach, so setup once;
_approachConfig.aoa = aoa; // not strictly needed, see runConfig() // solver will change this for cruise config
_approachConfig.fuel = fuel; _config[cfg].state.setupOrientationFromAoa(aoa);
_approachConfig.glideAngle = gla; _config[cfg].aoa = aoa; // not strictly needed, see runConfig()
} _config[cfg].fuel = fuel;
_config[cfg].glideAngle = gla;
void Airplane::setCruise(float speed, float altitude, float fuel, float gla)
{
_cruiseConfig.speed = speed;
_cruiseConfig.altitude = altitude;
_cruiseConfig.fuel = fuel;
_cruiseConfig.glideAngle = gla;
} }
/// set property name for elevator /// set property name for elevator
@ -174,14 +168,7 @@ Airplane::ControlSetting* Airplane::_addControlSetting(Configuration cfg, const
ControlSetting* c = new ControlSetting(); ControlSetting* c = new ControlSetting();
c->propHandle = _controlMap.getInputPropertyHandle(prop); c->propHandle = _controlMap.getInputPropertyHandle(prop);
c->val = val; c->val = val;
switch (cfg) { _config[cfg].controls.add(c);
case APPROACH:
_approachConfig.controls.add(c);
break;
case CRUISE:
_cruiseConfig.controls.add(c);
break;
}
return c; return c;
} }
@ -202,7 +189,7 @@ void Airplane::addControlInput(const char* propName, ControlMap::ControlType typ
void Airplane::addSolutionWeight(Configuration cfg, int idx, float wgt) void Airplane::addSolutionWeight(Configuration cfg, int idx, float wgt)
{ {
SolveWeight* w = new SolveWeight(); SolveWeight* w = new SolveWeight();
w->approach = (cfg == APPROACH); w->cfg = cfg;
w->id = idx; w->id = idx;
w->wgt = wgt; w->wgt = wgt;
_solveWeights.add(w); _solveWeights.add(w);
@ -594,8 +581,8 @@ void Airplane::compile(bool verbose)
t->handle = body->addMass(0, t->pos); t->handle = body->addMass(0, t->pos);
totalFuel += t->cap; totalFuel += t->cap;
} }
_cruiseConfig.weight = _emptyWeight + totalFuel*_cruiseConfig.fuel; _config[CRUISE].weight = _emptyWeight + totalFuel*_config[CRUISE].fuel;
_approachConfig.weight = _emptyWeight + totalFuel*_approachConfig.fuel; _config[APPROACH].weight = _emptyWeight + totalFuel*_config[APPROACH].fuel;
body->recalc(); body->recalc();
@ -680,12 +667,12 @@ void Airplane::solveGear()
// The force at max compression should be sufficient to stop a // The force at max compression should be sufficient to stop a
// plane moving downwards at 2x the approach descent rate. Assume // plane moving downwards at 2x the approach descent rate. Assume
// a 3 degree approach. // a 3 degree approach.
float descentRate = 2.0f*_approachConfig.speed/19.1f; float descentRate = 2.0f*_config[APPROACH].speed/19.1f;
// Spread the kinetic energy according to the gear weights. This // Spread the kinetic energy according to the gear weights. This
// will results in an equal compression fraction (not distance) of // will results in an equal compression fraction (not distance) of
// each gear. // each gear.
float energy = 0.5f*_approachConfig.weight*descentRate*descentRate; float energy = 0.5f*_config[APPROACH].weight*descentRate*descentRate;
for(i=0; i<_gears.size(); i++) { for(i=0; i<_gears.size(); i++) {
GearRec* gr = (GearRec*)_gears.get(i); GearRec* gr = (GearRec*)_gears.get(i);
@ -700,7 +687,7 @@ void Airplane::solveGear()
gr->gear->setSpring(k * gr->gear->getSpring()); gr->gear->setSpring(k * gr->gear->getSpring());
// Critically damped (too damped, too!) // Critically damped (too damped, too!)
gr->gear->setDamping(2*Math::sqrt(k*_approachConfig.weight*gr->wgt) gr->gear->setDamping(2*Math::sqrt(k*_config[APPROACH].weight*gr->wgt)
* gr->gear->getDamping()); * gr->gear->getDamping());
} }
} }
@ -733,14 +720,14 @@ void Airplane::stabilizeThrust()
} }
/// Setup weights for cruise or approach during solve. /// Setup weights for cruise or approach during solve.
void Airplane::setupWeights(bool isApproach) void Airplane::setupWeights(Configuration cfg)
{ {
int i; int i;
for(i=0; i<_weights.size(); i++) for(i=0; i<_weights.size(); i++)
setWeight(i, 0); setWeight(i, 0);
for(i=0; i<_solveWeights.size(); i++) { for(i=0; i<_solveWeights.size(); i++) {
SolveWeight* w = (SolveWeight*)_solveWeights.get(i); SolveWeight* w = (SolveWeight*)_solveWeights.get(i);
if(w->approach == isApproach) if(w->cfg == cfg)
setWeight(w->id, w->wgt); setWeight(w->id, w->wgt);
} }
} }
@ -760,8 +747,8 @@ void Airplane::setControlValues(const Vector& controls)
void Airplane::runConfig(Config &cfg) void Airplane::runConfig(Config &cfg)
{ {
// aoa is consider to be given for approach so we calculate orientation // aoa is consider to be given for approach so we calculate orientation
// for approach only once in setApproach() but everytime for cruise here. // for approach only once in setConfig() but everytime for cruise here.
if (!cfg.isApproach) { if (!(cfg.id == APPROACH)) {
cfg.state.setupOrientationFromAoa(cfg.aoa); cfg.state.setupOrientationFromAoa(cfg.aoa);
} }
cfg.state.setupSpeedAndPosition(cfg.speed, cfg.glideAngle); cfg.state.setupSpeedAndPosition(cfg.speed, cfg.glideAngle);
@ -775,7 +762,7 @@ void Airplane::runConfig(Config &cfg)
cfg.state.globalToLocal(wind, wind); cfg.state.globalToLocal(wind, wind);
setFuelFraction(cfg.fuel); setFuelFraction(cfg.fuel);
setupWeights(cfg.isApproach); setupWeights(cfg.id);
// Set up the thruster parameters and iterate until the thrust // Set up the thruster parameters and iterate until the thrust
// stabilizes. // stabilizes.
@ -875,7 +862,7 @@ float Airplane::_getPitch(Config &cfg)
} }
///helper for solveAirplane() ///helper for solveAirplane()
float Airplane::_getLift(Config &cfg) float Airplane::_getLiftForce(Config &cfg)
{ {
float tmp[3]; float tmp[3];
_model.getBody()->getAccel(tmp); _model.getBody()->getAccel(tmp);
@ -883,6 +870,15 @@ float Airplane::_getLift(Config &cfg)
return cfg.weight * tmp[2]; return cfg.weight * tmp[2];
} }
///helper for solveAirplane()
float Airplane::_getDragForce(Config &cfg)
{
float tmp[3];
_model.getBody()->getAccel(tmp);
cfg.state.localToGlobal(tmp, tmp);
return cfg.weight * tmp[0];
}
void Airplane::solveAirplane(bool verbose) void Airplane::solveAirplane(bool verbose)
{ {
static const float ARCMIN = 0.0002909f; static const float ARCMIN = 0.0002909f;
@ -911,28 +907,26 @@ void Airplane::solveAirplane(bool verbose)
return; return;
} }
// Run an iteration at cruise, and extract the needed numbers: // Run an iteration at cruise, and extract the needed numbers:
runConfig(_cruiseConfig); runConfig(_config[CRUISE]);
_model.getThrust(tmp); _model.getThrust(tmp);
float thrust = tmp[0] + _cruiseConfig.weight * Math::sin(_cruiseConfig.glideAngle) * 9.81; float thrust = tmp[0] + _config[CRUISE].weight * Math::sin(_config[CRUISE].glideAngle) * 9.81;
_model.getBody()->getAccel(tmp); float cDragForce = _getDragForce(_config[CRUISE]);
_cruiseConfig.state.localToGlobal(tmp, tmp); float clift0 = _getLiftForce(_config[CRUISE]);
float xforce = _cruiseConfig.weight * tmp[0]; float cpitch0 = _getPitch(_config[CRUISE]);
float clift0 = _getLift(_cruiseConfig);
float cpitch0 = _getPitch(_cruiseConfig);
// Run an approach iteration, and do likewise // Run an approach iteration, and do likewise
runConfig(_approachConfig); runConfig(_config[APPROACH]);
double apitch0 = _getPitch(_approachConfig); double apitch0 = _getPitch(_config[APPROACH]);
float alift = _getLift(_approachConfig); float alift = _getLiftForce(_config[APPROACH]);
// Modify the cruise AoA a bit to get a derivative // Modify the cruise AoA a bit to get a derivative
_cruiseConfig.aoa += ARCMIN; _config[CRUISE].aoa += ARCMIN;
runConfig(_cruiseConfig); runConfig(_config[CRUISE]);
_cruiseConfig.aoa -= ARCMIN; _config[CRUISE].aoa -= ARCMIN;
float clift1 = _getLift(_cruiseConfig); float clift1 = _getLiftForce(_config[CRUISE]);
// Do the same with the tail incidence // Do the same with the tail incidence
float savedIncidence = _tailIncidence->val; float savedIncidence = _tailIncidence->val;
@ -942,16 +936,16 @@ void Airplane::solveAirplane(bool verbose)
_failureMsg = "Tail incidence out of bounds."; _failureMsg = "Tail incidence out of bounds.";
return; return;
}; };
runConfig(_cruiseConfig); runConfig(_config[CRUISE]);
_tailIncidenceCopy->val = _tailIncidence->val = savedIncidence; _tailIncidenceCopy->val = _tailIncidence->val = savedIncidence;
_tail->setIncidence(_tailIncidence->val); _tail->setIncidence(_tailIncidence->val);
float cpitch1 = _getPitch(_cruiseConfig); float cpitch1 = _getPitch(_config[CRUISE]);
// Now calculate: // Now calculate:
float awgt = 9.8f * _approachConfig.weight; float awgt = 9.8f * _config[APPROACH].weight;
float dragFactor = thrust / (thrust-xforce); float dragFactor = thrust / (thrust-cDragForce);
float liftFactor = awgt / (awgt+alift); float liftFactor = awgt / (awgt+alift);
// Sanity: // Sanity:
@ -964,10 +958,10 @@ void Airplane::solveAirplane(bool verbose)
// variable). // variable).
const float ELEVDIDDLE = 0.001f; const float ELEVDIDDLE = 0.001f;
_approachElevator->val += ELEVDIDDLE; _approachElevator->val += ELEVDIDDLE;
runConfig(_approachConfig); runConfig(_config[APPROACH]);
_approachElevator->val -= ELEVDIDDLE; _approachElevator->val -= ELEVDIDDLE;
double apitch1 = _getPitch(_approachConfig); double apitch1 = _getPitch(_config[APPROACH]);
// Now apply the values we just computed. Note that the // Now apply the values we just computed. Note that the
// "minor" variables are deferred until we get the lift/drag // "minor" variables are deferred until we get the lift/drag
@ -990,14 +984,14 @@ void Airplane::solveAirplane(bool verbose)
if (verbose) { if (verbose) {
fprintf(stdout,"%4d\t%f\t%f\t%f\t%f\n", _solutionIterations, aoaDelta, tailDelta, clift0, cpitch1); fprintf(stdout,"%4d\t%f\t%f\t%f\t%f\n", _solutionIterations, aoaDelta, tailDelta, clift0, cpitch1);
} }
_cruiseConfig.aoa += _solverDelta*aoaDelta; _config[CRUISE].aoa += _solverDelta*aoaDelta;
_tailIncidence->val += _solverDelta*tailDelta; _tailIncidence->val += _solverDelta*tailDelta;
_cruiseConfig.aoa = Math::clamp(_cruiseConfig.aoa, -0.175f, 0.175f); _config[CRUISE].aoa = Math::clamp(_config[CRUISE].aoa, -0.175f, 0.175f);
_tailIncidence->val = Math::clamp(_tailIncidence->val, -0.175f, 0.175f); _tailIncidence->val = Math::clamp(_tailIncidence->val, -0.175f, 0.175f);
if(abs(xforce/_cruiseConfig.weight) < _solverThreshold*0.0001 && if(abs(cDragForce/_config[CRUISE].weight) < _solverThreshold*0.0001 &&
abs(alift/_approachConfig.weight) < _solverThreshold*0.0001 && abs(alift/_config[APPROACH].weight) < _solverThreshold*0.0001 &&
abs(aoaDelta) < _solverThreshold*.000017 && abs(aoaDelta) < _solverThreshold*.000017 &&
abs(tailDelta) < _solverThreshold*.000017) abs(tailDelta) < _solverThreshold*.000017)
{ {
@ -1024,7 +1018,7 @@ void Airplane::solveAirplane(bool verbose)
} else if(_liftRatio < 1e-04 || _liftRatio > 1e4) { } else if(_liftRatio < 1e-04 || _liftRatio > 1e4) {
_failureMsg = "Lift ratio beyond reasonable bounds."; _failureMsg = "Lift ratio beyond reasonable bounds.";
return; return;
} else if(Math::abs(_cruiseConfig.aoa) >= .17453293) { } else if(Math::abs(_config[CRUISE].aoa) >= .17453293) {
_failureMsg = "Cruise AoA > 10 degrees"; _failureMsg = "Cruise AoA > 10 degrees";
return; return;
} else if(Math::abs(_tailIncidence->val) >= .17453293) { } else if(Math::abs(_tailIncidence->val) >= .17453293) {
@ -1058,12 +1052,12 @@ void Airplane::solveHelicopter(bool verbose)
applyDragFactor(Math::pow(15.7/1000, 1/_solverDelta)); applyDragFactor(Math::pow(15.7/1000, 1/_solverDelta));
applyLiftRatio(Math::pow(104, 1/_solverDelta)); applyLiftRatio(Math::pow(104, 1/_solverDelta));
} }
_cruiseConfig.state.setupState(0,0,0); _config[CRUISE].state.setupState(0,0,0);
_model.setState(&_cruiseConfig.state); _model.setState(&_config[CRUISE].state);
setupWeights(true); setupWeights(APPROACH);
_controlMap.reset(); _controlMap.reset();
_model.getBody()->reset(); _model.getBody()->reset();
_model.setStandardAtmosphere(_cruiseConfig.altitude); _model.setStandardAtmosphere(_config[CRUISE].altitude);
} }
float Airplane::getCGMAC() float Airplane::getCGMAC()

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

@ -25,8 +25,11 @@ public:
~Airplane(); ~Airplane();
enum Configuration { enum Configuration {
NONE,
APPROACH, APPROACH,
CRUISE, CRUISE,
TAKEOFF, // for testing
TEST, // for testing
}; };
void iterate(float dt); void iterate(float dt);
@ -61,8 +64,8 @@ public:
int addWeight(const float* pos, float size); int addWeight(const float* pos, float size);
void setWeight(int handle, float mass); void setWeight(int handle, float mass);
void setApproach(float speed, float altitude, float aoa, float fuel, float gla); void setConfig(Configuration cfg, float speed, float altitude, float fuel,
void setCruise(float speed, float altitude, float fuel, float gla); float gla = 0, float aoa = 0);
/// add (fixed) control setting to approach/cruise config (for solver) /// add (fixed) control setting to approach/cruise config (for solver)
void addControlSetting(Configuration cfg, const char* prop, float val); void addControlSetting(Configuration cfg, const char* prop, float val);
@ -100,14 +103,15 @@ public:
int getSolutionIterations() const { return _solutionIterations; } int getSolutionIterations() const { return _solutionIterations; }
float getDragCoefficient() const { return _dragFactor; } float getDragCoefficient() const { return _dragFactor; }
float getLiftRatio() const { return _liftRatio; } float getLiftRatio() const { return _liftRatio; }
float getCruiseAoA() const { return _cruiseConfig.aoa; } float getCruiseAoA() const { return _config[CRUISE].aoa; }
float getTailIncidence()const; float getTailIncidence()const;
float getApproachElevator() const; float getApproachElevator() const;
const char* getFailureMsg() const { return _failureMsg; } const char* getFailureMsg() const { return _failureMsg; }
float getMass() const { return _model.getMass(); };
// next two are used only in yasim CLI tool // next two are used only in yasim CLI tool
void setApproachControls() { setControlValues(_approachConfig.controls); } void setApproachControls() { setControlValues(_config[APPROACH].controls); }
void setCruiseControls() { setControlValues(_cruiseConfig.controls); } void setCruiseControls() { setControlValues(_config[CRUISE].controls); }
float getCGHardLimitXMin() const { return _cgMin; } // get min x-coordinate for c.g (from main gear) float getCGHardLimitXMin() const { return _cgMin; } // get min x-coordinate for c.g (from main gear)
float getCGHardLimitXMax() const { return _cgMax; } // get max x-coordinate for c.g (from nose gear) float getCGHardLimitXMax() const { return _cgMax; } // get max x-coordinate for c.g (from nose gear)
@ -166,7 +170,7 @@ private:
}; };
struct SolveWeight { struct SolveWeight {
int id {-1}; int id {-1};
bool approach {false}; Configuration cfg {APPROACH};
float wgt {0}; float wgt {0};
}; };
struct ContactRec { struct ContactRec {
@ -174,18 +178,17 @@ private:
float p[3] {0,0,0}; float p[3] {0,0,0};
}; };
struct Config { struct Config {
bool isApproach {false}; Configuration id;
float speed {0}; float speed {0};
float fuel {0}; float fuel {0};
float glideAngle {0}; float glideAngle {0};
float aoa {0}; float aoa {0};
float altitude {0}; float altitude {0};
float weight {0}; float weight {0};
State state; State state;
Vector controls; Vector controls;
}; };
Config _cruiseConfig; Config _config[Configuration::TEST];
Config _approachConfig;
/// load values for controls as defined in cruise/approach configuration /// load values for controls as defined in cruise/approach configuration
void setControlValues(const Vector& controls); void setControlValues(const Vector& controls);
@ -193,7 +196,8 @@ private:
void runConfig(Config &cfg); void runConfig(Config &cfg);
void solveGear(); void solveGear();
float _getPitch(Config &cfg); float _getPitch(Config &cfg);
float _getLift(Config &cfg); float _getLiftForce(Config &cfg);
float _getDragForce(Config &cfg);
void solveAirplane(bool verbose = false); void solveAirplane(bool verbose = false);
void solveHelicopter(bool verbose = false); void solveHelicopter(bool verbose = false);
float compileWing(Wing* w); float compileWing(Wing* w);
@ -206,7 +210,7 @@ private:
void compileContactPoints(); void compileContactPoints();
float normFactor(float f); float normFactor(float f);
void updateGearState(); void updateGearState();
void setupWeights(bool isApproach); void setupWeights(Configuration cfg);
void calculateCGHardLimits(); void calculateCGHardLimits();
///calculate mass divided by area of main wing ///calculate mass divided by area of main wing
float _getWingLoad(float mass) const; float _getWingLoad(float mass) const;

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

@ -302,12 +302,12 @@ void FGFDM::parseApproachCruise(const XMLAttributes* a, const char* name)
float gla = attrf(a, "glide-angle", 0) * DEG2RAD; float gla = attrf(a, "glide-angle", 0) * DEG2RAD;
if (!strcmp(name, "approach")) { if (!strcmp(name, "approach")) {
float aoa = attrf(a, "aoa", 0) * DEG2RAD; float aoa = attrf(a, "aoa", 0) * DEG2RAD;
_airplane.setApproach(spd, alt, aoa, attrf(a, "fuel", 0.2), gla);
_airplaneCfg = Airplane::Configuration::APPROACH; _airplaneCfg = Airplane::Configuration::APPROACH;
_airplane.setConfig(_airplaneCfg, spd, alt, attrf(a, "fuel", 0.2), gla, aoa);
} }
else { else {
_airplane.setCruise(spd, alt, attrf(a, "fuel", 0.5),gla);
_airplaneCfg = Airplane::Configuration::CRUISE; _airplaneCfg = Airplane::Configuration::CRUISE;
_airplane.setConfig(_airplaneCfg, spd, alt, attrf(a, "fuel", 0.5), gla);
} }
} }

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

@ -99,32 +99,30 @@ void Model::getThrust(float* out) const
void Model::initIteration() void Model::initIteration()
{ {
// Precompute torque and angular momentum for the thrusters // Precompute torque and angular momentum for the thrusters
int i; Math::zero3(_torque);
for(i=0; i<3; i++) Math::zero3(_gyro);
_gyro[i] = _torque[i] = 0;
// Need a local altitude for the wind calculation // Need a local altitude for the wind calculation
float lground[4]; float lground[4];
_s->planeGlobalToLocal(_global_ground, lground); _s->planeGlobalToLocal(_global_ground, lground);
float alt = Math::abs(lground[3]); float alt = Math::abs(lground[3]);
for(i=0; i<_thrusters.size(); i++) { for(int i=0; i<_thrusters.size(); i++) {
Thruster* t = (Thruster*)_thrusters.get(i); Thruster* t = (Thruster*)_thrusters.get(i);
// Get the wind velocity at the thruster location // Get the wind velocity at the thruster location
float pos[3], v[3]; float pos[3], v[3];
t->getPosition(pos); t->getPosition(pos);
localWind(pos, _s, v, alt); localWind(pos, _s, v, alt);
t->setWind(v); t->setWind(v);
t->setAtmosphere(_atmo); t->setAtmosphere(_atmo);
t->integrate(_integrator.getInterval()); t->integrate(_integrator.getInterval());
t->getTorque(v); t->getTorque(v);
Math::add3(v, _torque, _torque); Math::add3(v, _torque, _torque);
t->getGyro(v); t->getGyro(v);
Math::add3(v, _gyro, _gyro); Math::add3(v, _gyro, _gyro);
} }
// Displace the turbulence coordinates according to the local wind. // Displace the turbulence coordinates according to the local wind.
@ -134,12 +132,12 @@ void Model::initIteration()
_turb->offset(toff); _turb->offset(toff);
} }
for(i=0; i<_gears.size(); i++) { for(int i=0; i<_gears.size(); i++) {
Gear* g = (Gear*)_gears.get(i); Gear* g = (Gear*)_gears.get(i);
g->integrate(_integrator.getInterval()); g->integrate(_integrator.getInterval());
} }
for(i=0; i<_hitches.size(); i++) { for(int i=0; i<_hitches.size(); i++) {
Hitch* h = (Hitch*)_hitches.get(i); Hitch* h = (Hitch*)_hitches.get(i);
h->integrate(_integrator.getInterval()); h->integrate(_integrator.getInterval());
} }
@ -272,7 +270,7 @@ void Model::calcForces(State* s)
// calcForces. They get computed before we begin the integration // calcForces. They get computed before we begin the integration
// step. // step.
_body.setGyro(_gyro); _body.setGyro(_gyro);
_body.addTorque(_torque); _body.setTorque(_torque);
int i,j; int i,j;
for(i=0; i<_thrusters.size(); i++) { for(i=0; i<_thrusters.size(); i++) {
Thruster* t = (Thruster*)_thrusters.get(i); Thruster* t = (Thruster*)_thrusters.get(i);

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

@ -30,6 +30,7 @@ public:
RigidBody* getBody() { return &_body; } RigidBody* getBody() { return &_body; }
void getCG(float* cg) const { return _body.getCG(cg); } void getCG(float* cg) const { return _body.getCG(cg); }
float getMass() const {return _body.getTotalMass(); }
Integrator* getIntegrator() { return &_integrator; } Integrator* getIntegrator() { return &_integrator; }
void setTurbulence(Turbulence* turb) { _turb = turb; } void setTurbulence(Turbulence* turb) { _turb = turb; }

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

@ -73,6 +73,7 @@ public:
/// Adds a torque with the specified axis and magnitude /// Adds a torque with the specified axis and magnitude
void addTorque(const float* torque) { Math::add3(_torque, torque, _torque); } void addTorque(const float* torque) { Math::add3(_torque, torque, _torque); }
void setTorque(const float* torque) { Math::set3(torque, _torque); }
// Sets the rotation rate of the body (about its c.g.) within the // Sets the rotation rate of the body (about its c.g.) within the
// surrounding environment. This is needed to compute torque on // surrounding environment. This is needed to compute torque on

1
src/FDM/YASim/yasim-common.hpp
View file

@ -38,6 +38,7 @@ namespace yasim {
static const float INCIDENCE_MIN = -20*DEG2RAD; static const float INCIDENCE_MIN = -20*DEG2RAD;
static const float INCIDENCE_MAX = 20*DEG2RAD; static const float INCIDENCE_MAX = 20*DEG2RAD;
static const char* DEF_PROP_ELEVATOR_TRIM = "/controls/flight/elevator-trim";
}; //namespace yasim }; //namespace yasim
#endif // ifndef _YASIM_COMMON_HPP #endif // ifndef _YASIM_COMMON_HPP

395
src/FDM/YASim/yasim-test.cpp
View file

@ -28,11 +28,35 @@ double fgGetDouble (const char * name, double defaultValue = 0.0) { return 0; }
bool fgSetDouble (const char * name, double defaultValue = 0.0) { return 0; } bool fgSetDouble (const char * name, double defaultValue = 0.0) { return 0; }
enum Config void _setup(Airplane* a, Airplane::Configuration cfgID, float altitude)
{ {
CONFIG_NONE, Model* m = a->getModel();
CONFIG_APPROACH, m->setStandardAtmosphere(altitude);
CONFIG_CRUISE, switch (cfgID) {
case Airplane::APPROACH:
fprintf(stderr,"Setting approach controls.\n");
a->setApproachControls();
break;
case Airplane::CRUISE:
fprintf(stderr,"Setting cruise controls.\n");
a->setCruiseControls();
break;
default:
break;
}
m->getBody()->recalc();
};
void _calculateAcceleration(Airplane* a, float aoa_rad, float speed_mps, float* output)
{
Model* m = a->getModel();
State s;
s.setupState(aoa_rad, speed_mps, 0);
m->getBody()->reset();
m->initIteration();
m->calcForces(&s);
m->getBody()->getAccel(output);
s.localToGlobal(output, output);
}; };
// Generate a graph of lift, drag and L/D against AoA at the specified // Generate a graph of lift, drag and L/D against AoA at the specified
@ -45,123 +69,97 @@ enum Config
"dat" using 1:3 with lines title 'drag', \ "dat" using 1:3 with lines title 'drag', \
"dat" using 1:4 with lines title 'LD' "dat" using 1:4 with lines title 'LD'
*/ */
void yasim_graph(Airplane* a, const float alt, const float kts, int cfg = CONFIG_NONE) void yasim_graph(Airplane* a, const float alt, const float kts, Airplane::Configuration cfgID)
{ {
Model* m = a->getModel(); _setup(a, cfgID, alt);
State s; float speed = kts * KTS2MPS;
float acc[3] {0,0,0};
m->setStandardAtmosphere(alt); float cl_max = 0, cd_min = 1e6, ld_max = 0;
int cl_max_deg = 0, cd_min_deg = 0, ld_max_deg = 0;
switch (cfg) {
case CONFIG_APPROACH:
a->setApproachControls();
break;
case CONFIG_CRUISE:
a->setCruiseControls();
break;
case CONFIG_NONE:
break;
}
//if we fake the properties we could also use FGFDM::getExternalInput()
m->getBody()->recalc();
float cl_max = 0, cd_min = 1e6, ld_max = 0;
int cl_max_deg = 0, cd_min_deg = 0, ld_max_deg = 0;
for(int deg=-15; deg<=90; deg++) {
float aoa = deg * DEG2RAD;
s.setupState(aoa, kts * KTS2MPS, 0);
m->getBody()->reset();
m->initIteration();
m->calcForces(&s);
float acc[3];
m->getBody()->getAccel(acc);
Math::tmul33(s.orient, acc, acc);
float drag = acc[0] * (-1/9.8);
float lift = 1 + acc[2] * (1/9.8);
float ld = lift/drag;
if (cd_min > drag) { printf("aoa\tlift\tdrag\n");
cd_min = drag; for(int deg=-15; deg<=90; deg++) {
cd_min_deg = deg; float aoa = deg * DEG2RAD;
_calculateAcceleration(a, aoa, speed, acc);
float drag = acc[0] * (-1/9.8);
float lift = 1 + acc[2] * (1/9.8);
float ld = lift/drag;
if (cd_min > drag) {
cd_min = drag;
cd_min_deg = deg;
}
if (cl_max < lift) {
cl_max = lift;
cl_max_deg = deg;
}
if (ld_max < ld) {
ld_max= ld;
ld_max_deg = deg;
}
printf("%2d\t%.4f\t%.4f\n", deg, lift, drag);
} }
if (cl_max < lift) { printf("# cl_max %.4f at %d deg\n", cl_max, cl_max_deg);
cl_max = lift; printf("# cd_min %.4f at %d deg\n", cd_min, cd_min_deg);
cl_max_deg = deg; printf("# ld_max %.4f at %d deg\n", ld_max, ld_max_deg);
}
if (ld_max < ld) {
ld_max= ld;
ld_max_deg = deg;
}
printf("%2d\t%.4f\t%.4f\t%.4f\n", deg, lift, drag, ld);
}
printf("# cl_max %.4f at %d deg\n", cl_max, cl_max_deg);
printf("# cd_min %.4f at %d deg\n", cd_min, cd_min_deg);
printf("# ld_max %.4f at %d deg\n", ld_max, ld_max_deg);
} }
void yasim_masses(Airplane* a) void yasim_masses(Airplane* a)
{ {
RigidBody* body = a->getModel()->getBody(); RigidBody* body = a->getModel()->getBody();
int i, N = body->numMasses(); int N = body->numMasses();
float pos[3]; float pos[3];
float m, mass = 0; float m, mass = 0;
printf("id posx posy posz mass\n"); printf("id\tposx\tposy\tposz\tmass\n");
for (i = 0; i < N; i++) for (int i = 0; i < N; i++)
{ {
body->getMassPosition(i, pos); body->getMassPosition(i, pos);
m = body->getMass(i); m = body->getMass(i);
printf("%d %.3f %.3f %.3f %.3f\n", i, pos[0], pos[1], pos[2], m); printf("%d\t%.3f\t%.3f\t%.3f\t%.3f\n", i, pos[0], pos[1], pos[2], m);
mass += m; mass += m;
} }
printf("Total mass: %g", mass); printf("Total mass: %g", mass);
} }
void yasim_drag(Airplane* a, const float aoa, const float alt, int cfg = CONFIG_NONE) void yasim_drag(Airplane* a, const float aoa, const float alt, Airplane::Configuration cfgID)
{ {
fprintf(stderr,"yasim_drag"); _setup(a, cfgID, alt);
Model* m = a->getModel();
State s;
m->setStandardAtmosphere(alt);
switch (cfg) {
case CONFIG_APPROACH:
a->setApproachControls();
break;
case CONFIG_CRUISE:
a->setCruiseControls();
break;
case CONFIG_NONE:
break;
}
m->getBody()->recalc();
float cd_min = 1e6;
int cd_min_kts = 0;
printf("#kts, drag\n");
for(int kts=15; kts<=150; kts++) {
s.setupState(aoa, kts * KTS2MPS, 0);
m->getBody()->reset();
m->initIteration();
m->calcForces(&s);
float acc[3]; float cd_min = 1e6;
m->getBody()->getAccel(acc); int cd_min_kts = 0;
Math::tmul33(s.orient, acc, acc); float acc[3] {0,0,0};
float drag = acc[0] * (-1/9.8); printf("#kts, drag\n");
for(int kts=15; kts<=150; kts++) {
if (cd_min > drag) { _calculateAcceleration(a, aoa,kts * KTS2MPS, acc);
cd_min = drag; float drag = acc[0] * (-1/9.8);
cd_min_kts = kts; if (cd_min > drag) {
cd_min = drag;
cd_min_kts = kts;
}
printf("%d %g\n", kts, drag);
}
printf("# cd_min %g at %d kts\n", cd_min, cd_min_kts);
}
void findMinSpeed(Airplane* a, float alt)
{
a->addControlSetting(Airplane::CRUISE, DEF_PROP_ELEVATOR_TRIM, 0.7f);
_setup(a, Airplane::CRUISE, alt);
float acc[3];
printf("aoa\tknots\tlift\n");
for(int deg=0; deg<=20; deg++) {
float aoa = deg * DEG2RAD;
for(int kts=15; kts<=180; kts++) {
_calculateAcceleration(a, aoa, kts * KTS2MPS, acc);
float lift = acc[2];
if (lift > 0) {
printf("%d\t%d\t%f\n", deg, kts, lift);
break;
}
}
} }
printf("%d %g\n", kts, drag);
}
printf("# cd_min %g at %d kts\n", cd_min, cd_min_kts);
} }
void report(Airplane* a) void report(Airplane* a)
@ -233,97 +231,108 @@ void report(Airplane* a)
int usage() int usage()
{ {
fprintf(stderr, "Usage: \n"); fprintf(stderr, "Usage: \n");
fprintf(stderr, " yasim <aircraft.xml> [-g [-a meters] [-s kts] [-approach | -cruise] ]\n"); fprintf(stderr, " yasim <aircraft.xml> [-g [-a meters] [-s kts] [-approach | -cruise] ]\n");
fprintf(stderr, " yasim <aircraft.xml> [-d [-a meters] [-approach | -cruise] ]\n"); fprintf(stderr, " yasim <aircraft.xml> [-d [-a meters] [-approach | -cruise] ]\n");
fprintf(stderr, " yasim <aircraft.xml> [-m]\n"); fprintf(stderr, " yasim <aircraft.xml> [-m]\n");
fprintf(stderr, " yasim <aircraft.xml> [-test] [-a meters] [-s kts] [-approach | -cruise] ]\n"); fprintf(stderr, " yasim <aircraft.xml> [-test] [-a meters] [-s kts] [-approach | -cruise] ]\n");
fprintf(stderr, " -g print lift/drag table: aoa, lift, drag, lift/drag \n"); fprintf(stderr, " -g print lift/drag table: aoa, lift, drag, lift/drag \n");
fprintf(stderr, " -d print drag over TAS: kts, drag\n"); fprintf(stderr, " -d print drag over TAS: kts, drag\n");
fprintf(stderr, " -a set altitude in meters!\n"); fprintf(stderr, " -a set altitude in meters!\n");
fprintf(stderr, " -s set speed in knots\n"); fprintf(stderr, " -s set speed in knots\n");
fprintf(stderr, " -m print mass distribution table: id, x, y, z, mass \n"); fprintf(stderr, " -m print mass distribution table: id, x, y, z, mass \n");
fprintf(stderr, " -test print summary and output like -g -m \n"); fprintf(stderr, " -test print summary and output like -g -m \n");
return 1; return 1;
} }
int main(int argc, char** argv) int main(int argc, char** argv)
{ {
FGFDM* fdm = new FGFDM(); FGFDM* fdm = new FGFDM();
Airplane* a = fdm->getAirplane(); Airplane* a = fdm->getAirplane();
if(argc < 2) return usage(); if(argc < 2) return usage();
// Read // Read
try { try {
string file = argv[1]; string file = argv[1];
readXML(SGPath(file), *fdm); readXML(SGPath(file), *fdm);
}
catch (const sg_exception &e) {
printf("XML parse error: %s (%s)\n", e.getFormattedMessage().c_str(), e.getOrigin());
}
// ... and run
bool verbose {false};
if (argc > 2 && strcmp(argv[2], "-v") == 0) {
verbose=true;
}
if ((argc == 4) && (strcmp(argv[2], "--tweak") == 0)) {
float tweak = std::atof(argv[3]);
a->setSolverTweak(tweak);
a->setSolverMaxIterations(2000);
verbose=true;
}
a->compile(verbose);
if(a->getFailureMsg()) {
printf("SOLUTION FAILURE: %s\n", a->getFailureMsg());
}
if(!a->getFailureMsg() && argc > 2 ) {
bool test = (strcmp(argv[2], "-test") == 0);
if((strcmp(argv[2], "-g") == 0) || test)
{
float alt = 5000, kts = 100;
int cfg = CONFIG_NONE;
for(int i=3; i<argc; i++) {
if (std::strcmp(argv[i], "-a") == 0) {
if (i+1 < argc) alt = std::atof(argv[++i]);
}
else if(std::strcmp(argv[i], "-s") == 0) {
if(i+1 < argc) kts = std::atof(argv[++i]);
}
else if(std::strcmp(argv[i], "-approach") == 0) cfg = CONFIG_APPROACH;
else if(std::strcmp(argv[i], "-cruise") == 0) cfg = CONFIG_CRUISE;
else return usage();
}
if (test) {
report(a);
printf("\n#-- lift, drag at altitude %.0f meters, %.0f knots, Config %d --\n", alt, kts, cfg);
}
yasim_graph(a, alt, kts, cfg);
if (test) {
printf("\n#-- mass distribution --\n");
yasim_masses(a);
}
} }
else if(strcmp(argv[2], "-d") == 0) { catch (const sg_exception &e) {
float alt = 2000, aoa = a->getCruiseAoA(); printf("XML parse error: %s (%s)\n", e.getFormattedMessage().c_str(), e.getOrigin());
int cfg = CONFIG_NONE;
for(int i=3; i<argc; i++) {
if (std::strcmp(argv[i], "-a") == 0) {
if (i+1 < argc) alt = std::atof(argv[++i]);
}
else if(std::strcmp(argv[i], "-approach") == 0) cfg = CONFIG_APPROACH;
else if(std::strcmp(argv[i], "-cruise") == 0) cfg = CONFIG_CRUISE;
else return usage();
}
yasim_drag(a, aoa, alt, cfg);
}
else if(strcmp(argv[2], "-m") == 0) {
yasim_masses(a);
} }
} // ... and run
else { bool verbose {false};
report(a); if (argc > 2 && strcmp(argv[2], "-v") == 0) {
} verbose=true;
delete fdm; }
return 0; if ((argc == 4) && (strcmp(argv[2], "--tweak") == 0)) {
float tweak = std::atof(argv[3]);
a->setSolverTweak(tweak);
a->setSolverMaxIterations(2000);
verbose=true;
}
a->compile(verbose);
if(a->getFailureMsg()) {
printf("SOLUTION FAILURE: %s\n", a->getFailureMsg());
}
if(!a->getFailureMsg() && argc > 2 ) {
bool test = (strcmp(argv[2], "-test") == 0);
Airplane::Configuration cfg = Airplane::NONE;
float alt = 5000, kts = 100;
if((strcmp(argv[2], "-g") == 0) || test) {
for(int i=3; i<argc; i++) {
if (std::strcmp(argv[i], "-a") == 0) {
if (i+1 < argc) alt = std::atof(argv[++i]);
}
else if(std::strcmp(argv[i], "-s") == 0) {
if(i+1 < argc) kts = std::atof(argv[++i]);
}
else if(std::strcmp(argv[i], "-approach") == 0) cfg = Airplane::APPROACH;
else if(std::strcmp(argv[i], "-cruise") == 0) cfg = Airplane::CRUISE;
else return usage();
}
if (test) {
report(a);
printf("\n#-- lift, drag at altitude %.0f meters, %.0f knots, Config %d --\n", alt, kts, cfg);
}
yasim_graph(a, alt, kts, cfg);
if (test) {
printf("\n#-- mass distribution --\n");
yasim_masses(a);
}
}
else if(strcmp(argv[2], "-d") == 0) {
float alt = 2000, aoa = a->getCruiseAoA();
for(int i=3; i<argc; i++) {
if (std::strcmp(argv[i], "-a") == 0) {
if (i+1 < argc) alt = std::atof(argv[++i]);
}
else if(std::strcmp(argv[i], "-approach") == 0) cfg = Airplane::APPROACH;
else if(std::strcmp(argv[i], "-cruise") == 0) cfg = Airplane::CRUISE;
else return usage();
}
yasim_drag(a, aoa, alt, cfg);
}
else if(strcmp(argv[2], "-m") == 0) {
yasim_masses(a);
}
else if(strcmp(argv[2], "--min-speed") == 0) {
alt = 10;
for(int i=3; i<argc; i++) {
if (std::strcmp(argv[i], "-a") == 0) {
if (i+1 < argc) alt = std::atof(argv[++i]);
}
else if(std::strcmp(argv[i], "-approach") == 0) cfg = Airplane::APPROACH;
else if(std::strcmp(argv[i], "-cruise") == 0) cfg = Airplane::CRUISE;
else return usage();
}
findMinSpeed(a, alt);
}
}
else {
report(a);
}
delete fdm;
return 0;
} }