Initial revision.
This commit is contained in:
curt
parent
408742c48c
commit
5513830175
2 changed files with 272 additions and 0 deletions
184
src/FDM/JSBSim/filtersjb/FGKinemat.cpp
Normal file
184
src/FDM/JSBSim/filtersjb/FGKinemat.cpp
Normal file
|
|
@ -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
|
||||||
|
}
|
||||||
|
|
||||||
88
src/FDM/JSBSim/filtersjb/FGKinemat.h
Normal file
88
src/FDM/JSBSim/filtersjb/FGKinemat.h
Normal file
|
|
@ -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
|
||||||
Loading…
Add table
Reference in a new issue