Move InputValue class into SimGear
This commit is contained in:
parent
0f291ff417
commit
60d5bba0c5
15 changed files with 138 additions and 616 deletions
|
@ -190,42 +190,26 @@ void FGSubmodelMgr::update(double dt)
|
|||
|
||||
_contrail_trigger->setBoolValue(_user_alt_node->getDoubleValue() > contrail_altitude);
|
||||
|
||||
bool trigger = false;
|
||||
int i = -1;
|
||||
|
||||
submodel_iterator = submodels.begin();
|
||||
while (submodel_iterator != submodels.end()) {
|
||||
i++;
|
||||
|
||||
/*SG_LOG(SG_AI, SG_DEBUG,
|
||||
"Submodels: " << (*submodel_iterator)->id
|
||||
<< " name " << (*submodel_iterator)->name
|
||||
);*/
|
||||
|
||||
if ((*submodel_iterator)->trigger_node != 0) {
|
||||
_trigger_node = (*submodel_iterator)->trigger_node;
|
||||
for (auto sm : submodels) {
|
||||
bool trigger = false;
|
||||
if (sm->trigger_node) {
|
||||
trigger = _trigger_node->getBoolValue();
|
||||
//cout << (*submodel_iterator)->name << "trigger node found " << trigger << endl;
|
||||
} else {
|
||||
trigger = false;
|
||||
//cout << (*submodel_iterator)->name << " trigger node not found " << trigger << endl;
|
||||
}
|
||||
|
||||
if (trigger && (*submodel_iterator)->count != 0) {
|
||||
if (trigger && sm->count != 0) {
|
||||
//int id = (*submodel_iterator)->id;
|
||||
//const string& name = (*submodel_iterator)->name;
|
||||
|
||||
SG_LOG(SG_AI, SG_DEBUG,
|
||||
"Submodels release: " << (*submodel_iterator)->id
|
||||
<< " name " << (*submodel_iterator)->name
|
||||
<< " count " << (*submodel_iterator)->count
|
||||
<< " slaved " << (*submodel_iterator)->slaved);
|
||||
"Submodels release: " << sm->id
|
||||
<< " name " << sm->name
|
||||
<< " count " << sm->count
|
||||
<< " slaved " << sm->slaved);
|
||||
|
||||
release(*submodel_iterator, dt);
|
||||
} else
|
||||
(*submodel_iterator)->first_time = true;
|
||||
|
||||
++submodel_iterator;
|
||||
release(sm, dt);
|
||||
} else {
|
||||
sm->first_time = true; // reset first-time flag
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -537,40 +521,40 @@ void FGSubmodelMgr::setData(int id, const string& path, bool serviceable, const
|
|||
|
||||
if (a) {
|
||||
b = a->getNode("x-m");
|
||||
sm->x_offset = new FGXMLAutopilot::InputValue(*prop_root, b ? *b : n);
|
||||
sm->x_offset = new simgear::Value(*prop_root, b ? *b : n);
|
||||
|
||||
b = a->getNode("y-m");
|
||||
sm->y_offset = new FGXMLAutopilot::InputValue(*prop_root, b ? *b : n);
|
||||
sm->y_offset = new simgear::Value(*prop_root, b ? *b : n);
|
||||
|
||||
b = a->getNode("z-m");
|
||||
sm->z_offset = new FGXMLAutopilot::InputValue(*prop_root, b ? *b : n);
|
||||
sm->z_offset = new simgear::Value(*prop_root, b ? *b : n);
|
||||
|
||||
b = a->getNode("heading-deg");
|
||||
sm->yaw_offset = new FGXMLAutopilot::InputValue(*prop_root, b ? *b : n);
|
||||
sm->yaw_offset = new simgear::Value(*prop_root, b ? *b : n);
|
||||
|
||||
b = a->getNode("pitch-deg");
|
||||
sm->pitch_offset = new FGXMLAutopilot::InputValue(*prop_root, b ? *b : n);
|
||||
sm->pitch_offset = new simgear::Value(*prop_root, b ? *b : n);
|
||||
} else {
|
||||
bool old = false;
|
||||
|
||||
b = entry_node->getNode("x-offset");
|
||||
sm->x_offset = new FGXMLAutopilot::InputValue(*prop_root, b ? *b : n);
|
||||
sm->x_offset = new simgear::Value(*prop_root, b ? *b : n);
|
||||
if (b) old = true;
|
||||
|
||||
b = entry_node->getNode("y-offset");
|
||||
sm->y_offset = new FGXMLAutopilot::InputValue(*prop_root, b ? *b : n);
|
||||
sm->y_offset = new simgear::Value(*prop_root, b ? *b : n);
|
||||
if (b) old = true;
|
||||
|
||||
b = entry_node->getNode("z-offset");
|
||||
sm->z_offset = new FGXMLAutopilot::InputValue(*prop_root, b ? *b : n);
|
||||
sm->z_offset = new simgear::Value(*prop_root, b ? *b : n);
|
||||
if (b) old = true;
|
||||
|
||||
b = entry_node->getNode("yaw-offset");
|
||||
sm->yaw_offset = new FGXMLAutopilot::InputValue(*prop_root, b ? *b : n);
|
||||
sm->yaw_offset = new simgear::Value(*prop_root, b ? *b : n);
|
||||
if (b) old = true;
|
||||
|
||||
b = entry_node->getNode("pitch-offset");
|
||||
sm->pitch_offset = new FGXMLAutopilot::InputValue(*prop_root, b ? *b : n);
|
||||
sm->pitch_offset = new simgear::Value(*prop_root, b ? *b : n);
|
||||
if (b) old = true;
|
||||
|
||||
if (old) {
|
||||
|
@ -583,11 +567,11 @@ void FGSubmodelMgr::setData(int id, const string& path, bool serviceable, const
|
|||
|
||||
// Maximum azimuth randomness error in degrees
|
||||
b = a->getNode("azimuth");
|
||||
sm->azimuth_error = new FGXMLAutopilot::InputValue(*prop_root, b ? *b : n);
|
||||
sm->azimuth_error = new simgear::Value(*prop_root, b ? *b : n);
|
||||
|
||||
// Maximum elevation randomness error in degrees
|
||||
b = a->getNode("elevation");
|
||||
sm->elevation_error = new FGXMLAutopilot::InputValue(*prop_root, b ? *b : n);
|
||||
sm->elevation_error = new simgear::Value(*prop_root, b ? *b : n);
|
||||
|
||||
// Randomness of Cd (plus or minus)
|
||||
b = a->getNode("cd");
|
||||
|
@ -595,7 +579,7 @@ void FGSubmodelMgr::setData(int id, const string& path, bool serviceable, const
|
|||
b = a->getNode("cd", true);
|
||||
b->setDoubleValue(0.1);
|
||||
}
|
||||
sm->cd_randomness = new FGXMLAutopilot::InputValue(*prop_root, *b);
|
||||
sm->cd_randomness = new simgear::Value(*prop_root, *b);
|
||||
|
||||
// Randomness of life (plus or minus)
|
||||
b = a->getNode("life");
|
||||
|
@ -603,7 +587,7 @@ void FGSubmodelMgr::setData(int id, const string& path, bool serviceable, const
|
|||
b = a->getNode("life", true);
|
||||
b->setDoubleValue(0.5);
|
||||
}
|
||||
sm->life_randomness = new FGXMLAutopilot::InputValue(*prop_root, *b);
|
||||
sm->life_randomness = new simgear::Value(*prop_root, *b);
|
||||
|
||||
if (sm->contents_node != 0)
|
||||
sm->contents = sm->contents_node->getDoubleValue();
|
||||
|
|
|
@ -12,7 +12,7 @@
|
|||
#include <simgear/props/props.hxx>
|
||||
#include <simgear/structure/subsystem_mgr.hxx>
|
||||
|
||||
#include <Autopilot/inputvalue.hxx>
|
||||
#include <simgear/misc/inputvalue.hxx>
|
||||
|
||||
class FGAIBase;
|
||||
class FGAIManager;
|
||||
|
@ -37,18 +37,18 @@ public:
|
|||
double timer;
|
||||
int count;
|
||||
bool offsets_in_meter;
|
||||
FGXMLAutopilot::InputValue_ptr x_offset;
|
||||
FGXMLAutopilot::InputValue_ptr y_offset;
|
||||
FGXMLAutopilot::InputValue_ptr z_offset;
|
||||
FGXMLAutopilot::InputValue_ptr yaw_offset;
|
||||
FGXMLAutopilot::InputValue_ptr pitch_offset;
|
||||
simgear::Value_ptr x_offset;
|
||||
simgear::Value_ptr y_offset;
|
||||
simgear::Value_ptr z_offset;
|
||||
simgear::Value_ptr yaw_offset;
|
||||
simgear::Value_ptr pitch_offset;
|
||||
double drag_area;
|
||||
double life;
|
||||
double buoyancy;
|
||||
FGXMLAutopilot::InputValue_ptr azimuth_error;
|
||||
FGXMLAutopilot::InputValue_ptr elevation_error;
|
||||
FGXMLAutopilot::InputValue_ptr cd_randomness;
|
||||
FGXMLAutopilot::InputValue_ptr life_randomness;
|
||||
simgear::Value_ptr azimuth_error;
|
||||
simgear::Value_ptr elevation_error;
|
||||
simgear::Value_ptr cd_randomness;
|
||||
simgear::Value_ptr life_randomness;
|
||||
bool wind;
|
||||
bool first_time;
|
||||
double cd;
|
||||
|
|
|
@ -8,7 +8,6 @@ set(SOURCES
|
|||
digitalcomponent.cxx
|
||||
digitalfilter.cxx
|
||||
flipflop.cxx
|
||||
inputvalue.cxx
|
||||
logic.cxx
|
||||
pidcontroller.cxx
|
||||
pisimplecontroller.cxx
|
||||
|
@ -24,7 +23,6 @@ set(HEADERS
|
|||
digitalcomponent.hxx
|
||||
digitalfilter.hxx
|
||||
flipflop.hxx
|
||||
inputvalue.hxx
|
||||
logic.hxx
|
||||
pidcontroller.hxx
|
||||
pisimplecontroller.hxx
|
||||
|
@ -32,4 +30,4 @@ set(HEADERS
|
|||
route_mgr.hxx
|
||||
)
|
||||
|
||||
flightgear_component(Autopilot "${SOURCES}" "${HEADERS}")
|
||||
flightgear_component(Autopilot "${SOURCES}" "${HEADERS}")
|
||||
|
|
|
@ -90,31 +90,31 @@ bool AnalogComponent::configure( SGPropertyNode& cfg_node,
|
|||
|
||||
if( cfg_name == "input" )
|
||||
{
|
||||
_valueInput.push_back( new InputValue(prop_root, cfg_node) );
|
||||
_valueInput.push_back(new simgear::Value(prop_root, cfg_node));
|
||||
return true;
|
||||
}
|
||||
|
||||
if( cfg_name == "reference" )
|
||||
{
|
||||
_referenceInput.push_back( new InputValue(prop_root, cfg_node) );
|
||||
_referenceInput.push_back(new simgear::Value(prop_root, cfg_node));
|
||||
return true;
|
||||
}
|
||||
|
||||
if( cfg_name == "min" || cfg_name == "u_min" )
|
||||
{
|
||||
_minInput.push_back( new InputValue(prop_root, cfg_node) );
|
||||
_minInput.push_back(new simgear::Value(prop_root, cfg_node));
|
||||
return true;
|
||||
}
|
||||
|
||||
if( cfg_name == "max" || cfg_name == "u_max" )
|
||||
{
|
||||
_maxInput.push_back( new InputValue(prop_root, cfg_node) );
|
||||
_maxInput.push_back(new simgear::Value(prop_root, cfg_node));
|
||||
return true;
|
||||
}
|
||||
|
||||
if( cfg_name == "period" )
|
||||
{
|
||||
_periodical = new PeriodicalValue(prop_root, cfg_node);
|
||||
_periodical = new simgear::PeriodicalValue(prop_root, cfg_node);
|
||||
return true;
|
||||
}
|
||||
|
||||
|
|
|
@ -23,7 +23,7 @@
|
|||
#ifndef __ANALOGCOMPONENT_HXX
|
||||
#define __ANALOGCOMPONENT_HXX 1
|
||||
|
||||
#include "inputvalue.hxx"
|
||||
#include <simgear/misc/inputvalue.hxx>
|
||||
#include "component.hxx"
|
||||
|
||||
namespace FGXMLAutopilot {
|
||||
|
@ -54,27 +54,27 @@ protected:
|
|||
/**
|
||||
* @brief the value input
|
||||
*/
|
||||
InputValueList _valueInput;
|
||||
simgear::ValueList _valueInput;
|
||||
|
||||
/**
|
||||
* @brief the reference input
|
||||
*/
|
||||
InputValueList _referenceInput;
|
||||
simgear::ValueList _referenceInput;
|
||||
|
||||
/**
|
||||
* @brief the minimum output clamp input
|
||||
*/
|
||||
InputValueList _minInput;
|
||||
simgear::ValueList _minInput;
|
||||
|
||||
/**
|
||||
* @brief the maximum output clamp input
|
||||
*/
|
||||
InputValueList _maxInput;
|
||||
simgear::ValueList _maxInput;
|
||||
|
||||
/**
|
||||
* @brief the configuration for periodical outputs
|
||||
*/
|
||||
PeriodicalValue_ptr _periodical;
|
||||
simgear::PeriodicalValue_ptr _periodical;
|
||||
|
||||
/**
|
||||
* @brief A constructor for an analog component. Call configure() to
|
||||
|
@ -138,7 +138,7 @@ protected:
|
|||
}
|
||||
|
||||
public:
|
||||
const PeriodicalValue * getPeriodicalValue() const { return _periodical; }
|
||||
const simgear::PeriodicalValue_ptr getPeriodicalValue() const { return _periodical; }
|
||||
|
||||
/**
|
||||
Add to <props> all properties that are used by this component. Similar to
|
||||
|
@ -150,9 +150,10 @@ public:
|
|||
inline void AnalogComponent::disabled( double dt )
|
||||
{
|
||||
if( _feedback_if_disabled && ! _output_list.empty() ) {
|
||||
InputValue * input;
|
||||
if( (input = _valueInput.get_active() ) != NULL )
|
||||
input->set_value( _output_list[0]->getDoubleValue() );
|
||||
auto activeInput = _valueInput.get_active();
|
||||
if (activeInput) {
|
||||
activeInput->set_value(_output_list[0]->getDoubleValue());
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
@ -63,10 +63,10 @@ class DigitalFilterImplementation:
|
|||
/* --------------------------------------------------------------------------------- */
|
||||
class GainFilterImplementation : public DigitalFilterImplementation {
|
||||
protected:
|
||||
InputValueList _gainInput;
|
||||
bool configure( SGPropertyNode& cfg_node,
|
||||
const std::string& cfg_name,
|
||||
SGPropertyNode& prop_root );
|
||||
simgear::ValueList _gainInput;
|
||||
bool configure(SGPropertyNode& cfg_node,
|
||||
const std::string& cfg_name,
|
||||
SGPropertyNode& prop_root);
|
||||
public:
|
||||
GainFilterImplementation() : _gainInput(1.0) {}
|
||||
double compute( double dt, double input );
|
||||
|
@ -82,7 +82,7 @@ public:
|
|||
};
|
||||
|
||||
class DerivativeFilterImplementation : public GainFilterImplementation {
|
||||
InputValueList _TfInput;
|
||||
simgear::ValueList _TfInput;
|
||||
double _input_1;
|
||||
bool configure( SGPropertyNode& cfg_node,
|
||||
const std::string& cfg_name,
|
||||
|
@ -100,12 +100,12 @@ public:
|
|||
|
||||
class ExponentialFilterImplementation : public GainFilterImplementation {
|
||||
protected:
|
||||
InputValueList _TfInput;
|
||||
bool configure( SGPropertyNode& cfg_node,
|
||||
const std::string& cfg_name,
|
||||
SGPropertyNode& prop_root );
|
||||
bool _isSecondOrder;
|
||||
double _output_1, _output_2;
|
||||
simgear::ValueList _TfInput;
|
||||
bool configure(SGPropertyNode& cfg_node,
|
||||
const std::string& cfg_name,
|
||||
SGPropertyNode& prop_root);
|
||||
bool _isSecondOrder;
|
||||
double _output_1, _output_2;
|
||||
public:
|
||||
ExponentialFilterImplementation();
|
||||
double compute( double dt, double input );
|
||||
|
@ -119,12 +119,12 @@ public:
|
|||
|
||||
class MovingAverageFilterImplementation : public DigitalFilterImplementation {
|
||||
protected:
|
||||
InputValueList _samplesInput;
|
||||
double _output_1;
|
||||
std::deque <double> _inputQueue;
|
||||
bool configure( SGPropertyNode& cfg_node,
|
||||
const std::string& cfg_name,
|
||||
SGPropertyNode& prop_root );
|
||||
simgear::ValueList _samplesInput;
|
||||
double _output_1;
|
||||
std::deque<double> _inputQueue;
|
||||
bool configure(SGPropertyNode& cfg_node,
|
||||
const std::string& cfg_name,
|
||||
SGPropertyNode& prop_root);
|
||||
public:
|
||||
MovingAverageFilterImplementation();
|
||||
double compute( double dt, double input );
|
||||
|
@ -138,7 +138,7 @@ public:
|
|||
class NoiseSpikeFilterImplementation : public DigitalFilterImplementation {
|
||||
protected:
|
||||
double _output_1;
|
||||
InputValueList _rateOfChangeInput;
|
||||
simgear::ValueList _rateOfChangeInput;
|
||||
bool configure( SGPropertyNode& cfg_node,
|
||||
const std::string& cfg_name,
|
||||
SGPropertyNode& prop_root );
|
||||
|
@ -155,8 +155,8 @@ public:
|
|||
class RateLimitFilterImplementation : public DigitalFilterImplementation {
|
||||
protected:
|
||||
double _output_1;
|
||||
InputValueList _rateOfChangeMax;
|
||||
InputValueList _rateOfChangeMin ;
|
||||
simgear::ValueList _rateOfChangeMax;
|
||||
simgear::ValueList _rateOfChangeMin;
|
||||
bool configure( SGPropertyNode& cfg_node,
|
||||
const std::string& cfg_name,
|
||||
SGPropertyNode& prop_root );
|
||||
|
@ -173,14 +173,14 @@ public:
|
|||
|
||||
class IntegratorFilterImplementation : public GainFilterImplementation {
|
||||
protected:
|
||||
InputValueList _TfInput;
|
||||
InputValueList _minInput;
|
||||
InputValueList _maxInput;
|
||||
double _input_1;
|
||||
double _output_1;
|
||||
bool configure( SGPropertyNode& cfg_node,
|
||||
const std::string& cfg_name,
|
||||
SGPropertyNode& prop_root );
|
||||
simgear::ValueList _TfInput;
|
||||
simgear::ValueList _minInput;
|
||||
simgear::ValueList _maxInput;
|
||||
double _input_1;
|
||||
double _output_1;
|
||||
bool configure(SGPropertyNode& cfg_node,
|
||||
const std::string& cfg_name,
|
||||
SGPropertyNode& prop_root);
|
||||
public:
|
||||
IntegratorFilterImplementation();
|
||||
double compute( double dt, double input );
|
||||
|
@ -197,15 +197,15 @@ public:
|
|||
// integrates x" + ax' + bx + c = 0
|
||||
class DampedOscillationFilterImplementation : public GainFilterImplementation {
|
||||
protected:
|
||||
InputValueList _aInput;
|
||||
InputValueList _bInput;
|
||||
InputValueList _cInput;
|
||||
double _x2;
|
||||
double _x1;
|
||||
double _x0;
|
||||
bool configure( SGPropertyNode& cfg_node,
|
||||
const std::string& cfg_name,
|
||||
SGPropertyNode& prop_root );
|
||||
simgear::ValueList _aInput;
|
||||
simgear::ValueList _bInput;
|
||||
simgear::ValueList _cInput;
|
||||
double _x2;
|
||||
double _x1;
|
||||
double _x0;
|
||||
bool configure(SGPropertyNode& cfg_node,
|
||||
const std::string& cfg_name,
|
||||
SGPropertyNode& prop_root);
|
||||
public:
|
||||
DampedOscillationFilterImplementation();
|
||||
double compute( double dt, double input );
|
||||
|
@ -221,12 +221,12 @@ public:
|
|||
|
||||
class HighPassFilterImplementation : public GainFilterImplementation {
|
||||
protected:
|
||||
InputValueList _TfInput;
|
||||
double _input_1;
|
||||
double _output_1;
|
||||
bool configure( SGPropertyNode& cfg_node,
|
||||
const std::string& cfg_name,
|
||||
SGPropertyNode& prop_root );
|
||||
simgear::ValueList _TfInput;
|
||||
double _input_1;
|
||||
double _output_1;
|
||||
bool configure(SGPropertyNode& cfg_node,
|
||||
const std::string& cfg_name,
|
||||
SGPropertyNode& prop_root);
|
||||
public:
|
||||
HighPassFilterImplementation();
|
||||
double compute( double dt, double input );
|
||||
|
@ -239,13 +239,13 @@ public:
|
|||
};
|
||||
class LeadLagFilterImplementation : public GainFilterImplementation {
|
||||
protected:
|
||||
InputValueList _TfaInput;
|
||||
InputValueList _TfbInput;
|
||||
double _input_1;
|
||||
double _output_1;
|
||||
bool configure( SGPropertyNode& cfg_node,
|
||||
const std::string& cfg_name,
|
||||
SGPropertyNode& prop_root );
|
||||
simgear::ValueList _TfaInput;
|
||||
simgear::ValueList _TfbInput;
|
||||
double _input_1;
|
||||
double _output_1;
|
||||
bool configure(SGPropertyNode& cfg_node,
|
||||
const std::string& cfg_name,
|
||||
SGPropertyNode& prop_root);
|
||||
public:
|
||||
LeadLagFilterImplementation();
|
||||
double compute( double dt, double input );
|
||||
|
@ -261,7 +261,7 @@ public:
|
|||
class CoherentNoiseFilterImplementation : public DigitalFilterImplementation
|
||||
{
|
||||
protected:
|
||||
InputValueList _amplitude;
|
||||
simgear::ValueList _amplitude;
|
||||
|
||||
std::vector<double> _discreteValues;
|
||||
bool _absoluteVal = false;
|
||||
|
@ -306,7 +306,7 @@ bool GainFilterImplementation::configure( SGPropertyNode& cfg_node,
|
|||
SGPropertyNode& prop_root )
|
||||
{
|
||||
if (cfg_name == "gain" ) {
|
||||
_gainInput.push_back( new InputValue(prop_root, cfg_node, 1) );
|
||||
_gainInput.push_back(new simgear::Value(prop_root, cfg_node, 1));
|
||||
return true;
|
||||
}
|
||||
|
||||
|
@ -347,7 +347,7 @@ bool DerivativeFilterImplementation::configure( SGPropertyNode& cfg_node,
|
|||
return true;
|
||||
|
||||
if (cfg_name == "filter-time" ) {
|
||||
_TfInput.push_back( new InputValue(prop_root, cfg_node, 1) );
|
||||
_TfInput.push_back(new simgear::Value(prop_root, cfg_node, 1));
|
||||
return true;
|
||||
}
|
||||
|
||||
|
@ -405,7 +405,7 @@ bool MovingAverageFilterImplementation::configure( SGPropertyNode& cfg_node,
|
|||
SGPropertyNode& prop_root )
|
||||
{
|
||||
if (cfg_name == "samples" ) {
|
||||
_samplesInput.push_back( new InputValue(prop_root, cfg_node, 1) );
|
||||
_samplesInput.push_back(new simgear::Value(prop_root, cfg_node, 1));
|
||||
return true;
|
||||
}
|
||||
|
||||
|
@ -431,7 +431,7 @@ double NoiseSpikeFilterImplementation::compute( double dt, double input )
|
|||
if( fabs(delta) <= SGLimitsd::min() ) return input; // trivial
|
||||
|
||||
double maxChange = _rateOfChangeInput.get_value() * dt;
|
||||
const PeriodicalValue * periodical = _digitalFilter->getPeriodicalValue();
|
||||
const auto periodical = _digitalFilter->getPeriodicalValue();
|
||||
if( periodical ) delta = periodical->normalizeSymmetric( delta );
|
||||
|
||||
if( fabs(delta) <= maxChange )
|
||||
|
@ -446,7 +446,7 @@ bool NoiseSpikeFilterImplementation::configure( SGPropertyNode& cfg_node,
|
|||
SGPropertyNode& prop_root )
|
||||
{
|
||||
if (cfg_name == "max-rate-of-change" ) {
|
||||
_rateOfChangeInput.push_back( new InputValue(prop_root, cfg_node, 1) );
|
||||
_rateOfChangeInput.push_back(new simgear::Value(prop_root, cfg_node, 1));
|
||||
return true;
|
||||
}
|
||||
|
||||
|
@ -491,11 +491,11 @@ bool RateLimitFilterImplementation::configure( SGPropertyNode& cfg_node,
|
|||
{
|
||||
// std::cout << "RateLimitFilterImplementation " << cfg_name << std::endl;
|
||||
if (cfg_name == "max-rate-of-change" ) {
|
||||
_rateOfChangeMax.push_back( new InputValue(prop_root, cfg_node, 1) );
|
||||
_rateOfChangeMax.push_back(new simgear::Value(prop_root, cfg_node, 1));
|
||||
return true;
|
||||
}
|
||||
if (cfg_name == "min-rate-of-change" ) {
|
||||
_rateOfChangeMin.push_back( new InputValue(prop_root, cfg_node, 1) );
|
||||
_rateOfChangeMin.push_back(new simgear::Value(prop_root, cfg_node, 1));
|
||||
return true;
|
||||
}
|
||||
|
||||
|
@ -549,7 +549,7 @@ bool ExponentialFilterImplementation::configure( SGPropertyNode& cfg_node,
|
|||
return true;
|
||||
|
||||
if (cfg_name == "filter-time" ) {
|
||||
_TfInput.push_back( new InputValue(prop_root, cfg_node, 1) );
|
||||
_TfInput.push_back(new simgear::Value(prop_root, cfg_node, 1));
|
||||
return true;
|
||||
}
|
||||
|
||||
|
@ -583,11 +583,11 @@ bool IntegratorFilterImplementation::configure( SGPropertyNode& cfg_node,
|
|||
return true;
|
||||
|
||||
if (cfg_name == "u_min" ) {
|
||||
_minInput.push_back( new InputValue(prop_root, cfg_node, 1) );
|
||||
_minInput.push_back(new simgear::Value(prop_root, cfg_node, 1));
|
||||
return true;
|
||||
}
|
||||
if (cfg_name == "u_max" ) {
|
||||
_maxInput.push_back( new InputValue(prop_root, cfg_node, 1) );
|
||||
_maxInput.push_back(new simgear::Value(prop_root, cfg_node, 1));
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
|
@ -625,15 +625,15 @@ bool DampedOscillationFilterImplementation::configure( SGPropertyNode& cfg_node,
|
|||
return true;
|
||||
|
||||
if (cfg_name == "a" ) {
|
||||
_aInput.push_back( new InputValue(prop_root, cfg_node, 1) );
|
||||
_aInput.push_back(new simgear::Value(prop_root, cfg_node, 1));
|
||||
return true;
|
||||
}
|
||||
if (cfg_name == "b" ) {
|
||||
_bInput.push_back( new InputValue(prop_root, cfg_node, 1) );
|
||||
_bInput.push_back(new simgear::Value(prop_root, cfg_node, 1));
|
||||
return true;
|
||||
}
|
||||
if (cfg_name == "c" ) {
|
||||
_cInput.push_back( new InputValue(prop_root, cfg_node, 1) );
|
||||
_cInput.push_back(new simgear::Value(prop_root, cfg_node, 1));
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
|
@ -724,7 +724,7 @@ bool HighPassFilterImplementation::configure( SGPropertyNode& cfg_node,
|
|||
return true;
|
||||
|
||||
if (cfg_name == "filter-time" ) {
|
||||
_TfInput.push_back( new InputValue(prop_root, cfg_node, 1) );
|
||||
_TfInput.push_back(new simgear::Value(prop_root, cfg_node, 1));
|
||||
return true;
|
||||
}
|
||||
|
||||
|
@ -774,11 +774,11 @@ bool LeadLagFilterImplementation::configure( SGPropertyNode& cfg_node,
|
|||
return true;
|
||||
|
||||
if (cfg_name == "filter-time-a" ) {
|
||||
_TfaInput.push_back( new InputValue(prop_root, cfg_node, 1) );
|
||||
_TfaInput.push_back(new simgear::Value(prop_root, cfg_node, 1));
|
||||
return true;
|
||||
}
|
||||
if (cfg_name == "filter-time-b" ) {
|
||||
_TfbInput.push_back( new InputValue(prop_root, cfg_node, 1) );
|
||||
_TfbInput.push_back(new simgear::Value(prop_root, cfg_node, 1));
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
|
@ -833,7 +833,7 @@ bool CoherentNoiseFilterImplementation::configure(SGPropertyNode& cfg_node,
|
|||
}
|
||||
|
||||
if (cfg_name == "amplitude") {
|
||||
_amplitude.push_back(new InputValue(prop_root, cfg_node, 1.0));
|
||||
_amplitude.push_back(new simgear::Value(prop_root, cfg_node, 1.0));
|
||||
return true;
|
||||
}
|
||||
|
||||
|
|
|
@ -21,7 +21,7 @@
|
|||
|
||||
#include "flipflop.hxx"
|
||||
#include "functor.hxx"
|
||||
#include "inputvalue.hxx"
|
||||
#include <simgear/misc/inputvalue.hxx>
|
||||
#include <Main/fg_props.hxx>
|
||||
|
||||
using std::map;
|
||||
|
@ -267,7 +267,7 @@ protected:
|
|||
virtual bool configure( SGPropertyNode& cfg_node,
|
||||
const std::string& cfg_name,
|
||||
SGPropertyNode& prop_root );
|
||||
InputValueList _time;
|
||||
simgear::ValueList _time;
|
||||
double _t;
|
||||
|
||||
bool isConfigProperty(const std::string& cfg_name) const override
|
||||
|
@ -305,7 +305,7 @@ bool MonoFlopImplementation::configure( SGPropertyNode& cfg_node,
|
|||
return true;
|
||||
|
||||
if (cfg_name == "time") {
|
||||
_time.push_back( new InputValue(prop_root, cfg_node) );
|
||||
_time.push_back(new simgear::Value(prop_root, cfg_node));
|
||||
return true;
|
||||
}
|
||||
|
||||
|
|
|
@ -1,310 +0,0 @@
|
|||
// inputvalue.hxx - provide input to autopilot components
|
||||
//
|
||||
// Written by Torsten Dreyer
|
||||
// Copyright (C) 2010 Torsten Dreyer - Torsten (at) t3r (dot) de
|
||||
//
|
||||
// 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., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
|
||||
//
|
||||
|
||||
#include <cstdlib>
|
||||
|
||||
#include "inputvalue.hxx"
|
||||
|
||||
#include <simgear/misc/strutils.hxx>
|
||||
|
||||
using namespace FGXMLAutopilot;
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
PeriodicalValue::PeriodicalValue( SGPropertyNode& prop_root,
|
||||
SGPropertyNode& cfg )
|
||||
{
|
||||
SGPropertyNode_ptr minNode = cfg.getChild( "min" );
|
||||
SGPropertyNode_ptr maxNode = cfg.getChild( "max" );
|
||||
if( !minNode || !maxNode )
|
||||
{
|
||||
SG_LOG
|
||||
(
|
||||
SG_AUTOPILOT,
|
||||
SG_ALERT,
|
||||
"periodical defined, but no <min> and/or <max> tag. Period ignored."
|
||||
);
|
||||
}
|
||||
else
|
||||
{
|
||||
minPeriod = new InputValue(prop_root, *minNode);
|
||||
maxPeriod = new InputValue(prop_root, *maxNode);
|
||||
}
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
double PeriodicalValue::normalize( double value ) const
|
||||
{
|
||||
return SGMiscd::normalizePeriodic( minPeriod->get_value(),
|
||||
maxPeriod->get_value(),
|
||||
value );
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
double PeriodicalValue::normalizeSymmetric( double value ) const
|
||||
{
|
||||
double minValue = minPeriod->get_value();
|
||||
double maxValue = maxPeriod->get_value();
|
||||
|
||||
value = SGMiscd::normalizePeriodic( minValue, maxValue, value );
|
||||
double width_2 = (maxValue - minValue)/2;
|
||||
return value > width_2 ? width_2 - value : value;
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
InputValue::InputValue( SGPropertyNode& prop_root,
|
||||
SGPropertyNode& cfg,
|
||||
double value,
|
||||
double offset,
|
||||
double scale ):
|
||||
_value(0.0),
|
||||
_abs(false)
|
||||
{
|
||||
parse(prop_root, cfg, value, offset, scale);
|
||||
}
|
||||
|
||||
InputValue::~InputValue()
|
||||
{
|
||||
if (_pathNode) {
|
||||
_pathNode->removeChangeListener(this);
|
||||
}
|
||||
}
|
||||
|
||||
void InputValue::initPropertyFromInitialValue()
|
||||
{
|
||||
double s = get_scale();
|
||||
if( s != 0 )
|
||||
_property->setDoubleValue( (_value - get_offset())/s );
|
||||
else
|
||||
_property->setDoubleValue(0); // if scale is zero, value*scale is zero
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
void InputValue::parse( SGPropertyNode& prop_root,
|
||||
SGPropertyNode& cfg,
|
||||
double aValue,
|
||||
double aOffset,
|
||||
double aScale )
|
||||
{
|
||||
_value = aValue;
|
||||
_property = NULL;
|
||||
_offset = NULL;
|
||||
_scale = NULL;
|
||||
_min = NULL;
|
||||
_max = NULL;
|
||||
_periodical = NULL;
|
||||
|
||||
SGPropertyNode * n;
|
||||
|
||||
if( (n = cfg.getChild("condition")) != NULL )
|
||||
_condition = sgReadCondition(&prop_root, n);
|
||||
|
||||
if( (n = cfg.getChild( "scale" )) != NULL )
|
||||
_scale = new InputValue(prop_root, *n, aScale);
|
||||
|
||||
if( (n = cfg.getChild( "offset" )) != NULL )
|
||||
_offset = new InputValue(prop_root, *n, aOffset);
|
||||
|
||||
if( (n = cfg.getChild( "max" )) != NULL )
|
||||
_max = new InputValue(prop_root, *n);
|
||||
|
||||
if( (n = cfg.getChild( "min" )) != NULL )
|
||||
_min = new InputValue(prop_root, *n);
|
||||
|
||||
if( (n = cfg.getChild( "abs" )) != NULL )
|
||||
_abs = n->getBoolValue();
|
||||
|
||||
if( (n = cfg.getChild( "period" )) != NULL )
|
||||
_periodical = new PeriodicalValue(prop_root, *n);
|
||||
|
||||
|
||||
SGPropertyNode *valueNode = cfg.getChild("value");
|
||||
if( valueNode != NULL )
|
||||
_value = valueNode->getDoubleValue();
|
||||
|
||||
if( (n = cfg.getChild("expression")) != NULL )
|
||||
{
|
||||
_expression = SGReadDoubleExpression(&prop_root, n->getChild(0));
|
||||
return;
|
||||
}
|
||||
|
||||
if ((n = cfg.getChild("property-path"))) {
|
||||
// cache the root node, in case of changes
|
||||
_rootNode = &prop_root;
|
||||
const auto trimmed = simgear::strutils::strip(n->getStringValue());
|
||||
_pathNode = prop_root.getNode(trimmed, true);
|
||||
_pathNode->addChangeListener(this);
|
||||
|
||||
// if <property> is defined, should we use it to initialise
|
||||
// the path prop? not doing so for now.
|
||||
|
||||
const auto path = simgear::strutils::strip(_pathNode->getStringValue());
|
||||
if (!path.empty()) {
|
||||
_property = _rootNode->getNode(path);
|
||||
}
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
|
||||
// if no <property> element, check for <prop> element for backwards
|
||||
// compatibility
|
||||
if( (n = cfg.getChild("property"))
|
||||
|| (n = cfg.getChild("prop" )) )
|
||||
{
|
||||
// tolerate leading & trailing whitespace from XML, in the property name
|
||||
const auto trimmed = simgear::strutils::strip(n->getStringValue());
|
||||
_property = prop_root.getNode(trimmed, true);
|
||||
if( valueNode )
|
||||
{
|
||||
initPropertyFromInitialValue();
|
||||
}
|
||||
|
||||
return;
|
||||
} // of have a <property> or <prop>
|
||||
|
||||
if( !valueNode )
|
||||
{
|
||||
// no <value>, <prop> or <expression> element, use text node
|
||||
std::string textnode = cfg.getStringValue();
|
||||
char * endp = NULL;
|
||||
// try to convert to a double value. If the textnode does not start with a number
|
||||
// endp will point to the beginning of the string. We assume this should be
|
||||
// a property name
|
||||
_value = strtod( textnode.c_str(), &endp );
|
||||
if( endp == textnode.c_str() )
|
||||
_property = prop_root.getNode(textnode, true);
|
||||
}
|
||||
}
|
||||
|
||||
void InputValue::set_value( double aValue )
|
||||
{
|
||||
if (!_property)
|
||||
return;
|
||||
|
||||
double s = get_scale();
|
||||
if( s != 0 )
|
||||
_property->setDoubleValue( (aValue - get_offset())/s );
|
||||
else
|
||||
_property->setDoubleValue( 0 ); // if scale is zero, value*scale is zero
|
||||
}
|
||||
|
||||
double InputValue::get_value() const
|
||||
{
|
||||
double value = _value;
|
||||
|
||||
if (_expression) {
|
||||
// compute the expression value
|
||||
value = _expression->getValue(NULL);
|
||||
|
||||
if (SGMiscd::isNaN(value)) {
|
||||
SG_LOG(SG_AUTOPILOT, SG_DEV_ALERT, "AP input: read NaN from expression");
|
||||
}
|
||||
} else if( _property != NULL ) {
|
||||
value = _property->getDoubleValue();
|
||||
|
||||
if (SGMiscd::isNaN(value)) {
|
||||
SG_LOG(SG_AUTOPILOT, SG_DEV_ALERT, "AP input: read NaN from:" << _property->getPath() );
|
||||
}
|
||||
} else {
|
||||
if (SGMiscd::isNaN(value)) {
|
||||
SG_LOG(SG_AUTOPILOT, SG_DEV_ALERT, "AP input is NaN." );
|
||||
}
|
||||
}
|
||||
|
||||
if( _scale )
|
||||
value *= _scale->get_value();
|
||||
|
||||
if( _offset )
|
||||
value += _offset->get_value();
|
||||
|
||||
if( _min ) {
|
||||
double m = _min->get_value();
|
||||
if( value < m )
|
||||
value = m;
|
||||
}
|
||||
|
||||
if( _max ) {
|
||||
double m = _max->get_value();
|
||||
if( value > m )
|
||||
value = m;
|
||||
}
|
||||
|
||||
if( _periodical ) {
|
||||
value = _periodical->normalize( value );
|
||||
}
|
||||
|
||||
return _abs ? fabs(value) : value;
|
||||
}
|
||||
|
||||
bool InputValue::is_enabled() const
|
||||
{
|
||||
if (_pathNode && !_property) {
|
||||
// if we have a configurable path, and it's currently not valid,
|
||||
// mark ourselves as disabled
|
||||
return false;
|
||||
}
|
||||
|
||||
if (_condition) {
|
||||
return _condition->test();
|
||||
}
|
||||
|
||||
return true; // default to enab;ed
|
||||
}
|
||||
|
||||
void InputValue::collectDependentProperties(std::set<const SGPropertyNode*>& props) const
|
||||
{
|
||||
if (_property) props.insert(_property);
|
||||
if (_offset) _offset->collectDependentProperties(props);
|
||||
if (_scale) _scale->collectDependentProperties(props);
|
||||
if (_min) _min->collectDependentProperties(props);
|
||||
if (_max) _max->collectDependentProperties(props);
|
||||
if (_expression) _expression->collectDependentProperties(props);
|
||||
if (_pathNode) props.insert(_pathNode);
|
||||
}
|
||||
|
||||
void InputValue::valueChanged(SGPropertyNode *node)
|
||||
{
|
||||
assert(node == _pathNode);
|
||||
const auto path = simgear::strutils::strip(_pathNode->getStringValue());
|
||||
if (path.empty()) {
|
||||
// don't consider an empty string to mean the root node, that's not
|
||||
// useful behaviour
|
||||
_property.reset();
|
||||
return;
|
||||
}
|
||||
|
||||
// important we don't create here: this allows an invalid path
|
||||
// to give us a null _property, which causes us to be marked as
|
||||
// disabled, allowing another input to be used
|
||||
auto propNode = _rootNode->getNode(path);
|
||||
if (propNode) {
|
||||
_property = propNode;
|
||||
} else {
|
||||
_property.reset();
|
||||
}
|
||||
}
|
||||
|
||||
void InputValueList::collectDependentProperties(std::set<const SGPropertyNode*>& props) const
|
||||
{
|
||||
for (auto& iv: *this) {
|
||||
iv->collectDependentProperties(props);
|
||||
}
|
||||
}
|
||||
|
|
@ -1,151 +0,0 @@
|
|||
// inputvalue.hxx - provide input to autopilot components
|
||||
//
|
||||
// Written by Torsten Dreyer
|
||||
// Copyright (C) 2010 Torsten Dreyer - Torsten (at) t3r (dot) de
|
||||
//
|
||||
// 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., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
|
||||
//
|
||||
|
||||
#ifndef _INPUTVALUE_HXX
|
||||
#define _INPUTVALUE_HXX 1
|
||||
|
||||
#ifdef HAVE_CONFIG_H
|
||||
# include <config.h>
|
||||
#endif
|
||||
|
||||
|
||||
#include <simgear/structure/SGExpression.hxx>
|
||||
|
||||
namespace FGXMLAutopilot {
|
||||
|
||||
typedef SGSharedPtr<class InputValue> InputValue_ptr;
|
||||
typedef SGSharedPtr<class PeriodicalValue> PeriodicalValue_ptr;
|
||||
|
||||
/**
|
||||
* @brief Model a periodical value like angular values
|
||||
*
|
||||
* Most common use for periodical values are angular values.
|
||||
* If y = f(x) = f(x + n*period), this is a periodical function
|
||||
*/
|
||||
class PeriodicalValue : public SGReferenced {
|
||||
private:
|
||||
InputValue_ptr minPeriod; // The minimum value of the period
|
||||
InputValue_ptr maxPeriod; // The maximum value of the period
|
||||
public:
|
||||
PeriodicalValue( SGPropertyNode& prop_root,
|
||||
SGPropertyNode& cfg );
|
||||
double normalize( double value ) const;
|
||||
double normalizeSymmetric( double value ) const;
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief A input value for analog autopilot components
|
||||
*
|
||||
* Input values may be constants, property values, transformed with a scale
|
||||
* and/or offset, clamped to min/max values, be periodical, bound to
|
||||
* conditions or evaluated from expressions.
|
||||
*/
|
||||
class InputValue : public SGReferenced, public SGPropertyChangeListener {
|
||||
private:
|
||||
double _value; // The value as a constant or initializer for the property
|
||||
bool _abs; // return absolute value
|
||||
SGPropertyNode_ptr _property; // The name of the property containing the value
|
||||
InputValue_ptr _offset; // A fixed offset, defaults to zero
|
||||
InputValue_ptr _scale; // A constant scaling factor defaults to one
|
||||
InputValue_ptr _min; // A minimum clip defaults to no clipping
|
||||
InputValue_ptr _max; // A maximum clip defaults to no clipping
|
||||
PeriodicalValue_ptr _periodical; //
|
||||
SGSharedPtr<const SGCondition> _condition;
|
||||
SGSharedPtr<SGExpressiond> _expression; ///< expression to generate the value
|
||||
SGPropertyNode_ptr _pathNode;
|
||||
SGPropertyNode_ptr _rootNode;
|
||||
|
||||
void valueChanged(SGPropertyNode* node) override;
|
||||
void initPropertyFromInitialValue();
|
||||
public:
|
||||
InputValue( SGPropertyNode& prop_root,
|
||||
SGPropertyNode& node,
|
||||
double value = 0.0,
|
||||
double offset = 0.0,
|
||||
double scale = 1.0 );
|
||||
~InputValue();
|
||||
|
||||
/**
|
||||
*
|
||||
* @param prop_root Root node for all properties with relative path
|
||||
* @param cfg Configuration node
|
||||
* @param value Default initial value
|
||||
* @param offset Default initial offset
|
||||
* @param scale Default initial scale
|
||||
*/
|
||||
void parse( SGPropertyNode& prop_root,
|
||||
SGPropertyNode& cfg,
|
||||
double value = 0.0,
|
||||
double offset = 0.0,
|
||||
double scale = 1.0 );
|
||||
|
||||
/* get the value of this input, apply scale and offset and clipping */
|
||||
double get_value() const;
|
||||
|
||||
/* set the input value after applying offset and scale */
|
||||
void set_value( double value );
|
||||
|
||||
inline double get_scale() const {
|
||||
return _scale == NULL ? 1.0 : _scale->get_value();
|
||||
}
|
||||
|
||||
inline double get_offset() const {
|
||||
return _offset == NULL ? 0.0 : _offset->get_value();
|
||||
}
|
||||
|
||||
bool is_enabled() const;
|
||||
|
||||
void collectDependentProperties(std::set<const SGPropertyNode*>& props) const;
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief A chained list of InputValues
|
||||
*
|
||||
* Many compoments support InputValueLists as input. Each InputValue may be bound to
|
||||
* a condition. This list supports the get_value() function to retrieve the value
|
||||
* of the first InputValue in this list that has a condition evaluating to true.
|
||||
*/
|
||||
class InputValueList : public std::vector<InputValue_ptr> {
|
||||
public:
|
||||
InputValueList( double def = 0.0 ) : _def(def) { }
|
||||
|
||||
InputValue_ptr get_active() const {
|
||||
for (const_iterator it = begin(); it != end(); ++it) {
|
||||
if( (*it)->is_enabled() )
|
||||
return *it;
|
||||
}
|
||||
return NULL;
|
||||
}
|
||||
|
||||
double get_value() const {
|
||||
InputValue_ptr input = get_active();
|
||||
return input == NULL ? _def : input->get_value();
|
||||
}
|
||||
|
||||
void collectDependentProperties(std::set<const SGPropertyNode*>& props) const;
|
||||
private:
|
||||
|
||||
double _def;
|
||||
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#endif
|
|
@ -251,17 +251,17 @@ bool PIDController::configure( SGPropertyNode& cfg_node,
|
|||
}
|
||||
|
||||
if (cfg_name == "Kp") {
|
||||
Kp.push_back( new InputValue(prop_root, cfg_node) );
|
||||
Kp.push_back(new simgear::Value(prop_root, cfg_node));
|
||||
return true;
|
||||
}
|
||||
|
||||
if (cfg_name == "Ti") {
|
||||
Ti.push_back( new InputValue(prop_root, cfg_node) );
|
||||
Ti.push_back(new simgear::Value(prop_root, cfg_node));
|
||||
return true;
|
||||
}
|
||||
|
||||
if (cfg_name == "Td") {
|
||||
Td.push_back( new InputValue(prop_root, cfg_node) );
|
||||
Td.push_back(new simgear::Value(prop_root, cfg_node));
|
||||
return true;
|
||||
}
|
||||
|
||||
|
|
|
@ -41,9 +41,9 @@ class PIDController : public AnalogComponent
|
|||
{
|
||||
private:
|
||||
// Configuration values
|
||||
InputValueList Kp; // proportional gain
|
||||
InputValueList Ti; // Integrator time (sec)
|
||||
InputValueList Td; // Derivator time (sec)
|
||||
simgear::ValueList Kp; // proportional gain
|
||||
simgear::ValueList Ti; // Integrator time (sec)
|
||||
simgear::ValueList Td; // Derivator time (sec)
|
||||
|
||||
double alpha; // low pass filter weighing factor (usually 0.1)
|
||||
double beta; // process value weighing factor for
|
||||
|
|
|
@ -43,12 +43,12 @@ bool PISimpleController::configure( SGPropertyNode& cfg_node,
|
|||
}
|
||||
|
||||
if (cfg_name == "Kp") {
|
||||
_Kp.push_back( new InputValue(prop_root, cfg_node) );
|
||||
_Kp.push_back(new simgear::Value(prop_root, cfg_node));
|
||||
return true;
|
||||
}
|
||||
|
||||
if (cfg_name == "Ki") {
|
||||
_Ki.push_back( new InputValue(prop_root, cfg_node) );
|
||||
_Ki.push_back(new simgear::Value(prop_root, cfg_node));
|
||||
return true;
|
||||
}
|
||||
|
||||
|
|
|
@ -42,10 +42,10 @@ class PISimpleController : public AnalogComponent
|
|||
|
||||
private:
|
||||
// proportional component data
|
||||
InputValueList _Kp;
|
||||
simgear::ValueList _Kp;
|
||||
|
||||
// integral component data
|
||||
InputValueList _Ki;
|
||||
simgear::ValueList _Ki;
|
||||
double _int_sum;
|
||||
|
||||
protected:
|
||||
|
|
|
@ -44,12 +44,12 @@ bool Predictor::configure( SGPropertyNode& cfg_node,
|
|||
}
|
||||
|
||||
if (cfg_name == "seconds") {
|
||||
_seconds.push_back( new InputValue(prop_root, cfg_node, 0) );
|
||||
_seconds.push_back(new simgear::Value(prop_root, cfg_node, 0));
|
||||
return true;
|
||||
}
|
||||
|
||||
if (cfg_name == "filter-gain") {
|
||||
_filter_gain.push_back( new InputValue(prop_root, cfg_node, 0) );
|
||||
_filter_gain.push_back(new simgear::Value(prop_root, cfg_node, 0));
|
||||
return true;
|
||||
}
|
||||
|
||||
|
|
|
@ -49,8 +49,8 @@ class Predictor : public AnalogComponent
|
|||
private:
|
||||
double _last_value;
|
||||
double _average;
|
||||
InputValueList _seconds;
|
||||
InputValueList _filter_gain;
|
||||
simgear::ValueList _seconds;
|
||||
simgear::ValueList _filter_gain;
|
||||
|
||||
protected:
|
||||
virtual bool configure( SGPropertyNode& cfg_node,
|
||||
|
|
Loading…
Add table
Reference in a new issue