MACROS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
-//#define RegisterVariable(ID,DEF) coeffdef[#ID] = ID; paramdef[ID] = DEF
-#define RegisterVariable(ID,DEF) coeffdef[#ID] = ID;
-
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
CLASS IMPLEMENTATION
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
-//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-//
-// For every term registered here there must be a corresponding handler in
-// GetParameter() below that retrieves that parameter. Also, there must be an
-// entry in the enum eParam definition in FGJSBBase.h. The ID is what must be used
-// in any config file entry which references that item.
-
FGState::FGState(FGFDMExec* fdex)
{
FDMExec = fdex;
a = 1000.0;
sim_time = 0.0;
dt = 1.0/120.0;
- ActiveEngine = -1;
Aircraft = FDMExec->GetAircraft();
Translation = FDMExec->GetTranslation();
Propulsion = FDMExec->GetPropulsion();
PropertyManager = FDMExec->GetPropertyManager();
- InitPropertyMaps();
+ for(int i=0;i<3;i++) vQdot_prev[i].InitMatrix();
bind();
Debug(1);
}
-//***************************************************************************
-//
-// Reset: Assume all angles READ FROM FILE IN DEGREES !!
-//
-
-bool FGState::Reset(string path, string acname, string fname)
-{
- string resetDef;
- string token="";
-
- double U, V, W;
- double phi, tht, psi;
- double latitude, longitude, h;
- double wdir, wmag, wnorth, weast;
-
-# ifndef macintosh
- resetDef = path + "/" + acname + "/" + fname + ".xml";
-# else
- resetDef = path + ";" + acname + ";" + fname + ".xml";
-# endif
-
- FGConfigFile resetfile(resetDef);
- if (!resetfile.IsOpen()) return false;
-
- resetfile.GetNextConfigLine();
- token = resetfile.GetValue();
- if (token != string("initialize")) {
- cerr << "The reset file " << resetDef
- << " does not appear to be a reset file" << endl;
- return false;
- } else {
- resetfile.GetNextConfigLine();
- resetfile >> token;
- cout << "Resetting using: " << token << endl << endl;
- }
-
- while (token != string("/initialize") && token != string("EOF")) {
- if (token == "UBODY") resetfile >> U;
- if (token == "VBODY") resetfile >> V;
- if (token == "WBODY") resetfile >> W;
- if (token == "LATITUDE") resetfile >> latitude;
- if (token == "LONGITUDE") resetfile >> longitude;
- if (token == "PHI") resetfile >> phi;
- if (token == "THETA") resetfile >> tht;
- if (token == "PSI") resetfile >> psi;
- if (token == "ALTITUDE") resetfile >> h;
- if (token == "WINDDIR") resetfile >> wdir;
- if (token == "VWIND") resetfile >> wmag;
-
- resetfile >> token;
- }
-
-
- Position->SetLatitude(latitude*degtorad);
- Position->SetLongitude(longitude*degtorad);
- Position->Seth(h);
-
- wnorth = wmag*ktstofps*cos(wdir*degtorad);
- weast = wmag*ktstofps*sin(wdir*degtorad);
-
- Initialize(U, V, W, phi*degtorad, tht*degtorad, psi*degtorad,
- latitude*degtorad, longitude*degtorad, h, wnorth, weast, 0.0);
-
- return true;
-}
-
//***************************************************************************
//
// Initialize: Assume all angles GIVEN IN RADIANS !!
vQdot(2) = 0.5*(vQtrn(1)*vPQR(eP) + vQtrn(3)*vPQR(eR) - vQtrn(4)*vPQR(eQ));
vQdot(3) = 0.5*(vQtrn(1)*vPQR(eQ) + vQtrn(4)*vPQR(eP) - vQtrn(2)*vPQR(eR));
vQdot(4) = 0.5*(vQtrn(1)*vPQR(eR) + vQtrn(2)*vPQR(eQ) - vQtrn(3)*vPQR(eP));
- vQtrn += 0.5*dt*rate*(vlastQdot + vQdot);
- vQtrn.Normalize();
+ vQtrn += Integrate(TRAPZ, dt*rate, vQdot, vQdot_prev);
- vlastQdot = vQdot;
+ vQtrn.Normalize();
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-void FGState::InitPropertyMaps(void)
-{
- ParamNameToProp[ "FG_TIME" ]="sim-time-sec";
- ParamNameToProp[ "FG_QBAR" ]="aero/qbar-psf";
- ParamNameToProp[ "FG_WINGAREA" ]="metrics/Sw-sqft";
- ParamNameToProp[ "FG_WINGSPAN" ]="metrics/bw-ft";
- ParamNameToProp[ "FG_CBAR" ]="metrics/cbarw-ft";
- ParamNameToProp[ "FG_ALPHA" ]="aero/alpha-rad";
- ParamNameToProp[ "FG_ALPHADOT" ]="aero/alphadot-rad_sec";
- ParamNameToProp[ "FG_BETA" ]="aero/beta-rad";
- ParamNameToProp[ "FG_ABETA" ]="aero/mag-beta-rad";
- ParamNameToProp[ "FG_BETADOT" ]="aero/betadot-rad_sec";
- ParamNameToProp[ "FG_PHI" ]="attitude/phi-rad";
- ParamNameToProp[ "FG_THT" ]="attitude/theta-rad";
- ParamNameToProp[ "FG_PSI" ]="attitude/psi-true-rad";
- ParamNameToProp[ "FG_PITCHRATE" ]="velocities/q-rad_sec";
- ParamNameToProp[ "FG_ROLLRATE" ]="velocities/p-rad_sec";
- ParamNameToProp[ "FG_YAWRATE" ]="velocities/r-rad_sec";
- ParamNameToProp[ "FG_AEROP" ]="velocities/p-aero-rad_sec";
- ParamNameToProp[ "FG_AEROQ" ]="velocities/q-aero-rad_sec";
- ParamNameToProp[ "FG_AEROR" ]="velocities/r-aero-rad_sec";
- ParamNameToProp[ "FG_CL_SQRD" ]="aero/cl-squared-norm";
- ParamNameToProp[ "FG_MACH" ]="velocities/mach-norm";
- ParamNameToProp[ "FG_ALTITUDE" ]="position/h-sl-ft";
- ParamNameToProp[ "FG_BI2VEL" ]="aero/bi2vel";
- ParamNameToProp[ "FG_CI2VEL" ]="aero/ci2vel";
- ParamNameToProp[ "FG_ELEVATOR_POS" ]="fcs/elevator-pos-rad";
- ParamNameToProp[ "FG_AELEVATOR_POS" ]="fcs/mag-elevator-pos-rad";
- ParamNameToProp[ "FG_NELEVATOR_POS" ]="fcs/elevator-pos-norm";
- ParamNameToProp[ "FG_AILERON_POS" ]="fcs/left-aileron-pos-rad";
- ParamNameToProp[ "FG_AAILERON_POS" ]="fcs/mag-aileron-pos-rad";
- ParamNameToProp[ "FG_NAILERON_POS" ]="fcs/left-aileron-pos-norm";
- ParamNameToProp[ "FG_LEFT_AILERON_POS" ]="fcs/left-aileron-pos-rad";
- ParamNameToProp[ "FG_ALEFT_AILERON_POS" ]="fcs/mag-left-aileron-pos-rad";
- ParamNameToProp[ "FG_NLEFT_AILERON_POS" ]="fcs/left-aileron-pos-norm";
- ParamNameToProp[ "FG_RIGHT_AILERON_POS" ]="fcs/right-aileron-pos-rad";
- ParamNameToProp[ "FG_ARIGHT_AILERON_POS" ]="fcs/mag-aileron-pos-rad";
- ParamNameToProp[ "FG_NRIGHT_AILERON_POS" ]="fcs/right-aileron-pos-norm";
- ParamNameToProp[ "FG_RUDDER_POS" ]="fcs/rudder-pos-rad";
- ParamNameToProp[ "FG_ARUDDER_POS" ]="fcs/mag-rudder-pos-rad";
- ParamNameToProp[ "FG_NRUDDER_POS" ]="fcs/rudder-pos-norm";
- ParamNameToProp[ "FG_SPDBRAKE_POS" ]="fcs/speedbrake-pos-rad";
- ParamNameToProp[ "FG_NSPDBRAKE_POS" ]="fcs/speedbrake-pos-norm";
- ParamNameToProp[ "FG_SPOILERS_POS" ]="fcs/spoiler-pos-rad";
- ParamNameToProp[ "FG_NSPOILERS_POS" ]="fcs/spoiler-pos-norm";
- ParamNameToProp[ "FG_FLAPS_POS" ]="fcs/flap-pos-deg";
- ParamNameToProp[ "FG_NFLAPS_POS" ]="fcs/flap-pos-norm";
- ParamNameToProp[ "FG_ELEVATOR_CMD" ]="fcs/elevator-cmd-norm";
- ParamNameToProp[ "FG_AILERON_CMD" ]="fcs/aileron-cmd-norm";
- ParamNameToProp[ "FG_RUDDER_CMD" ]="fcs/rudder-cmd-norm";
- ParamNameToProp[ "FG_SPDBRAKE_CMD" ]="fcs/speedbrake-cmd-norm";
- ParamNameToProp[ "FG_SPOILERS_CMD" ]="fcs/spoiler-cmd-norm";
- ParamNameToProp[ "FG_FLAPS_CMD" ]="fcs/flap-cmd-norm";
- ParamNameToProp[ "FG_THROTTLE_CMD" ]="fcs/throttle-cmd-norm";
- ParamNameToProp[ "FG_THROTTLE_POS" ]="fcs/throttle-pos-norm";
- ParamNameToProp[ "FG_MIXTURE_CMD" ]="fcs/mixture-cmd-norm";
- ParamNameToProp[ "FG_MIXTURE_POS" ]="fcs/mixture-pos-norm";
- ParamNameToProp[ "FG_MAGNETO_CMD" ]="zero";
- ParamNameToProp[ "FG_STARTER_CMD" ]="zero";
- ParamNameToProp[ "FG_ACTIVE_ENGINE" ]="zero";
- ParamNameToProp[ "FG_HOVERB" ]="aero/h_b-mac-ft";
- ParamNameToProp[ "FG_PITCH_TRIM_CMD" ]="fcs/pitch-trim-cmd-norm";
- ParamNameToProp[ "FG_YAW_TRIM_CMD" ]="fcs/yaw-trim-cmd-norm";
- ParamNameToProp[ "FG_ROLL_TRIM_CMD" ]="fcs/roll-trim-cmd-norm";
- ParamNameToProp[ "FG_LEFT_BRAKE_CMD" ]="zero";
- ParamNameToProp[ "FG_CENTER_BRAKE_CMD" ]="zero";
- ParamNameToProp[ "FG_RIGHT_BRAKE_CMD" ]="zero";
- ParamNameToProp[ "FG_SET_LOGGING" ]="zero";
- ParamNameToProp[ "FG_ALPHAH" ]="aero/alpha-rad";
- ParamNameToProp[ "FG_ALPHAW" ]="aero/alpha-wing-rad";
- ParamNameToProp[ "FG_LBARH" ]="metrics/lh-norm";
- ParamNameToProp[ "FG_LBARV" ]="metrics/lv-norm";
- ParamNameToProp[ "FG_HTAILAREA" ]="metrics/Sh-sqft";
- ParamNameToProp[ "FG_VTAILAREA" ]="metrics/Sv-sqft";
- ParamNameToProp[ "FG_VBARH" ]="metrics/vbarh-norm";
- ParamNameToProp[ "FG_VBARV" ]="metrics/vbarv-norm";
- ParamNameToProp[ "FG_GEAR_CMD" ]="gear/gear-cmd-norm";
- ParamNameToProp[ "FG_GEAR_POS" ]="gear/gear-pos-norm";
- ParamNameToProp[ "FG_HYSTPARM" ]="aero/stall-hyst-norm";
-}
-
-//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-
void FGState::bind(void)
{
PropertyManager->Tie("sim-time-sec",this,
projects / flightgear.git / blobdiff