fly/flightgear
1
0
Fork 0
Code Activity

Maik JUSTUS: (OK'ed by Andy)

Browse source 
"""
- ground properties (e.g. feel bumpiness and the reduced friction of
  grass or go swimming with the beaver)
- initial load for yasim gears (to get rid of the jitter the beaver has
  on ground)
- glider/winch/aerotow (do winch start with YASim glider or do aerotow
  over the net) I will place a how-to on the wiki soon, here very short:
  use the sgs233y (or the bocian if you have AJ (up ot now) non-GPL
  bocian)
  winch start: Ctrl-w for placing the winch, hold w to winch, press
               Shift-w to release the tow
  aerotow: Place the glider within 60m to a MP-aircraft, press
           Ctrl-t to tow to this aircraft. If the MP-aircraft is the
           J3 and the patch is installed on both sides, the J3 feels the
           forces, too. The J3-pilot has to taxi very slow up to the
           moment, the glider starts moving. Increase the throttle gently.
           Don't lift the J3 early, wait for the glider being lifted,
           lift gently.
"""
This commit is contained in:
mfranz 2007-01-17 20:42:39 +00:00
parent 15754ccfc1
commit 53f09ff6a5
19 changed files with 1360 additions and 45 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

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

@ -7,6 +7,7 @@
#include "Surface.hpp"
#include "Rotorpart.hpp"
#include "Thruster.hpp"
#include "Hitch.hpp"
#include "Airplane.hpp"
namespace yasim {
@ -38,11 +39,13 @@ Airplane::Airplane()
_cruiseT = 0;
_cruiseSpeed = 0;
_cruiseWeight = 0;
_cruiseGlideAngle = 0;
_approachP = 0;
_approachT = 0;
_approachSpeed = 0;
_approachAoA = 0;
_approachWeight = 0;
_approachGlideAngle = 0;
_dragFactor = 1;
_liftRatio = 1;
@ -182,16 +185,17 @@ void Airplane::updateGearState()
}
}
void Airplane::setApproach(float speed, float altitude, float aoa, float fuel)
void Airplane::setApproach(float speed, float altitude, float aoa, float fuel, float gla)
{
_approachSpeed = speed;
_approachP = Atmosphere::getStdPressure(altitude);
_approachT = Atmosphere::getStdTemperature(altitude);
_approachAoA = aoa;
_approachFuel = fuel;
_approachGlideAngle = gla;
}
void Airplane::setCruise(float speed, float altitude, float fuel)
void Airplane::setCruise(float speed, float altitude, float fuel, float gla)
{
_cruiseSpeed = speed;
_cruiseP = Atmosphere::getStdPressure(altitude);
@ -199,6 +203,7 @@ void Airplane::setCruise(float speed, float altitude, float fuel)
_cruiseAoA = 0;
_tailIncidence = 0;
_cruiseFuel = fuel;
_cruiseGlideAngle = gla;
}
void Airplane::setElevatorControl(int control)
@ -323,6 +328,11 @@ void Airplane::addHook(Hook* hook)
_model.addHook(hook);
}
void Airplane::addHitch(Hitch* hitch)
{
_model.addHitch(hitch);
}
void Airplane::addLaunchbar(Launchbar* launchbar)
{
_model.addLaunchbar(launchbar);
@ -417,7 +427,7 @@ int Airplane::getSolutionIterations()
return _solutionIterations;
}
void Airplane::setupState(float aoa, float speed, State* s)
void Airplane::setupState(float aoa, float speed, float gla, State* s)
{
float cosAoA = Math::cos(aoa);
float sinAoA = Math::sin(aoa);
@ -425,7 +435,7 @@ void Airplane::setupState(float aoa, float speed, State* s)
s->orient[3] = 0; s->orient[4] = 1; s->orient[5] = 0;
s->orient[6] = -sinAoA; s->orient[7] = 0; s->orient[8] = cosAoA;
s->v[0] = speed; s->v[1] = 0; s->v[2] = 0;
s->v[0] = speed*Math::cos(gla); s->v[1] = -speed*Math::sin(gla); s->v[2] = 0;
int i;
for(i=0; i<3; i++)
@ -602,6 +612,7 @@ void Airplane::compileContactPoints()
// I made these up
g->setStaticFriction(0.6f);
g->setDynamicFriction(0.5f);
g->setContactPoint(1);
_model.addGear(g);
}
@ -709,7 +720,8 @@ void Airplane::solveGear()
g->getPosition(pos);
Math::sub3(cg, pos, pos);
gr->wgt = 1.0f/(0.5f+Math::sqrt(pos[0]*pos[0] + pos[1]*pos[1]));
total += gr->wgt;
if (!g->getIgnoreWhileSolving())
total += gr->wgt;
}
// Renormalize so they sum to 1
@ -731,7 +743,7 @@ void Airplane::solveGear()
float e = energy * gr->wgt;
float comp[3];
gr->gear->getCompression(comp);
float len = Math::mag3(comp);
float len = Math::mag3(comp)*(1+2*gr->gear->getInitialLoad());
// Energy in a spring: e = 0.5 * k * len^2
float k = 2 * e / (len*len);
@ -773,7 +785,7 @@ void Airplane::setupWeights(bool isApproach)
void Airplane::runCruise()
{
setupState(_cruiseAoA, _cruiseSpeed, &_cruiseState);
setupState(_cruiseAoA, _cruiseSpeed,_approachGlideAngle, &_cruiseState);
_model.setState(&_cruiseState);
_model.setAir(_cruiseP, _cruiseT,
Atmosphere::calcStdDensity(_cruiseP, _cruiseT));
@ -816,7 +828,7 @@ void Airplane::runCruise()
void Airplane::runApproach()
{
setupState(_approachAoA, _approachSpeed, &_approachState);
setupState(_approachAoA, _approachSpeed,_approachGlideAngle, &_approachState);
_model.setState(&_approachState);
_model.setAir(_approachP, _approachT,
Atmosphere::calcStdDensity(_approachP, _approachT));
@ -924,7 +936,7 @@ void Airplane::solve()
runCruise();
_model.getThrust(tmp);
float thrust = tmp[0];
float thrust = tmp[0] + _cruiseWeight * Math::sin(_cruiseGlideAngle) * 9.81;
_model.getBody()->getAccel(tmp);
Math::tmul33(_cruiseState.orient, tmp, tmp);
@ -1063,7 +1075,7 @@ void Airplane::solveHelicopter()
applyDragFactor(Math::pow(15.7/1000, 1/SOLVE_TWEAK));
applyLiftRatio(Math::pow(104, 1/SOLVE_TWEAK));
}
setupState(0,0, &_cruiseState);
setupState(0,0,0, &_cruiseState);
_model.setState(&_cruiseState);
setupWeights(true);
_controls.reset();

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

@ -13,6 +13,7 @@ class Gear;
class Hook;
class Launchbar;
class Thruster;
class Hitch;
class Airplane {
public:
@ -45,12 +46,13 @@ public:
void addLaunchbar(Launchbar* l);
void addThruster(Thruster* t, float mass, float* cg);
void addBallast(float* pos, float mass);
void addHitch(Hitch* h);
int addWeight(float* pos, float size);
void setWeight(int handle, float mass);
void setApproach(float speed, float altitude, float aoa, float fuel);
void setCruise(float speed, float altitude, float fuel);
void setApproach(float speed, float altitude, float aoa, float fuel, float gla);
void setCruise(float speed, float altitude, float fuel, float gla);
void setElevatorControl(int control);
void addApproachControl(int control, float val);
@ -61,6 +63,8 @@ public:
int numGear();
Gear* getGear(int g);
Hook* getHook();
int numHitches() { return _hitches.size(); }
Hitch* getHitch(int h);
Rotorgear* getRotorgear();
Launchbar* getLaunchbar();
@ -88,7 +92,7 @@ public:
float getApproachElevator() { return _approachElevator.val; }
char* getFailureMsg();
static void setupState(float aoa, float speed, State* s); // utility
static void setupState(float aoa, float speed, float gla, State* s); // utility
private:
struct Tank { float pos[3]; float cap; float fill;
@ -139,6 +143,7 @@ private:
Vector _contacts; // non-gear ground contact points
Vector _weights;
Vector _surfs; // NON-wing Surfaces
Vector _hitches; //for airtow and winch
Vector _solveWeights;
@ -149,6 +154,7 @@ private:
float _cruiseSpeed;
float _cruiseWeight;
float _cruiseFuel;
float _cruiseGlideAngle;
Vector _approachControls;
State _approachState;
@ -158,6 +164,7 @@ private:
float _approachAoA;
float _approachWeight;
float _approachFuel;
float _approachGlideAngle;
int _solutionIterations;
float _dragFactor;

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

@ -10,6 +10,7 @@
#include "Rotor.hpp"
#include "Math.hpp"
#include "Propeller.hpp"
#include "Hitch.hpp"
#include "ControlMap.hpp"
namespace yasim {
@ -219,6 +220,10 @@ void ControlMap::applyControls(float dt)
case WASTEGATE:
((PistonEngine*)((Thruster*)obj)->getEngine())->setWastegate(lval);
break;
case WINCHRELSPEED: ((Hitch*)obj)->setWinchRelSpeed(lval); break;
case HITCHOPEN: ((Hitch*)obj)->setOpen(lval!=0); break;
case PLACEWINCH: ((Hitch*)obj)->setWinchPositionAuto(lval!=0); break;
case FINDAITOW: ((Hitch*)obj)->findBestAIObject(lval!=0); break;
}
}
}
@ -233,6 +238,7 @@ float ControlMap::rangeMin(int type)
case CYCLICELE: return -1;
case CYCLICAIL: return -1;
case COLLECTIVE: return -1;
case WINCHRELSPEED: return -1;
case MAGNETOS: return 0; // [0:3]
default: return 0; // [0:1]
}

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

