Merge branch 'torsten/auto'
This commit is contained in:
commit
98e6b8212a
2 changed files with 129 additions and 36 deletions
|
@ -64,7 +64,7 @@ double FGPeriodicalValue::normalize( double value )
|
|||
double phase = fabs(max - min);
|
||||
|
||||
if( phase > SGLimitsd::min() ) {
|
||||
while( value < min ) value += phase;
|
||||
while( value < min ) value += phase;
|
||||
while( value >= max ) value -= phase;
|
||||
} else {
|
||||
value = min; // phase is zero
|
||||
|
@ -459,11 +459,8 @@ bool FGPIDController::parseConfigHook(const string& aName, SGPropertyNode* aNode
|
|||
*/
|
||||
|
||||
void FGPIDController::update( double dt ) {
|
||||
double ep_n; // proportional error with reference weighing
|
||||
double e_n; // error
|
||||
double ed_n; // derivative error
|
||||
double edf_n; // derivative error filter
|
||||
double Tf; // filter time
|
||||
double edf_n;
|
||||
double delta_u_n = 0.0; // incremental output
|
||||
double u_n = 0.0; // absolute output
|
||||
double Ts; // sampling interval (sec)
|
||||
|
@ -503,7 +500,7 @@ void FGPIDController::update( double dt ) {
|
|||
if ( debug ) cout << " input = " << y_n << " ref = " << r_n << endl;
|
||||
|
||||
// Calculates proportional error:
|
||||
ep_n = beta * r_n - y_n;
|
||||
double ep_n = beta * r_n - y_n;
|
||||
if ( debug ) cout << " ep_n = " << ep_n;
|
||||
if ( debug ) cout << " ep_n_1 = " << ep_n_1;
|
||||
|
||||
|
@ -511,14 +508,15 @@ void FGPIDController::update( double dt ) {
|
|||
e_n = r_n - y_n;
|
||||
if ( debug ) cout << " e_n = " << e_n;
|
||||
|
||||
// Calculates derivate error:
|
||||
ed_n = gamma * r_n - y_n;
|
||||
if ( debug ) cout << " ed_n = " << ed_n;
|
||||
|
||||
double td = Td.get_value();
|
||||
if ( td > 0.0 ) {
|
||||
if ( td > 0.0 ) { // do we need to calcluate derivative error?
|
||||
|
||||
// Calculates derivate error:
|
||||
double ed_n = gamma * r_n - y_n;
|
||||
if ( debug ) cout << " ed_n = " << ed_n;
|
||||
|
||||
// Calculates filter time:
|
||||
Tf = alpha * td;
|
||||
double Tf = alpha * td;
|
||||
if ( debug ) cout << " Tf = " << Tf;
|
||||
|
||||
// Filters the derivate error:
|
||||
|
@ -526,7 +524,7 @@ void FGPIDController::update( double dt ) {
|
|||
+ ed_n * (Ts/Tf) / (Ts/Tf + 1);
|
||||
if ( debug ) cout << " edf_n = " << edf_n;
|
||||
} else {
|
||||
edf_n = ed_n;
|
||||
edf_n_2 = edf_n_1 = edf_n = 0.0;
|
||||
}
|
||||
|
||||
// Calculates the incremental output:
|
||||
|
@ -535,14 +533,14 @@ void FGPIDController::update( double dt ) {
|
|||
delta_u_n = Kp.get_value() * ( (ep_n - ep_n_1)
|
||||
+ ((Ts/ti) * e_n)
|
||||
+ ((td/Ts) * (edf_n - 2*edf_n_1 + edf_n_2)) );
|
||||
}
|
||||
|
||||
if ( debug ) {
|
||||
cout << " delta_u_n = " << delta_u_n << endl;
|
||||
cout << "P:" << Kp.get_value() * (ep_n - ep_n_1)
|
||||
<< " I:" << Kp.get_value() * ((Ts/ti) * e_n)
|
||||
<< " D:" << Kp.get_value() * ((td/Ts) * (edf_n - 2*edf_n_1 + edf_n_2))
|
||||
<< endl;
|
||||
if ( debug ) {
|
||||
cout << " delta_u_n = " << delta_u_n << endl;
|
||||
cout << "P:" << Kp.get_value() * (ep_n - ep_n_1)
|
||||
<< " I:" << Kp.get_value() * ((Ts/ti) * e_n)
|
||||
<< " D:" << Kp.get_value() * ((td/Ts) * (edf_n - 2*edf_n_1 + edf_n_2))
|
||||
<< endl;
|
||||
}
|
||||
}
|
||||
|
||||
// Integrator anti-windup logic:
|
||||
|
@ -566,13 +564,8 @@ void FGPIDController::update( double dt ) {
|
|||
|
||||
set_output_value( u_n );
|
||||
} else if ( !enabled ) {
|
||||
ep_n = 0.0;
|
||||
edf_n = 0.0;
|
||||
// Updates indexed values;
|
||||
u_n_1 = u_n;
|
||||
ep_n_1 = ep_n;
|
||||
edf_n_2 = edf_n_1;
|
||||
edf_n_1 = edf_n;
|
||||
ep_n_1 = 0.0;
|
||||
edf_n_2 = edf_n_1 = edf_n = 0.0;
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -622,17 +615,20 @@ void FGPISimpleController::update( double dt ) {
|
|||
<< " error = " << error
|
||||
<< endl;
|
||||
|
||||
double prop_comp = error * Kp.get_value();
|
||||
double prop_comp = clamp(error * Kp.get_value());
|
||||
int_sum += error * Ki.get_value() * dt;
|
||||
|
||||
|
||||
double output = prop_comp + int_sum;
|
||||
double clamped_output = clamp( output );
|
||||
if( output != clamped_output ) // anti-windup
|
||||
int_sum = clamped_output - prop_comp;
|
||||
|
||||
if ( debug ) cout << "prop_comp = " << prop_comp
|
||||
<< " int_sum = " << int_sum << endl;
|
||||
|
||||
double output = prop_comp + int_sum;
|
||||
output = clamp( output );
|
||||
set_output_value( output );
|
||||
if ( debug ) cout << "output = " << output << endl;
|
||||
set_output_value( clamped_output );
|
||||
if ( debug ) cout << "output = " << clamped_output << endl;
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -757,7 +753,7 @@ void FGDigitalFilter::update(double dt)
|
|||
{
|
||||
if ( isPropertyEnabled() ) {
|
||||
|
||||
input.push_front(valueInput.get_value());
|
||||
input.push_front(valueInput.get_value()-referenceInput.get_value());
|
||||
input.resize(samplesInput.get_value() + 1, 0.0);
|
||||
|
||||
if ( !enabled ) {
|
||||
|
@ -848,9 +844,58 @@ void FGDigitalFilter::update(double dt)
|
|||
}
|
||||
}
|
||||
|
||||
FGXMLAutoLogic::FGXMLAutoLogic(SGPropertyNode * node ) :
|
||||
FGXMLAutoComponent(),
|
||||
inverted(false)
|
||||
{
|
||||
parseNode(node);
|
||||
}
|
||||
|
||||
bool FGXMLAutoLogic::parseNodeHook(const std::string& aName, SGPropertyNode* aNode)
|
||||
{
|
||||
if (aName == "input") {
|
||||
input = sgReadCondition( fgGetNode("/"), aNode );
|
||||
} else if (aName == "inverted") {
|
||||
inverted = aNode->getBoolValue();
|
||||
} else {
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void FGXMLAutoLogic::update(double dt)
|
||||
{
|
||||
if ( isPropertyEnabled() ) {
|
||||
if ( !enabled ) {
|
||||
// we have just been enabled
|
||||
}
|
||||
enabled = true;
|
||||
} else {
|
||||
enabled = false;
|
||||
do_feedback();
|
||||
}
|
||||
|
||||
if ( !enabled || dt < SGLimitsd::min() )
|
||||
return;
|
||||
|
||||
if( input == NULL ) {
|
||||
if ( debug ) cout << "No input for " << get_name() << endl;
|
||||
return;
|
||||
}
|
||||
|
||||
bool i = input->test();
|
||||
|
||||
if ( debug ) cout << "Updating " << get_name() << ": " << (inverted ? !i : i) << endl;
|
||||
|
||||
set_output_value( i );
|
||||
}
|
||||
|
||||
|
||||
FGXMLAutopilotGroup::FGXMLAutopilotGroup() :
|
||||
SGSubsystemGroup(),
|
||||
average(0.0), // average/filtered prediction
|
||||
SGSubsystemGroup()
|
||||
#ifdef XMLAUTO_USEHELPER
|
||||
,average(0.0), // average/filtered prediction
|
||||
v_last(0.0), // last velocity
|
||||
last_static_pressure(0.0),
|
||||
vel(fgGetNode( "/velocities/airspeed-kt", true )),
|
||||
|
@ -874,6 +919,7 @@ FGXMLAutopilotGroup::FGXMLAutopilotGroup() :
|
|||
static_pressure(fgGetNode( "/systems/static[0]/pressure-inhg", true )),
|
||||
pressure_rate(fgGetNode( "/autopilot/internal/pressure-rate", true )),
|
||||
track(fgGetNode( "/orientation/track-deg", true ))
|
||||
#endif
|
||||
{
|
||||
}
|
||||
|
||||
|
@ -881,7 +927,7 @@ void FGXMLAutopilotGroup::update( double dt )
|
|||
{
|
||||
// update all configured autopilots
|
||||
SGSubsystemGroup::update( dt );
|
||||
|
||||
#ifdef XMLAUTO_USEHELPER
|
||||
// update helper values
|
||||
double v = vel->getDoubleValue();
|
||||
double a = 0.0;
|
||||
|
@ -940,6 +986,7 @@ void FGXMLAutopilotGroup::update( double dt )
|
|||
pressure_rate->setDoubleValue(current_pressure_rate);
|
||||
last_static_pressure = current_static_pressure;
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
void FGXMLAutopilotGroup::reinit()
|
||||
|
@ -1062,6 +1109,8 @@ bool FGXMLAutopilot::build( SGPropertyNode_ptr config_props ) {
|
|||
components.push_back( new FGPredictor( node ) );
|
||||
} else if ( name == "filter" ) {
|
||||
components.push_back( new FGDigitalFilter( node ) );
|
||||
} else if ( name == "logic" ) {
|
||||
components.push_back( new FGXMLAutoLogic( node ) );
|
||||
} else {
|
||||
SG_LOG( SG_AUTOPILOT, SG_WARN, "Unknown top level autopilot section: " << name );
|
||||
// return false;
|
||||
|
|
|
@ -24,6 +24,21 @@
|
|||
#ifndef _XMLAUTO_HXX
|
||||
#define _XMLAUTO_HXX 1
|
||||
|
||||
/*
|
||||
Torsten Dreyer:
|
||||
I'd like to deprecate the so called autopilot helper function
|
||||
(which is now part of the AutopilotGroup::update() method).
|
||||
Every property calculated within this helper can be calculated
|
||||
using filters defined in an external autopilot definition file.
|
||||
The complete set of calculations may be extracted into a separate
|
||||
configuration file. The current implementation is able to hande
|
||||
multiple config files and autopilots. The helper doubles code
|
||||
and writes properties used only by a few aircraft.
|
||||
*/
|
||||
// FIXME: this should go into config.h and/or configure
|
||||
// or removed along with the "helper" one day.
|
||||
#define XMLAUTO_USEHELPER
|
||||
|
||||
#include <simgear/compiler.h>
|
||||
|
||||
#include <string>
|
||||
|
@ -190,6 +205,17 @@ public:
|
|||
(*it)->setDoubleValue( clamp( value ) );
|
||||
}
|
||||
|
||||
inline void set_output_value( bool value ) {
|
||||
// passive_ignore == true means that we go through all the
|
||||
// motions, but drive the outputs. This is analogous to
|
||||
// running the autopilot with the "servos" off. This is
|
||||
// helpful for things like flight directors which position
|
||||
// their vbars from the autopilot computations.
|
||||
if ( honor_passive && passive_mode->getBoolValue() ) return;
|
||||
for( std::vector <SGPropertyNode_ptr>::iterator it = output_list.begin(); it != output_list.end(); ++it)
|
||||
(*it)->setBoolValue( value ); // don't use clamp here, bool is clamped anyway
|
||||
}
|
||||
|
||||
inline double get_output_value() {
|
||||
return output_list.size() == 0 ? 0.0 : clamp(output_list[0]->getDoubleValue());
|
||||
}
|
||||
|
@ -367,6 +393,22 @@ public:
|
|||
void update(double dt);
|
||||
};
|
||||
|
||||
class FGXMLAutoLogic : public FGXMLAutoComponent
|
||||
{
|
||||
private:
|
||||
SGSharedPtr<SGCondition> input;
|
||||
bool inverted;
|
||||
|
||||
protected:
|
||||
bool parseNodeHook(const std::string& aName, SGPropertyNode* aNode);
|
||||
|
||||
public:
|
||||
FGXMLAutoLogic(SGPropertyNode * node );
|
||||
~FGXMLAutoLogic() {}
|
||||
|
||||
void update(double dt);
|
||||
};
|
||||
|
||||
/**
|
||||
* Model an autopilot system.
|
||||
*
|
||||
|
@ -382,6 +424,7 @@ public:
|
|||
private:
|
||||
std::vector<std::string> _autopilotNames;
|
||||
|
||||
#ifdef XMLAUTO_USEHELPER
|
||||
double average;
|
||||
double v_last;
|
||||
double last_static_pressure;
|
||||
|
@ -406,6 +449,7 @@ private:
|
|||
SGPropertyNode_ptr static_pressure;
|
||||
SGPropertyNode_ptr pressure_rate;
|
||||
SGPropertyNode_ptr track;
|
||||
#endif
|
||||
};
|
||||
|
||||
class FGXMLAutopilot : public SGSubsystem
|
||||
|
|
Loading…
Add table
Reference in a new issue