Author: Jon Berndt
Date started: 21 February 2006
- ------------- Copyright (C) 2007 Jon S. Berndt (jsb@hal-pc.org) -------------
+ ------------- Copyright (C) 2007 Jon S. Berndt (jon@jsbsim.org) -------------
This program is free software; you can redistribute it and/or modify it under
the terms of the GNU Lesser General Public License as published by the Free Software
#include "FGActuator.h"
+using namespace std;
+
namespace JSBSim {
-static const char *IdSrc = "$Id$";
+static const char *IdSrc = "$Id: FGActuator.cpp,v 1.23 2012/04/08 15:04:41 jberndt Exp $";
static const char *IdHdr = ID_ACTUATOR;
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FGActuator::FGActuator(FGFCS* fcs, Element* element) : FGFCSComponent(fcs, element)
{
double denom;
- dt = fcs->GetDt();
// inputs are read from the base class constructor
rate_limit = 0.0; // no limit
fail_zero = fail_hardover = fail_stuck = false;
ca = cb = 0.0;
+ initialized = 0;
+ saturated = false;
if ( element->FindElement("deadband_width") ) {
deadband_width = element->FindElementValueAsNumber("deadband_width");
hysteresis_width = element->FindElementValueAsNumber("hysteresis_width");
}
if ( element->FindElement("rate_limit") ) {
- rate_limit = element->FindElementValueAsNumber("rate_limit");
+ rate_limit = fabs(element->FindElementValueAsNumber("rate_limit"));
}
if ( element->FindElement("bias") ) {
bias = element->FindElementValueAsNumber("bias");
bool FGActuator::Run(void )
{
- dt = fcs->GetDt();
-
Input = InputNodes[0]->getDoubleValue() * InputSigns[0];
+ if( fcs->GetTrimStatus() ) initialized = 0;
+
if (fail_zero) Input = 0;
if (fail_hardover) Input = clipmax*sign(Input);
// the Input will be further processed and the eventual Output
// will be overwritten from this perfect value.
- if (lag != 0.0) Lag(); // models actuator lag
- if (rate_limit != 0) RateLimit(); // limit the actuator rate
- if (deadband_width != 0.0) Deadband();
- if (hysteresis_width != 0.0) Hysteresis();
- if (bias != 0.0) Bias(); // models a finite bias
+ if (fail_stuck) {
+ Output = PreviousOutput;
+ } else {
+ if (lag != 0.0) Lag(); // models actuator lag
+ if (rate_limit != 0) RateLimit(); // limit the actuator rate
+ if (deadband_width != 0.0) Deadband();
+ if (hysteresis_width != 0.0) Hysteresis();
+ if (bias != 0.0) Bias(); // models a finite bias
+ }
- if (fail_stuck) Output = PreviousOutput;
PreviousOutput = Output; // previous value needed for "stuck" malfunction
+
+ initialized = 1;
Clip();
+
+ if (clip) {
+ saturated = false;
+ if (Output >= clipmax) saturated = true;
+ else if (Output <= clipmin) saturated = true;
+ }
+
if (IsOutput) SetOutput();
return true;
// "Output" on the right side of the "=" is the current frame input
// for this Lag filter
double input = Output;
- Output = ca * (input + PreviousLagInput) + PreviousLagOutput * cb;
+
+ if ( initialized )
+ Output = ca * (input + PreviousLagInput) + PreviousLagOutput * cb;
+
PreviousLagInput = input;
PreviousLagOutput = Output;
}
// method.
double input = Output;
- if (input > PreviousHystOutput) {
- Output = max(PreviousHystOutput, input-0.5*hysteresis_width);
- } else if (input < PreviousHystOutput) {
- Output = min(PreviousHystOutput, input+0.5*hysteresis_width);
+ if ( initialized ) {
+ if (input > PreviousHystOutput)
+ Output = max(PreviousHystOutput, input-0.5*hysteresis_width);
+ else if (input < PreviousHystOutput)
+ Output = min(PreviousHystOutput, input+0.5*hysteresis_width);
}
PreviousHystOutput = Output;
// is - for the purposes of this RateLimit method - really the input to the
// method.
double input = Output;
- if (dt > 0.0) {
- double rate = (input - PreviousRateLimOutput)/dt;
- if (fabs(rate) > rate_limit) {
- Output = PreviousRateLimOutput + (rate_limit*fabs(rate)/rate)*dt;
+ if ( initialized ) {
+ double delta = input - PreviousRateLimOutput;
+ if (fabs(delta) > dt * rate_limit) {
+ double signed_rate_limit = delta > 0.0 ? rate_limit : -rate_limit;
+ Output = PreviousRateLimOutput + signed_rate_limit * dt;
}
}
PreviousRateLimOutput = Output;
void FGActuator::Deadband(void)
{
+ // Note: this function acts cumulatively on the "Output" parameter. So, "Output"
+ // is - for the purposes of this Deadband method - really the input to the
+ // method.
+ double input = Output;
+
+ if (input < -deadband_width/2.0) {
+ Output = (input + deadband_width/2.0);
+ } else if (input > deadband_width/2.0) {
+ Output = (input - deadband_width/2.0);
+ } else {
+ Output = 0.0;
+ }
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
const string tmp_zero = tmp + "/malfunction/fail_zero";
const string tmp_hardover = tmp + "/malfunction/fail_hardover";
const string tmp_stuck = tmp + "/malfunction/fail_stuck";
+ const string tmp_sat = tmp + "/saturated";
PropertyManager->Tie( tmp_zero, this, &FGActuator::GetFailZero, &FGActuator::SetFailZero);
PropertyManager->Tie( tmp_hardover, this, &FGActuator::GetFailHardover, &FGActuator::SetFailHardover);
PropertyManager->Tie( tmp_stuck, this, &FGActuator::GetFailStuck, &FGActuator::SetFailStuck);
+ PropertyManager->Tie( tmp_sat, this, &FGActuator::IsSaturated);
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if (debug_lvl & 1) { // Standard console startup message output
if (from == 0) { // Constructor
if (InputSigns[0] < 0)
- cout << " INPUT: -" << InputNodes[0]->getName() << endl;
+ cout << " INPUT: -" << InputNames[0] << endl;
else
- cout << " INPUT: " << InputNodes[0]->getName() << endl;
+ cout << " INPUT: " << InputNames[0] << endl;
- if (IsOutput) cout << " OUTPUT: " << OutputNode->getName() << endl;
+ if (IsOutput) {
+ for (unsigned int i=0; i<OutputNodes.size(); i++)
+ cout << " OUTPUT: " << OutputNodes[i]->getName() << endl;
+ }
if (bias != 0.0) cout << " Bias: " << bias << endl;
if (rate_limit != 0) cout << " Rate limit: " << rate_limit << endl;
if (lag != 0) cout << " Actuator lag: " << lag << endl;
projects / flightgear.git / blobdiff