@ -16,7 +16,8 @@ public:
BOOST, CASTERING, PROPPITCH, PROPFEATHER,
COLLECTIVE, CYCLICAIL, CYCLICELE, ROTORENGINEON,
ROTORBRAKE,
REVERSE_THRUST, WASTEGATE };
REVERSE_THRUST, WASTEGATE,
WINCHRELSPEED, HITCHOPEN, PLACEWINCH, FINDAITOW};
enum { OPT_SPLIT = 0x01,
OPT_INVERT = 0x02,

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

@ -16,6 +16,7 @@
#include "TurbineEngine.hpp"
#include "Rotor.hpp"
#include "Rotorpart.hpp"
#include "Hitch.hpp"
#include "FGFDM.hpp"
@ -158,12 +159,14 @@ void FGFDM::startElement(const char* name, const XMLAttributes &atts)
float spd = attrf(a, "speed") * KTS2MPS;
float alt = attrf(a, "alt", 0) * FT2M;
float aoa = attrf(a, "aoa", 0) * DEG2RAD;
_airplane.setApproach(spd, alt, aoa, attrf(a, "fuel", 0.2));
float gla = attrf(a, "glide-angle", 0) * DEG2RAD;
_airplane.setApproach(spd, alt, aoa, attrf(a, "fuel", 0.2),gla);
_cruiseCurr = false;
} else if(eq(name, "cruise")) {
float spd = attrf(a, "speed") * KTS2MPS;
float alt = attrf(a, "alt") * FT2M;
_airplane.setCruise(spd, alt, attrf(a, "fuel", 0.5));
float gla = attrf(a, "glide-angle", 0) * DEG2RAD;
_airplane.setCruise(spd, alt, attrf(a, "fuel", 0.5),gla);
_cruiseCurr = true;
} else if(eq(name, "solve-weight")) {
int idx = attri(a, "idx");
@ -245,6 +248,46 @@ void FGFDM::startElement(const char* name, const XMLAttributes &atts)
er->eng = j;
er->prefix = dup(buf);
_thrusters.add(er);
} else if(eq(name, "hitch")) {
Hitch* h = new Hitch(a->getValue("name"));
_currObj = h;
v[0] = attrf(a, "x");
v[1] = attrf(a, "y");
v[2] = attrf(a, "z");
h->setPosition(v);
if(a->hasAttribute("force-is-calculated-by-other")) h->setForceIsCalculatedByOther(attrb(a,"force-is-calculated-by-other"));
_airplane.addHitch(h);
} else if(eq(name, "tow")) {
Hitch* h = (Hitch*)_currObj;
if(a->hasAttribute("length"))
h->setTowLength(attrf(a, "length"));
if(a->hasAttribute("elastic-constant"))
h->setTowElasticConstant(attrf(a, "elastic-constant"));
if(a->hasAttribute("break-force"))
h->setTowBreakForce(attrf(a, "break-force"));
if(a->hasAttribute("weight-per-meter"))
h->setTowWeightPerM(attrf(a, "weight-per-meter"));
if(a->hasAttribute("mp-auto-connect-period"))
h->setMpAutoConnectPeriod(attrf(a, "mp-auto-connect-period"));
} else if(eq(name, "winch")) {
Hitch* h = (Hitch*)_currObj;
double pos[3];
pos[0] = attrd(a, "x",0);
pos[1] = attrd(a, "y",0);
pos[2] = attrd(a, "z",0);
h->setWinchPosition(pos);
if(a->hasAttribute("max-speed"))
h->setWinchMaxSpeed(attrf(a, "max-speed"));
if(a->hasAttribute("power"))
h->setWinchPower(attrf(a, "power") * 1000);
if(a->hasAttribute("max-force"))
h->setWinchMaxForce(attrf(a, "max-force"));
if(a->hasAttribute("initial-tow-length"))
h->setWinchInitialTowLength(attrf(a, "initial-tow-length"));
if(a->hasAttribute("max-tow-length"))
h->setWinchMaxTowLength(attrf(a, "max-tow-length"));
if(a->hasAttribute("min-tow-length"))
h->setWinchMinTowLength(attrf(a, "min-tow-length"));
} else if(eq(name, "gear")) {
Gear* g = new Gear();
_currObj = g;
@ -269,10 +312,17 @@ void FGFDM::startElement(const char* name, const XMLAttributes &atts)
v[i] *= attrf(a, "compression", 1);
g->setCompression(v);
g->setBrake(attrf(a, "skid", 0));
g->setInitialLoad(attrf(a, "initial-load", 0));
g->setStaticFriction(attrf(a, "sfric", 0.8));
g->setDynamicFriction(attrf(a, "dfric", 0.7));
g->setSpring(attrf(a, "spring", 1));
g->setDamping(attrf(a, "damp", 1));
if(a->hasAttribute("on-water")) g->setOnWater(attrb(a,"on-water"));
if(a->hasAttribute("on-solid")) g->setOnSolid(attrb(a,"on-solid"));
if(a->hasAttribute("ignored-by-solver")) g->setIgnoreWhileSolving(attrb(a,"ignored-by-solver"));
g->setSpringFactorNotPlaning(attrf(a, "spring-factor-not-planing", 1));
g->setSpeedPlaning(attrf(a, "speed-planing", 0) * KTS2MPS);
g->setReduceFrictionByExtension(attrf(a, "reduce-friction-by-extension", 0));
_airplane.addGear(g);
} else if(eq(name, "hook")) {
Hook* h = new Hook();
@ -898,6 +948,11 @@ int FGFDM::parseOutput(const char* name)
if(eq(name, "ROTORBRAKE")) return ControlMap::ROTORBRAKE;
if(eq(name, "REVERSE_THRUST")) return ControlMap::REVERSE_THRUST;
if(eq(name, "WASTEGATE")) return ControlMap::WASTEGATE;
if(eq(name, "WINCHRELSPEED")) return ControlMap::WINCHRELSPEED;
if(eq(name, "HITCHOPEN")) return ControlMap::HITCHOPEN;
if(eq(name, "PLACEWINCH")) return ControlMap::PLACEWINCH;
if(eq(name, "FINDAITOW")) return ControlMap::FINDAITOW;
SG_LOG(SG_FLIGHT,SG_ALERT,"Unrecognized control type '"
<< name << "' in YASim aircraft description.");
exit(1);
@ -972,6 +1027,23 @@ float FGFDM::attrf(XMLAttributes* atts, char* attr, float def)
else return (float)atof(val);
}
double FGFDM::attrd(XMLAttributes* atts, char* attr)
{
if(!atts->hasAttribute(attr)) {
SG_LOG(SG_FLIGHT,SG_ALERT,"Missing '" << attr <<
"' in YASim aircraft description");
exit(1);
}
return attrd(atts, attr, 0);
}
double FGFDM::attrd(XMLAttributes* atts, char* attr, double def)
{
const char* val = atts->getValue(attr);
if(val == 0) return def;
else return atof(val);
}
// 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

2
src/FDM/YASim/FGFDM.hpp
View file

@ -53,6 +53,8 @@ private:
int attri(XMLAttributes* atts, char* attr, int def);
float attrf(XMLAttributes* atts, char* attr);
float attrf(XMLAttributes* atts, char* attr, float def);
double attrd(XMLAttributes* atts, char* attr);
double attrd(XMLAttributes* atts, char* attr, double def);
bool attrb(XMLAttributes* atts, char* attr);
// The core Airplane object we manage.

17
src/FDM/YASim/FGGround.cpp
View file

