From b5d116dad7816125dbaf94989aba774349f01017 Mon Sep 17 00:00:00 2001 From: tony Date: Wed, 28 Aug 2002 13:46:42 +0000 Subject: [PATCH] Latest JSBSim updates. --- src/FDM/JSBSim/FGEngine.h | 2 +- src/FDM/JSBSim/FGFCS.cpp | 283 ++++++++++++++++++++++++++++------ src/FDM/JSBSim/FGFCS.h | 165 +++++++++++++++++++- src/FDM/JSBSim/FGFDMExec.cpp | 29 +++- src/FDM/JSBSim/FGInertial.cpp | 3 +- src/FDM/JSBSim/FGInertial.h | 1 - src/FDM/JSBSim/FGJSBBase.cpp | 6 +- src/FDM/JSBSim/FGJSBBase.h | 21 ++- src/FDM/JSBSim/FGOutput.cpp | 8 + src/FDM/JSBSim/FGPiston.cpp | 51 ++---- src/FDM/JSBSim/FGState.cpp | 17 ++ 11 files changed, 493 insertions(+), 93 deletions(-) diff --git a/src/FDM/JSBSim/FGEngine.h b/src/FDM/JSBSim/FGEngine.h index ac5ea6420..fde9f0adb 100644 --- a/src/FDM/JSBSim/FGEngine.h +++ b/src/FDM/JSBSim/FGEngine.h @@ -111,7 +111,7 @@ public: FGEngine(FGFDMExec* exec); virtual ~FGEngine(); - enum EngineType {etUnknown, etRocket, etPiston, etTurboProp, etTurboJet, etTurboShaft}; + enum EngineType {etUnknown, etRocket, etPiston, etTurbine}; virtual double GetThrottleMin(void) { return MinThrottle; } virtual double GetThrottleMax(void) { return MaxThrottle; } diff --git a/src/FDM/JSBSim/FGFCS.cpp b/src/FDM/JSBSim/FGFCS.cpp index d14fbe322..ed6dbf356 100644 --- a/src/FDM/JSBSim/FGFCS.cpp +++ b/src/FDM/JSBSim/FGFCS.cpp @@ -74,11 +74,14 @@ FGFCS::FGFCS(FGFDMExec* fdmex) : FGModel(fdmex) Name = "FGFCS"; DaCmd = DeCmd = DrCmd = DfCmd = DsbCmd = DspCmd = 0.0; + AP_DaCmd = AP_DeCmd = AP_DrCmd = AP_ThrottleCmd = 0.0; PTrimCmd = YTrimCmd = RTrimCmd = 0.0; GearCmd = GearPos = 1; // default to gear down LeftBrake = RightBrake = CenterBrake = 0.0; DoNormalize=true; + eMode = mNone; + bind(); for (i=0;i<=NForms;i++) { DePos[i] = DaLPos[i] = DaRPos[i] = DrPos[i] = 0.0; @@ -100,11 +103,13 @@ FGFCS::~FGFCS() PropAdvanceCmd.clear(); PropAdvance.clear(); - unsigned int i; - unbind(); - for (i=0;iRun(); + for (i=0; iRun(); + eMode = mNone; + } + for (i=0; iRun(); + eMode = mNone; + } if (DoNormalize) Normalize(); - return false; + return false; } else { - return true; + return true; } } @@ -265,17 +279,63 @@ void FGFCS::SetPropAdvance(int engineNum, double setting) bool FGFCS::Load(FGConfigFile* AC_cfg) { - string token; + string token, delimiter; + string name, file, fname; unsigned i; + vector *Components; + FGConfigFile *FCS_cfg; + + // Determine if the FCS/Autopilot is defined inline in the aircraft configuration + // file or in a separate file. Set up the config file class as appropriate. + + delimiter = AC_cfg->GetValue(); + name = AC_cfg->GetValue("NAME"); + fname = AC_cfg->GetValue("FILE"); + + if ( AC_cfg->GetValue("NORMALIZE") == "FALSE") { + DoNormalize = false; + cout << " Automatic Control Surface Normalization Disabled" << endl; + } + +# ifndef macintosh + file = "control/" + fname + ".xml"; +# else + file = "control;" + fname + ".xml"; +# endif + + if (name.empty()) { + name = fname; + if (file.empty()) { + cerr << "FCS/Autopilot does not appear to be defined inline nor in a file" << endl; + } else { + FCS_cfg = new FGConfigFile(file); + if (!FCS_cfg->IsOpen()) { + cerr << "Could not open " << delimiter << " file: " << file << endl; + return false; + } else { + AC_cfg = FCS_cfg; // set local config file object pointer to FCS config + // file object pointer + } + } + } else { + AC_cfg->GetNextConfigLine(); + } + + if (delimiter == "AUTOPILOT") { + Components = &APComponents; + eMode = mAP; + Name = "Autopilot:" + name; + } else if (delimiter == "FLIGHT_CONTROL") { + Components = &FCSComponents; + eMode = mFCS; + Name = "FCS:" + name; + } else { + cerr << endl << "Unknown FCS delimiter" << endl << endl; + } - Name = Name + ":" + AC_cfg->GetValue("NAME"); if (debug_lvl > 0) cout << " Control System Name: " << Name << endl; - if ( AC_cfg->GetValue("NORMALIZE") == "FALSE") { - DoNormalize=false; - cout << " Automatic Control Surface Normalization Disabled" << endl; - } - AC_cfg->GetNextConfigLine(); - while ((token = AC_cfg->GetValue()) != string("/FLIGHT_CONTROL")) { + + while ((token = AC_cfg->GetValue()) != string("/" + delimiter)) { if (token == "COMPONENT") { token = AC_cfg->GetValue("TYPE"); if (debug_lvl > 0) cout << endl << " Loading Component \"" @@ -286,38 +346,40 @@ bool FGFCS::Load(FGConfigFile* AC_cfg) (token == "SECOND_ORDER_FILTER") || (token == "WASHOUT_FILTER") || (token == "INTEGRATOR") ) { - Components.push_back(new FGFilter(this, AC_cfg)); + Components->push_back(new FGFilter(this, AC_cfg)); } else if ((token == "PURE_GAIN") || (token == "SCHEDULED_GAIN") || (token == "AEROSURFACE_SCALE") ) { - Components.push_back(new FGGain(this, AC_cfg)); + Components->push_back(new FGGain(this, AC_cfg)); } else if (token == "SUMMER") { - Components.push_back(new FGSummer(this, AC_cfg)); + Components->push_back(new FGSummer(this, AC_cfg)); } else if (token == "DEADBAND") { - Components.push_back(new FGDeadBand(this, AC_cfg)); + Components->push_back(new FGDeadBand(this, AC_cfg)); } else if (token == "GRADIENT") { - Components.push_back(new FGGradient(this, AC_cfg)); + Components->push_back(new FGGradient(this, AC_cfg)); } else if (token == "SWITCH") { - Components.push_back(new FGSwitch(this, AC_cfg)); + Components->push_back(new FGSwitch(this, AC_cfg)); } else if (token == "KINEMAT") { - Components.push_back(new FGKinemat(this, AC_cfg)); + Components->push_back(new FGKinemat(this, AC_cfg)); } else { cerr << "Unknown token [" << token << "] in FCS portion of config file" << endl; return false; } - AC_cfg->GetNextConfigLine(); + if (AC_cfg->GetNextConfigLine() == "EOF") break; } } + //collect information for normalizing control surfaces + string nodeName; - for (i=0;isize(); i++) { - if ( (Components[i]->GetType() == "AEROSURFACE_SCALE" - || Components[i]->GetType() == "KINEMAT") - && Components[i]->GetOutputNode() ) { - nodeName= Components[i]->GetOutputNode()->GetName(); + if ( (((*Components)[i])->GetType() == "AEROSURFACE_SCALE" + || ((*Components)[i])->GetType() == "KINEMAT") + && ((*Components)[i])->GetOutputNode() ) { + nodeName = ((*Components)[i])->GetOutputNode()->GetName(); if ( nodeName == "elevator-pos-rad" ) { ToNormalize[iDe]=i; } else if ( nodeName == "left-aileron-pos-rad" @@ -337,21 +399,41 @@ bool FGFCS::Load(FGConfigFile* AC_cfg) } } - bindModel(); - + if (delimiter == "FLIGHT_CONTROL") bindModel(); + + eMode = mNone; + return true; } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -double FGFCS::GetComponentOutput(int idx) { - return Components[idx]->GetOutput(); +double FGFCS::GetComponentOutput(int idx) +{ + switch (eMode) { + case mFCS: + return FCSComponents[idx]->GetOutput(); + case mAP: + return APComponents[idx]->GetOutput(); + case mNone: + cerr << "Unknown FCS mode" << endl; + break; + } } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -string FGFCS::GetComponentName(int idx) { - return Components[idx]->GetName(); +string FGFCS::GetComponentName(int idx) +{ + switch (eMode) { + case mFCS: + return FCSComponents[idx]->GetName(); + case mAP: + return APComponents[idx]->GetName(); + case mNone: + cerr << "Unknown FCS mode" << endl; + break; + } } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -379,11 +461,17 @@ string FGFCS::GetComponentStrings(void) string CompStrings = ""; bool firstime = true; - for (comp = 0; comp < Components.size(); comp++) { + for (comp = 0; comp < FCSComponents.size(); comp++) { if (firstime) firstime = false; else CompStrings += ", "; - CompStrings += Components[comp]->GetName(); + CompStrings += FCSComponents[comp]->GetName(); + } + + for (comp = 0; comp < APComponents.size(); comp++) + { + CompStrings += ", "; + CompStrings += APComponents[comp]->GetName(); } return CompStrings; @@ -398,11 +486,16 @@ string FGFCS::GetComponentValues(void) char buffer[10]; bool firstime = true; - for (comp = 0; comp < Components.size(); comp++) { + for (comp = 0; comp < FCSComponents.size(); comp++) { if (firstime) firstime = false; else CompValues += ", "; - sprintf(buffer, "%9.6f", Components[comp]->GetOutput()); + sprintf(buffer, "%9.6f", FCSComponents[comp]->GetOutput()); + CompValues += string(buffer); + } + + for (comp = 0; comp < APComponents.size(); comp++) { + sprintf(buffer, ", %9.6f", APComponents[comp]->GetOutput()); CompValues += string(buffer); } @@ -430,31 +523,31 @@ void FGFCS::Normalize(void) { //ToNormalize is filled in Load() if ( ToNormalize[iDe] > -1 ) { - DePos[ofNorm] = Components[ToNormalize[iDe]]->GetOutputPct(); + DePos[ofNorm] = FCSComponents[ToNormalize[iDe]]->GetOutputPct(); } if ( ToNormalize[iDaL] > -1 ) { - DaLPos[ofNorm] = Components[ToNormalize[iDaL]]->GetOutputPct(); + DaLPos[ofNorm] = FCSComponents[ToNormalize[iDaL]]->GetOutputPct(); } if ( ToNormalize[iDaR] > -1 ) { - DaRPos[ofNorm] = Components[ToNormalize[iDaR]]->GetOutputPct(); + DaRPos[ofNorm] = FCSComponents[ToNormalize[iDaR]]->GetOutputPct(); } if ( ToNormalize[iDr] > -1 ) { - DrPos[ofNorm] = Components[ToNormalize[iDr]]->GetOutputPct(); + DrPos[ofNorm] = FCSComponents[ToNormalize[iDr]]->GetOutputPct(); } if ( ToNormalize[iDsb] > -1 ) { - DsbPos[ofNorm] = Components[ToNormalize[iDsb]]->GetOutputPct(); + DsbPos[ofNorm] = FCSComponents[ToNormalize[iDsb]]->GetOutputPct(); } if ( ToNormalize[iDsp] > -1 ) { - DspPos[ofNorm] = Components[ToNormalize[iDsp]]->GetOutputPct(); + DspPos[ofNorm] = FCSComponents[ToNormalize[iDsp]]->GetOutputPct(); } if ( ToNormalize[iDf] > -1 ) { - DfPos[ofNorm] = Components[ToNormalize[iDf]]->GetOutputPct(); + DfPos[ofNorm] = FCSComponents[ToNormalize[iDf]]->GetOutputPct(); } DePos[ofMag] = fabs(DePos[ofRad]); @@ -603,6 +696,91 @@ void FGFCS::bind(void) &FGFCS::GetGearPos, &FGFCS::SetGearPos, true); + + PropertyManager->Tie("jsbsim/ap/elevator_cmd", this, + &FGFCS::GetAPDeCmd, + &FGFCS::SetAPDeCmd, + true); + + PropertyManager->Tie("jsbsim/ap/aileron_cmd", this, + &FGFCS::GetAPDaCmd, + &FGFCS::SetAPDaCmd, + true); + + PropertyManager->Tie("jsbsim/ap/rudder_cmd", this, + &FGFCS::GetAPDrCmd, + &FGFCS::SetAPDrCmd, + true); + + PropertyManager->Tie("jsbsim/ap/throttle_cmd", this, + &FGFCS::GetAPThrottleCmd, + &FGFCS::SetAPThrottleCmd, + true); + + PropertyManager->Tie("jsbsim/ap/attitude_setpoint", this, + &FGFCS::GetAPAttitudeSetPt, + &FGFCS::SetAPAttitudeSetPt, + true); + + PropertyManager->Tie("jsbsim/ap/altitude_setpoint", this, + &FGFCS::GetAPAltitudeSetPt, + &FGFCS::SetAPAltitudeSetPt, + true); + + PropertyManager->Tie("jsbsim/ap/heading_setpoint", this, + &FGFCS::GetAPHeadingSetPt, + &FGFCS::SetAPHeadingSetPt, + true); + + PropertyManager->Tie("jsbsim/ap/airspeed_setpoint", this, + &FGFCS::GetAPAirspeedSetPt, + &FGFCS::SetAPAirspeedSetPt, + true); + + PropertyManager->Tie("jsbsim/ap/acquire_attitude", this, + &FGFCS::GetAPAcquireAttitude, + &FGFCS::SetAPAcquireAttitude, + true); + + PropertyManager->Tie("jsbsim/ap/acquire_altitude", this, + &FGFCS::GetAPAcquireAltitude, + &FGFCS::SetAPAcquireAltitude, + true); + + PropertyManager->Tie("jsbsim/ap/acquire_heading", this, + &FGFCS::GetAPAcquireHeading, + &FGFCS::SetAPAcquireHeading, + true); + + PropertyManager->Tie("jsbsim/ap/acquire_airspeed", this, + &FGFCS::GetAPAcquireAirspeed, + &FGFCS::SetAPAcquireAirspeed, + true); + + PropertyManager->Tie("jsbsim/ap/attitude_hold", this, + &FGFCS::GetAPAttitudeHold, + &FGFCS::SetAPAttitudeHold, + true); + + PropertyManager->Tie("jsbsim/ap/altitude_hold", this, + &FGFCS::GetAPAltitudeHold, + &FGFCS::SetAPAltitudeHold, + true); + + PropertyManager->Tie("jsbsim/ap/heading_hold", this, + &FGFCS::GetAPHeadingHold, + &FGFCS::SetAPHeadingHold, + true); + + PropertyManager->Tie("jsbsim/ap/airspeed_hold", this, + &FGFCS::GetAPAirspeedHold, + &FGFCS::SetAPAirspeedHold, + true); + + PropertyManager->Tie("jsbsim/ap/wingslevel_hold", this, + &FGFCS::GetAPWingsLevelHold, + &FGFCS::SetAPWingsLevelHold, + true); } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -685,6 +863,23 @@ void FGFCS::unbind(void) PropertyManager->Untie("fcs/mag-spoiler-pos-rad"); PropertyManager->Untie("fcs/spoiler-pos-norm"); PropertyManager->Untie("gear/gear-pos-norm"); + PropertyManager->Untie("jsbsim/ap/elevator_cmd"); + PropertyManager->Untie("jsbsim/ap/aileron_cmd"); + PropertyManager->Untie("jsbsim/ap/rudder_cmd"); + PropertyManager->Untie("jsbsim/ap/throttle_cmd"); + PropertyManager->Untie("jsbsim/ap/attitude_setpoint"); + PropertyManager->Untie("jsbsim/ap/altitude_setpoint"); + PropertyManager->Untie("jsbsim/ap/heading_setpoint"); + PropertyManager->Untie("jsbsim/ap/airspeed_setpoint"); + PropertyManager->Untie("jsbsim/ap/acquire_attitude"); + PropertyManager->Untie("jsbsim/ap/acquire_altitude"); + PropertyManager->Untie("jsbsim/ap/acquire_heading"); + PropertyManager->Untie("jsbsim/ap/acquire_airspeed"); + PropertyManager->Untie("jsbsim/ap/attitude_hold"); + PropertyManager->Untie("jsbsim/ap/altitude_hold"); + PropertyManager->Untie("jsbsim/ap/heading_hold"); + PropertyManager->Untie("jsbsim/ap/airspeed_hold"); + PropertyManager->Untie("jsbsim/ap/wingslevel_hold"); } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% diff --git a/src/FDM/JSBSim/FGFCS.h b/src/FDM/JSBSim/FGFCS.h index fa0a8baf1..a5387de27 100644 --- a/src/FDM/JSBSim/FGFCS.h +++ b/src/FDM/JSBSim/FGFCS.h @@ -232,6 +232,138 @@ public: inline double GetGearCmd(void) const { return GearCmd; } //@} + /// @name AUTOPilot -> FCS effectors command retrieval + //@{ + /** Gets the AUTOPilot aileron command. + @return aileron command in radians */ + inline double GetAPDaCmd(void) const { return AP_DaCmd; } + + /** Gets the AUTOPilot elevator command. + @return elevator command in radians */ + inline double GetAPDeCmd(void) const { return AP_DeCmd; } + + /** Gets the AUTOPilot rudder command. + @return rudder command in radians */ + inline double GetAPDrCmd(void) const { return AP_DrCmd; } + + /** Gets the AUTOPilot throttle (all engines) command. + @return throttle command in percent */ + inline double GetAPThrottleCmd(void) const { return AP_ThrottleCmd; } + //@} + + /// @name AUTOPilot setpoint retrieval + //@{ + /** Gets the autopilot pitch attitude setpoint + @return Pitch attitude setpoint in radians */ + inline double GetAPAttitudeSetPt(void) const {return APAttitudeSetPt;} + + /** Gets the autopilot altitude setpoint + @return Altitude setpoint in feet */ + inline double GetAPAltitudeSetPt(void) const {return APAltitudeSetPt;} + + /** Gets the autopilot heading setpoint + @return Heading setpoint in radians */ + inline double GetAPHeadingSetPt(void) const {return APHeadingSetPt;} + + /** Gets the autopilot airspeed setpoint + @return Airspeed setpoint in fps */ + inline double GetAPAirspeedSetPt(void) const {return APAirspeedSetPt;} + //@} + + /// @name AUTOPilot setpoint setting + //@{ + /// Sets the autopilot pitch attitude setpoint + inline void SetAPAttitudeSetPt(double set) {APAttitudeSetPt = set;} + + /// Sets the autopilot altitude setpoint + inline void SetAPAltitudeSetPt(double set) {APAltitudeSetPt = set;} + + /// Sets the autopilot heading setpoint + inline void SetAPHeadingSetPt(double set) {APHeadingSetPt = set;} + + /// Sets the autopilot airspeed setpoint + inline void SetAPAirspeedSetPt(double set) {APAirspeedSetPt = set;} + //@} + + + /// @name AUTOPilot mode setting + //@{ + /** Turns on/off the attitude-seeking autopilot. + @param set true turns the mode on, false turns it off **/ + inline void SetAPAcquireAttitude(bool set) {APAcquireAttitude = set;} + + /** Turns on/off the altitude-seeking autopilot. + @param set true turns the mode on, false turns it off **/ + inline void SetAPAcquireAltitude(bool set) {APAcquireAltitude = set;} + + /** Turns on/off the heading-seeking autopilot. + @param set true turns the mode on, false turns it off **/ + inline void SetAPAcquireHeading(bool set) {APAcquireHeading = set;} + + /** Turns on/off the airspeed-seeking autopilot. + @param set true turns the mode on, false turns it off **/ + inline void SetAPAcquireAirspeed(bool set) {APAcquireAirspeed = set;} + + /** Turns on/off the attitude-holding autopilot. + @param set true turns the mode on, false turns it off **/ + inline void SetAPAttitudeHold(bool set) {APAttitudeHold = set;} + + /** Turns on/off the altitude-holding autopilot. + @param set true turns the mode on, false turns it off **/ + inline void SetAPAltitudeHold(bool set) {APAltitudeHold = set;} + + /** Turns on/off the heading-holding autopilot. + @param set true turns the mode on, false turns it off **/ + inline void SetAPHeadingHold(bool set) {APHeadingHold = set;} + + /** Turns on/off the airspeed-holding autopilot. + @param set true turns the mode on, false turns it off **/ + inline void SetAPAirspeedHold(bool set) {APAirspeedHold = set;} + + /** Turns on/off the wing-leveler autopilot. + @param set true turns the mode on, false turns it off **/ + inline void SetAPWingsLevelHold(bool set) {APWingsLevelHold = set;} + //@} + + /// @name AUTOPilot mode retrieval + //@{ + /** Retrieves the on/off mode of the autopilot AcquireAttitude mode + @return true if on, false if off */ + inline bool GetAPAcquireAttitude(void) const {return APAcquireAttitude;} + + /** Retrieves the on/off mode of the autopilot AcquireAltitude mode + @return true if on, false if off */ + inline bool GetAPAcquireAltitude(void) const {return APAcquireAltitude;} + + /** Retrieves the on/off mode of the autopilot AcquireHeading mode + @return true if on, false if off */ + inline bool GetAPAcquireHeading(void) const {return APAcquireHeading;} + + /** Retrieves the on/off mode of the autopilot AcquireAirspeed mode + @return true if on, false if off */ + inline bool GetAPAcquireAirspeed(void) const {return APAcquireAirspeed;} + + /** Retrieves the on/off mode of the autopilot AttitudeHold mode + @return true if on, false if off */ + inline bool GetAPAttitudeHold(void) const {return APAttitudeHold;} + + /** Retrieves the on/off mode of the autopilot AltitudeHold mode + @return true if on, false if off */ + inline bool GetAPAltitudeHold(void) const {return APAltitudeHold;} + + /** Retrieves the on/off mode of the autopilot HeadingHold mode + @return true if on, false if off */ + inline bool GetAPHeadingHold(void) const {return APHeadingHold;} + + /** Retrieves the on/off mode of the autopilot AirspeedHold mode + @return true if on, false if off */ + inline bool GetAPAirspeedHold(void) const {return APAirspeedHold;} + + /** Retrieves the on/off mode of the autopilot WingsLevelHold mode + @return true if on, false if off */ + inline bool GetAPWingsLevelHold(void) const {return APWingsLevelHold;} + //@} + /// @name Aerosurface position retrieval //@{ /** Gets the left aileron position. @@ -370,6 +502,25 @@ public: void SetPropAdvanceCmd(int engine, double cmd); //@} + /// @name AUTOPilot -> FCS effector command setting + //@{ + /** Sets the AUTOPilot aileron command + @param cmd AUTOPilot aileron command in radians*/ + inline void SetAPDaCmd( double cmd ) { AP_DaCmd = cmd; } + + /** Sets the AUTOPilot elevator command + @param cmd AUTOPilot elevator command in radians*/ + inline void SetAPDeCmd(double cmd ) { AP_DeCmd = cmd; } + + /** Sets the AUTOPilot rudder command + @param cmd AUTOPilot rudder command in radians*/ + inline void SetAPDrCmd(double cmd) { AP_DrCmd = cmd; } + + /** Sets the AUTOPilot throttle command + @param cmd AUTOPilot throttle command in percent*/ + inline void SetAPThrottleCmd(double cmd) { AP_ThrottleCmd = cmd; } + //@} + /// @name Aerosurface position setting //@{ /** Sets the left aileron position @@ -428,7 +579,7 @@ public: void SetPropAdvance(int engine, double cmd); //@} - /// @name Landing Gear brakes + /// @name Landing Gear brakes //@{ /** Sets the left brake group @param cmd brake setting in percent (0.0 - 1.0) */ @@ -466,6 +617,7 @@ public: private: double DaCmd, DeCmd, DrCmd, DfCmd, DsbCmd, DspCmd; + double AP_DaCmd, AP_DeCmd, AP_DrCmd, AP_ThrottleCmd; double DePos[NForms], DaLPos[NForms], DaRPos[NForms], DrPos[NForms]; double DfPos[NForms], DsbPos[NForms], DspPos[NForms]; double PTrimCmd, YTrimCmd, RTrimCmd; @@ -477,11 +629,18 @@ private: vector PropAdvance; double LeftBrake, RightBrake, CenterBrake; // Brake settings double GearCmd,GearPos; - + + enum Mode {mAP, mFCS, mNone} eMode; + + double APAttitudeSetPt, APAltitudeSetPt, APHeadingSetPt, APAirspeedSetPt; + bool APAcquireAttitude, APAcquireAltitude, APAcquireHeading, APAcquireAirspeed; + bool APAttitudeHold, APAltitudeHold, APHeadingHold, APAirspeedHold, APWingsLevelHold; + bool DoNormalize; void Normalize(void); - vector Components; + vector FCSComponents; + vector APComponents; int ToNormalize[NNorm]; void Debug(int from); }; diff --git a/src/FDM/JSBSim/FGFDMExec.cpp b/src/FDM/JSBSim/FGFDMExec.cpp index 2b1d08973..039ac7798 100644 --- a/src/FDM/JSBSim/FGFDMExec.cpp +++ b/src/FDM/JSBSim/FGFDMExec.cpp @@ -421,6 +421,9 @@ bool FGFDMExec::LoadModel(string APath, string EPath, string model) } else if (token == "FLIGHT_CONTROL") { if (debug_lvl > 0) cout << fgcyan << "\n Reading Flight Control" << fgdef << endl; if (!ReadFlightControls(&AC_cfg)) result = false; + } else if (token == "AUTOPILOT") { + if (debug_lvl > 0) cout << fgcyan << "\n Reading Autopilot" << fgdef << endl; + if (!ReadFlightControls(&AC_cfg)) result = false; } else if (token == "OUTPUT") { if (debug_lvl > 0) cout << fgcyan << "\n Reading Output directives" << fgdef << endl; if (!ReadOutput(&AC_cfg)) result = false; @@ -483,6 +486,7 @@ bool FGFDMExec::ReadSlave(FGConfigFile* AC_cfg) // reset debug level to prior setting int saved_debug_lvl = debug_lvl; + string token; SlaveFDMList.push_back(new slaveData); SlaveFDMList.back()->exec = new FGFDMExec(); @@ -490,9 +494,30 @@ bool FGFDMExec::ReadSlave(FGConfigFile* AC_cfg) string AircraftName = AC_cfg->GetValue("FILE"); - debug_lvl = 0; + debug_lvl = 0; // turn off debug output for slave vehicle SlaveFDMList.back()->exec->LoadModel("aircraft", "engine", AircraftName); - debug_lvl = saved_debug_lvl; + debug_lvl = saved_debug_lvl; // turn debug output back on for master vehicle + + AC_cfg->GetNextConfigLine(); + while ((token = AC_cfg->GetValue()) != string("/SLAVE")) { + *AC_cfg >> token; + if (token == "XLOC") { *AC_cfg >> SlaveFDMList.back()->x; } + else if (token == "YLOC") { *AC_cfg >> SlaveFDMList.back()->y; } + else if (token == "ZLOC") { *AC_cfg >> SlaveFDMList.back()->z; } + else if (token == "PITCH") { *AC_cfg >> SlaveFDMList.back()->pitch;} + else if (token == "YAW") { *AC_cfg >> SlaveFDMList.back()->yaw; } + else if (token == "ROLL") { *AC_cfg >> SlaveFDMList.back()->roll; } + else cerr << "Unknown identifier: " << token << " in slave vehicle definition" << endl; + } + + if (debug_lvl > 0) { + cout << " X = " << SlaveFDMList.back()->x << endl; + cout << " Y = " << SlaveFDMList.back()->y << endl; + cout << " Z = " << SlaveFDMList.back()->z << endl; + cout << " Pitch = " << SlaveFDMList.back()->pitch << endl; + cout << " Yaw = " << SlaveFDMList.back()->yaw << endl; + cout << " Roll = " << SlaveFDMList.back()->roll << endl; + } return true; } diff --git a/src/FDM/JSBSim/FGInertial.cpp b/src/FDM/JSBSim/FGInertial.cpp index 854ed26da..091f6647d 100644 --- a/src/FDM/JSBSim/FGInertial.cpp +++ b/src/FDM/JSBSim/FGInertial.cpp @@ -51,14 +51,13 @@ FGInertial::FGInertial(FGFDMExec* fgex) : FGModel(fgex) { Name = "FGInertial"; - vRadius.InitMatrix(); - // Defaults RotationRate = 0.00007272205217; GM = 14.06252720E15; RadiusReference = 20925650.00; gAccelReference = GM/(RadiusReference*RadiusReference); gAccel = GM/(RadiusReference*RadiusReference); + vRadius.InitMatrix(); vCoriolis.InitMatrix(); vCentrifugal.InitMatrix(); vGravity.InitMatrix(); diff --git a/src/FDM/JSBSim/FGInertial.h b/src/FDM/JSBSim/FGInertial.h index 1e4a461e9..5130c92db 100644 --- a/src/FDM/JSBSim/FGInertial.h +++ b/src/FDM/JSBSim/FGInertial.h @@ -87,7 +87,6 @@ public: void bind(void); void unbind(void); - private: FGColumnVector3 vOmegaLocal; FGColumnVector3 vForces; diff --git a/src/FDM/JSBSim/FGJSBBase.cpp b/src/FDM/JSBSim/FGJSBBase.cpp index bc38f011f..591a9897e 100644 --- a/src/FDM/JSBSim/FGJSBBase.cpp +++ b/src/FDM/JSBSim/FGJSBBase.cpp @@ -59,13 +59,15 @@ char FGJSBBase::fgdef[6] = {27, '[', '3', '9', 'm', '\0' }; const double FGJSBBase::radtodeg = 57.29578; const double FGJSBBase::degtorad = 1.745329E-2; const double FGJSBBase::hptoftlbssec = 550.0; +const double FGJSBBase::psftoinhg = 0.014138; const double FGJSBBase::fpstokts = 0.592484; const double FGJSBBase::ktstofps = 1.68781; const double FGJSBBase::inchtoft = 0.08333333; +const double FGJSBBase::in3tom3 = 1.638706E-5; const double FGJSBBase::Reng = 1716.0; const double FGJSBBase::SHRatio = 1.40; -const string FGJSBBase::needed_cfg_version = "1.57"; -const string FGJSBBase::JSBSim_version = "0.9.1"; +const string FGJSBBase::needed_cfg_version = "1.58"; +const string FGJSBBase::JSBSim_version = "0.9.2"; queue FGJSBBase::Messages; FGJSBBase::Message FGJSBBase::localMsg; diff --git a/src/FDM/JSBSim/FGJSBBase.h b/src/FDM/JSBSim/FGJSBBase.h index 8a6de13ca..6f4fe91c9 100644 --- a/src/FDM/JSBSim/FGJSBBase.h +++ b/src/FDM/JSBSim/FGJSBBase.h @@ -165,7 +165,24 @@ enum eParam { FG_VBARV, //vertical tail volume FG_GEAR_CMD, FG_GEAR_POS, - FG_HYSTPARM + FG_HYSTPARM, + AP_ELEVATOR_CMD, + AP_AILERON_CMD, + AP_RUDDER_CMD, + AP_THROTTLE_CMD, + AP_SET_ATTITUDE, + AP_SET_ALTITUDE, + AP_SET_HEADING, + AP_SET_AIRSPEED, + AP_ACQUIRE_ATTITUDE, + AP_ACQUIRE_ALTITUDE, + AP_ACQUIRE_HEADING, + AP_ACQUIRE_AIRSPEED, + AP_ATTITUDE_HOLD_ON, + AP_ALTITUDE_HOLD_ON, + AP_HEADING_HOLD_ON, + AP_AIRSPEED_HOLD_ON, + AP_WINGSLEVEL_HOLD_ON }; /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -299,9 +316,11 @@ protected: static const double radtodeg; static const double degtorad; static const double hptoftlbssec; + static const double psftoinhg; static const double fpstokts; static const double ktstofps; static const double inchtoft; + static const double in3tom3; static const double Reng; // Specific Gas Constant,ft^2/(sec^2*R) static const double SHRatio; static const string needed_cfg_version; diff --git a/src/FDM/JSBSim/FGOutput.cpp b/src/FDM/JSBSim/FGOutput.cpp index 52fa53841..b3b385808 100644 --- a/src/FDM/JSBSim/FGOutput.cpp +++ b/src/FDM/JSBSim/FGOutput.cpp @@ -211,6 +211,10 @@ void FGOutput::DelimitedOutput(string fname) outstream << ", "; outstream << Aerodynamics->GetCoefficientStrings(); } + if (SubSystems & ssFCS) { + outstream << ", "; + outstream << FCS->GetComponentStrings(); + } if (SubSystems & ssGroundReactions) { outstream << ", "; outstream << GroundReactions->GetGroundReactionStrings(); @@ -294,6 +298,10 @@ void FGOutput::DelimitedOutput(string fname) outstream << ", "; outstream << Aerodynamics->GetCoefficientValues(); } + if (SubSystems & ssFCS) { + outstream << ", "; + outstream << FCS->GetComponentValues(); + } if (SubSystems & ssGroundReactions) { outstream << ", "; outstream << GroundReactions->GetGroundReactionValues(); diff --git a/src/FDM/JSBSim/FGPiston.cpp b/src/FDM/JSBSim/FGPiston.cpp index cdafc5e98..f258d387a 100644 --- a/src/FDM/JSBSim/FGPiston.cpp +++ b/src/FDM/JSBSim/FGPiston.cpp @@ -50,7 +50,6 @@ CLASS IMPLEMENTATION %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ FGPiston::FGPiston(FGFDMExec* exec, FGConfigFile* Eng_cfg) : FGEngine(exec), - CONVERT_CUBIC_INCHES_TO_METERS_CUBED(1.638706e-5), R_air(287.3), rho_fuel(800), // estimate calorific_value_fuel(47.3e6), @@ -85,7 +84,7 @@ FGPiston::FGPiston(FGFDMExec* exec, FGConfigFile* Eng_cfg) : FGEngine(exec), crank_counter = 0; EngineNumber = 0; OilTemp_degK = 298; - ManifoldPressure_inHg = Atmosphere->GetPressure() * 0.014138; // psf to in Hg + ManifoldPressure_inHg = Atmosphere->GetPressure() * psftoinhg; // psf to in Hg dt = State->Getdt(); @@ -150,7 +149,6 @@ double FGPiston::Calculate(double PowerRequired) T_amb = Atmosphere->GetTemperature() * (5.0 / 9.0); // convert from Rankine to Kelvin RPM = Propulsion->GetThruster(EngineNumber)->GetRPM(); - //if (RPM < IdleRPM) RPM = IdleRPM; // kludge IAS = Auxiliary->GetVcalibratedKTS(); @@ -216,49 +214,28 @@ void FGPiston::doEngineStartup(void) crank_counter = 0; } - //Check mode of engine operation - if (Cranking) { - crank_counter++; - if (RPM <= 480) { - // Do nothing !! - cranking power output is now handled in the doPower section - } else { - // consider making a horrible noise if the starter is engaged with - // the engine running - } - } + if (Cranking) crank_counter++; //Check mode of engine operation - // if ((!Running) && (spark) && (fuel) && (crank_counter > 120)) { - - if ((!Running) && (spark) && (fuel)) { - // start the engine if revs high enough + if (!Running && spark && fuel) { // start the engine if revs high enough if (Cranking) { - if ((RPM > 450) && (crank_counter > 175)) { - //Add a little delay to startup on the starter - Running = true; - } + if ((RPM > 450) && (crank_counter > 175)) // Add a little delay to startup + Running = true; // on the starter } else { - if (RPM > 450) { - Running = true; - //This allows us to in-air start when windmilling - } + if (RPM > 450) // This allows us to in-air start + Running = true; // when windmilling } } - if ( (Running) && ((!spark)||(!fuel)) ) { - // Cut the engine - // note that we only cut the power - the engine may continue to - // spin if the prop is in a moving airstream - Running = false; - } + // Cut the engine *power* - Note: the engine may continue to + // spin if the prop is in a moving airstream + + if ( Running && (!spark || !fuel) ) Running = false; - // And finally a last check for stalling + // Check for stalling (RPM = 0). if (Running) { - //Check if we have stalled the engine if (RPM == 0) { Running = false; } else if ((RPM <= 480) && (Cranking)) { - // Make sure the engine noise dosn't play if the engine won't - // start due to eg mixture lever pulled out. Running = false; } } @@ -290,7 +267,7 @@ void FGPiston::doManifoldPressure(void) ManifoldPressure_inHg = MinManifoldPressure_inHg + (Throttle * (MaxManifoldPressure_inHg - MinManifoldPressure_inHg)); } else { - ManifoldPressure_inHg = Atmosphere->GetPressure() * 0.014138; // psf to in Hg + ManifoldPressure_inHg = Atmosphere->GetPressure() * psftoinhg; // psf to in Hg } } @@ -311,7 +288,7 @@ void FGPiston::doAirFlow(void) { rho_air = p_amb / (R_air * T_amb); double rho_air_manifold = rho_air * ManifoldPressure_inHg / 29.6; - double displacement_SI = Displacement * CONVERT_CUBIC_INCHES_TO_METERS_CUBED; + double displacement_SI = Displacement * in3tom3; double swept_volume = (displacement_SI * (RPM/60)) / 2; double v_dot_air = swept_volume * volumetric_efficiency; m_dot_air = v_dot_air * rho_air_manifold; diff --git a/src/FDM/JSBSim/FGState.cpp b/src/FDM/JSBSim/FGState.cpp index 5759173d0..dd1a4ddca 100644 --- a/src/FDM/JSBSim/FGState.cpp +++ b/src/FDM/JSBSim/FGState.cpp @@ -567,6 +567,23 @@ void FGState::InitPropertyMaps(void) ParamNameToProp[ "FG_GEAR_CMD" ]="gear/gear-cmd-norm"; ParamNameToProp[ "FG_GEAR_POS" ]="gear/gear-pos-norm"; ParamNameToProp[ "FG_HYSTPARM" ]="aero/stall-hyst-norm"; + ParamNameToProp[ "AP_ELEVATOR_CMD" ]="jsbsim/ap/elevator_cmd"; + ParamNameToProp[ "AP_AILERON_CMD" ]="jsbsim/ap/aileron_cmd"; + ParamNameToProp[ "AP_RUDDER_CMD" ]="jsbsim/ap/rudder_cmd"; + ParamNameToProp[ "AP_THROTTLE_CMD" ]="jsbsim/ap/throttle_cmd"; + ParamNameToProp[ "AP_SET_ATTITUDE" ]="jsbsim/ap/set_attitude"; + ParamNameToProp[ "AP_SET_ALTITUDE" ]="jsbsim/ap/set_altitude"; + ParamNameToProp[ "AP_SET_HEADING" ]="jsbsim/ap/set_heading"; + ParamNameToProp[ "AP_SET_AIRSPEED" ]="jsbsim/ap/set_airspeed"; + ParamNameToProp[ "AP_ACQUIRE_ATTITUDE" ]="jsbsim/ap/acquire_attitude"; + ParamNameToProp[ "AP_ACQUIRE_ALTITUDE" ]="jsbsim/ap/acquire_altitude"; + ParamNameToProp[ "AP_ACQUIRE_HEADING" ]="jsbsim/ap/acquire_heading"; + ParamNameToProp[ "AP_ACQUIRE_AIRSPEED" ]="jsbsim/ap/acquire_aispeed"; + ParamNameToProp[ "AP_ATTITUDE_HOLD_ON" ]="jsbsim/ap/attitude_hold_on"; + ParamNameToProp[ "AP_ALTITUDE,_HOLD_ON" ]="jsbsim/ap/altitude_hold_on"; + ParamNameToProp[ "AP_HEADING_HOLD_ON" ]="jsbsim/ap/heading_hold_on"; + ParamNameToProp[ "AP_AIRSPEED_HOLD_ON" ]="jsbsim/ap/airspeed_hold_on"; + ParamNameToProp[ "AP_WINGSLEVEL_HOLD_ON" ]="jsbsim/ap/wingslevel_hold_on"; } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -- 2.39.5