fly/flightgear
1
0
Fork 0
Code Activity

JSBSim bug fixes (candidate for cherry picking to release 2018.3)

Browse source 
The following bugs are fixed by this commit:
* PID integration with the 3rd order Adams-Bashforth was inccorect.
* The fail_stuck property of sensors (accelerometers, magnetometers, gyro, etc.) without a <lag> element was setting the output to zero instead of sticking to the last output value. Thanks to Jonathan Redpath (aka legoboyvdlp) for the bug report.
* When a sensor was stuck, the drift, gain, bias and quantization of the last output before being stuck were ignored. Thanks to Dennis J. Linse for the bug report.
This commit is contained in:
Bertrand Coconnier 2018-10-28 14:38:02 +01:00
parent 66d635dc9c
commit 94e1cdc551
4 changed files with 86 additions and 101 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

4
src/FDM/JSBSim/models/atmosphere/FGStandardAtmosphere.cpp
View file

@ -258,8 +258,8 @@ double FGStandardAtmosphere::GetStdTemperature(double altitude) const
if (GeoPotAlt >= 0.0) if (GeoPotAlt >= 0.0)
return StdAtmosTemperatureTable.GetValue(GeoPotAlt); return StdAtmosTemperatureTable.GetValue(GeoPotAlt);
else
return StdAtmosTemperatureTable.GetValue(0.0) + GeoPotAlt*LapseRates[0]; return StdAtmosTemperatureTable.GetValue(0.0) + GeoPotAlt*LapseRates[0];
} }
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

118
src/FDM/JSBSim/models/flight_control/FGPID.cpp
View file

