Initial revision.

diff --git a/src/FDM/JSBSim/filtersjb/FGKinemat.cpp b/src/FDM/JSBSim/filtersjb/FGKinemat.cpp
new file mode 100644 (file)
index 0000000..934d64d
--- /dev/null
+++ b/src/FDM/JSBSim/filtersjb/FGKinemat.cpp
@@ -0,0 +1,184 @@
+/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+ 
+ Module:       FGKinemat.cpp
+ Author:       Tony Peden, for flight control system authored by Jon S. Berndt
+ Date started: 12/02/01
+ 
+ ------------- Copyright (C) 2000 Anthony K. Peden -------------
+ 
+ This program is free software; you can redistribute it and/or modify it under
+ the terms of the GNU General Public License as published by the Free Software
+ Foundation; either version 2 of the License, or (at your option) any later
+ version.
+ 
+ This program is distributed in the hope that it will be useful, but WITHOUT
+ ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+ FOR A PARTICULAR PURPOSE.  See the GNU General Public License for more
+ details.
+ 
+ You should have received a copy of the GNU General Public License along with
+ this program; if not, write to the Free Software Foundation, Inc., 59 Temple
+ Place - Suite 330, Boston, MA  02111-1307, USA.
+ 
+ Further information about the GNU General Public License can also be found on
+ the world wide web at http://www.gnu.org.
+ 
+FUNCTIONAL DESCRIPTION
+--------------------------------------------------------------------------------
+ 
+HISTORY
+--------------------------------------------------------------------------------
+ 
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+COMMENTS, REFERENCES,  and NOTES
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+ 
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+INCLUDES
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
+
+#include "FGKinemat.h"
+
+static const char *IdSrc = "$Id$";
+static const char *IdHdr = ID_FLAPS;
+
+/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+CLASS IMPLEMENTATION
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
+
+FGKinemat::FGKinemat(FGFCS* fcs, FGConfigFile* AC_cfg) : FGFCSComponent(fcs),
+AC_cfg(AC_cfg) {
+  string token;
+  double tmpDetent;
+  double tmpTime;
+
+  Detents.clear();
+  TransitionTimes.clear();
+
+  Type = AC_cfg->GetValue("TYPE");
+  Name = AC_cfg->GetValue("NAME");
+  AC_cfg->GetNextConfigLine();
+
+  while ((token = AC_cfg->GetValue()) != string("/COMPONENT")) {
+    *AC_cfg >> token;
+    if (token == "ID") {
+      *AC_cfg >> ID;
+    } else if (token == "INPUT") {
+      token = AC_cfg->GetValue("INPUT");
+      if (token.find("FG_") != token.npos) {
+        *AC_cfg >> token;
+        InputIdx = fcs->GetState()->GetParameterIndex(token);
+        InputType = itPilotAC;
+      }
+    } else if ( token == "DETENTS" ) {
+      *AC_cfg >> NumDetents;
+      for(int i=0;i<NumDetents;i++) {
+        *AC_cfg >> tmpDetent;
+        *AC_cfg >> tmpTime;
+        Detents.push_back(tmpDetent);
+        TransitionTimes.push_back(tmpTime);
+      }
+    } else if (token == "OUTPUT") {
+
+      IsOutput = true;
+      *AC_cfg >> sOutputIdx;
+      OutputIdx = fcs->GetState()->GetParameterIndex(sOutputIdx);
+    }
+  }
+
+  if (debug_lvl > 1) {
+    cout << "      ID: " << ID << endl;
+    cout << "      INPUT: " << InputIdx << endl;
+    cout << "      DETENTS: " << NumDetents << endl;
+    for(int i=0;i<NumDetents;i++) {
+      cout << "        " << Detents[i] << " " << TransitionTimes[i] << endl;
+    }
+    if (IsOutput) cout << "      OUTPUT: " <<sOutputIdx << endl;
+  }
+
+  if (debug_lvl & 2) cout << "Instantiated: FGKinemat" << endl;
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+FGKinemat::~FGKinemat()
+{
+  if (debug_lvl & 2) cout << "Destroyed:    FGKinemat" << endl;
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+bool FGKinemat::Run(void ) {
+  double dt=fcs->GetState()->Getdt();
+  double output_transit_rate=0;
+
+  FGFCSComponent::Run(); // call the base class for initialization of Input
+  InputCmd = Input*Detents[NumDetents-1];
+  OutputPos = fcs->GetState()->GetParameter(OutputIdx);
+
+  if(InputCmd < Detents[0]) {
+    fi=0;
+    InputCmd=Detents[0];
+    lastInputCmd=InputCmd;
+    OutputPos=Detents[0];
+    Output=OutputPos;
+  } else if(InputCmd > Detents[NumDetents-1]) {
+    fi=NumDetents-1;
+    InputCmd=Detents[fi];
+    lastInputCmd=InputCmd;
+    OutputPos=Detents[fi];
+    Output=OutputPos;
+  } else {
+    //cout << "FGKinemat::Run Handle: " << InputCmd << " Position: " << OutputPos << endl;
+    if(dt <= 0)
+      OutputPos=InputCmd;
+    else {
+      if(InputCmd != lastInputCmd) {
+
+        InTransit=1;
+      }
+      if(InTransit) {
+
+        //fprintf(stderr,"InputCmd: %g, OutputPos: %g\n",InputCmd,OutputPos);
+        fi=0;
+        while(Detents[fi] < InputCmd) {
+          fi++;
+        }
+        if(OutputPos < InputCmd) {
+          if(TransitionTimes[fi] > 0)
+            output_transit_rate=(Detents[fi] - Detents[fi-1])/TransitionTimes[fi];
+          else
+            output_transit_rate=(Detents[fi] - Detents[fi-1])/5;
+        } else {
+          if(TransitionTimes[fi+1] > 0)
+            output_transit_rate=(Detents[fi] - Detents[fi+1])/TransitionTimes[fi+1];
+          else
+            output_transit_rate=(Detents[fi] - Detents[fi+1])/5;
+        }
+        if(fabs(OutputPos - InputCmd) > dt*output_transit_rate)
+          OutputPos+=output_transit_rate*dt;
+        else {
+          InTransit=0;
+          OutputPos=InputCmd;
+        }
+      }
+    }
+    lastInputCmd=InputCmd;
+    Output=OutputPos;
+  }
+  //cout << "FGKinemat::Run Handle: " << InputCmd << " Position: " << OutputPos << " Output: " << Output << endl;
+  if (IsOutput) {
+    //cout << "Calling SetOutput()" << endl;
+    SetOutput();
+  }
+  //cout << "Out FGKinemat::Run" << endl;
+  return true;
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+void FGKinemat::Debug(void)
+{
+    //TODO: Add your source code here
+}
+

diff --git a/src/FDM/JSBSim/filtersjb/FGKinemat.h b/src/FDM/JSBSim/filtersjb/FGKinemat.h
new file mode 100644 (file)
index 0000000..bf04a4a
--- /dev/null
+++ b/src/FDM/JSBSim/filtersjb/FGKinemat.h
@@ -0,0 +1,88 @@
+/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+ 
+ Header:       FGKinemat.h
+ Author:       Tony Peden, for flight control system authored by Jon S. Berndt
+ Date started: 12/02/01 
+ 
+ ------------- Copyright (C) Anthony K. Peden -------------
+ 
+ This program is free software; you can redistribute it and/or modify it under
+ the terms of the GNU General Public License as published by the Free Software
+ Foundation; either version 2 of the License, or (at your option) any later
+ version.
+ 
+ This program is distributed in the hope that it will be useful, but WITHOUT
+ ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+ FOR A PARTICULAR PURPOSE.  See the GNU General Public License for more
+ details.
+ 
+ You should have received a copy of the GNU General Public License along with
+ this program; if not, write to the Free Software Foundation, Inc., 59 Temple
+ Place - Suite 330, Boston, MA  02111-1307, USA.
+ 
+ Further information about the GNU General Public License can also be found on
+ the world wide web at http://www.gnu.org.
+ 
+HISTORY
+--------------------------------------------------------------------------------
+ 
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+COMMENTS, REFERENCES,  and NOTES
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+ 
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+SENTRY
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
+
+#ifndef FGKinemat_H
+#define FGKinemat_H
+
+/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+INCLUDES
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
+
+#ifdef FGFS
+#  include <simgear/compiler.h>
+#  ifdef SG_HAVE_STD_INCLUDES
+#    include <vector>
+#  else
+#    include <vector.h>
+#  endif
+#else
+#  include <vector>
+#endif
+
+#include <string>
+#include "FGFCSComponent.h"
+#include "../FGConfigFile.h"
+
+/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+DEFINES
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
+
+#define ID_FLAPS "$Id$"
+
+/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+CLASS DECLARATION
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
+
+class FGKinemat  : public FGFCSComponent {
+public:
+  FGKinemat(FGFCS* fcs, FGConfigFile* AC_cfg);
+  ~FGKinemat();
+  bool Run (void );
+  
+private:
+  FGConfigFile* AC_cfg;
+  vector<double> Detents;
+  vector<double> TransitionTimes;
+  int NumDetents,fi;
+  double lastInputCmd;
+  double InputCmd;
+  double OutputPos;
+  bool  InTransit;
+
+  void Debug(void);
+};
+
+#endif