@ -35,6 +35,23 @@ void FGGround::getGroundPlane(const double pos[3],
for(int i=0; i<3; i++) vel[i] = dvel[i];
}
void FGGround::getGroundPlane(const double pos[3],
double plane[4], float vel[3],
int *type, const SGMaterial **material
)
{
// Return values for the callback.
double agl;
double cp[3], dvel[3];
_iface->get_agl_m(_toff, pos, cp, plane, dvel,
type, material, &agl);
// The plane below the actual contact point.
plane[3] = plane[0]*cp[0] + plane[1]*cp[1] + plane[2]*cp[2];
for(int i=0; i<3; i++) vel[i] = dvel[i];
}
bool FGGround::caughtWire(const double pos[4][3])
{
return _iface->caught_wire_m(_toff, pos);

11
src/FDM/YASim/FGGround.hpp
View file

@ -4,6 +4,7 @@
#include "Ground.hpp"
class FGInterface;
class SGMaterial;
namespace yasim {
@ -18,6 +19,16 @@ public:
virtual void getGroundPlane(const double pos[3],
double plane[4], float vel[3]);
virtual void getGroundPlane(const double pos[3],
double plane[4], float vel[3],
int *type, const SGMaterial **material);/*
double *frictionFactor,
double *rollingFriction,
double *loadCapacity,
double *loadResistance,
double *bumpiness,
bool *isSolid);*/
virtual bool caughtWire(const double pos[4][3]);
virtual bool getWire(double end[2][3], float vel[2][3]);

210
src/FDM/YASim/Gear.cpp
View file

@ -2,8 +2,13 @@
#include "BodyEnvironment.hpp"
#include "RigidBody.hpp"
#include <simgear/scene/material/mat.hxx>
#include <FDM/flight.hxx>
#include "Gear.hpp"
namespace yasim {
static const float YASIM_PI = 3.14159265358979323846;
static const float maxGroundBumpAmplitude=0.4;
//Amplitude can be positive and negative
Gear::Gear()
{
@ -16,9 +21,26 @@ Gear::Gear()
_dfric = 0.7f;
_brake = 0;
_rot = 0;
_initialLoad = 0;
_extension = 1;
_castering = false;
_frac = 0;
_ground_type = 0;
_ground_frictionFactor = 1;
_ground_rollingFriction = 0.02;
_ground_loadCapacity = 1e30;
_ground_loadResistance = 1e30;
_ground_isSolid = 1;
_ground_bumpiness = 0;
_onWater = 0;
_onSolid = 1;
_global_x = 0.0;
_global_y = 0.0;
_reduceFrictionByExtension = 0;
_spring_factor_not_planing = 1;
_speed_planing = 0;
_isContactPoint = 0;
_ignoreWhileSolving = 0;
for(i=0; i<3; i++)
_global_ground[i] = _global_vel[i] = 0;
@ -78,12 +100,99 @@ void Gear::setCastering(bool c)
_castering = c;
}
void Gear::setGlobalGround(double *global_ground, float* global_vel)
void Gear::setContactPoint(bool c)
{
_isContactPoint=c;
}
void Gear::setOnWater(bool c)
{
_onWater = c;
}
void Gear::setOnSolid(bool c)
{
_onSolid = c;
}
void Gear::setIgnoreWhileSolving(bool c)
{
_ignoreWhileSolving = c;
}
void Gear::setSpringFactorNotPlaning(float f)
{
_spring_factor_not_planing = f;
}
void Gear::setSpeedPlaning(float s)
{
_speed_planing = s;
}
void Gear::setReduceFrictionByExtension(float s)
{
_reduceFrictionByExtension = s;
}
void Gear::setInitialLoad(float l)
{
_initialLoad = l;
}
void Gear::setGlobalGround(double *global_ground, float* global_vel,
double globalX, double globalY,
int type, const SGMaterial *material)
{
int i;
double frictionFactor,rollingFriction,loadCapacity,loadResistance,bumpiness;
bool isSolid;
for(i=0; i<4; i++) _global_ground[i] = global_ground[i];
for(i=0; i<3; i++) _global_vel[i] = global_vel[i];
}
if (material) {
loadCapacity = (*material).get_load_resistence();
frictionFactor =(*material).get_friction_factor();
rollingFriction = (*material).get_rolling_friction();
loadResistance = (*material).get_load_resistence();
bumpiness = (*material).get_bumpiness();
isSolid = (*material).get_solid();
} else {
if (type == FGInterface::Solid) {
loadCapacity = DBL_MAX;
frictionFactor = 1.0;
rollingFriction = 0.02;
loadResistance = DBL_MAX;
bumpiness = 0.0;
isSolid = true;
} else if (type == FGInterface::Water) {
loadCapacity = DBL_MAX;
frictionFactor = 1.0;
rollingFriction = 2;
loadResistance = DBL_MAX;
bumpiness = 0.8;
isSolid = false;
} else {
loadCapacity = DBL_MAX;
frictionFactor = 0.9;
rollingFriction = 0.1;
loadResistance = DBL_MAX;
bumpiness = 0.2;
isSolid = true;
}
}
_ground_type = type;
_ground_frictionFactor = frictionFactor;
_ground_rollingFriction = rollingFriction;
_ground_loadCapacity = loadCapacity;
_ground_loadResistance = loadResistance;
_ground_bumpiness = bumpiness;
_ground_isSolid = isSolid;
_global_x = globalX;
_global_y = globalY;
}
void Gear::getPosition(float* out)
{
@ -159,6 +268,31 @@ bool Gear::getCastering()
return _castering;
}
bool Gear::getGroundIsSolid()
{
return _ground_isSolid;
}
float Gear::getBumpAltitude()
{
if (_ground_bumpiness<0.001) return 0.0;
double x = _global_x*0.1;
double y = _global_y*0.1;
x -= Math::floor(x);
y -= Math::floor(y);
x *= 2*YASIM_PI;
y *= 2*YASIM_PI;
//now x and y are in the range of 0..2pi
//we need a function, that is periodically on 2pi and gives some
//height. This is not very fast, but for a beginning.
//maybe this should be done by interpolating between some precalculated
//values
float h = Math::sin(x)+Math::sin(7*x)+Math::sin(8*x)+Math::sin(13*x);
h += Math::sin(2*y)+Math::sin(5*y)+Math::sin(9*y*x)+Math::sin(17*y);
return h*(1/8.)*_ground_bumpiness*maxGroundBumpAmplitude;
}
void Gear::calcForce(RigidBody* body, State *s, float* v, float* rot)
{
// Init the return values
@ -169,6 +303,16 @@ void Gear::calcForce(RigidBody* body, State *s, float* v, float* rot)
if(_extension < 1)
return;
// Dont bother if we are in the "wrong" ground
if (!((_onWater&&!_ground_isSolid)||(_onSolid&&_ground_isSolid))) {
_wow = 0;
_frac = 0;
_compressDist = 0;
_rollSpeed = 0;
_casterAngle = 0;
return;
}
// The ground plane transformed to the local frame.
float ground[4];
s->planeGlobalToLocal(_global_ground, ground);
@ -180,6 +324,12 @@ void Gear::calcForce(RigidBody* body, State *s, float* v, float* rot)
// First off, make sure that the gear "tip" is below the ground.
// If it's not, there's no force.
float a = ground[3] - Math::dot3(_pos, ground);
float BumpAltitude=0;
if (a<maxGroundBumpAmplitude)
{
BumpAltitude=getBumpAltitude();
a+=BumpAltitude;
}
_compressDist = -a;
if(a > 0) {
_wow = 0;
@ -196,7 +346,7 @@ void Gear::calcForce(RigidBody* body, State *s, float* v, float* rot)
// above ground is negative.
float tmp[3];
Math::add3(_cmpr, _pos, tmp);
float b = ground[3] - Math::dot3(tmp, ground);
float b = ground[3] - Math::dot3(tmp, ground)+BumpAltitude;
// Calculate the point of ground _contact.
_frac = a/(a-b);
@ -218,8 +368,25 @@ void Gear::calcForce(RigidBody* body, State *s, float* v, float* rot)
// Finally, we can start adding up the forces. First the spring
// compression. (note the clamping of _frac to 1):
_frac = (_frac > 1) ? 1 : _frac;
float fmag = _frac*clen*_spring;
// Add the initial load to frac, but with continous transistion around 0
float frac_with_initial_load;
if (_frac>0.2 || _initialLoad==0.0)
frac_with_initial_load = _frac+_initialLoad;
else
frac_with_initial_load = (_frac+_initialLoad)
*_frac*_frac*3*25-_frac*_frac*_frac*2*125;
float fmag = frac_with_initial_load*clen*_spring;
if (_speed_planing>0)
{
float v = Math::mag3(cv);
if (v < _speed_planing)
{
float frac = v/_speed_planing;
fmag = fmag*_spring_factor_not_planing*(1-frac)+fmag*frac;
}
}
// Then the damping. Use the only the velocity into the ground
// (projection along "ground") projected along the compression
// axis. So Vdamp = ground*(ground dot cv) dot cmpr
@ -284,11 +451,28 @@ void Gear::calcForce(RigidBody* body, State *s, float* v, float* rot)
_rollSpeed = vsteer;
_casterAngle = _rot;
}
float fsteer = _brake * calcFriction(wgt, vsteer);
float fskid = calcFriction(wgt, vskid);
float fsteer,fskid;
if(_ground_isSolid)
{
fsteer = (_brake * _ground_frictionFactor
+(1-_brake)*_ground_rollingFriction
)*calcFriction(wgt, vsteer);
fskid = calcFriction(wgt, vskid)*(_ground_frictionFactor);
}
else
{
fsteer = calcFrictionFluid(wgt, vsteer)*_ground_frictionFactor;
fskid = 10*calcFrictionFluid(wgt, vskid)*_ground_frictionFactor;
//factor 10: floats have different drag in x and y.
}
if(vsteer > 0) fsteer = -fsteer;
if(vskid > 0) fskid = -fskid;
//reduce friction if wanted by _reduceFrictionByExtension
float factor = (1-_frac)*(1-_reduceFrictionByExtension)+_frac*1;
factor = Math::clamp(factor,0,1);
fsteer *= factor;
fskid *= factor;
// Phoo! All done. Add it up and get out of here.
Math::mul3(fsteer, steer, tmp);
@ -298,7 +482,7 @@ void Gear::calcForce(RigidBody* body, State *s, float* v, float* rot)
Math::add3(tmp, _force, _force);
}
float Gear::calcFriction(float wgt, float v)
float Gear::calcFriction(float wgt, float v) //used on solid ground
{
// How slow is stopped? 10 cm/second?
const float STOP = 0.1f;
@ -308,5 +492,15 @@ float Gear::calcFriction(float wgt, float v)
else return wgt * _dfric;
}
float Gear::calcFrictionFluid(float wgt, float v) //used on fluid ground
{
// How slow is stopped? 1 cm/second?
const float STOP = 0.01f;
const float iSTOP = 1.0f/STOP;
v = Math::abs(v);
if(v < STOP) return v*iSTOP * wgt * _sfric;
else return wgt * _dfric*v*v*0.01;
//*0.01: to get _dfric of the same size than _dfric on solid
}
}; // namespace yasim

39
src/FDM/YASim/Gear.hpp
View file

@ -1,6 +1,8 @@
#ifndef _GEAR_HPP
#define _GEAR_HPP
class SGMaterial;
namespace yasim {
class RigidBody;
@ -39,8 +41,16 @@ public:
void setRotation(float rotation);
void setExtension(float extension);
void setCastering(bool castering);
void setGlobalGround(double* global_ground, float* global_vel);
void setOnWater(bool c);
void setOnSolid(bool c);
void setSpringFactorNotPlaning(float f);
void setSpeedPlaning(float s);
void setReduceFrictionByExtension(float s);
void setInitialLoad(float l);
void setIgnoreWhileSolving(bool c);
void setGlobalGround(double* global_ground, float* global_vel,
double globalX, double globalY,
int type, const SGMaterial *material);
void getPosition(float* out);
void getCompression(float* out);
void getGlobalGround(double* global_ground);
@ -51,9 +61,12 @@ public:
float getBrake();
float getRotation();
float getExtension();
float getInitialLoad() {return _initialLoad; }
bool getCastering();
float getCasterAngle() { return _casterAngle; }
float getRollSpeed() { return _rollSpeed; }
float getBumpAltitude();
bool getGroundIsSolid();
// Takes a velocity of the aircraft relative to ground, a rotation
// vector, and a ground plane (all specified in local coordinates)
@ -67,9 +80,13 @@ public:
float getWoW();
float getCompressFraction();
float getCompressDist() { return _compressDist; }
bool getSubmergable() {return (!_ground_isSolid)&&(!_isContactPoint); }
bool getIgnoreWhileSolving() {return _ignoreWhileSolving; }
void setContactPoint(bool c);
private:
float calcFriction(float wgt, float v);
float calcFrictionFluid(float wgt, float v);
bool _castering;
float _pos[3];
@ -85,11 +102,29 @@ private:
float _contact[3];
float _wow;
float _frac;
float _initialLoad;
float _compressDist;
double _global_ground[4];
float _global_vel[3];
float _casterAngle;
float _rollSpeed;
bool _isContactPoint;
bool _onWater;
bool _onSolid;
float _spring_factor_not_planing;
float _speed_planing;
float _reduceFrictionByExtension;
bool _ignoreWhileSolving;
int _ground_type;
double _ground_frictionFactor;
double _ground_rollingFriction;
double _ground_loadCapacity;
double _ground_loadResistance;
double _ground_bumpiness;
bool _ground_isSolid;
double _global_x;
double _global_y;
};
}; // namespace yasim