@ -37,6 +37,7 @@ INCLUDES
#include "FGPID.h" #include "FGPID.h"
#include "input_output/FGXMLElement.h" #include "input_output/FGXMLElement.h"
#include "math/FGRealValue.h"
#include <string> #include <string>
#include <iostream> #include <iostream>
@ -50,15 +51,8 @@ CLASS IMPLEMENTATION
FGPID::FGPID(FGFCS* fcs, Element* element) : FGFCSComponent(fcs, element) FGPID::FGPID(FGFCS* fcs, Element* element) : FGFCSComponent(fcs, element)
{ {
string kp_string, ki_string, kd_string; Element *el;
Kp = Ki = Kd = 0.0;
KpPropertyNode = 0;
KiPropertyNode = 0;
KdPropertyNode = 0;
KpPropertySign = 1.0;
KiPropertySign = 1.0;
KdPropertySign = 1.0;
I_out_total = 0.0; I_out_total = 0.0;
Input_prev = Input_prev2 = 0.0; Input_prev = Input_prev2 = 0.0;
Trigger = 0; Trigger = 0;
@ -70,23 +64,21 @@ FGPID::FGPID(FGFCS* fcs, Element* element) : FGFCSComponent(fcs, element)
if (pid_type == "standard") IsStandard = true; if (pid_type == "standard") IsStandard = true;
if ( element->FindElement("kp") ) { el = element->FindElement("kp");
kp_string = element->FindElementValue("kp"); if (el) {
if (!is_number(kp_string)) { // property string kp_string = el->GetDataLine();
if (kp_string[0] == '-') {
KpPropertySign = -1.0; if (!is_number(kp_string)) // property
kp_string.erase(0,1); Kp = new FGPropertyValue(kp_string, PropertyManager);
} else
KpPropertyNode = PropertyManager->GetNode(kp_string); Kp = new FGRealValue(element->FindElementValueAsNumber("kp"));
} else {
Kp = element->FindElementValueAsNumber("kp");
}
} }
else
Kp = new FGRealValue(0.0);
if ( element->FindElement("ki") ) { el = element->FindElement("ki");
ki_string = element->FindElementValue("ki"); if (el) {
string integ_type = el->GetAttributeValue("type");
string integ_type = element->FindElement("ki")->GetAttributeValue("type");
if (integ_type == "rect") { // Use rectangular integration if (integ_type == "rect") { // Use rectangular integration
IntType = eRectEuler; IntType = eRectEuler;
} else if (integ_type == "trap") { // Use trapezoidal integration } else if (integ_type == "trap") { // Use trapezoidal integration
@ -99,37 +91,35 @@ FGPID::FGPID(FGFCS* fcs, Element* element) : FGFCSComponent(fcs, element)
IntType = eAdamsBashforth2; IntType = eAdamsBashforth2;
} }
if (!is_number(ki_string)) { // property string ki_string = el->GetDataLine();
if (ki_string[0] == '-') {
KiPropertySign = -1.0;
ki_string.erase(0,1);
}
KiPropertyNode = PropertyManager->GetNode(ki_string);
} else {
Ki = element->FindElementValueAsNumber("ki");
}
}
if ( element->FindElement("kd") ) { if (!is_number(ki_string)) // property
kd_string = element->FindElementValue("kd"); Ki = new FGPropertyValue(ki_string, PropertyManager);
if (!is_number(kd_string)) { // property else
if (kd_string[0] == '-') { Ki = new FGRealValue(element->FindElementValueAsNumber("ki"));
KdPropertySign = -1.0;
kd_string.erase(0,1);
}
KdPropertyNode = PropertyManager->GetNode(kd_string);
} else {
Kd = element->FindElementValueAsNumber("kd");
}
} }
else
Ki = new FGRealValue(0.0);
if (element->FindElement("pvdot")) { el = element->FindElement("kd");
ProcessVariableDot = PropertyManager->GetNode(element->FindElementValue("pvdot")); if (el) {
} string kd_string = el->GetDataLine();
if (element->FindElement("trigger")) { if (!is_number(kd_string)) // property
Trigger = PropertyManager->GetNode(element->FindElementValue("trigger")); Kd = new FGPropertyValue(kd_string, PropertyManager);
else
Kd = new FGRealValue(element->FindElementValueAsNumber("kd"));
} }
else
Kd = new FGRealValue(0.0);
el = element->FindElement("pvdot");
if (el)
ProcessVariableDot = PropertyManager->GetNode(el->GetDataLine());
el = element->FindElement("trigger");
if (el)
Trigger = PropertyManager->GetNode(el->GetDataLine());
FGFCSComponent::bind(); FGFCSComponent::bind();
string tmp; string tmp;
@ -148,6 +138,9 @@ FGPID::FGPID(FGFCS* fcs, Element* element) : FGFCSComponent(fcs, element)
FGPID::~FGPID() FGPID::~FGPID()
{ {
delete Kp;
delete Ki;
delete Kd;
Debug(1); Debug(1);
} }
@ -169,10 +162,6 @@ bool FGPID::Run(void )
Input = InputNodes[0]->getDoubleValue() * InputSigns[0]; Input = InputNodes[0]->getDoubleValue() * InputSigns[0];
if (KpPropertyNode != 0) Kp = KpPropertyNode->getDoubleValue() * KpPropertySign;
if (KiPropertyNode != 0) Ki = KiPropertyNode->getDoubleValue() * KiPropertySign;
if (KdPropertyNode != 0) Kd = KdPropertyNode->getDoubleValue() * KdPropertySign;
if (ProcessVariableDot) { if (ProcessVariableDot) {
Dval = ProcessVariableDot->getDoubleValue(); Dval = ProcessVariableDot->getDoubleValue();
} else { } else {
@ -190,19 +179,19 @@ bool FGPID::Run(void )
if (fabs(test) < 0.000001) { if (fabs(test) < 0.000001) {
switch(IntType) { switch(IntType) {
case eRectEuler: case eRectEuler:
I_out_delta = Ki * dt * Input; // Normal rectangular integrator I_out_delta = Input; // Normal rectangular integrator
break; break;
case eTrapezoidal: case eTrapezoidal:
I_out_delta = (Ki/2.0) * dt * (Input + Input_prev); // Trapezoidal integrator I_out_delta = 0.5 * (Input + Input_prev); // Trapezoidal integrator
break; break;
case eAdamsBashforth2: case eAdamsBashforth2:
I_out_delta = Ki * dt * (1.5*Input - 0.5*Input_prev); // 2nd order Adams Bashforth integrator I_out_delta = 1.5*Input - 0.5*Input_prev; // 2nd order Adams Bashforth integrator
break; break;
case eAdamsBashforth3: // 3rd order Adams Bashforth integrator case eAdamsBashforth3: // 3rd order Adams Bashforth integrator
I_out_delta = (Ki/12.0) * dt * (23.0*Input - 16.0*Input_prev + 5.0*Input_prev2); I_out_delta = (23.0*Input - 16.0*Input_prev + 5.0*Input_prev2) / 12.0;
break; break;
case eNone: case eNone:
// No integator is defined or used. // No integrator is defined or used.
I_out_delta = 0.0; I_out_delta = 0.0;
break; break;
} }
@ -210,16 +199,15 @@ bool FGPID::Run(void )
if (test < 0.0) I_out_total = 0.0; // Reset integrator to 0.0 if (test < 0.0) I_out_total = 0.0; // Reset integrator to 0.0
I_out_total += I_out_delta; I_out_total += Ki->GetValue() * dt * I_out_delta;
if (IsStandard) { if (IsStandard)
Output = Kp * (Input + I_out_total + Kd*Dval); Output = Kp->GetValue() * (Input + I_out_total + Kd->GetValue()*Dval);
} else { else
Output = Kp*Input + I_out_total + Kd*Dval; Output = Kp->GetValue()*Input + I_out_total + Kd->GetValue()*Dval;
}
Input_prev2 = test < 0.0 ? 0.0:Input_prev;
Input_prev = Input; Input_prev = Input;
Input_prev2 = Input_prev;
Clip(); Clip();
if (IsOutput) SetOutput(); if (IsOutput) SetOutput();

55
src/FDM/JSBSim/models/flight_control/FGPID.h
View file

@ -48,6 +48,7 @@ namespace JSBSim {
class FGFCS; class FGFCS;
class Element; class Element;
class FGParameter;
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
CLASS DOCUMENTATION CLASS DOCUMENTATION
@ -59,10 +60,12 @@ CLASS DOCUMENTATION
@code @code
<pid name="{string}" [type="standard"]> <pid name="{string}" [type="standard"]>
<kp> {number|property} </kp> <input> {[-]property} </input>
<ki> {number|property} </ki> <kp> {number|[-]property} </kp>
<kd> {number|property} </kd> <ki type="rect|trap|ab2|ab3"> {number|[-]property} </ki>
<kd> {number|[-]property} </kd>
<trigger> {property} </trigger> <trigger> {property} </trigger>
<pvdot> {property} </pvdot>
</pid> </pid>
@endcode @endcode
@ -78,39 +81,41 @@ For example,
@code @code
<pid name="fcs/heading-control"> <pid name="fcs/heading-control">
<input> fcs/heading-error </input>
<kp> 3 </kp> <kp> 3 </kp>
<ki type="ab3"> 1 </ki> <ki type="ab3"> 1 </ki>
<kd> 1 </kd> <kd> 1 </kd>
</pid> </pid>
@endcode @endcode
<h3>Configuration Parameters:</h3> <h3>Configuration Parameters:</h3>
<pre>
The values of kp, ki, and kd have slightly different interpretations depending on The values of kp, ki, and kd have slightly different interpretations depending
whether the PID controller is a standard one, or an ideal/parallel one - with the latter on whether the PID controller is a standard one, or an ideal/parallel one -
being the default. with the latter being the default.
By default, the PID controller computes the derivative as being the slope of
the line joining the value of the previous input to the value of the current
input. However if a better estimation can be determined for the derivative,
you can provide its value to the PID controller via the property supplied in
pvdot.
kp - Proportional constant, default value 0. kp - Proportional constant, default value 0.
ki - Integrative constant, default value 0. ki - Integrative constant, default value 0.
kd - Derivative constant, default value 0. kd - Derivative constant, default value 0.
trigger - Property which is used to sense wind-up, optional. Most often, the trigger trigger - Property which is used to sense wind-up, optional. Most often, the
will be driven by the "saturated" property of a particular actuator. When trigger will be driven by the "saturated" property of a particular
the relevant actuator has reached it's limits (if there are any, specified actuator. When the relevant actuator has reached it's limits (if
by the <clipto> element) the automatically generated saturated property will there are any, specified by the <clipto> element) the automatically
be greater than zero (true). If this property is used as the trigger for the generated saturated property will be greater than zero (true). If
integrator, the integrator will not continue to integrate while the property this property is used as the trigger for the integrator, the
is still true (> 1), preventing wind-up. integrator will not continue to integrate while the property is
still true (> 1), preventing wind-up.
The integrator can also be reset to 0.0 if the property is set to a
negative value.
pvdot - The property to be used as the process variable time derivative. pvdot - The property to be used as the process variable time derivative.
</pre>
@author Jon S. Berndt @author Jon S. Berndt
@version $Revision: 1.16 $
*/ */
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@ -135,21 +140,15 @@ public:
} }
private: private:
double Kp, Ki, Kd;
double I_out_total; double I_out_total;
double Input_prev, Input_prev2; double Input_prev, Input_prev2;
double KpPropertySign;
double KiPropertySign;
double KdPropertySign;
bool IsStandard; bool IsStandard;
eIntegrateType IntType; eIntegrateType IntType;
FGParameter *Kp, *Ki, *Kd;
FGPropertyNode_ptr Trigger; FGPropertyNode_ptr Trigger;
FGPropertyNode_ptr KpPropertyNode;
FGPropertyNode_ptr KiPropertyNode;
FGPropertyNode_ptr KdPropertyNode;
FGPropertyNode_ptr ProcessVariableDot; FGPropertyNode_ptr ProcessVariableDot;
void Debug(int from); void Debug(int from);

8
src/FDM/JSBSim/models/flight_control/FGSensor.cpp
View file

@ -157,13 +157,11 @@ bool FGSensor::Run(void)
void FGSensor::ProcessSensorSignal(void) void FGSensor::ProcessSensorSignal(void)
{ {
Output = Input; // perfect sensor
// Degrade signal as specified // Degrade signal as specified
if (fail_stuck) { if (!fail_stuck) {
Output = PreviousOutput; Output = Input; // perfect sensor
} else {
if (lag != 0.0) Lag(); // models sensor lag and filter if (lag != 0.0) Lag(); // models sensor lag and filter
if (noise_variance != 0.0) Noise(); // models noise if (noise_variance != 0.0) Noise(); // models noise
if (drift_rate != 0.0) Drift(); // models drift over time if (drift_rate != 0.0) Drift(); // models drift over time