YASim-0.1.3 updates.

src/FDM/YASim/Atmosphere.cpp

@@ -31,6 +31,10 @@ float Atmosphere::data[][4] = {{ 0,     288.20, 101325, 1.22500 },
 			       { 18000, 216.66,   7565, 0.12165 },
 			       { 18900, 216.66,   6570, 0.10564 }};
 
+// Universal gas constant for air, in SI units.  P = R * rho * T.
+// P in pascals (N/m^2), rho is kg/m^3, T in kelvin.
+const float R = 287.1;
+
 float Atmosphere::getStdTemperature(float alt)
 {
     return getRecord(alt, 1);
@@ -48,7 +52,9 @@ float Atmosphere::getStdDensity(float alt)
 
 float Atmosphere::calcVEAS(float spd, float pressure, float temp)
 {
-    return 0; //FIXME
+    static float rho0 = getStdDensity(0);
+    float densityRatio = calcDensity(pressure, temp) / rho0;
+    return spd * Math::sqrt(densityRatio);
 }
 
 float Atmosphere::calcVCAS(float spd, float pressure, float temp)
@@ -84,13 +90,12 @@ float Atmosphere::calcVCAS(float spd, float pressure, float temp)
 
 float Atmosphere::calcDensity(float pressure, float temp)
 {
-    // P = rho*R*T, R == 287 kPa*m^3 per kg*kelvin for air
+    return pressure / (R * temp);
-    return pressure / (287 * temp);
 }
 
 float Atmosphere::calcMach(float spd, float temp)
 {
-    return spd / Math::sqrt(1.4 * 287 * temp);
+    return spd / Math::sqrt(1.4 * R * temp);
 }
 
 float Atmosphere::getRecord(float alt, int recNum)

src/FDM/YASim/FGFDM.cpp

@@ -77,23 +77,10 @@ Airplane* FGFDM::getAirplane()
 
 void FGFDM::init()
 {
-    // We don't want to use these ties (we set the values ourselves,
+    // We don't want to use these ties (we set the values ourselves)
-    // and this works only for the first piston engine anyway).
-    fgUntie("/engines/engine[0]/rpm");
-    fgUntie("/engines/engine[0]/egt-degf");
-    fgUntie("/engines/engine[0]/cht-degf");
-    fgUntie("/engines/engine[0]/oil-temperature-degf");
-    fgUntie("/engines/engine[0]/mp-osi");
-    fgUntie("/engines/engine[0]/fuel-flow-gph");
-    fgUntie("/engines/engine[0]/running");
-    fgUntie("/engines/engine[0]/cranking");
     fgUntie("/consumables/fuel/tank[0]/level-gal_us");
     fgUntie("/consumables/fuel/tank[1]/level-gal_us");
 
-    // Set these to sane values.  We don't support engine start yet.
-    fgSetBool("/engines/engine[0]/running", true);
-    fgSetBool("/engines/engine[0]/cranking", false);
-
     // Allows the user to start with something other than full fuel
     _airplane.setFuelFraction(fgGetFloat("/yasim/fuel-fraction", 1));
 

src/FDM/YASim/Gear.cpp

@@ -143,8 +143,11 @@ void Gear::calcForce(RigidBody* body, float* v, float* rot, float* ground)
     // 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);
-    if(a > 0)
+    if(a > 0) {
+	_wow = 0;
+	_frac = 0;
 	return;
+    }
 
     // Now a is the distance from the tip to ground, so make b the
     // distance from the base to ground.  We can get the fraction
@@ -169,11 +172,10 @@ void Gear::calcForce(RigidBody* body, float* v, float* rot, float* ground)
     body->pointVelocity(_contact, rot, cv);
     Math::add3(cv, v, cv);
 
-    // Finally, we can start adding up the forces.  First the
+    // Finally, we can start adding up the forces.  First the spring
-    // spring compression (note the clamping of _frac to 1):
+    // compression.   (note the clamping of _frac to 1):
     _frac = (_frac > 1) ? 1 : _frac;
     float fmag = _frac*clen*_spring;
-    Math::mul3(fmag, cmpr, _force);
 
     // Then the damping.  Use the only the velocity into the ground
     // (projection along "ground") projected along the compression
@@ -183,10 +185,11 @@ void Gear::calcForce(RigidBody* body, float* v, float* rot, float* ground)
     float damp = _damp * dv;
     if(damp > fmag) damp = fmag; // can't pull the plane down!
     if(damp < -fmag) damp = -fmag; // sanity
-    Math::mul3(-damp, cmpr, tmp);
-    Math::add3(_force, tmp, _force);
 
-    _wow = fmag - damp;    
+    // The actual force applied is only the component perpendicular to
+    // the ground.  Side forces come from velocity only.
+    _wow = (fmag - damp) * -Math::dot3(cmpr, ground);
+    Math::mul3(-_wow, ground, _force);
 
     // Wheels are funky.  Split the velocity along the ground plane
     // into rolling and skidding components.  Assuming small angles,

src/FDM/YASim/Math.cpp

@@ -20,11 +20,6 @@ float Math::sqrt(float f)
     return ::sqrt(f);
 }
 
-float Math::pow(float base, float exp)
-{
-    return ::pow(base, exp);
-}
-
 float Math::ceil(float f)
 {
     return ::ceil(f);
@@ -60,7 +55,7 @@ double Math::sqrt(double f)
     return ::sqrt(f);
 }
 
-double Math::pow(double base, double exp)
+float Math::pow(double base, double exp)
 {
     return ::pow(base, exp);
 }

src/FDM/YASim/Math.hpp

@@ -12,17 +12,18 @@ public:
     // Simple wrappers around library routines
     static float abs(float f);
     static float sqrt(float f);
-    static float pow(float base, float exp);
     static float ceil(float f);
     static float sin(float f);
     static float cos(float f);
     static float tan(float f);
     static float atan2(float y, float x);
 
+    // Takes two args and runs afoul of the Koenig rules.
+    static float pow(double base, double exp);
+
     // double variants of the above
     static double abs(double f);
     static double sqrt(double f);
-    static double pow(double base, double exp);
     static double ceil(double f);
     static double sin(double f);
     static double cos(double f);

src/FDM/YASim/Model.cpp

@@ -47,6 +47,9 @@ Model::Model()
 
     // Default value of 30 Hz
     _integrator.setInterval(1.0/30.0);
+
+    _agl = 0;
+    _crashed = false;
 }
 
 Model::~Model()
