X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FFDM%2FJSBSim%2FFGFCS.cpp;h=c6ee9bd831788338d6bafc9eda2d540bc31e542c;hb=eb05a298e9a9a830b23b6337d812dafa48d39bba;hp=adcf66554548447cb3c5e622e5fdde556b1d500b;hpb=317d794f5c23a8952906d84a02cceda6626e854a;p=flightgear.git diff --git a/src/FDM/JSBSim/FGFCS.cpp b/src/FDM/JSBSim/FGFCS.cpp index adcf66554..c6ee9bd83 100644 --- a/src/FDM/JSBSim/FGFCS.cpp +++ b/src/FDM/JSBSim/FGFCS.cpp @@ -60,6 +60,10 @@ INCLUDES static const char *IdSrc = "$Id$"; static const char *IdHdr = ID_FCS; +#if defined(WIN32) && !defined(__CYGWIN__) +#define snprintf _snprintf +#endif + /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% CLASS IMPLEMENTATION %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ @@ -70,18 +74,21 @@ 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++) { + for (i=0;i<=NForms;i++) { DePos[i] = DaLPos[i] = DaRPos[i] = DrPos[i] = 0.0; DfPos[i] = DsbPos[i] = DspPos[i] = 0.0; } - for(i=0;iGetNode("fcs") ); + unbind( PropertyManager->GetNode("ap") ); + PropertyManager->Untie( "gear/gear-cmd-norm" ); + PropertyManager->Untie( "gear/gear-pos-norm" ); + ThrottleCmd.clear(); ThrottlePos.clear(); MixtureCmd.clear(); @@ -96,11 +108,12 @@ FGFCS::~FGFCS() PropAdvanceCmd.clear(); PropAdvance.clear(); + unsigned int i; - - unbind(); - for (i=0;iRun(); - if(DoNormalize) Normalize(); + for (i=0; iRun(); + eMode = mNone; + } + for (i=0; iRun(); + eMode = mNone; + } + if (DoNormalize) Normalize(); + + return false; } else { + return true; } - - return false; } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -162,7 +185,7 @@ void FGFCS::SetThrottlePos(int engineNum, double setting) //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -double FGFCS::GetThrottleCmd(int engineNum) +double FGFCS::GetThrottleCmd(int engineNum) const { if (engineNum < (int)ThrottlePos.size()) { if (engineNum < 0) { @@ -180,7 +203,7 @@ double FGFCS::GetThrottleCmd(int engineNum) //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -double FGFCS::GetThrottlePos(int engineNum) +double FGFCS::GetThrottlePos(int engineNum) const { if (engineNum < (int)ThrottlePos.size()) { if (engineNum < 0) { @@ -260,17 +283,64 @@ 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; + + Components=0; + // 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 \"" @@ -281,69 +351,97 @@ 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 - - for(i=0;isize(); i++) { - if(Components[i]->GetType() == "AEROSURFACE_SCALE" - || Components[i]->GetType() == "KINEMAT" ) { - if( Components[i]->GetOutputIdx() == FG_ELEVATOR_POS ) { + 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 ( Components[i]->GetOutputIdx() == FG_LEFT_AILERON_POS - || Components[i]->GetOutputIdx() == FG_AILERON_POS ) { + } else if ( nodeName == "left-aileron-pos-rad" + || nodeName == "aileron-pos-rad" ) { ToNormalize[iDaL]=i; - } else if ( Components[i]->GetOutputIdx() == FG_RIGHT_AILERON_POS ) { + } else if ( nodeName == "right-aileron-pos-rad" ) { ToNormalize[iDaR]=i; - } else if ( Components[i]->GetOutputIdx() == FG_RUDDER_POS ) { + } else if ( nodeName == "rudder-pos-rad" ) { ToNormalize[iDr]=i; - } else if ( Components[i]->GetOutputIdx() == FG_SPDBRAKE_POS ) { + } else if ( nodeName == "speedbrake-pos-rad" ) { ToNormalize[iDsb]=i; - } else if ( Components[i]->GetOutputIdx() == FG_SPOILERS_POS ) { + } else if ( nodeName == "spoiler-pos-rad" ) { ToNormalize[iDsp]=i; - } else if ( Components[i]->GetOutputIdx() == FG_FLAPS_POS ) { + } else if ( nodeName == "flap-pos-deg" ) { ToNormalize[iDf]=i; } } } + if (delimiter == "FLIGHT_CONTROL") bindModel(); + + eMode = mNone; + return true; } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -double FGFCS::GetComponentOutput(eParam 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; + } + return 0.0; } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -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; + } + return string(""); +} //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -370,11 +468,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; @@ -389,11 +493,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); } @@ -420,32 +529,32 @@ void FGFCS::Normalize(void) { //those that are have an index >=0 in the ToNormalize array //ToNormalize is filled in Load() - if( ToNormalize[iDe] > -1 ) { - DePos[ofNorm] = Components[ToNormalize[iDe]]->GetOutputPct(); + if ( ToNormalize[iDe] > -1 ) { + DePos[ofNorm] = FCSComponents[ToNormalize[iDe]]->GetOutputPct(); } - if( ToNormalize[iDaL] > -1 ) { - DaLPos[ofNorm] = Components[ToNormalize[iDaL]]->GetOutputPct(); + if ( ToNormalize[iDaL] > -1 ) { + DaLPos[ofNorm] = FCSComponents[ToNormalize[iDaL]]->GetOutputPct(); } - if( ToNormalize[iDaR] > -1 ) { - DaRPos[ofNorm] = Components[ToNormalize[iDaR]]->GetOutputPct(); + if ( ToNormalize[iDaR] > -1 ) { + DaRPos[ofNorm] = FCSComponents[ToNormalize[iDaR]]->GetOutputPct(); } - if( ToNormalize[iDr] > -1 ) { - DrPos[ofNorm] = Components[ToNormalize[iDr]]->GetOutputPct(); + if ( ToNormalize[iDr] > -1 ) { + DrPos[ofNorm] = FCSComponents[ToNormalize[iDr]]->GetOutputPct(); } - if( ToNormalize[iDsb] > -1 ) { - DsbPos[ofNorm] = Components[ToNormalize[iDsb]]->GetOutputPct(); + if ( ToNormalize[iDsb] > -1 ) { + DsbPos[ofNorm] = FCSComponents[ToNormalize[iDsb]]->GetOutputPct(); } - if( ToNormalize[iDsp] > -1 ) { - DspPos[ofNorm] = Components[ToNormalize[iDsp]]->GetOutputPct(); + if ( ToNormalize[iDsp] > -1 ) { + DspPos[ofNorm] = FCSComponents[ToNormalize[iDsp]]->GetOutputPct(); } - if( ToNormalize[iDf] > -1 ) { - DfPos[ofNorm] = Components[ToNormalize[iDf]]->GetOutputPct(); + if ( ToNormalize[iDf] > -1 ) { + DfPos[ofNorm] = FCSComponents[ToNormalize[iDf]]->GetOutputPct(); } DePos[ofMag] = fabs(DePos[ofRad]); @@ -460,7 +569,8 @@ void FGFCS::Normalize(void) { //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -void FGFCS::bind(void){ +void FGFCS::bind(void) +{ PropertyManager->Tie("fcs/aileron-cmd-norm", this, &FGFCS::GetDaCmd, &FGFCS::SetDaCmd, @@ -593,45 +703,152 @@ void FGFCS::bind(void){ &FGFCS::GetGearPos, &FGFCS::SetGearPos, true); -} -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + PropertyManager->Tie("ap/elevator_cmd", this, + &FGFCS::GetAPDeCmd, + &FGFCS::SetAPDeCmd, + true); + + PropertyManager->Tie("ap/aileron_cmd", this, + &FGFCS::GetAPDaCmd, + &FGFCS::SetAPDaCmd, + true); + + PropertyManager->Tie("ap/rudder_cmd", this, + &FGFCS::GetAPDrCmd, + &FGFCS::SetAPDrCmd, + true); + + PropertyManager->Tie("ap/throttle_cmd", this, + &FGFCS::GetAPThrottleCmd, + &FGFCS::SetAPThrottleCmd, + true); + + PropertyManager->Tie("ap/attitude_setpoint", this, + &FGFCS::GetAPAttitudeSetPt, + &FGFCS::SetAPAttitudeSetPt, + true); + + PropertyManager->Tie("ap/altitude_setpoint", this, + &FGFCS::GetAPAltitudeSetPt, + &FGFCS::SetAPAltitudeSetPt, + true); -void FGFCS::unbind(void){ - PropertyManager->Untie("fcs/aileron-cmd-norm"); - PropertyManager->Untie("fcs/elevator-cmd-norm"); - PropertyManager->Untie("fcs/rudder-cmd-norm"); - PropertyManager->Untie("fcs/flap-cmd-norm"); - PropertyManager->Untie("fcs/speedbrake-cmd-norm"); - PropertyManager->Untie("fcs/spoiler-cmd-norm"); - PropertyManager->Untie("fcs/pitch-trim-cmd-norm"); - PropertyManager->Untie("fcs/roll-trim-cmd-norm"); - PropertyManager->Untie("fcs/yaw-trim-cmd-norm"); - PropertyManager->Untie("gear/gear-cmd-norm"); - PropertyManager->Untie("fcs/left-aileron-pos-rad"); - PropertyManager->Untie("fcs/mag-left-aileron-pos-rad"); - PropertyManager->Untie("fcs/left-aileron-pos-norm"); - PropertyManager->Untie("fcs/right-aileron-pos-rad"); - PropertyManager->Untie("fcs/mag-right-aileron-pos-rad"); - PropertyManager->Untie("fcs/right-aileron-pos-norm"); - PropertyManager->Untie("fcs/elevator-pos-rad"); - PropertyManager->Untie("fcs/mag-elevator-pos-rad"); - PropertyManager->Untie("fcs/elevator-pos-norm"); - PropertyManager->Untie("fcs/rudder-pos-rad"); - PropertyManager->Untie("fcs/mag-rudder-pos-rad"); - PropertyManager->Untie("fcs/rudder-pos-norm"); - PropertyManager->Untie("fcs/flap-pos-deg"); - PropertyManager->Untie("fcs/flap-pos-norm"); - PropertyManager->Untie("fcs/speedbrake-pos-rad"); - PropertyManager->Untie("fcs/mag-speedbrake-pos-rad"); - PropertyManager->Untie("fcs/speedbrake-pos-norm"); - PropertyManager->Untie("fcs/spoiler-pos-rad"); - PropertyManager->Untie("fcs/mag-spoiler-pos-rad"); - PropertyManager->Untie("fcs/spoiler-pos-norm"); - PropertyManager->Untie("gear/gear-pos-norm"); + PropertyManager->Tie("ap/heading_setpoint", this, + &FGFCS::GetAPHeadingSetPt, + &FGFCS::SetAPHeadingSetPt, + true); + + PropertyManager->Tie("ap/airspeed_setpoint", this, + &FGFCS::GetAPAirspeedSetPt, + &FGFCS::SetAPAirspeedSetPt, + true); + + PropertyManager->Tie("ap/acquire_attitude", this, + &FGFCS::GetAPAcquireAttitude, + &FGFCS::SetAPAcquireAttitude, + true); + + PropertyManager->Tie("ap/acquire_altitude", this, + &FGFCS::GetAPAcquireAltitude, + &FGFCS::SetAPAcquireAltitude, + true); + + PropertyManager->Tie("ap/acquire_heading", this, + &FGFCS::GetAPAcquireHeading, + &FGFCS::SetAPAcquireHeading, + true); + + PropertyManager->Tie("ap/acquire_airspeed", this, + &FGFCS::GetAPAcquireAirspeed, + &FGFCS::SetAPAcquireAirspeed, + true); + + PropertyManager->Tie("ap/attitude_hold", this, + &FGFCS::GetAPAttitudeHold, + &FGFCS::SetAPAttitudeHold, + true); + + PropertyManager->Tie("ap/altitude_hold", this, + &FGFCS::GetAPAltitudeHold, + &FGFCS::SetAPAltitudeHold, + true); + + PropertyManager->Tie("ap/heading_hold", this, + &FGFCS::GetAPHeadingHold, + &FGFCS::SetAPHeadingHold, + true); + + PropertyManager->Tie("ap/airspeed_hold", this, + &FGFCS::GetAPAirspeedHold, + &FGFCS::SetAPAirspeedHold, + true); + + PropertyManager->Tie("ap/wingslevel_hold", this, + &FGFCS::GetAPWingsLevelHold, + &FGFCS::SetAPWingsLevelHold, + true); } +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +void FGFCS::bindModel(void) +{ + unsigned i; + char tmp[80]; + + + for (i=0; iTie( tmp,this,i, + &FGFCS::GetThrottleCmd, + &FGFCS::SetThrottleCmd, + true ); + snprintf(tmp,80,"fcs/throttle-pos-norm[%u]",i); + PropertyManager->Tie( tmp,this,i, + &FGFCS::GetThrottlePos, + &FGFCS::SetThrottlePos, + true ); + if ( MixtureCmd.size() > i ) { + snprintf(tmp,80,"fcs/mixture-cmd-norm[%u]",i); + PropertyManager->Tie( tmp,this,i, + &FGFCS::GetMixtureCmd, + &FGFCS::SetMixtureCmd, + true ); + snprintf(tmp,80,"fcs/mixture-pos-norm[%u]",i); + PropertyManager->Tie( tmp,this,i, + &FGFCS::GetMixturePos, + &FGFCS::SetMixturePos, + true ); + } + if ( PropAdvanceCmd.size() > i ) { + snprintf(tmp,80,"fcs/advance-cmd-norm[%u]",i); + PropertyManager->Tie( tmp,this,i, + &FGFCS::GetPropAdvanceCmd, + &FGFCS::SetPropAdvanceCmd, + true ); + snprintf(tmp,80,"fcs/advance-pos-norm[%u]",i); + PropertyManager->Tie( tmp,this,i, + &FGFCS::GetPropAdvance, + &FGFCS::SetPropAdvance, + true ); + } + } +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +void FGFCS::unbind(FGPropertyManager *node) +{ + int N = node->nChildren(); + for(int i=0;igetChild(i)->nChildren() ) { + unbind( (FGPropertyManager*)node->getChild(i) ); + } else if( node->getChild(i)->isTied() ) { + node->getChild(i)->untie(); + } + } +} //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% // The bitmasked value choices are as follows: