fly/flightgear
1
0
Fork 0
Code Activity

Sync'ed with JSBSim: avoid the lag in <kinematic> while trimming. This allows the trim on ground for the P51 to succeed.

Browse source
This commit is contained in:
Bertrand Coconnier 2016-06-12 17:04:35 +02:00
parent dc7f065e08
commit a61c0bfdb4
2 changed files with 48 additions and 52 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

93
src/FDM/JSBSim/models/flight_control/FGKinemat.cpp
View file

@ -39,6 +39,7 @@ INCLUDES
#include "FGKinemat.h" #include "FGKinemat.h"
#include "input_output/FGXMLElement.h" #include "input_output/FGXMLElement.h"
#include "models/FGFCS.h"
#include <iostream> #include <iostream>
#include <cstdlib> #include <cstdlib>
@ -46,7 +47,7 @@ using namespace std;
namespace JSBSim { namespace JSBSim {
IDENT(IdSrc,"$Id: FGKinemat.cpp,v 1.15 2015/04/02 17:39:28 bcoconni Exp $"); IDENT(IdSrc,"$Id: FGKinemat.cpp,v 1.16 2016/06/12 14:47:46 bcoconni Exp $");
IDENT(IdHdr,ID_FLAPS); IDENT(IdHdr,ID_FLAPS);
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@ -62,7 +63,7 @@ FGKinemat::FGKinemat(FGFCS* fcs, Element* element) : FGFCSComponent(fcs, element
Detents.clear(); Detents.clear();
TransitionTimes.clear(); TransitionTimes.clear();
Output = OutputPct = 0; Output = 0;
DoScale = true; DoScale = true;
if (element->FindElement("noscale")) DoScale = false; if (element->FindElement("noscale")) DoScale = false;
@ -76,16 +77,14 @@ FGKinemat::FGKinemat(FGFCS* fcs, Element* element) : FGFCSComponent(fcs, element
TransitionTimes.push_back(tmpTime); TransitionTimes.push_back(tmpTime);
setting_element = traverse_element->FindNextElement("setting"); setting_element = traverse_element->FindNextElement("setting");
} }
NumDetents = Detents.size();
if (NumDetents <= 1) { if (Detents.size() <= 1) {
cerr << "Kinematic component " << Name cerr << "Kinematic component " << Name
<< " must have more than 1 setting element" << endl; << " must have more than 1 setting element" << endl;
exit(-1); exit(-1);
} }
FGFCSComponent::bind(); FGFCSComponent::bind();
// treenode->Tie("output-norm", this, &FGKinemat::GetOutputPct );
Debug(0); Debug(0);
} }
@ -105,58 +104,56 @@ bool FGKinemat::Run(void )
Input = InputNodes[0]->getDoubleValue() * InputSigns[0]; Input = InputNodes[0]->getDoubleValue() * InputSigns[0];
if (DoScale) Input *= Detents[NumDetents-1]; if (DoScale) Input *= Detents.back();
if (IsOutput) Output = OutputNodes[0]->getDoubleValue(); if (IsOutput) Output = OutputNodes[0]->getDoubleValue();
if (Input < Detents[0]) Input = Constrain(Detents.front(), Input, Detents.back());
Input = Detents[0];
else if (Detents[NumDetents-1] < Input)
Input = Detents[NumDetents-1];
// Process all detent intervals the movement traverses until either the if (fcs->GetTrimStatus())
// final value is reached or the time interval has finished. // When trimming the output must be reached in one step
while ( dt0 > 0.0 && !EqualToRoundoff(Input, Output) ) { Output = Input;
else {
// Process all detent intervals the movement traverses until either the
// final value is reached or the time interval has finished.
while ( dt0 > 0.0 && !EqualToRoundoff(Input, Output) ) {
// Find the area where Output is in // Find the area where Output is in
unsigned int ind; unsigned int ind;
for (ind = 1; (Input < Output) ? Detents[ind] < Output : Detents[ind] <= Output ; ++ind) for (ind = 1; (Input < Output) ? Detents[ind] < Output : Detents[ind] <= Output ; ++ind)
if (NumDetents <= ind) if (ind >= Detents.size())
break;
// A transition time of 0.0 means an infinite rate.
// The output is reached in one step
if (TransitionTimes[ind] <= 0.0) {
Output = Input;
break; break;
} else {
// Compute the rate in this area
double Rate = (Detents[ind] - Detents[ind-1])/TransitionTimes[ind];
// Compute the maximum input value inside this area
double ThisInput = Constrain(Detents[ind-1], Input, Detents[ind]);
// Compute the time to reach the value in ThisInput
double ThisDt = fabs((ThisInput-Output)/Rate);
// A transition time of 0.0 means an infinite rate. // and clip to the timestep size
// The output is reached in one step if (dt0 < ThisDt) {
if (TransitionTimes[ind] <= 0.0) { ThisDt = dt0;
Output = Input; if (Output < Input)
break; Output += ThisDt*Rate;
} else { else
// Compute the rate in this area Output -= ThisDt*Rate;
double Rate = (Detents[ind] - Detents[ind-1])/TransitionTimes[ind]; } else
// Compute the maximum input value inside this area // Handle this case separate to make shure the termination condition
double ThisInput = Input; // is met even in inexact arithmetics ...
if (ThisInput < Detents[ind-1]) ThisInput = Detents[ind-1]; Output = ThisInput;
if (Detents[ind] < ThisInput) ThisInput = Detents[ind];
// Compute the time to reach the value in ThisInput
double ThisDt = fabs((ThisInput-Output)/Rate);
// and clip to the timestep size dt0 -= ThisDt;
if (dt0 < ThisDt) { }
ThisDt = dt0;
if (Output < Input)
Output += ThisDt*Rate;
else
Output -= ThisDt*Rate;
} else
// Handle this case separate to make shure the termination condition
// is met even in inexact arithmetics ...
Output = ThisInput;
dt0 -= ThisDt;
} }
} }
OutputPct = (Output-Detents[0])/(Detents[NumDetents-1]-Detents[0]);
Clip(); Clip();
if (IsOutput) SetOutput(); if (IsOutput) SetOutput();
@ -189,8 +186,8 @@ void FGKinemat::Debug(int from)
if (debug_lvl & 1) { // Standard console startup message output if (debug_lvl & 1) { // Standard console startup message output
if (from == 0) { // Constructor if (from == 0) { // Constructor
cout << " INPUT: " << InputNodes[0]->GetName() << endl; cout << " INPUT: " << InputNodes[0]->GetName() << endl;
cout << " DETENTS: " << NumDetents << endl; cout << " DETENTS: " << Detents.size() << endl;
for (unsigned int i=0;i<NumDetents;i++) { for (unsigned int i=0;i<Detents.size();i++) {
cout << " " << Detents[i] << " " << TransitionTimes[i] << endl; cout << " " << Detents[i] << " " << TransitionTimes[i] << endl;
} }
if (IsOutput) { if (IsOutput) {

7
src/FDM/JSBSim/models/flight_control/FGKinemat.h
View file

@ -44,7 +44,7 @@ INCLUDES
DEFINITIONS DEFINITIONS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#define ID_FLAPS "$Id: FGKinemat.h,v 1.11 2015/03/28 14:49:02 bcoconni Exp $" #define ID_FLAPS "$Id: FGKinemat.h,v 1.12 2016/06/12 14:47:46 bcoconni Exp $"
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FORWARD DECLARATIONS FORWARD DECLARATIONS
@ -121,7 +121,8 @@ public:
/** Kinematic component output value. /** Kinematic component output value.
@return the current output of the kinematic object on the range of [0,1]. */ @return the current output of the kinematic object on the range of [0,1]. */
double GetOutputPct() const { return OutputPct; } double GetOutputPct() const
{ return (Output-Detents[0])/(Detents.back()-Detents[0]); }
/** Run method, overrides FGModel::Run(). /** Run method, overrides FGModel::Run().
@return false on success, true on failure. @return false on success, true on failure.
@ -131,8 +132,6 @@ public:
private: private:
std::vector<double> Detents; std::vector<double> Detents;
std::vector<double> TransitionTimes; std::vector<double> TransitionTimes;
size_t NumDetents;
double OutputPct;
bool DoScale; bool DoScale;
void Debug(int from); void Debug(int from);