@@ -99,6 +102,21 @@ void Model::iterate()
     _integrator.calcNewInterval();
 }
 
+bool Model::isCrashed()
+{
+    return _crashed;
+}
+
+void Model::setCrashed(bool crashed)
+{
+    _crashed = crashed;
+}
+
+float Model::getAGL()
+{
+    return _agl;
+}
+
 State* Model::getState()
 {
     return _s;
@@ -269,20 +287,27 @@ void Model::newState(State* s)
     //printState(s);
 
     // Some simple collision detection
+    float min = 1e8;
     float ground[4], pos[3], cmpr[3];
     ground[3] = localGround(s, ground);
     int i;
     for(i=0; i<_gears.size(); i++) {
 	Gear* g = (Gear*)_gears.get(i);
+
+	// Get the point of ground contact
 	g->getPosition(pos);
 	g->getCompression(cmpr);
+	Math::mul3(g->getCompressFraction(), cmpr, cmpr);
 	Math::add3(cmpr, pos, pos);
 	float dist = ground[3] - Math::dot3(pos, ground);
-	if(dist < 0) {
+
-	    printf("CRASH: gear %d\n", i);
+	// Find the lowest one
-	    *(int*)0=0;
+	if(dist < min)
-	}
+	    min = dist;
     }
+    _agl = min;
+    if(_agl < -1) // Allow for some integration slop
+	_crashed = true;
 }
 
 // Returns a unit "down" vector for the ground in out, and the

src/FDM/YASim/Model.hpp

@@ -24,6 +24,9 @@ public:
 
     State* getState();
     void setState(State* s);
+    bool isCrashed();
+    void setCrashed(bool crashed);
+    float getAGL();
 
     void iterate();
 
@@ -81,6 +84,8 @@ private:
     float _torque[3];
 
     State* _s;
+    bool _crashed;
+    float _agl;
 };
 
 }; // namespace yasim

src/FDM/YASim/PropEngine.cpp

@@ -126,9 +126,6 @@ void PropEngine::integrate(float dt)
     // needs to go away when the startup code gets written.
     if(_omega < 52.3) _omega = 52.3;
 
-    // FIXME: Integrate the propeller governor here, when that gets
-    // implemented...
-
     // Store the total angular momentum into _gyro
     Math::mul3(_omega*_moment, _dir, _gyro);
 
@@ -140,15 +137,25 @@ void PropEngine::integrate(float dt)
     float tau = _moment < 0 ? engTorque : -engTorque;
     Math::mul3(tau, _dir, _torque);
 
