X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FCockpit%2Fsteam.cxx;h=cd78bf303259ab843f2575299c14f3eb0854a6c0;hb=d13b46b8f1efc36057c98fd396c306cbfd7a3612;hp=8ec8f211e09c078ffa88e715d029b92cbdcd5d68;hpb=b23f930aa67e175d8e90add10129de69a407c6dd;p=flightgear.git diff --git a/src/Cockpit/steam.cxx b/src/Cockpit/steam.cxx index 8ec8f211e..cd78bf303 100644 --- a/src/Cockpit/steam.cxx +++ b/src/Cockpit/steam.cxx @@ -23,39 +23,40 @@ # include #endif -#if defined( FG_HAVE_NATIVE_SGI_COMPILERS ) -# include -#else -# include -#endif +#include + +#include STL_IOSTREAM #include -#include -#include
-#include
+#include +#include +#include -FG_USING_NAMESPACE(std); +SG_USING_NAMESPACE(std); #include "radiostack.hxx" #include "steam.hxx" +static bool isTied = false; + //////////////////////////////////////////////////////////////////////// // Declare the functions that read the variables //////////////////////////////////////////////////////////////////////// -// Anything that reads the BFI directly is not implemented at all! - - -#define VARY_E (14) - - double FGSteam::the_STATIC_inhg = 29.92; -double FGSteam::the_ALT_ft = 0.0; +double FGSteam::the_ALT_ft = 0.0; // Indicated altitude double FGSteam::get_ALT_ft() { _CatchUp(); return the_ALT_ft; } -double FGSteam::get_ASI_kias() { return FGBFI::getAirspeed(); } +double FGSteam::the_ALT_datum_mb = 1013.0; +double FGSteam::get_ALT_datum_mb() { return the_ALT_datum_mb; } + +void FGSteam::set_ALT_datum_mb ( double datum_mb ) { + the_ALT_datum_mb = datum_mb; +} + +double FGSteam::get_ASI_kias() { return fgGetDouble("/velocities/airspeed-kt"); } double FGSteam::the_VSI_case = 29.92; double FGSteam::the_VSI_fps = 0.0; @@ -64,15 +65,26 @@ double FGSteam::get_VSI_fps() { _CatchUp(); return the_VSI_fps; } double FGSteam::the_VACUUM_inhg = 0.0; double FGSteam::get_VACUUM_inhg() { _CatchUp(); return the_VACUUM_inhg; } -double FGSteam::get_MH_deg () { - return FGBFI::getHeading () - FGBFI::getMagVar (); -} -double FGSteam::get_DG_deg () { - return FGBFI::getHeading () - FGBFI::getMagVar (); +double FGSteam::the_MH_err = 0.0; +double FGSteam::the_MH_deg = 0.0; +double FGSteam::the_MH_degps = 0.0; +double FGSteam::get_MH_deg () { _CatchUp(); return the_MH_deg; } + +double FGSteam::the_DG_err = 0.0; +double FGSteam::the_DG_deg = 0.0; +double FGSteam::the_DG_degps = 0.0; +double FGSteam::the_DG_inhg = 0.0; +double FGSteam::get_DG_deg () { _CatchUp(); return the_DG_deg; } +double FGSteam::get_DG_err () { _CatchUp(); return the_DG_err; } + +void FGSteam::set_DG_err ( double approx_magvar ) { + the_DG_err = approx_magvar; } -double FGSteam::get_TC_rad () { return FGBFI::getSideSlip (); } -double FGSteam::get_TC_radps () { return FGBFI::getRoll (); } +double FGSteam::the_TC_rad = 0.0; +double FGSteam::the_TC_std = 0.0; +double FGSteam::get_TC_rad () { _CatchUp(); return the_TC_rad; } +double FGSteam::get_TC_std () { _CatchUp(); return the_TC_std; } //////////////////////////////////////////////////////////////////////// @@ -80,15 +92,46 @@ double FGSteam::get_TC_radps () { return FGBFI::getRoll (); } //////////////////////////////////////////////////////////////////////// -int FGSteam::_UpdatesPending = 9999; /* Forces filter to reset */ +int FGSteam::_UpdatesPending = 1000000; /* Forces filter to reset */ + + // FIXME: no need to use static + // functions any longer. void FGSteam::update ( int timesteps ) { - _UpdatesPending += timesteps; + if (!isTied) { + isTied = true; + fgTie("/steam/airspeed-kt", FGSteam::get_ASI_kias); + fgSetArchivable("/steam/airspeed-kt"); + fgTie("/steam/altitude-ft", FGSteam::get_ALT_ft); + fgSetArchivable("/steam/altitude-ft"); + fgTie("/steam/altimeter-datum-mb", + FGSteam::get_ALT_datum_mb, FGSteam::set_ALT_datum_mb, + false); /* don't modify the value */ + fgSetArchivable("/steam/altimeter-datum-mb"); + fgTie("/steam/turn-rate", FGSteam::get_TC_std); + fgSetArchivable("/steam/turn-rate"); + fgTie("/steam/slip-skid", FGSteam::get_TC_rad); + fgSetArchivable("/steam/slip-skid"); + fgTie("/steam/vertical-speed-fpm", FGSteam::get_VSI_fps); + fgSetArchivable("/steam/vertical-speed-fpm"); + fgTie("/steam/gyro-compass-deg", FGSteam::get_DG_deg); + fgSetArchivable("/steam/gyro-compass-deg"); + fgTie("/steam/adf-deg", FGSteam::get_HackADF_deg); + fgSetArchivable("/steam/adf-deg"); + fgTie("/steam/gyro-compass-error-deg", + FGSteam::get_DG_err, FGSteam::set_DG_err, + false); /* don't modify the value */ + fgSetArchivable("/steam/gyro-compass-error-deg"); + fgTie("/steam/mag-compass-deg", FGSteam::get_MH_deg); + fgSetArchivable("/steam/mag-compass-deg"); + } + _UpdatesPending += timesteps; } +/* tc should be (elapsed_time_between_updates / desired_smoothing_time) */ void FGSteam::set_lowpass ( double *outthe, double inthe, double tc ) { if ( tc < 0.0 ) @@ -100,7 +143,7 @@ void FGSteam::set_lowpass ( double *outthe, double inthe, double tc ) } } else if ( tc < 0.2 ) - { /* Normal mode of operation */ + { /* Normal mode of operation; fast approximation to exp(-tc) */ (*outthe) = (*outthe) * ( 1.0 - tc ) + inthe * tc; } else @@ -117,6 +160,38 @@ void FGSteam::set_lowpass ( double *outthe, double inthe, double tc ) } +#define INHG_TO_MB 33.86388 /* Inches_of_mercury * INHG_TO_MB == millibars. */ + +// Convert air pressure to altitude by ICAO Standard Atmosphere +double pressInHgToAltFt(double p_inhg) +{ + // Ref. Aviation Formulary, Ed Williams, www.best.com/~williams/avform.htm + const double P_0 = 29.92126; // Std. MSL pressure, inHg. (=1013.25 mb) + const double p_Tr = 0.2233609 * P_0; // Pressure at tropopause, same units. + const double h_Tr = 36089.24; // Alt of tropopause, ft. (=11.0 km) + + if (p_inhg > p_Tr) // 0.0 to 11.0 km + return (1.0 - pow((p_inhg / P_0), 1.0 / 5.2558797)) / 6.8755856e-6; + return h_Tr + log10(p_inhg / p_Tr) / -4.806346e-5; // 11.0 to 20.0 km + // We could put more code for higher altitudes here. +} + + +// Convert altitude to air pressure by ICAO Standard Atmosphere +double altFtToPressInHg(double alt_ft) +{ + // Ref. Aviation Formulary, Ed Williams, www.best.com/~williams/avform.htm + const double P_0 = 29.92126; // Std. MSL pressure, inHg. (=1013.25 mb) + const double p_Tr = 0.2233609 * P_0; // Pressure at tropopause, same units. + const double h_Tr = 36089.24; // Alt of tropopause, ft. (=11.0 km) + + if (alt_ft < h_Tr) // 0.0 to 11.0 km + return P_0 * pow(1.0 - 6.8755856e-6 * alt_ft, 5.2558797); + return p_Tr * exp(-4.806346e-5 * (alt_ft - h_Tr)); // 11.0 to 20.0 km + // We could put more code for higher altitudes here. +} + + //////////////////////////////////////////////////////////////////////// // Here the fun really begins @@ -125,40 +200,143 @@ void FGSteam::set_lowpass ( double *outthe, double inthe, double tc ) void FGSteam::_CatchUp() { if ( _UpdatesPending != 0 ) - { double dt = _UpdatesPending * 1.0 / current_options.get_model_hz(); - int i,j; + { double dt = _UpdatesPending * 1.0 / + fgGetInt("/sim/model-hz"); // FIXME: inefficient + double AccN, AccE, AccU; + /* int i, j; */ double d, the_ENGINE_rpm; - /* + +#if 0 + /************************** + There is the possibility that this is the first call. + If this is the case, we will emit the feature registrations + just to be on the safe side. Doing it more than once will + waste CPU time but doesn't hurt anything really. + */ + if ( _UpdatesPending > 999999 ) + { FGFeature::register_int ( "Avionics/NAV1/Localizer", &NAV1_LOC ); + FGFeature::register_double ( "Avionics/NAV1/Latitude", &NAV1_Lat ); + FGFeature::register_double ( "Avionics/NAV1/Longitude", &NAV1_Lon ); + FGFeature::register_double ( "Avionics/NAV1/Radial", &NAV1_Rad ); + FGFeature::register_double ( "Avionics/NAV1/Altitude", &NAV1_Alt ); + FGFeature::register_int ( "Avionics/NAV2/Localizer", &NAV2_LOC ); + FGFeature::register_double ( "Avionics/NAV2/Latitude", &NAV2_Lat ); + FGFeature::register_double ( "Avionics/NAV2/Longitude", &NAV2_Lon ); + FGFeature::register_double ( "Avionics/NAV2/Radial", &NAV2_Rad ); + FGFeature::register_double ( "Avionics/NAV2/Altitude", &NAV2_Alt ); + FGFeature::register_double ( "Avionics/ADF/Latitude", &ADF_Lat ); + FGFeature::register_double ( "Avionics/ADF/Longitude", &ADF_Lon ); + } +#endif + + /************************** Someone has called our update function and it turns out that we are running somewhat behind. Here, we recalculate everything for a 'dt' second step. */ /************************** - This is not actually correct, but provides a - scaling capability for the vacuum pump later on. - When we have a real engine model, we can ask it. + The ball responds to the acceleration vector in the body + frame, only the components perpendicular to the longitudinal + axis of the aircraft. This is only related to actual + side slip for a symmetrical aircraft which is not touching + the ground and not changing its attitude. Math simplifies + by assuming (for small angles) arctan(x)=x in radians. + Obvious failure mode is the absence of liquid in the + tube, which is there to damp the motion, so that instead + the ball will bounce around, hitting the tube ends. + More subtle flaw is having it not move or a travel limit + occasionally due to some dirt in the tube or on the ball. */ - the_ENGINE_rpm = FGBFI::getThrottle() * 26.0; + d = - current_aircraft.fdm_state->get_A_Z_pilot(); + if ( d < 1 ) d = 1; + set_lowpass ( & the_TC_rad, + current_aircraft.fdm_state->get_A_Y_pilot () / d, + dt ); /************************** - This is just temporary, until the static source works, - so we just filter the actual value by one second to - account for the line impedance of the plumbing. + The rate of turn indication is from an electric gyro. + We should have it spin up with the master switch. + It is mounted at a funny angle so that it detects + both rate of bank (i.e. rolling into and out of turns) + and the rate of turn (i.e. how fast heading is changing). */ - set_lowpass ( & the_ALT_ft, FGBFI::getAltitude(), dt ); + set_lowpass ( & the_TC_std, + current_aircraft.fdm_state->get_Phi_dot () + * SGD_RADIANS_TO_DEGREES / 20.0 + + current_aircraft.fdm_state->get_Psi_dot () + * SGD_RADIANS_TO_DEGREES / 3.0 , dt ); + + /************************** + We want to know the pilot accelerations, + to compute the magnetic compass errors. + */ + AccN = current_aircraft.fdm_state->get_V_dot_north(); + AccE = current_aircraft.fdm_state->get_V_dot_east(); + AccU = current_aircraft.fdm_state->get_V_dot_down() + - 9.81 / 0.3; + if ( fabs(the_TC_rad) > 0.2 /* 2.0 */ ) + { /* Massive sideslip jams it; it stops turning */ + the_MH_degps = 0.0; + the_MH_err = fgGetDouble("/orientation/heading-deg") - the_MH_deg; + } else + { double MagDip, MagVar, CosDip; + double FrcN, FrcE, FrcU, AccTot; + double EdgN, EdgE, EdgU; + double TrqN, TrqE, TrqU, Torque; + /* Find a force vector towards exact magnetic north */ + MagVar = fgGetDouble("/environment/magnetic-variation-deg") + / SGD_RADIANS_TO_DEGREES; + MagDip = fgGetDouble("/environment/magnetic-dip-deg") + / SGD_RADIANS_TO_DEGREES; + CosDip = cos ( MagDip ); + FrcN = CosDip * cos ( MagVar ); + FrcE = CosDip * sin ( MagVar ); + FrcU = sin ( MagDip ); + /* Rotation occurs around acceleration axis, + but axis magnitude is irrelevant. So compute it. */ + AccTot = AccN*AccN + AccE*AccE + AccU*AccU; + if ( AccTot > 1.0 ) AccTot = sqrt ( AccTot ); + else AccTot = 1.0; + /* Force applies to north marking on compass card */ + EdgN = cos ( the_MH_err / SGD_RADIANS_TO_DEGREES ); + EdgE = sin ( the_MH_err / SGD_RADIANS_TO_DEGREES ); + EdgU = 0.0; + /* Apply the force to the edge to get torques */ + TrqN = EdgE * FrcU - EdgU * FrcE; + TrqE = EdgU * FrcN - EdgN * FrcU; + TrqU = EdgN * FrcE - EdgE * FrcN; + /* Select the component parallel to the axis */ + Torque = ( TrqN * AccN + + TrqE * AccE + + TrqU * AccU ) * 5.0 / AccTot; + /* The magnetic compass has angular momentum, + so we apply a torque to it and wait */ + if ( dt < 1.0 ) + { the_MH_degps= the_MH_degps * (1.0 - dt) - Torque; + the_MH_err += dt * the_MH_degps; + } + if ( the_MH_err > 180.0 ) the_MH_err -= 360.0; else + if ( the_MH_err < -180.0 ) the_MH_err += 360.0; + the_MH_deg = fgGetDouble("/orientation/heading-deg") - the_MH_err; + } + + /************************** + This is not actually correct, but provides a + scaling capability for the vacuum pump later on. + When we have a real engine model, we can ask it. + */ + the_ENGINE_rpm = globals->get_controls()->get_throttle(0) * 26.0; /************************** First, we need to know what the static line is reporting, which is a whole simulation area in itself. For now, we cheat. + We filter the actual value by one second to + account for the line impedance of the plumbing. */ - the_STATIC_inhg = 29.92; - i = (int) the_ALT_ft; - while ( i > 9000 ) - { the_STATIC_inhg *= 0.707; - i -= 9000; - } - the_STATIC_inhg *= ( 1.0 - 0.293 * i / 9000.0 ); + double static_inhg + = altFtToPressInHg(fgGetDouble("/position/altitude-ft")); + set_lowpass ( & the_STATIC_inhg, static_inhg, dt ); /* NO alternate static source error (student feature), @@ -166,6 +344,21 @@ void FGSteam::_CatchUp() NO slip-induced error, important for C172 for example. */ + /************************** + Altimeter. + ICAO standard atmosphere MSL pressure is 1013.25 mb, and pressure + gradient is about 28 ft per mb at MSL increasing to about 32 at + 5000 and 38 at 10000 ft. + Standard altimeters apply the subscale offset to the output altitude, + not to the input pressure; I don't know exactly what pressure gradient + they assume for this. I choose to make it accurate at low altitudes. + Remember, we are trying to simulate a real altimeter, not an ideal one. + */ + set_lowpass ( & the_ALT_ft, + pressInHgToAltFt(the_STATIC_inhg) + + (the_ALT_datum_mb - 1013.25) * 28.0, /* accurate at low alt. */ + dt * 10 ); /* smoothing time 0.1 s */ + /************************** The VSI case is a low-pass filter of the static line pressure. The instrument reports the difference, scaled to approx ft. @@ -201,11 +394,18 @@ void FGSteam::_CatchUp() > have it tumble when you exceed the usual pitch or bank limits, > put in those insidious turning errors ... for now anyway. */ + if ( _UpdatesPending > 999999 ) + the_DG_err = fgGetDouble("/environment/magnetic-variation-deg"); + the_DG_degps = 0.01; /* HACK! */ + if (dt<1.0) the_DG_err += dt * the_DG_degps; + the_DG_deg = fgGetDouble("/orientation/heading-deg") - the_DG_err; /************************** Finished updates, now clear the timer */ _UpdatesPending = 0; + } else { + // cout << "0 Updates pending" << endl; } } @@ -214,86 +414,94 @@ void FGSteam::_CatchUp() // Everything below is a transient hack; expect it to disappear //////////////////////////////////////////////////////////////////////// -/* KMYF ILS */ -#define NAV1_LOC (1) -#define NAV1_Lat ( 32.0 + 48.94/60.0) -#define NAV1_Lon (-117.0 - 08.37/60.0) -#define NAV1_Rad 280.0 -#define NAV1_Alt 423 - -/* MZB stepdown radial */ -#define NAV2_Lat ( 32.0 + 46.93/60.0) -#define NAV2_Lon (-117.0 - 13.53/60.0) -#define NAV2_Rad 068.0 - -/* HAILE intersection */ -#define ADF_Lat ( 32.0 + 46.79/60.0) -#define ADF_Lon (-117.0 - 02.70/60.0) - - - -double FGSteam::get_HackGS_deg () -{ double x,y,dme; - if (0==NAV1_LOC) return 0.0; - y = 60.0 * ( NAV1_Lat - FGBFI::getLatitude () ); - x = 60.0 * ( NAV1_Lon - FGBFI::getLongitude() ) - * cos ( FGBFI::getLatitude () / RAD_TO_DEG ); - dme = x*x + y*y; - if ( dme > 0.1 ) x = sqrt ( dme ); else x = 0.3; - y = FGBFI::getAltitude() - NAV1_Alt; - return 3.0 - (y/x) * 60.0 / 6000.0; + +#if 0 + +double FGSteam::get_HackGS_deg () { + if ( current_radiostack->get_nav1_inrange() && + current_radiostack->get_nav1_has_gs() ) + { + double x = current_radiostack->get_nav1_gs_dist(); + double y = (fgGetDouble("/position/altitude-ft") + - current_radiostack->get_nav1_elev()) + * SG_FEET_TO_METER; + double angle = atan2( y, x ) * SGD_RADIANS_TO_DEGREES; + return (current_radiostack->get_nav1_target_gs() - angle) * 5.0; + } else { + return 0.0; + } } -double FGSteam::get_HackVOR1_deg () -{ double r; - double x,y; - y = 60.0 * ( NAV1_Lat - FGBFI::getLatitude () ); - x = 60.0 * ( NAV1_Lon - FGBFI::getLongitude() ) - * cos ( FGBFI::getLatitude () / RAD_TO_DEG ); - r = atan2 ( x, y ) * RAD_TO_DEG - NAV1_Rad - FGBFI::getMagVar(); +double FGSteam::get_HackVOR1_deg () { + double r; + + if ( current_radiostack->get_nav1_inrange() ) { + r = current_radiostack->get_nav1_heading() + - current_radiostack->get_nav1_radial(); + // cout << "Radial = " << current_radiostack->get_nav1_radial() + // << " Bearing = " << current_radiostack->get_nav1_heading() + // << endl; + if (r> 180.0) r-=360.0; else - if (r<-180.0) r+=360.0; + if (r<-180.0) r+=360.0; if ( fabs(r) > 90.0 ) - r = ( r<0.0 ? -r-180.0 : -r+180.0 ); - if (NAV1_LOC) r*=5.0; - return r; + r = ( r<0.0 ? -r-180.0 : -r+180.0 ); + // According to Robin Peel, the ILS is 4x more sensitive than a vor + if ( current_radiostack->get_nav1_loc() ) r *= 4.0; + } else { + r = 0.0; + } + + return r; } -double FGSteam::get_HackVOR2_deg () -{ double r; - double x,y; - y = 60.0 * ( NAV2_Lat - FGBFI::getLatitude () ); - x = 60.0 * ( NAV2_Lon - FGBFI::getLongitude() ) - * cos ( FGBFI::getLatitude () / RAD_TO_DEG ); - r = atan2 ( x, y ) * RAD_TO_DEG - NAV2_Rad - FGBFI::getMagVar(); +double FGSteam::get_HackVOR2_deg () { + double r; + + if ( current_radiostack->get_nav2_inrange() ) { + r = current_radiostack->get_nav2_heading() + - current_radiostack->get_nav2_radial(); + // cout << "Radial = " << current_radiostack->get_nav1_radial() + // << " Bearing = " << current_radiostack->get_nav1_heading() << endl; + if (r> 180.0) r-=360.0; else - if (r<-180.0) r+=360.0; + if (r<-180.0) r+=360.0; if ( fabs(r) > 90.0 ) - r = ( r<0.0 ? -r-180.0 : -r+180.0 ); - return r; + r = ( r<0.0 ? -r-180.0 : -r+180.0 ); + } else { + r = 0.0; + } + + return r; } +#endif -double FGSteam::get_HackOBS1_deg () -{ return NAV1_Rad; +double FGSteam::get_HackOBS1_deg () { + return current_radiostack->get_nav1_radial(); } -double FGSteam::get_HackOBS2_deg () -{ return NAV2_Rad; +double FGSteam::get_HackOBS2_deg () { + return current_radiostack->get_nav2_radial(); } -double FGSteam::get_HackADF_deg () -{ double r; - double x,y; - y = 60.0 * ( ADF_Lat - FGBFI::getLatitude () ); - x = 60.0 * ( ADF_Lon - FGBFI::getLongitude() ) - * cos ( FGBFI::getLatitude () / RAD_TO_DEG ); - r = atan2 ( x, y ) * RAD_TO_DEG - FGBFI::getHeading (); +double FGSteam::get_HackADF_deg () { + static double last_r = 0; + + if ( current_radiostack->get_adf_inrange() ) { + double r = current_radiostack->get_adf_heading() + - fgGetDouble("/orientation/heading-deg"); + last_r = r; + // cout << "Radial = " << current_radiostack->get_adf_heading() + // << " Heading = " << FGBFI::getHeading() << endl; return r; + } else { + return last_r; + } }