8
src/FDM/YASim/Ground.cpp
View file

@ -1,5 +1,6 @@
#include "Glue.hpp"
#include <simgear/scene/material/mat.hxx>
#include "Ground.hpp"
namespace yasim {
@ -28,6 +29,13 @@ void Ground::getGroundPlane(const double pos[3],
vel[2] = 0.0;
}
void Ground::getGroundPlane(const double pos[3],
double plane[4], float vel[3],
int *type, const SGMaterial **material)
{
getGroundPlane(pos,plane,vel);
}
bool Ground::caughtWire(const double pos[4][3])
{
return false;

5
src/FDM/YASim/Ground.hpp
View file

@ -1,6 +1,7 @@
#ifndef _GROUND_HPP
#define _GROUND_HPP
class SGMaterial;
namespace yasim {
class Ground {
@ -11,6 +12,10 @@ public:
virtual void getGroundPlane(const double pos[3],
double plane[4], float vel[3]);
virtual void getGroundPlane(const double pos[3],
double plane[4], float vel[3],
int *type, const SGMaterial **material);
virtual bool caughtWire(const double pos[4][3]);
virtual bool getWire(double end[2][3], float vel[2][3]);

778
src/FDM/YASim/Hitch.cpp Executable file
View file

@ -0,0 +1,778 @@
#include "Math.hpp"
#include "BodyEnvironment.hpp"
#include "RigidBody.hpp"
#include <Main/fg_props.hxx>
#include <string.h>
#include "Hitch.hpp"
namespace yasim {
Hitch::Hitch(const char *name)
{
int i;
strncpy(_name,name,128);
_name[127]=0;
for(i=0; i<3; i++)
_pos[i] = _force[i] = _winchPos[i] = _mp_lpos[i]=_towEndForce[i]=_mp_force[i]=0;
for(i=0; i<2; i++)
_global_ground[i] = 0;
_global_ground[2] = 1;
_global_ground[3] = -1e5;
_forceMagnitude=0;
_open=true;
_oldOpen=_open;
_towLength=60;
_towElasticConstant=1e5;
_towBrakeForce=100000;
_towWeightPerM=1;
_winchMaxSpeed=40;
_winchRelSpeed=0;
_winchInitialTowLength=1000;
_winchPower=100000;
_winchMaxForce=10000;
_winchActualForce=0;
_winchMaxTowLength=1000;
_winchMinTowLength=0;
_dist=0;
_towEndIsConnectedToProperty=false;
_towEndNode=0;
_nodeIsMultiplayer=false;
_nodeIsAiAircraft=false;
_forceIsCalculatedByMaster=false;
_nodeID=0;
//_ai_MP_callsign=0;
_height_above_ground=0;
_winch_height_above_ground=0;
_loPosFrac=0;
_lowest_tow_height=0;
_state=new State;
_displayed_len_lower_dist_message=false;
_last_wish=true;
_isSlave=false;
_mpAutoConnectPeriod=0;
_timeToNextAutoConnectTry=0;
_timeToNextReConnectTry=10;
_speed_in_tow_direction=0;
_mp_time_lag=1;
_mp_last_reported_dist=0;
_mp_last_reported_v=0;
_mp_is_slave=false;
_mp_open_last_state=false;
_timeLagCorrectedDist=0;
//tie the properties
char text[128];
sprintf(text,"/sim/hitches/%s", _name);
SGPropertyNode * node = fgGetNode(text, true);
node->tie("tow/length",SGRawValuePointer<float>(&_towLength));
node->tie("tow/elastic-constant",SGRawValuePointer<float>(&_towElasticConstant));
node->tie("tow/weight-per-m-kg-m",SGRawValuePointer<float>(&_towWeightPerM));
node->tie("tow/brake-force",SGRawValuePointer<float>(&_towBrakeForce));
node->tie("winch/max-speed-m-s",SGRawValuePointer<float>(&_winchMaxSpeed));
node->tie("winch/rel-speed",SGRawValuePointer<float>(&_winchRelSpeed));
node->tie("winch/initial-tow-length-m",SGRawValuePointer<float>(&_winchInitialTowLength));
node->tie("winch/min-tow-length-m",SGRawValuePointer<float>(&_winchMinTowLength));
node->tie("winch/max-tow-length-m",SGRawValuePointer<float>(&_winchMaxTowLength));
node->tie("winch/global-pos-x",SGRawValuePointer<double>(&_winchPos[0]));
node->tie("winch/global-pos-y",SGRawValuePointer<double>(&_winchPos[1]));
node->tie("winch/global-pos-z",SGRawValuePointer<double>(&_winchPos[2]));
node->tie("winch/max-power",SGRawValuePointer<float>(&_winchPower));
node->tie("winch/max-force",SGRawValuePointer<float>(&_winchMaxForce));
node->tie("winch/actual-force",SGRawValuePointer<float>(&_winchActualForce));
node->tie("tow/end-force-x",SGRawValuePointer<float>(&_reportTowEndForce[0]));
node->tie("tow/end-force-y",SGRawValuePointer<float>(&_reportTowEndForce[1]));
node->tie("tow/end-force-z",SGRawValuePointer<float>(&_reportTowEndForce[2]));
node->tie("force",SGRawValuePointer<float>(&_forceMagnitude));
node->tie("open",SGRawValuePointer<bool>(&_open));
node->tie("force-is-calculated-by-other",SGRawValuePointer<bool>(&_forceIsCalculatedByMaster));
node->tie("local-pos-x",SGRawValuePointer<float>(&_pos[0]));
node->tie("local-pos-y",SGRawValuePointer<float>(&_pos[1]));
node->tie("local-pos-z",SGRawValuePointer<float>(&_pos[2]));
node->tie("tow/dist",SGRawValuePointer<float>(&_dist));
node->tie("tow/dist-time-lag-corrected",SGRawValuePointer<float>(&_timeLagCorrectedDist));
node->tie("tow/connected-to-property-node",SGRawValuePointer<bool>(&_towEndIsConnectedToProperty));
node->tie("tow/connected-to-mp-node",SGRawValuePointer<bool>(&_nodeIsMultiplayer));
node->tie("tow/connected-to-ai-node",SGRawValuePointer<bool>(&_nodeIsAiAircraft));
node->tie("tow/connected-to-ai-or-mp-id",SGRawValuePointer<int>(&_nodeID));
node->tie("debug/hitch-height-above-ground",SGRawValuePointer<float>(&_height_above_ground));
node->tie("debug/tow-end-height-above-ground",SGRawValuePointer<float>(&_winch_height_above_ground));
node->tie("debug/tow-rel-lo-pos",SGRawValuePointer<float>(&_loPosFrac));
node->tie("debug/tow-lowest-pos-height",SGRawValuePointer<float>(&_lowest_tow_height));
node->tie("is-slave",SGRawValuePointer<bool>(&_isSlave));
node->tie("speed-in-tow-direction",SGRawValuePointer<float>(&_speed_in_tow_direction));
node->tie("mp-auto-connect-period",SGRawValuePointer<float>(&_mpAutoConnectPeriod));
node->tie("mp-time-lag",SGRawValuePointer<float>(&_mp_time_lag));
node->setStringValue("tow/Node","");
node->setStringValue("tow/connectedToAIorMP-callsign");
node->setBoolValue("broken",false);
}
Hitch::~Hitch()
{
char text[128];
sprintf(text,"/sim/hitches/%s", _name);
SGPropertyNode * node = fgGetNode(text, true);
node->untie("tow/length");
node->untie("tow/elastic-constant");
node->untie("tow/weight-per-m-kg-m");
node->untie("tow/brake-force");
node->untie("winch/max-speed-m-s");
node->untie("winch/rel-speed");
node->untie("winch/initial-tow-length-m");
node->untie("winch/min-tow-length-m");
node->untie("winch/max-tow-length-m");
node->untie("winch/global-pos-x");
node->untie("winch/global-pos-y");
node->untie("winch/global-pos-z");
node->untie("winch/max-power");
node->untie("winch/max-force");
node->untie("winch/actual-force");
node->untie("tow/end-force-x");
node->untie("tow/end-force-y");
node->untie("tow/end-force-z");
node->untie("force");
node->untie("open");
node->untie("force-is-calculated-by-other");
node->untie("local-pos-x");
node->untie("local-pos-y");
node->untie("local-pos-z");
node->untie("tow/dist");
node->untie("tow/dist-time-lag-corrected");
node->untie("tow/connected-to-property-node");
node->untie("tow/connected-to-mp-node");
node->untie("tow/connected-to-ai-node");
node->untie("tow/connected-to-ai-or-mp-id");
node->untie("debug/hitch-height-above-ground");
node->untie("debug/tow-end-height-above-ground");
node->untie("debug/tow-rel-lo-pos");
node->untie("debug/tow-lowest-pos-height");
node->untie("is-slave");
node->untie("speed-in-tow-direction");
node->untie("mp-auto-connect-period");
node->untie("mp-time-lag");
delete _state;
}
void Hitch::setPosition(float* position)
{
int i;
for(i=0; i<3; i++) _pos[i] = position[i];
}
void Hitch::setTowLength(float length)
{
_towLength = length;
}
void Hitch::setOpen(bool isOpen)
{
//test if we already processed this before
//without this test a binded property could
//try to close the Hitch every run
//it will close, if we are near the end
//e.g. if we are flying over the parked
//tow-aircraft....
if (isOpen==_last_wish)
return;
_last_wish=isOpen;
_open=isOpen;
}
void Hitch::setTowElasticConstant(float sc)
{
_towElasticConstant=sc;
}
void Hitch::setTowBreakForce(float bf)
{
_towBrakeForce=bf;
}
void Hitch::setWinchMaxForce(float f)
{
_winchMaxForce=f;
}
void Hitch::setTowWeightPerM(float rw)
{
_towWeightPerM=rw;
}
void Hitch::setWinchMaxSpeed(float mws)
{
_winchMaxSpeed=mws;
}
void Hitch::setWinchRelSpeed(float rws)
{
_winchRelSpeed=rws;
}
void Hitch::setWinchPosition(double *winchPosition)//in global coordinates!
{
for (int i=0; i<3;i++)
_winchPos[i]=winchPosition[i];
}
void Hitch::setMpAutoConnectPeriod(float dt)
{
_mpAutoConnectPeriod=dt;
}
void Hitch::setForceIsCalculatedByOther(bool b)
{
_forceIsCalculatedByMaster=b;
}
const char *Hitch::getConnectedPropertyNode() const
{
if (_towEndNode)
return _towEndNode->getDisplayName();
else
return 0;
}
void Hitch::setConnectedPropertyNode(const char *nodename)
{
_towEndNode=fgGetNode(nodename,false);
}
void Hitch::setWinchPositionAuto(bool doit)
{
static bool lastState=false;
if(!_state)
return;
if (!doit)
{
lastState=false;
return;
}
if(lastState)
return;
lastState=true;
float lWinchPos[3];
// The ground plane transformed to the local frame.
float ground[4];
_state->planeGlobalToLocal(_global_ground, ground);
float help[3];
//find a normalized vector pointing forward parallel to the ground
help[0]=0;
help[1]=1;
help[2]=0;
Math::cross3(ground,help,help);
//multiplay by initial tow length;
//reduced by 1m to be able to close the
//hitch either if the glider slips backwards a bit
Math::mul3((_winchInitialTowLength-1.),help,help);
//add to the actual pos
Math::add3(_pos,help,lWinchPos);
//put it onto the ground plane
Math::mul3(ground[3],ground,help);
Math::add3(lWinchPos,help,lWinchPos);
_state->posLocalToGlobal(lWinchPos,_winchPos);
_towLength=_winchInitialTowLength;
SG_LOG(SG_GENERAL, SG_ALERT,"Set the winch pos to "<<_winchPos[0]<<","<<_winchPos[1]<<","<<_winchPos[2]<<endl);
_open=false;
char text[512];
sprintf(text,"/sim/hitches/%s", _name);
SGPropertyNode * node = fgGetNode(text, true);
node->setBoolValue("broken",false);
//set the dist value (if not, the hitch would open in the next calcforce run
//float delta[3];
//Math::sub3(lWinchPos,_pos,delta);
//_dist=Math::mag3(delta);
_dist=Math::mag3(lWinchPos); //use the aircraft center as reference for distance calculation
//this position is transferred to the MP-Aircraft.
//With this trick, both player in aerotow get the same length
}
void Hitch::findBestAIObject(bool doit,bool running_as_autoconnect)
{
static bool lastState=false;
if(!_state)
return;
if (!doit)
{
lastState=false;
return;
}
if(lastState)
return;
lastState=true;
double gpos[3];
_state->posLocalToGlobal(_pos,gpos);
double bestdist=_towLength*_towLength;//squared!
_towEndIsConnectedToProperty=false;
SGPropertyNode * ainode = fgGetNode("/ai/models",false);
if(!ainode) return;
char myCallsign[256]="***********";
if (running_as_autoconnect)
{
//get own callsign
SGPropertyNode *cs=fgGetNode("/sim/multiplay/callsign",false);
if (cs)
{
strncpy(myCallsign,cs->getStringValue(),256);
myCallsign[255]=0;
}
//reset tow length for search radius. Lentgh will be later copied from master
_towLength=_winchInitialTowLength;
}
bool found=false;
vector <SGPropertyNode_ptr> nodes;//<SGPropertyNode_ptr>
for (int i=0;i<ainode->nChildren();i++)
{
SGPropertyNode * n=ainode->getChild(i);
_nodeIsMultiplayer = strncmp("multiplayer",n->getName(),11)==0;
_nodeIsAiAircraft = strncmp("aircraft",n->getName(),8)==0;
if (!(_nodeIsMultiplayer || _nodeIsAiAircraft))
continue;
if (running_as_autoconnect)
{
if (!_nodeIsMultiplayer)
continue;
if(n->getBoolValue("sim/hitches/aerotow/open",true)) continue;
if(strncmp(myCallsign,n->getStringValue("sim/hitches/aerotow/tow/connectedToAIorMP-callsign"),255)!=0)
continue;
}
double pos[3];
pos[0]=n->getDoubleValue("position/global-x",0);
pos[1]=n->getDoubleValue("position/global-y",0);
pos[2]=n->getDoubleValue("position/global-z",0);
double dist=0;
for (int j=0;j<3;j++)
dist+=(pos[j]-gpos[j])*(pos[j]-gpos[j]);
if (dist<bestdist)
{
bestdist=dist;
_towEndNode=n;
_towEndIsConnectedToProperty=true;
char text[512];
sprintf(text,"/sim/hitches/%s", _name);
SGPropertyNode * node = fgGetNode(text, true);
//node->setStringValue("tow/Node",n->getPath());
node->setStringValue("tow/Node",n->getDisplayName());
_nodeID=n->getIntValue("id",0);
node->setStringValue("tow/connectedToAIorMP-callsign",n->getStringValue("callsign"));
_open=false;
found = true;
}
}
if (found)
{
char text[512];
sprintf(text,"/sim/hitches/%s", _name);
SGPropertyNode * node = fgGetNode(text, true);
//if (!running_as_autoconnect)
SG_LOG(SG_GENERAL, SG_ALERT,"Found aircraft for aerotow: "
<<node->getStringValue("tow/connectedToAIorMP-callsign")
<<", distance "<<Math::sqrt(bestdist)<<"m at "
<<node->getStringValue("tow/Node")<<endl);
if (running_as_autoconnect)
_isSlave=true;
//set the dist value to some value below the tow lentgh (if not, the hitch
//would open in the next calc force run
_dist=_towLength*0.5;
_mp_open_last_state=true;
}
else
if (!running_as_autoconnect)
SG_LOG(SG_GENERAL, SG_ALERT,"Found no aircraft for aerotow!"<<endl);
}
void Hitch::setWinchInitialTowLength(float length)
{
_winchInitialTowLength=length;
}
void Hitch::setWinchPower(float power)
{
_winchPower=power;
}
void Hitch::setWinchMaxTowLength(float length)
{
_winchMaxTowLength=length;
}
void Hitch::setWinchMinTowLength(float length)
{
_winchMinTowLength=length;
}
void Hitch::setGlobalGround(double *global_ground, float *global_vel)
{
int i;
for(i=0; i<4; i++) _global_ground[i] = global_ground[i];
for(i=0; i<3; i++) _global_vel[i] = global_vel[i];
}
void Hitch::getPosition(float* out)
{
int i;
for(i=0; i<3; i++) out[i] = _pos[i];
}
float Hitch::getTowLength(void)
{
return _towLength;
}
void Hitch::calcForce(Ground *g_cb, RigidBody* body, State* s)
{
float lWinchPos[3],delta[3],deltaN[3];
*_state=*s;
s->posGlobalToLocal(_winchPos,lWinchPos);
Math::sub3(lWinchPos,_pos,delta);
//_dist=Math::mag3(delta);
_dist=Math::mag3(lWinchPos); //use the aircraft center as reference for distance calculation
//this position is transferred to the MP-Aircraft.
//With this trick, both player in aerotow get the same length
Math::unit3(delta,deltaN);
float lvel[3];
s->velGlobalToLocal(s->v,lvel);
_speed_in_tow_direction=Math::dot3(lvel,deltaN);
if (_towEndIsConnectedToProperty && _nodeIsMultiplayer)
{
float mp_delta_dist_due_to_time_lag=0.5*_mp_time_lag*(-_mp_v+_speed_in_tow_direction);
_timeLagCorrectedDist=_dist+mp_delta_dist_due_to_time_lag;
if(_forceIsCalculatedByMaster && !_open)
{
s->velGlobalToLocal(_mp_force,_force);
return;
}
}
else
_timeLagCorrectedDist=_dist;
if (_open)
{
_force[0]=_force[1]=_force[2]=0;
return;
}
if(_dist>_towLength)
if(_towLength>1e-3)
_forceMagnitude=(_dist-_towLength)/_towLength*_towElasticConstant;
else
_forceMagnitude=2*_towBrakeForce;
else
_forceMagnitude=0;
if(_forceMagnitude>=_towBrakeForce)
{
_forceMagnitude=0;
_open=true;
char text[128];
sprintf(text,"/sim/hitches/%s", _name);
SGPropertyNode * node = fgGetNode(text, true);
node->setBoolValue("broken",true);
_force[0]=_force[1]=_force[2]=0;
_towEndForce[0]=_towEndForce[1]=_towEndForce[2]=0;
_reportTowEndForce[0]=_reportTowEndForce[1]=_reportTowEndForce[2]=0;
return;
}
Math::mul3(_forceMagnitude,deltaN,_force);
Math::mul3(-1.,_force,_towEndForce);
_winchActualForce=_forceMagnitude; //missing: gravity on this end and friction
//Add the gravitiy of the rope.
//calculate some numbers:
float grav_force=_towWeightPerM*_towLength*9.81;
//the length of the gravity-expanded row:
float leng=_towLength+grav_force*_towLength/_towElasticConstant;
// The ground plane transformed to the local frame.
float ground[4];
s->planeGlobalToLocal(_global_ground, ground);
// The velocity of the contact patch transformed to local coordinates.
//float glvel[3];
//s->velGlobalToLocal(_global_vel, glvel);
_height_above_ground = ground[3] - Math::dot3(_pos, ground);
//the same for the winch-pos (the pos of the tow end)
_winch_height_above_ground = ground[3] - Math::dot3(lWinchPos, ground);
//the frac of the grav force acting on _pos:
float grav_frac=0.5*(1+(_height_above_ground-_winch_height_above_ground)/leng);
grav_frac=Math::clamp(grav_frac,0,1);
float grav_frac_tow_end=1-grav_frac;
//reduce grav_frac, if the tow has ground contact.
if (_height_above_ground<leng) //if not, the tow can not be on ground
{
float fa[3],fb[3],fg[3];
//the grav force an the hitch position:
Math::mul3(-grav_frac*grav_force,ground,fg);
//the total force on hitch postion:
Math::add3(fg,_force,fa);
//the grav force an the tow end position:
Math::mul3(-(1-grav_frac)*grav_force,ground,fg);
//the total force on tow end postion:
//note: sub: _force on tow-end is negative of force on hitch postion
Math::sub3(fg,_force,fb);
float fa_=Math::mag3(fa);
float fb_=Math::mag3(fb);
float stretchedTowLen;
stretchedTowLen=_towLength*(1.+(fa_+fb_)/(2*_towElasticConstant));
//the relative position of the lowest postion of the tow:
if ((fa_+fb_)>1e-3)
_loPosFrac=fa_/(fa_+fb_);
else
_loPosFrac=0.5;
//dist to tow-end parallel to ground
float ground_dist;
float help[3];
//Math::cross3(delta,ground,help);//as long as we calculate the dist without _pos, od it with lWinchpos, the dist to our center....
Math::cross3(lWinchPos,ground,help);
ground_dist=Math::mag3(help);
//height of lowest tow pos (relative to _pos)
_lowest_tow_height=_loPosFrac*Math::sqrt(Math::abs(stretchedTowLen*stretchedTowLen-ground_dist*ground_dist));
if (_height_above_ground<_lowest_tow_height)
{
if (_height_above_ground>1e-3)
grav_frac*=_height_above_ground/_lowest_tow_height;
else
grav_frac=0;
}
if (_winch_height_above_ground<(_lowest_tow_height-_height_above_ground+_winch_height_above_ground))
{
if (_winch_height_above_ground>1e-3)
grav_frac_tow_end*=_winch_height_above_ground/
(_lowest_tow_height-_height_above_ground+_winch_height_above_ground);
else
grav_frac_tow_end=0;
}
}
else _lowest_tow_height=_loPosFrac=-1; //for debug output
float grav_force_v[3];
Math::mul3(grav_frac*grav_force,ground,grav_force_v);
Math::add3(grav_force_v,_force,_force);
_forceMagnitude=Math::mag3(_force);
//the same for the tow end:
Math::mul3(grav_frac_tow_end*grav_force,ground,grav_force_v);
Math::add3(grav_force_v,_towEndForce,_towEndForce);
s->velLocalToGlobal(_towEndForce,_towEndForce);
if(_forceMagnitude>=_towBrakeForce)
{
_forceMagnitude=0;
_open=true;
char text[128];
sprintf(text,"/sim/hitches/%s", _name);
SGPropertyNode * node = fgGetNode(text, true);
node->setBoolValue("broken",true);
_force[0]=_force[1]=_force[2]=0;
_towEndForce[0]=_towEndForce[1]=_towEndForce[2]=0;
}
}
// Computed values: total force
void Hitch::getForce(float* force, float* off)
{
Math::set3(_force, force);
Math::set3(_pos, off);
}
void Hitch::integrate (float dt)
{
//check if hitch has opened or closed, if yes: message
if (_open !=_oldOpen)
{
if (_oldOpen)
{
if (_dist>_towLength*1.00001)
{
SG_LOG(SG_GENERAL, SG_ALERT,"Could not lock Hinch (tow length is insufficient) on hitch '"<<_name<<"' !"<<endl);
_open=true;
return;
}
char text[512];
sprintf(text,"/sim/hitches/%s", _name);
SGPropertyNode * node = fgGetNode(text, true);
node->setBoolValue("broken",false);
}
SG_LOG(SG_GENERAL, SG_ALERT,(_open?"Opened hitch (or broken tow) '":"Locked hitch '")<<_name<<"' !"<<endl);
_oldOpen=_open;
}
//check, if tow end should be searched in all MP-aircrafts
if(_open && _mpAutoConnectPeriod)
{
_isSlave=false;
_timeToNextAutoConnectTry-=dt;
if ((_timeToNextAutoConnectTry>_mpAutoConnectPeriod) || (_timeToNextAutoConnectTry<0))
{
_timeToNextAutoConnectTry=_mpAutoConnectPeriod;
//search for MP-Aircraft, which is towed with us
findBestAIObject(true,true);
}
}
//check, if tow end can be modified by property, if yes: update
if(_towEndIsConnectedToProperty)
{
char text[128];
sprintf(text,"/sim/hitches/%s", _name);
SGPropertyNode * node = fgGetNode(text, true);
if (node)
{
//_towEndNode=fgGetNode(node->getStringValue("tow/Node"), false);
char towNode[256];
strncpy(towNode,node->getStringValue("tow/Node"),256);
towNode[255]=0;
_towEndNode=fgGetNode("ai/models")->getNode(towNode, false);
//AI and multiplayer objects seem to change node?
//Check if we have the right one by callsign
if (_nodeIsMultiplayer || _nodeIsAiAircraft)
{
char MPcallsign[256]="";
const char *MPc;
MPc=node->getStringValue("tow/connectedToAIorMP-callsign");
if (MPc)
{
strncpy(MPcallsign,MPc,256);
MPcallsign[255]=0;
}
if (((_towEndNode)&&(strncmp(_towEndNode->getStringValue("callsign"),MPcallsign,255)!=0))||!_towEndNode)
{
_timeToNextReConnectTry-=dt;
if((_timeToNextReConnectTry<0)||(_timeToNextReConnectTry>10))
{
_timeToNextReConnectTry=10;
SGPropertyNode * ainode = fgGetNode("/ai/models",false);
if(ainode)
{
for (int i=0;i<ainode->nChildren();i++)
{
SGPropertyNode * n=ainode->getChild(i);
if(_nodeIsMultiplayer?strncmp("multiplayer",n->getName(),11)==0:strncmp("aircraft",n->getName(),8))
if (strcmp(n->getStringValue("callsign"),MPcallsign)==0)//found
{
_towEndNode=n;
//node->setStringValue("tow/Node",n->getPath());
node->setStringValue("tow/Node",n->getDisplayName());
}
}
}
}
}
}
if(_towEndNode)
{
_winchPos[0]=_towEndNode->getDoubleValue("position/global-x",_winchPos[0]);
_winchPos[1]=_towEndNode->getDoubleValue("position/global-y",_winchPos[1]);
_winchPos[2]=_towEndNode->getDoubleValue("position/global-z",_winchPos[2]);
_mp_lpos[0]=_towEndNode->getFloatValue("sim/hitches/aerotow/local-pos-x",0);
_mp_lpos[1]=_towEndNode->getFloatValue("sim/hitches/aerotow/local-pos-y",0);
_mp_lpos[2]=_towEndNode->getFloatValue("sim/hitches/aerotow/local-pos-z",0);
_mp_dist=_towEndNode->getFloatValue("sim/hitches/aerotow/tow/dist");
_mp_v=_towEndNode->getFloatValue("sim/hitches/aerotow/speed-in-tow-direction");
_mp_force[0]=_towEndNode->getFloatValue("sim/hitches/aerotow/tow/end-force-x",0);
_mp_force[1]=_towEndNode->getFloatValue("sim/hitches/aerotow/tow/end-force-y",0);
_mp_force[2]=_towEndNode->getFloatValue("sim/hitches/aerotow/tow/end-force-z",0);
if(_isSlave)
{
#define gf(a,b) a=_towEndNode->getFloatValue(b,a)
#define gb(a,b) a=_towEndNode->getBoolValue(b,a)
gf(_towLength,"sim/hitches/aerotow/tow/length");
gf(_towElasticConstant,"sim/hitches/aerotow/tow/elastic-constant");
gf(_towWeightPerM,"sim/hitches/aerotow/tow/weight-per-m-kg-m");
gf(_towBrakeForce,"sim/hitches/aerotow/brake-force");
gb(_open,"sim/hitches/aerotow/open");
gb(_mp_is_slave,"sim/hitches/aerotow/is-slave");
#undef gf
#undef gb
if (_mp_is_slave) _isSlave=false; //someone should be master
}
else
{
//check if other has opened hitch, but is neccessary, that it was closed before
bool mp_open=_towEndNode->getBoolValue("sim/hitches/aerotow/open",_mp_open_last_state);
if (mp_open != _mp_open_last_state) //state has changed
{
_mp_open_last_state=mp_open; //store that value
if(!_open)
{
if(mp_open)
{
_open=true;
char text[512];
sprintf(text,"/sim/hitches/%s", _name);
SGPropertyNode * node = fgGetNode(text, true);
node->getStringValue("tow/Node","");
SG_LOG(SG_GENERAL, SG_ALERT,"'"<<node->getStringValue("tow/Node","")<<"' has opened hitch!"<<endl);
}
}
}
}
//try to calculate the time lag
if ((_mp_last_reported_dist!=_mp_dist)||(_mp_last_reported_v!=_mp_v)) //new data;
{
_mp_last_reported_dist=_mp_dist;
_mp_last_reported_v=_mp_v;
float total_v=-_mp_v+_speed_in_tow_direction;//mp has opposite tow direction
float abs_v=Math::abs(total_v);
if (abs_v>0.1)
{
float actual_time_lag_guess=(_mp_dist-_dist)/total_v;
//check, if it sounds ok
if((actual_time_lag_guess>0)&&(actual_time_lag_guess<5))
{
float frac=abs_v*0.01;
if (frac>0.05) frac=0.05;
// if we are slow, the guess of the lag can be rather wrong. as faster we are
// the better the guess. Therefore frac is proportiona to the speed. Clamp it
// at 5m/s
_mp_time_lag=(1-frac)*_mp_time_lag+frac*actual_time_lag_guess;
}
}
}
}
}
}
//set the _reported_tow_end_force (smoothed)
//smooth it a bit and store it
float sc=10.*dt; //100ms
float tmp[3];
Math::mul3(sc,_towEndForce,tmp);
Math::mul3(1.-sc,_reportTowEndForce,_reportTowEndForce);
Math::add3(tmp,_reportTowEndForce,_reportTowEndForce);
if (_open) return;
if (_winchRelSpeed==0) return;
float factor=1,offset=0;
if (_winchActualForce>_winchMaxForce)
offset=-(_winchActualForce-_winchMaxForce)/_winchMaxForce*20;
if (_winchRelSpeed>0.01) // to avoit div by zero
{
float maxForcePowerLimit=_winchPower/(_winchRelSpeed*_winchMaxSpeed);
if (_winchActualForce>maxForcePowerLimit)
factor-=(_winchActualForce-maxForcePowerLimit)/maxForcePowerLimit;
}
_towLength-=dt*(factor*_winchRelSpeed+offset)*_winchMaxSpeed;
if (_towLength<=_winchMinTowLength)
{
if (_winchRelSpeed<0)
_winchRelSpeed=0;
_towLength=_winchMinTowLength;
return;
}
if (_towLength>=_winchMaxTowLength)
{
if (_winchRelSpeed<0)
_winchRelSpeed=0;
_towLength=_winchMaxTowLength;
return;
}
}
}; // namespace yasim

112
src/FDM/YASim/Hitch.hpp Executable file
View file

@ -0,0 +1,112 @@
#ifndef _HITCH_HPP
#define _HITCH_HPP
class SGPropertyNode;
namespace yasim {
class Ground;
class RigidBody;
struct State;
class Hitch {
public:
Hitch(const char *name);
~Hitch();
// Externally set values
void setPosition(float* position);
void setOpen(bool isOpen);
//void setName(const char *text);
void setTowLength(float length);
void setTowElasticConstant(float sc);
void setTowBreakForce(float bf);
void setTowWeightPerM(float rw);
void setWinchMaxSpeed(float mws);
void setWinchRelSpeed(float rws);
void setWinchPosition(double *winchPosition); //in global coordinates!
void setWinchPositionAuto(bool doit);
void findBestAIObject(bool doit,bool running_as_autoconnect=false);
void setWinchInitialTowLength(float length);
void setWinchPower(float power);
void setWinchMaxForce(float force);
void setWinchMaxTowLength(float length);
void setWinchMinTowLength(float length);
void setMpAutoConnectPeriod(float dt);
void setForceIsCalculatedByOther(bool b);
void setGlobalGround(double *global_ground, float *global_vel);
void getPosition(float* out);
float getTowLength(void);
void calcForce(Ground *g_cb, RigidBody* body, State* s);
// Computed values: total force
void getForce(float* force, float* off);
void integrate (float dt);
const char *getConnectedPropertyNode() const;
void setConnectedPropertyNode(const char *nodename);
private:
float _pos[3];
bool _open;
bool _oldOpen;
float _towLength;
float _towElasticConstant;
float _towBrakeForce;
float _towWeightPerM;
float _winchMaxSpeed;
float _winchRelSpeed;
float _winchInitialTowLength;
float _winchMaxTowLength;
float _winchMinTowLength;
float _winchPower;
float _winchMaxForce;
float _winchActualForce;
double _winchPos[3];
float _force[3];
float _towEndForce[3];
float _reportTowEndForce[3];
float _forceMagnitude;
double _global_ground[4];
float _global_vel[3];
char _name[256];
State* _state;
float _dist;
float _timeLagCorrectedDist;
SGPropertyNode *_towEndNode;
const char *_towEndPropertyName;
bool _towEndIsConnectedToProperty;
bool _nodeIsMultiplayer;
bool _nodeIsAiAircraft;
bool _forceIsCalculatedByMaster;
int _nodeID;
//const char *_ai_MP_callsign;
bool _isSlave;
float _mpAutoConnectPeriod;
float _timeToNextAutoConnectTry;
float _timeToNextReConnectTry;
float _height_above_ground;
float _winch_height_above_ground;
float _loPosFrac;
float _lowest_tow_height;
float _speed_in_tow_direction;
float _mp_time_lag;
float _mp_last_reported_dist;
float _mp_last_reported_v;
float _mp_v;
float _mp_dist;
float _mp_lpos[3];
float _mp_force[3];
bool _mp_is_slave;
bool _mp_open_last_state;
bool _displayed_len_lower_dist_message;
bool _last_wish;
};
}; // namespace yasim
#endif // _HITCH_HPP

1
src/FDM/YASim/Makefile.am
View file

@ -15,6 +15,7 @@ SHARED_SOURCE_FILES = \
Gear.cpp Gear.hpp \
Glue.cpp Glue.hpp \
Ground.cpp Ground.hpp \
Hitch.cpp Hitch.hpp \
Hook.cpp Hook.hpp \
Launchbar.cpp Launchbar.hpp \
Integrator.cpp Integrator.hpp \

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

@ -11,6 +11,7 @@
#include "Surface.hpp"
#include "Rotor.hpp"
#include "Rotorpart.hpp"
#include "Hitch.hpp"
#include "Glue.hpp"
#include "Ground.hpp"
@ -75,6 +76,9 @@ Model::~Model()
delete _ground_cb;
delete _hook;
delete _launchbar;
for(int i=0; i<_hitches.size();i++)
delete (Hitch*)_hitches.get(i);
}
void Model::getThrust(float* out)
@ -127,6 +131,11 @@ void Model::initIteration()
_turb->offset(toff);
}
for(i=0; i<_hitches.size(); i++) {
Hitch* h = (Hitch*)_hitches.get(i);
h->integrate(_integrator.getInterval());
}
}
@ -252,6 +261,11 @@ void Model::addLaunchbar(Launchbar* launchbar)
_launchbar = launchbar;
}
int Model::addHitch(Hitch* hitch)
{
return _hitches.add(hitch);
}
void Model::setGroundCallback(Ground* ground_cb)
{
delete _ground_cb;
@ -307,9 +321,34 @@ void Model::updateGround(State* s)
// 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);
g->setGlobalGround(global_ground, global_vel);
int type;
const SGMaterial* material;
_ground_cb->getGroundPlane(pt, global_ground, global_vel,
&type,&material);
static int h=0;
g->setGlobalGround(global_ground, global_vel, pt[0], pt[1],
type,material);
}
for(i=0; i<_hitches.size(); i++) {
Hitch* h = (Hitch*)_hitches.get(i);
// Get the point of interest
float pos[3];
h->getPosition(pos);
// Transform the local coordinates of the contact point 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);
h->setGlobalGround(global_ground, global_vel);
}
for(i=0; i<_rotorgear.getRotors()->size(); i++) {
Rotor* r = (Rotor*)_rotorgear.getRotors()->get(i);
r->findGroundEffectAltitude(_ground_cb,s);
@ -468,8 +507,16 @@ void Model::calcForces(State* s)
_body.addForce(contactlb, forcelb);
_body.addForce(contacthb, forcehb);
}
}
// The hitches
for(i=0; i<_hitches.size(); i++) {
float force[3], contact[3];
Hitch* h = (Hitch*)_hitches.get(i);
h->calcForce(_ground_cb,&_body, s);
h->getForce(force, contact);
_body.addForce(contact, force);
}
}
void Model::newState(State* s)
{
_s = s;
@ -480,23 +527,26 @@ void Model::newState(State* s)
for(i=0; i<_gears.size(); i++) {
Gear* g = (Gear*)_gears.get(i);
// Get the point of ground contact
float pos[3], cmpr[3];
g->getPosition(pos);
g->getCompression(cmpr);
Math::mul3(g->getCompressFraction(), cmpr, cmpr);
Math::add3(cmpr, pos, pos);
if (!g->getSubmergable())
{
// Get the point of ground contact
float pos[3], cmpr[3];
g->getPosition(pos);
g->getCompression(cmpr);
Math::mul3(g->getCompressFraction(), cmpr, cmpr);
Math::add3(cmpr, pos, pos);
// The plane transformed to local coordinates.
double global_ground[4];
g->getGlobalGround(global_ground);
float ground[4];
s->planeGlobalToLocal(global_ground, ground);
float dist = ground[3] - Math::dot3(pos, ground);
// The plane transformed to local coordinates.
double global_ground[4];
g->getGlobalGround(global_ground);
float ground[4];
s->planeGlobalToLocal(global_ground, ground);
float dist = ground[3] - Math::dot3(pos, ground);
// Find the lowest one
if(dist < min)
min = dist;
// Find the lowest one
if(dist < min)
min = dist;
}
}
_agl = min;
if(_agl < -1) // Allow for some integration slop

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

@ -19,6 +19,7 @@ class Gear;
class Ground;
class Hook;
class Launchbar;
class Hitch;
class Model : public BodyEnvironment {
public:
@ -49,6 +50,7 @@ public:
Rotorgear* getRotorgear(void);
Gear* getGear(int handle);
Hook* getHook(void);
int addHitch(Hitch* hitch);
Launchbar* getLaunchbar(void);
// Semi-private methods for use by the Airplane solver.
@ -92,6 +94,7 @@ private:
Vector _gears;
Hook* _hook;
Launchbar* _launchbar;
Vector _hitches;
float _groundEffectSpan;
float _groundEffect;

1
src/FDM/YASim/YASim.cxx
View file

@ -477,6 +477,7 @@ void YASim::copyFromYASim()
node->setFloatValue("compression-m", g->getCompressDist());
node->setFloatValue("caster-angle-deg", g->getCasterAngle() * RAD2DEG);
node->setFloatValue("rollspeed-ms", g->getRollSpeed());
node->setBoolValue("ground-is-solid", g->getGroundIsSolid()!=0);
}
Hook* h = airplane->getHook();

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

@ -46,7 +46,7 @@ void yasim_graph(Airplane* a, float alt, float kts)
for(int deg=-179; deg<=179; deg++) {
float aoa = deg * DEG2RAD;
Airplane::setupState(aoa, kts * KTS2MPS, &s);
Airplane::setupState(aoa, kts * KTS2MPS, 0 ,&s);
m->getBody()->reset();
m->initIteration();
m->calcForces(&s);