-    // Play with the variable propeller, but only if the propeller is
+    // Iterate the propeller governor, if we have one.  Since engine
-    // vaguely stable alread (accelerating less than 100 rpm/s)
+    // torque is basically constant with RPM, we want to make the
-    if(_variable && Math::abs(rotacc) < 20) {
+    // propeller torque at the target RPM equal to the engine by
-	float target = _minOmega + _advance*(_maxOmega-_minOmega);
+    // varying the pitch.  Assume the the torque goes as the square of
-	float mod = 1.04;
+    // the RPM (roughly correct) and compute a "target" torque for the
-	if(target > _omega) mod = 1/mod;
+    // _current_ RPM.  Seek to that.  This is sort of a continuous
-	float diff = Math::abs(target - _omega);
+    // Newton-Raphson, basically.
-	if(diff < 1) mod = 1 + (mod-1)*diff;
+    if(_variable) {
-	if(thrust < 0) mod = 1;
+	float targetOmega = _minOmega + _advance*(_maxOmega-_minOmega);
+	float ratio2 = (_omega*_omega)/(targetOmega*targetOmega);
+	float targetTorque = engTorque * ratio2;
+
+	float mod = propTorque < targetTorque ? 1.04 : (1/1.04);
+
+	// Convert to an acceleration here, so that big propellers
+	// don't seek faster than small ones.
+	float diff = Math::abs(propTorque - targetTorque) / _moment;
+	if(diff < 10) mod = 1 + (mod-1)*(0.1*diff);
+
 	_prop->modPitch(mod);
     }
 }

src/FDM/YASim/YASim.cxx

@@ -105,6 +105,8 @@ void YASim::init()
     // Superclass hook
     common_init();
 
+    m->setCrashed(false);
+
     // Build a filename and parse it
     SGPath f(globals->get_fg_root());
     f.append("Aircraft-yasim");
@@ -141,16 +143,19 @@ void YASim::init()
     }
     
 
-    // Lift the plane up so the gear clear the ground
+    // Are we at ground level?  If so, lift the plane up so the gear
-    float minGearZ = 1e18;
+    // clear the ground
-    for(i=0; i<a->numGear(); i++) {
+    if(get_Altitude() - get_Runway_altitude() < 50) {
-	Gear* g = a->getGear(i);
+	float minGearZ = 1e18;
-	float pos[3];
+	for(i=0; i<a->numGear(); i++) {
-	g->getPosition(pos);
+	    Gear* g = a->getGear(i);
-	if(pos[2] < minGearZ)
+	    float pos[3];
-	    minGearZ = pos[2];
+	    g->getPosition(pos);
+	    if(pos[2] < minGearZ)
+		minGearZ = pos[2];
+	}
+	_set_Altitude(get_Runway_altitude() - minGearZ*M2FT);
     }
-    _set_Altitude(get_Altitude() - minGearZ*M2FT);
 
     // The pilot's eyepoint
     float pilot[3];
@@ -169,6 +174,10 @@ void YASim::init()
 
 bool YASim::update(int iterations)
 {
+    // If we're crashed, then we don't care
+    if(_fdm->getAirplane()->getModel()->isCrashed())
+	return true;
+
     int i;
     for(i=0; i<iterations; i++) {
         // Remember, update only every 4th call
@@ -310,10 +319,7 @@ void YASim::copyFromYASim()
     // UNUSED
     //_set_Geocentric_Position(Glue::geod2geocLat(lat), lon, alt*M2FT);
 
-    // FIXME: there's a normal vector available too, use it.
+    _set_Altitude_AGL(model->getAGL() * M2FT);
-    float groundMSL = scenery.get_cur_elev();
-    _set_Runway_altitude(groundMSL*M2FT);
-    _set_Altitude_AGL((alt - groundMSL)*M2FT);
 
     // useful conversion matrix
     float xyz2ned[9];
@@ -390,6 +396,8 @@ void YASim::copyFromYASim()
             fgg->SetBrake(true);
         if(g->getCompressFraction() != 0)
             fgg->SetWoW(true);
+	else
+	    fgg->SetWoW(false);
         fgg->SetPosition(g->getExtension());
     }
 
@@ -398,6 +406,7 @@ void YASim::copyFromYASim()
         Thruster* t = model->getThruster(i);
 
         fge->set_Running_Flag(true);
+        fge->set_Cranking_Flag(false);
 
         // Note: assumes all tanks have the same fuel density!
         fge->set_Fuel_Flow(CM2GALS * t->getFuelFlow()