public DeadBand(FlightControlSystem fcs, XmlElement element)
: base(fcs, element)
{
width = null;
gain = 1.0;
XmlElement width_element = element.FindElement("width");
if (width_element != null)
{
width = new ParameterValue(width_element, propertyManager);
}
else
{
width = new RealValue(0.0);
}
XmlElement gain_element = element.FindElement("gain");
if (gain_element != null)
{
gain = FormatHelper.ValueAsNumber(gain_element);
}
Bind(element);
Debug(0);
}
AstronautControlPanel()
{
CommunicationSystem comms = new CommunicationSystem();
ExperimentSystem experiment = new ExperimentSystem();
FlightControlSystem flight = new FlightControlSystem();
LifeSupportSystem lifeSupport = new LifeSupportSystem();
PowerDistributionSystem power = new PowerDistributionSystem();
}
Gyro(FlightControlSystem fcs, XmlElement element)
: base(fcs, element)
{
sensorOrientation = new SensorOrientation(element);
Propagate = fcs.GetExec().GetPropagate();
Debug(0);
}
private void Awake()
{
_flightControlSystem = GetComponentInParent <FlightControlSystem>();
_particleSystem = GetComponent <ParticleSystem>();
_emissionModule = _particleSystem.emission;
StartCoroutine(FxCoroutine());
}
public Summer(FlightControlSystem fcs, XmlElement element)
: base(fcs, element)
{
XmlNodeList childs = element.GetElementsByTagName("bias");
if (childs != null && childs.Count > 0)
{
Bias = FormatHelper.ValueAsNumber(childs[0] as XmlElement);
}
base.Bind();
}
public FCSFunction(FlightControlSystem fcs, XmlElement element)
: base(fcs, element)
{
XmlNodeList childs = element.GetElementsByTagName("function");
if (childs != null && childs.Count > 0)
{
function = new Function(fcs.GetPropertyManager(), childs[0] as XmlElement);
}
base.Bind();
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="fcs">the parent FGFCS object.</param>
/// <param name="element">the configuration file node.the configuration file node.</param>
public Summer(FlightControlSystem fcs, XmlElement element)
: base(fcs, element)
{
XmlElement elem = element.FindElement("bias");
if (elem != null)
{
Bias = FormatHelper.ValueAsNumber(elem);
}
base.Bind();
Debug(0);
}
/// <summary>
/// Initializer.
/// Initializes the FGKinemat object from the given configuration
/// file. The Configuration file is expected to be at the stream
/// position where the KINEMAT object starts. Also it is expected to
/// be past the end of the current KINEMAT configuration on exit.
/// </summary>
/// <param name="fcs">A reference to the ccurrent flightcontrolsystem.</param>
/// <param name="element">reference to the current aircraft configuration element</param>
public Kinemat(FlightControlSystem fcs, XmlElement element)
: base(fcs, element)
{
XmlElement traverse_element, setting_element;
double tmpDetent;
double tmpTime;
detents.Clear();
transitionTimes.Clear();
output = 0;
DoScale = true;
if (element.FindElement("noscale") != null)
{
DoScale = false;
}
traverse_element = element.FindElement("traverse");
var nodeList = traverse_element.GetElementsByTagName("setting");
foreach (var elem in nodeList)
{
if (elem is XmlElement)
{
setting_element = elem as XmlElement;
XmlElement tmpel = setting_element.FindElement("position");
tmpDetent = FormatHelper.ValueAsNumber(tmpel);
tmpel = setting_element.FindElement("time");
tmpTime = FormatHelper.ValueAsNumber(tmpel);
detents.Add(tmpDetent);
transitionTimes.Add(tmpTime);
}
}
if (detents.Count <= 1)
{
log.Error("Kinematic component " + name
+ " must have more than 1 setting element");
throw new Exception();
}
Bind(element);
Debug(0);
}
public FCSFunction(FlightControlSystem fcs, XmlElement element)
: base(fcs, element)
{
XmlElement function_element = element.FindElement("function");
if (function_element != null)
{
function = new Function(fcs.GetExec(), function_element);
}
else
{
log.Error("FCS Function should contain a \"function\" element");
throw new Exception("Malformed FCS function specification.");
}
Bind(element);
Debug(0);
}
public Switch(FlightControlSystem fcs, XmlElement element)
: base(fcs, element)
{
string value;
TestSwitch current_test;
Bind(element); // Bind() this component here in case it is used in its own
// definition for a sample-and-hold
XmlElement test_element = element.FindElement("default");
if (test_element != null)
{
current_test = new TestSwitch();
value = test_element.GetAttribute("value");
current_test.SetTestValue(value, name, propertyManager);
current_test.Default = true;
double tmp;
if (delay > 0 && double.TryParse(value, out tmp))
{ // If there is a delay, initialize the
for (int i = 0; i < delay - 1; i++)
{ // delay buffer to the default value
output_array[i] = tmp; // for the switch if that value is a number.
}
}
tests.Add(current_test);
}
var nodeList = element.GetElementsByTagName("test");
foreach (var elem in nodeList)
{
if (elem is XmlElement)
{
test_element = elem as XmlElement;
current_test = new TestSwitch();
current_test.condition = new Condition(test_element, propertyManager);
value = test_element.GetAttribute("value");
current_test.SetTestValue(value, name, propertyManager);
tests.Add(current_test);
}
}
Debug(0);
}
/// <summary>
/// Initializer.
/// Initializes the FGKinemat object from the given configuration
/// file. The Configuration file is expected to be at the stream
/// position where the KINEMAT object starts. Also it is expected to
/// be past the end of the current KINEMAT configuration on exit.
/// </summary>
/// <param name="fcs">A reference to the ccurrent flightcontrolsystem.</param>
/// <param name="element">reference to the current aircraft configuration element</param>
public Kinemat(FlightControlSystem fcs, XmlElement element)
: base(fcs, element)
{
XmlElement traverse_element;
double tmpDetent;
double tmpTime;
output = 0.0;
DoScale = true;
XmlNodeList childs = element.GetElementsByTagName("noscale");
if (childs != null && childs.Count > 0)
{
DoScale = false;
}
traverse_element = element.GetElementsByTagName("traverse")[0] as XmlElement;
XmlNodeList settingsElements = traverse_element.GetElementsByTagName("setting");
foreach (XmlElement setting_element in settingsElements)
{
childs = setting_element.GetElementsByTagName("position");
tmpDetent = FormatHelper.ValueAsNumber(childs[0] as XmlElement);
childs = setting_element.GetElementsByTagName("time");
tmpTime = FormatHelper.ValueAsNumber(childs[0] as XmlElement);
detents.Add(tmpDetent);
transitionTimes.Add(tmpTime);
}
NumDetents = detents.Count;
if (NumDetents <= 1)
{
if (log.IsErrorEnabled)
{
log.Error("Kinematic component " + name + " must have more than 1 setting element");
}
throw new Exception("Kinematic component must have more than 1 setting element");
}
base.Bind();
}
public virtual void ParametrizeUnityFromModel()
{
// Set mass and inertia
body.mass = (float)model.Mass;
Vector3 inertia = (model.Inertia * Vector <double> .Build.DenseOfArray(new double[] { 1, 1, 1 })).ToUnity();
inertia.y *= -1; // all inertia tensor components must be positive
body.inertiaTensor = inertia;
Debug.Log("Inertia tensor " + body.inertiaTensor);
// TODO what about body.inertiaTensorRotation ?
fcs = model.FCS;
engine = model.Engine;
gearBox = model.GearBox;
fuselage = model.Fuselage;
foreach (var submodelName in submodelTransforms.Keys)
{
var submodel = model.SubModels[submodelName];
var childTransform = submodelTransforms[submodelName];
Debug.Log("Transform from submodel: " + childTransform.name);
childTransform.localPosition = submodel.Translation.ToUnity();
childTransform.localRotation = submodel.Rotation.ToUnity();
if (submodel is Rotor)
{
BoxCollider collider = childTransform.GetComponent <BoxCollider> ();
var sizey = 0.1f;
if (collider == null)
{
collider = childTransform.gameObject.AddComponent <BoxCollider> ();
}
else
{
sizey = collider.size.y;
}
var radius = (float)((Rotor)submodel).radius;
var size = Mathf.Sqrt(2 * radius * radius);
collider.size = new Vector3(size, sizey, size);
}
}
}
public DeadBand(FlightControlSystem fcs, XmlElement element)
: base(fcs, element)
{
foreach (XmlNode currentNode in element.ChildNodes)
{
if (currentNode.NodeType == XmlNodeType.Element)
{
XmlElement currentElement = (XmlElement)currentNode;
if (currentElement.LocalName.Equals("width"))
{
width = FormatHelper.ValueAsNumber(currentElement);
}
else if (currentElement.LocalName.Equals("gain"))
{
gain = FormatHelper.ValueAsNumber(currentElement);
}
}
}
base.Bind();
}
public Filter(FlightControlSystem fcs, XmlElement element)
: base(fcs, element)
{
DynamicFilter = false; initialize = true;
C[1] = C[2] = C[3] = C[4] = C[5] = C[6] = null;
for (int i = 1; i < 7; i++)
{
ReadFilterCoefficients(element, i);
}
if (compType == "LAG_FILTER")
{
FilterType = FilterTypeEnum.Lag;
}
else if (compType == "LEAD_LAG_FILTER")
{
FilterType = FilterTypeEnum.LeadLag;
}
else if (compType == "SECOND_ORDER_FILTER")
{
FilterType = FilterTypeEnum.Order2;
}
else if (compType == "WASHOUT_FILTER")
{
FilterType = FilterTypeEnum.Washout;
}
else
{
FilterType = FilterTypeEnum.Unknown;
}
CalculateDynamicFilters();
Bind(element);
Debug(0);
}
public Accelerometer(FlightControlSystem fcs, XmlElement element)
: base(fcs, element)
{
sensorOrientation = new SensorOrientation(element);
Propagate = fcs.GetExec().GetPropagate();
Accelerations = fcs.GetExec().Accelerations;
MassBalance = fcs.GetExec().MassBalance;
XmlElement location_element = element.FindElement("location");
if (location_element != null)
{
vLocation = FormatHelper.TripletConvertTo(location_element, "IN");
}
else
{
log.Error("No location given for accelerometer. ");
throw new Exception("Malformed accelerometer specification");
}
vRadius = MassBalance.StructuralToBody(vLocation);
Debug(0);
}
/// Constructor
public FCSComponent(FlightControlSystem fcsParent, XmlElement element)
{
fcs = fcsParent;
propertyManager = fcs.GetPropertyManager();
compType = "";
isOutput = false;
name = element.GetAttribute("name");
compType = element.GetAttribute("type"); // Old, deprecated format
if (compType.Length == 0)
{
if (element.LocalName.Equals("lag_filter"))
{
compType = "LAG_FILTER";
}
else if (element.LocalName.Equals("lead_lag_filter"))
{
compType = "LEAD_LAG_FILTER";
}
else if (element.LocalName.Equals("washout_filter"))
{
compType = "WASHOUT_FILTER";
}
else if (element.LocalName.Equals("second_order_filter"))
{
compType = "SECOND_ORDER_FILTER";
}
else if (element.LocalName.Equals("integrator"))
{
compType = "INTEGRATOR";
}
else if (element.LocalName.Equals("summer"))
{
compType = "SUMMER";
}
else if (element.LocalName.Equals("pure_gain"))
{
compType = "PURE_GAIN";
}
else if (element.LocalName.Equals("scheduled_gain"))
{
compType = "SCHEDULED_GAIN";
}
else if (element.LocalName.Equals("aerosurface_scale"))
{
compType = "AEROSURFACE_SCALE";
}
else if (element.LocalName.Equals("switch"))
{
compType = "SWITCH";
}
else if (element.LocalName.Equals("kinematic"))
{
compType = "KINEMATIC";
}
else if (element.LocalName.Equals("deadband"))
{
compType = "DEADBAND";
}
else if (element.LocalName.Equals("fcs_function"))
{
compType = "FCS_FUNCTION";
}
else if (element.LocalName.Equals("sensor"))
{
compType = "SENSOR";
}
else
{ // illegal component in this channel
compType = "UNKNOWN";
}
}
foreach (XmlNode currentNode in element.ChildNodes)
{
if (currentNode.NodeType == XmlNodeType.Element)
{
XmlElement currentElement = (XmlElement)currentNode;
if (currentElement.LocalName.Equals("input"))
{
string inputTxt = currentElement.InnerText.Trim();
if (inputTxt[0] == '-')
{
inputSigns.Add(-1.0f);
inputTxt = inputTxt.Remove(0, 1);
}
else
{
inputSigns.Add(1.0f);
}
inputNodes.Add(ResolveSymbol(inputTxt));
}
else if (currentElement.LocalName.Equals("output"))
{
isOutput = true;
outputNode = propertyManager.GetPropertyNode(currentElement.InnerText.Trim());
if (outputNode == null)
{
log.Error(" Unable to process property: " + currentElement.InnerText);
throw new Exception("Invalid output property name in flight control definition");
}
else if (currentElement.LocalName.Equals("clipto"))
{
XmlNodeList childs;
string clip_string;
childs = currentElement.GetElementsByTagName("min");
if (childs != null)
{
clip_string = ((XmlElement)childs[0]).InnerText.Trim();
}
else
{
throw new Exception("clipto doesn't have a min tag");
}
if (!clip_string.StartsWith("+-.0123456789"))
{ // it's a property
if (clip_string[0] == '-')
{
clipMinSign = -1.0f;
}
clip_string = clip_string.Remove(0, 1); //TODO test it
ClipMinPropertyNode = propertyManager.GetPropertyNode(clip_string);
}
else
{
clipmin = double.Parse(clip_string, FormatHelper.numberFormatInfo);
}
childs = currentElement.GetElementsByTagName("max");
if (childs != null)
{
clip_string = ((XmlElement)childs[0]).InnerText.Trim();
}
else
{
throw new Exception("clipto doesn't have a max tag");
}
if (!clip_string.StartsWith("+-.0123456789"))
{ // it's a property
if (clip_string[0] == '-')
{
clipMaxSign = -1.0f;
}
clip_string = clip_string.Remove(0, 1); //TODO test it
ClipMaxPropertyNode = propertyManager.GetPropertyNode(clip_string);
}
else
{
clipmax = double.Parse(clip_string, FormatHelper.numberFormatInfo);
}
clip = true;
}
}
}
}
}
// Use this for initialization
void Start()
{
FCS = GetComponent <FlightControlSystem>();
}
public Actuator(FlightControlSystem fcs, XmlElement element)
: base(fcs, element)
{
// inputs are read from the base class constructor
PreviousOutput = 0.0;
PreviousHystOutput = 0.0;
PreviousRateLimOutput = 0.0;
PreviousLagInput = PreviousLagOutput = 0.0;
bias = lag = hysteresis_width = deadband_width = 0.0;
rate_limit_incr = rate_limit_decr = null; // no limit
fail_zero = fail_hardover = fail_stuck = false;
ca = cb = 0.0;
initialized = false;
saturated = false;
if (element.FindElement("deadband_width") != null)
{
deadband_width = element.FindElementValueAsNumber("deadband_width");
}
if (element.FindElement("hysteresis_width") != null)
{
hysteresis_width = element.FindElementValueAsNumber("hysteresis_width");
}
// There can be a single rate limit specified, or increasing and
// decreasing rate limits specified, and rate limits can be numeric, or
// a property.
XmlNodeList nodeList = element.GetElementsByTagName("rate_limit");
foreach (var node in nodeList)
{
XmlElement ratelim_el = node as XmlElement;
if (ratelim_el != null)
{
string rate_limit_str = ratelim_el.InnerText;
Parameter rate_limit = new ParameterValue(rate_limit_str,
propertyManager);
if (ratelim_el.HasAttribute("sense"))
{
string sense = ratelim_el.GetAttribute("sense");
if (sense.StartsWith("incr"))
{
rate_limit_incr = rate_limit;
}
else if (sense.StartsWith("decr"))
{
rate_limit_decr = rate_limit;
}
}
else
{
rate_limit_incr = rate_limit;
rate_limit_decr = rate_limit;
}
}
}
if (element.FindElement("bias") != null)
{
bias = element.FindElementValueAsNumber("bias");
}
if (element.FindElement("lag") != null)
{
lag = element.FindElementValueAsNumber("lag");
double denom = 2.00 + dt * lag;
ca = dt * lag / denom;
cb = (2.00 - dt * lag) / denom;
}
Bind(element);
Debug(0);
}
public Gain(FlightControlSystem fcs, XmlElement element)
: base(fcs, element)
{
XmlElement scale_element, zero_centered;
XmlNodeList childs;
string gain_string, sZeroCentered;
XmlElement gainElement = element.GetElementsByTagName("gain")[0] as XmlElement;
if (compType.Equals("PURE_GAIN"))
{
if (gainElement == null)
{
if (log.IsErrorEnabled)
{
log.Error("No GAIN specified (default: 1.0)");
}
}
}
if (gainElement != null)
{
gain_string = gainElement.InnerText.Trim();
Match match = testRegex.Match(gain_string);
if (match.Success && match.Groups["prop1"].Value.Length != 0)
{ // property
gainPropertyNode = fcs.GetPropertyManager().GetPropertyNode(match.Groups["prop1"].Value);
}
else
{
gain = FormatHelper.ValueAsNumber(gainElement);
}
}
if (compType.Equals("AEROSURFACE_SCALE"))
{
scale_element = element.GetElementsByTagName("domain")[0] as XmlElement;
if (scale_element != null)
{
childs = scale_element.GetElementsByTagName("max");
inMax = FormatHelper.ValueAsNumber(childs[0] as XmlElement);
childs = scale_element.GetElementsByTagName("min");
inMin = FormatHelper.ValueAsNumber(childs[0] as XmlElement);
}
scale_element = element.GetElementsByTagName("range")[0] as XmlElement;
if (scale_element == null)
{
if (log.IsErrorEnabled)
{
log.Error("No range supplied for aerosurface scale component");
}
throw new Exception("No range supplied for aerosurface scale component");
}
try
{
childs = scale_element.GetElementsByTagName("max");
outMax = FormatHelper.ValueAsNumber(childs[0] as XmlElement);
childs = scale_element.GetElementsByTagName("min");
outMin = FormatHelper.ValueAsNumber(childs[0] as XmlElement);
}
catch (Exception e)
{
if (log.IsErrorEnabled)
{
log.Error("Maximum and minimum output values must be supplied for the " +
"aerosurface scale component");
}
throw new Exception("Maximum and minimum output values must be supplied. Catch exception: " + e);
}
zeroCentered = true;
zero_centered = element.GetElementsByTagName("zero_centered")[0] as XmlElement;
if (zero_centered != null)
{
sZeroCentered = zero_centered.InnerText.Trim();
if (sZeroCentered.Equals("0") || sZeroCentered.Equals("false"))
{
zeroCentered = false;
}
}
}
if (compType.Equals("SCHEDULED_GAIN"))
{
XmlElement tableElement = element.GetElementsByTagName("table")[0] as XmlElement;
if (tableElement != null)
{
table = new Table(fcs.GetPropertyManager(), tableElement);
}
else
{
if (log.IsErrorEnabled)
{
log.Error("A table must be provided for the scheduled gain component");
}
throw new Exception("A table must be provided for the scheduled gain component");
}
}
base.Bind();
}
public Switch(FlightControlSystem fcs, XmlElement element)
: base(fcs, element)
{
string value, logic;
TestSwitch current_test;
foreach (XmlNode currentNode in element.ChildNodes)
{
if (currentNode.NodeType == XmlNodeType.Element)
{
XmlElement currentElement = (XmlElement)currentNode;
current_test = new TestSwitch();
if (currentElement.LocalName.Equals("default"))
{
tests.Add(current_test);
current_test.Logic = eLogic.eDefault;
}
else if (currentElement.LocalName.Equals("test"))
{
tests.Add(current_test);
logic = currentElement.GetAttribute("logic");
if (logic.Equals("OR"))
{
current_test.Logic = eLogic.eOR;
}
else if (logic.Equals("AND"))
{
current_test.Logic = eLogic.eAND;
}
else if (logic.Length == 0)
{
current_test.Logic = eLogic.eAND; // default
}
else
{ // error
if (log.IsErrorEnabled)
{
log.Error("Unrecognized LOGIC token " + logic + " in switch component: " + name);
}
}
ReaderText rtxt = new ReaderText(new StringReader(currentElement.InnerText));
while (rtxt.Done)
{
string tmp = rtxt.ReadLine().Trim();
if (tmp.Length != 0)
{
current_test.conditions.Add(new Condition(tmp, propertyManager));
}
}
foreach (XmlNode currentNode2 in currentElement.ChildNodes)
{
if (currentNode2.NodeType == XmlNodeType.Element)
{
current_test.conditions.Add(new Condition(currentNode2 as XmlElement, propertyManager));
}
}
}
if (!currentElement.LocalName.Equals("output"))
{
value = currentElement.GetAttribute("value");
if (value.Length == 0)
{
if (log.IsErrorEnabled)
{
log.Error("No VALUE supplied for switch component: " + name);
}
}
else
{
Match match = testRegex.Match(value);
if (match.Success)
{
if (match.Groups["prop"].Value == "") // if true (and execution falls into this block), "value" is a number.
{
current_test.OutputVal = double.Parse(value, FormatHelper.numberFormatInfo);
}
else
{
// "value" must be a property if execution passes to here.
if (value[0] == '-')
{
current_test.sign = -1.0;
value = value.Remove(0, 1);
}
else
{
current_test.sign = 1.0;
}
current_test.OutputProp = propertyManager.GetPropertyNode(value);
}
}
}
}
}
}
base.Bind();
}
public PID(FlightControlSystem fcs, XmlElement element)
: base(fcs, element)
{
XmlElement el;
I_out_total = 0.0;
Input_prev = Input_prev2 = 0.0;
Trigger = null;
ProcessVariableDot = null;
IsStandard = false;
IntType = eIntegrateType.eNone; // No integrator initially defined.
string pid_type = element.GetAttribute("type");
if (pid_type == "standard")
{
IsStandard = true;
}
el = element.FindElement("kp");
if (el != null)
{
Kp = new ParameterValue(el, propertyManager);
}
else
{
Kp = new RealValue(0.0);
}
el = element.FindElement("ki");
if (el != null)
{
string integ_type = el.GetAttribute("type");
if (integ_type == "rect")
{ // Use rectangular integration
IntType = eIntegrateType.eRectEuler;
}
else if (integ_type == "trap")
{ // Use trapezoidal integration
IntType = eIntegrateType.eTrapezoidal;
}
else if (integ_type == "ab2")
{ // Use Adams Bashforth 2nd order integration
IntType = eIntegrateType.eAdamsBashforth2;
}
else if (integ_type == "ab3")
{ // Use Adams Bashforth 3rd order integration
IntType = eIntegrateType.eAdamsBashforth3;
}
else
{ // Use default Adams Bashforth 2nd order integration
IntType = eIntegrateType.eAdamsBashforth2;
}
Ki = new ParameterValue(el, propertyManager);
}
else
{
Ki = new RealValue(0.0);
}
el = element.FindElement("kd");
if (el != null)
{
Kd = new ParameterValue(el, propertyManager);
}
else
{
Kd = new RealValue(0.0);
}
el = element.FindElement("pvdot");
if (el != null)
{
ProcessVariableDot = new PropertyValue(el.InnerText, propertyManager);
}
el = element.FindElement("trigger");
if (el != null)
{
Trigger = new PropertyValue(el.InnerText, propertyManager);
}
Bind(el);
}
public Filter(FlightControlSystem fcs, XmlElement element)
: base(fcs, element)
{
double denom;
dt = fcs.GetState().DeltaTime;
if (compType == "LAG_FILTER")
{
filterType = FilterType.Lag;
}
else if (compType == "LEAD_LAG_FILTER")
{
filterType = FilterType.LeadLag;
}
else if (compType == "SECOND_ORDER_FILTER")
{
filterType = FilterType.Order2;
}
else if (compType == "WASHOUT_FILTER")
{
filterType = FilterType.Washout;
}
else if (compType == "INTEGRATOR")
{
filterType = FilterType.Integrator;
}
else
{
filterType = FilterType.Unknown;
}
foreach (XmlNode currentNode in element.ChildNodes)
{
if (currentNode.NodeType == XmlNodeType.Element)
{
XmlElement currentElement = (XmlElement)currentNode;
if (currentElement.LocalName.Equals("c1"))
{
C1 = FormatHelper.ValueAsNumber(currentElement);
}
else if (currentElement.LocalName.Equals("c2"))
{
C1 = FormatHelper.ValueAsNumber(currentElement);
}
else if (currentElement.LocalName.Equals("c3"))
{
C1 = FormatHelper.ValueAsNumber(currentElement);
}
else if (currentElement.LocalName.Equals("c4"))
{
C1 = FormatHelper.ValueAsNumber(currentElement);
}
else if (currentElement.LocalName.Equals("c5"))
{
C1 = FormatHelper.ValueAsNumber(currentElement);
}
else if (currentElement.LocalName.Equals("c6"))
{
C1 = FormatHelper.ValueAsNumber(currentElement);
}
else if (currentElement.LocalName.Equals("trigger"))
{
trigger = ResolveSymbol(currentElement.InnerText);
}
/*
* else
* {
* if (log.IsErrorEnabled)
* log.Error("Error reading Filter. Tag unknown: " + currentElement.LocalName);
* throw new Exception("Error reading Filter.");
* }
*/
}
}
initialize = true;
switch (filterType)
{
case FilterType.Lag:
denom = 2.00 + dt * C1;
ca = dt * C1 / denom;
cb = (2.00 - dt * C1) / denom;
break;
case FilterType.LeadLag:
denom = 2.00 * C3 + dt * C4;
ca = (2.00 * C1 + dt * C2) / denom;
cb = (dt * C2 - 2.00 * C1) / denom;
cc = (2.00 * C3 - dt * C4) / denom;
break;
case FilterType.Order2:
denom = 4.0 * C4 + 2.0 * C5 * dt + C6 * dt * dt;
ca = (4.0 * C1 + 2.0 * C2 * dt + C3 * dt * dt) / denom;
cb = (2.0 * C3 * dt * dt - 8.0 * C1) / denom;
cc = (4.0 * C1 - 2.0 * C2 * dt + C3 * dt * dt) / denom;
cd = (2.0 * C6 * dt * dt - 8.0 * C4) / denom;
ce = (4.0 * C4 - 2.0 * C5 * dt + C6 * dt * dt) / denom;
break;
case FilterType.Washout:
denom = 2.00 + dt * C1;
ca = 2.00 / denom;
cb = (2.00 - dt * C1) / denom;
break;
case FilterType.Integrator:
ca = dt * C1 / 2.00;
break;
case FilterType.Unknown:
if (log.IsErrorEnabled)
{
log.Error("Error reading Filter. Unknown filter type.");
}
throw new Exception("Unknown filter type.");
//break;
}
base.Bind();
}
public Gain(FlightControlSystem fcs, XmlElement element)
: base(fcs, element)
{
gain = null;
Table = null;
InMin = -1.0;
InMax = 1.0;
OutMin = OutMax = 0.0;
if (compType == "PURE_GAIN")
{
if (element.FindElement("gain") == null)
{
log.Error(" No GAIN specified (default: 1.0)");
}
}
XmlElement gain_element = element.FindElement("gain");
if (gain_element != null)
{
gain = new ParameterValue(gain_element, propertyManager);
}
else
{
gain = new RealValue(1.0);
}
if (compType == "AEROSURFACE_SCALE")
{
XmlElement scale_element = element.FindElement("domain");
if (scale_element != null)
{
if (scale_element.FindElement("max") != null && scale_element.FindElement("min") != null)
{
XmlElement elem = scale_element.FindElement("max");
InMax = FormatHelper.ValueAsNumber(elem);
elem = scale_element.FindElement("min");
InMin = FormatHelper.ValueAsNumber(elem);
}
}
scale_element = element.FindElement("range");
if (scale_element == null)
{
throw new Exception("No range supplied for aerosurface scale component");
}
if (scale_element.FindElement("max") != null && scale_element.FindElement("min") != null)
{
XmlElement elem = scale_element.FindElement("max");
OutMax = FormatHelper.ValueAsNumber(elem);
elem = scale_element.FindElement("min");
OutMin = FormatHelper.ValueAsNumber(elem);
}
else
{
log.Error("Maximum and minimum output values must be supplied for the " +
"aerosurface scale component");
throw new Exception("Some inputs are missing.");
}
ZeroCentered = true;
XmlElement zero_centered = element.FindElement("zero_centered");
//ToDo if zero centered, then mins must be <0 and max's must be >0
if (zero_centered != null)
{
string sZeroCentered = element.FindElementValue("zero_centered");
if (sZeroCentered == "0" || sZeroCentered == "false")
{
ZeroCentered = false;
}
}
}
if (compType == "SCHEDULED_GAIN")
{
if (element.FindElement("table") != null)
{
Table = new Table(propertyManager, element.FindElement("table"));
}
else
{
log.Error("A table must be provided for the scheduled gain component");
throw new Exception("Some inputs are missing.");
}
}
Bind(element);
Debug(0);
}
private bool Allocate()
{
bool result = true;
atmosphere = new Atmosphere(this);
propulsion = new Propulsion(this);
aerodynamics = new Aerodynamics(this);
FCS = new FlightControlSystem(this);
groundReactions = new GroundReactions(this);
inertial = new Inertial(this);
massBalance = new MassBalance(this);
propagate = new Propagate(this);
auxiliary = new Auxiliary(this);
aircraft = new Aircraft(this);
//output = new Output(this);
input = new Input(this);
groundCallback = new GroundCallback();
state = new State(this); // This must be done here, as the State
// class needs valid pointers to the above model classes
// Initialize models so they can communicate with each other
if (!atmosphere.InitModel())
{
if (log.IsErrorEnabled)
{
log.Error("Atmosphere model init failed");
}
error += 1;
}
if (!FCS.InitModel())
{
if (log.IsErrorEnabled)
{
log.Error("FCS model init failed");
}
error += 2;
}
if (!propulsion.InitModel())
{
if (log.IsErrorEnabled)
{
log.Error("Propulsion model init failed");
}
error += 4;
}
if (!massBalance.InitModel())
{
if (log.IsErrorEnabled)
{
log.Error("MassBalance model init failed");
}
error += 8;
}
if (!aerodynamics.InitModel())
{
if (log.IsErrorEnabled)
{
log.Error("Aerodynamics model init failed");
}
error += 16;
}
if (!inertial.InitModel())
{
if (log.IsErrorEnabled)
{
log.Error("Inertial model init failed");
}
error += 32;
}
if (!groundReactions.InitModel())
{
if (log.IsErrorEnabled)
{
log.Error("Ground Reactions model init failed");
}
error += 64;
}
if (!aircraft.InitModel())
{
if (log.IsErrorEnabled)
{
log.Error("Aircraft model init failed");
}
error += 128;
}
if (!propagate.InitModel())
{
if (log.IsErrorEnabled)
{
log.Error("Propagate model init failed");
}
error += 256;
}
if (!auxiliary.InitModel())
{
if (log.IsErrorEnabled)
{
log.Error("Auxiliary model init failed");
}
error += 512;
}
if (!input.InitModel())
{
if (log.IsErrorEnabled)
{
log.Error("Intput model init failed");
}
error += 1024;
}
if (error > 0)
{
result = false;
}
ic = new InitialCondition(this);
// Schedule a model. The second arg (the integer) is the pass number. For
// instance, the atmosphere model gets executed every fifth pass it is called
// by the executive. Everything else here gets executed each pass.
// IC and Trim objects are NOT scheduled.
Schedule(input, 1);
Schedule(atmosphere, 1);
Schedule(FCS, 1);
Schedule(propulsion, 1);
Schedule(massBalance, 1);
Schedule(aerodynamics, 1);
Schedule(inertial, 1);
Schedule(groundReactions, 1);
Schedule(aircraft, 1);
Schedule(propagate, 1);
Schedule(auxiliary, 1);
//Schedule(output, 1);
modelLoaded = false;
return(result);
}
public Angles(FlightControlSystem fcs, XmlElement element)
: base(fcs, element)
{
source_angle = 0.0;
target_angle = 0.0;
source_angle_unit = 1.0;
target_angle_unit = 1.0;
output_unit = 1.0;
if (element.FindElement("target_angle") != null)
{
target_angle_pNode = propertyManager.GetNode(element.FindElementValue("target_angle"));
if (element.FindElement("target_angle").HasAttribute("unit"))
{
if (element.FindElement("target_angle").GetAttribute("unit") == "DEG")
{
target_angle_unit = 0.017453293;
}
}
}
else
{
throw new Exception("Target angle is required for component: " + name);
}
if (element.FindElement("source_angle") != null)
{
source_angle_pNode = propertyManager.GetNode(element.FindElementValue("source_angle"));
if (element.FindElement("source_angle").HasAttribute("unit"))
{
if (element.FindElement("source_angle").GetAttribute("unit") == "DEG")
{
source_angle_unit = 0.017453293;
}
}
}
else
{
throw new Exception("Source latitude is required for Angles component: " + name);
}
unit = element.GetAttribute("unit");
if (!string.IsNullOrEmpty(unit))
{
if (unit == "DEG")
{
output_unit = 180.0 / Math.PI;
}
else if (unit == "RAD")
{
output_unit = 1.0;
}
else
{
throw new Exception("Unknown unit " + unit + " in angle component, " + name);
}
}
else
{
output_unit = 1.0; // Default is radians (1.0) if unspecified
}
Bind(element);
Debug(0);
}
public Sensor(FlightControlSystem fcs, XmlElement element)
: base(fcs, element)
{
// inputs are read from the base class constructor
bits = quantized = divisions = 0;
PreviousInput = PreviousOutput = 0.0;
min = max = bias = gain = noise_variance = lag = drift_rate = drift = span = 0.0;
granularity = 0.0;
noise_type = 0;
fail_low = fail_high = fail_stuck = false;
XmlElement quantization_element = element.FindElement("quantization");
if (quantization_element != null)
{
if (quantization_element.FindElement("bits") != null)
{
bits = (int)quantization_element.FindElementValueAsNumber("bits");
}
divisions = (1 << bits);
if (quantization_element.FindElement("min") != null)
{
min = quantization_element.FindElementValueAsNumber("min");
}
if (quantization_element.FindElement("max") != null)
{
max = quantization_element.FindElementValueAsNumber("max");
}
quant_property = quantization_element.GetAttribute("name");
span = max - min;
granularity = span / divisions;
}
if (element.FindElement("bias") != null)
{
bias = element.FindElementValueAsNumber("bias");
}
if (element.FindElement("gain") != null)
{
gain = element.FindElementValueAsNumber("gain");
}
if (element.FindElement("drift_rate") != null)
{
drift_rate = element.FindElementValueAsNumber("drift_rate");
}
if (element.FindElement("lag") != null)
{
lag = element.FindElementValueAsNumber("lag");
double denom = 2.00 + dt * lag;
ca = dt * lag / denom;
cb = (2.00 - dt * lag) / denom;
}
if (element.FindElement("noise") != null)
{
noise_variance = element.FindElementValueAsNumber("noise");
string variation = element.FindElement("noise").GetAttribute("variation");
if (variation == "PERCENT")
{
NoiseType = eNoiseType.ePercent;
}
else if (variation == "ABSOLUTE")
{
NoiseType = eNoiseType.eAbsolute;
}
else
{
NoiseType = eNoiseType.ePercent;
log.Error("Unknown noise type in sensor: " + name);
log.Error(" defaulting to PERCENT.");
}
string distribution = element.FindElement("noise").GetAttribute("distribution");
if (distribution == "UNIFORM")
{
DistributionType = eDistributionType.eUniform;
}
else if (distribution == "GAUSSIAN")
{
DistributionType = eDistributionType.eGaussian;
}
else
{
DistributionType = eDistributionType.eUniform;
log.Error("Unknown random distribution type in sensor: " + name);
log.Error(" defaulting to UNIFORM.");
}
}
Bind(element);
Debug(0);
}
public Sensor(FlightControlSystem fcs, XmlElement element)
: base(fcs, element)
{
double denom;
dt = fcs.GetState().DeltaTime;
XmlElement tmpElem;
// inputs are read from the base class constructor
XmlElement quantization_element = element.GetElementsByTagName("quantization")[0] as XmlElement;
if (quantization_element != null)
{
tmpElem = quantization_element.GetElementsByTagName("bits")[0] as XmlElement;
if (tmpElem != null)
{
bits = (int)FormatHelper.ValueAsNumber(tmpElem);
}
divisions = (1 << bits);
tmpElem = quantization_element.GetElementsByTagName("min")[0] as XmlElement;
if (tmpElem != null)
{
min = FormatHelper.ValueAsNumber(tmpElem);
}
tmpElem = quantization_element.GetElementsByTagName("max")[0] as XmlElement;
if (tmpElem != null)
{
max = FormatHelper.ValueAsNumber(tmpElem);
}
span = max - min;
granularity = span / divisions;
}
tmpElem = quantization_element.GetElementsByTagName("bias")[0] as XmlElement;
if (tmpElem != null)
{
bias = FormatHelper.ValueAsNumber(tmpElem);
}
tmpElem = quantization_element.GetElementsByTagName("drift_rate")[0] as XmlElement;
if (tmpElem != null)
{
drift_rate = FormatHelper.ValueAsNumber(tmpElem);
}
tmpElem = quantization_element.GetElementsByTagName("lag")[0] as XmlElement;
if (tmpElem != null)
{
lag = FormatHelper.ValueAsNumber(tmpElem);
denom = 2.00 + dt * lag;
ca = dt * lag / denom;
cb = (2.00 - dt * lag) / denom;
}
tmpElem = quantization_element.GetElementsByTagName("noise")[0] as XmlElement;
if (tmpElem != null)
{
noise_variance = FormatHelper.ValueAsNumber(tmpElem);
string variation = tmpElem.GetAttribute("variation");
if (variation.Equals("PERCENT"))
{
noiseType = NoiseType.ePercent;
}
else if (variation.Equals("ABSOLUTE"))
{
noiseType = NoiseType.eAbsolute;
}
else
{
noiseType = NoiseType.ePercent;
if (log.IsErrorEnabled)
{
log.Error("Unknown noise type in sensor: " + name + ". Defaulting to PERCENT.");
}
}
}
base.Bind();
Bind();
}
public Waypoint(FlightControlSystem fcs, XmlElement element) : base(fcs, element)
{
if (compType == "WAYPOINT_HEADING")
{
WaypointType = eWaypointType.eHeading;
}
else if (compType == "WAYPOINT_DISTANCE")
{
WaypointType = eWaypointType.eDistance;
}
target_latitude_unit = 1.0;
target_longitude_unit = 1.0;
source_latitude_unit = 1.0;
source_longitude_unit = 1.0;
source = fcs.GetExec().GetIC().GetPosition();
if (element.FindElement("target_latitude") != null)
{
target_latitude = new PropertyValue(element.FindElementValue("target_latitude"),
propertyManager);
if (element.FindElement("target_latitude").HasAttribute("unit"))
{
if (element.FindElement("target_latitude").GetAttribute("unit") == "DEG")
{
target_latitude_unit = 0.017453293;
}
}
}
else
{
log.Error("Target latitude is required for waypoint component: " + name);
throw new Exception("Malformed waypoint definition");
}
if (element.FindElement("target_longitude") != null)
{
target_longitude = new PropertyValue(element.FindElementValue("target_longitude"),
propertyManager);
if (element.FindElement("target_longitude").HasAttribute("unit"))
{
if (element.FindElement("target_longitude").GetAttribute("unit") == "DEG")
{
target_longitude_unit = 0.017453293;
}
}
}
else
{
log.Error("Target longitude is required for waypoint component: " + name);
throw new Exception("Malformed waypoint definition");
}
if (element.FindElement("source_latitude") != null)
{
source_latitude = new PropertyValue(element.FindElementValue("source_latitude"),
propertyManager);
if (element.FindElement("source_latitude").HasAttribute("unit"))
{
if (element.FindElement("source_latitude").GetAttribute("unit") == "DEG")
{
source_latitude_unit = 0.017453293;
}
}
}
else
{
log.Error("Source latitude is required for waypoint component: " + name);
throw new Exception("Malformed waypoint definition");
}
if (element.FindElement("source_longitude") != null)
{
source_longitude = new PropertyValue(element.FindElementValue("source_longitude"),
propertyManager);
if (element.FindElement("source_longitude").HasAttribute("unit"))
{
if (element.FindElement("source_longitude").GetAttribute("unit") == "DEG")
{
source_longitude_unit = 0.017453293;
}
}
}
else
{
log.Error("Source longitude is required for waypoint component: " + name);
throw new Exception("Malformed waypoint definition");
}
if (element.FindElement("radius") != null)
{
XmlElement elem = element.FindElement("radius");
radius = FormatHelper.ValueAsNumberConvertTo(elem, "FT");
}
else
{
radius = -1.0;
}
unit = element.GetAttribute("unit");
if (WaypointType == eWaypointType.eHeading)
{
if (!string.IsNullOrEmpty(unit))
{
if (unit == "DEG")
{
eUnit = eUnitType.eDeg;
}
else if (unit == "RAD")
{
eUnit = eUnitType.eRad;
}
else
{
log.Error("Unknown unit " + unit + " in HEADING waypoint component, "
+ name);
throw new Exception("Malformed waypoint definition");
}
}
else
{
eUnit = eUnitType.eRad; // Default is radians if unspecified
}
}
else
{
if (!string.IsNullOrEmpty(unit))
{
if (unit == "FT")
{
eUnit = eUnitType.eFeet;
}
else if (unit == "M")
{
eUnit = eUnitType.eMeters;
}
else
{
log.Error("Unknown unit " + unit + " in DISTANCE waypoint component, "
+ name);
throw new Exception("Malformed waypoint definition");
}
}
else
{
eUnit = eUnitType.eFeet; // Default is feet if unspecified
}
}
Bind(element);
Debug(0);
}
public Gradient(FlightControlSystem fcs, XmlElement element)
: base(fcs, element)
{
base.Bind();
}
/// Constructor
public FCSComponent(FlightControlSystem fcsParent, XmlElement element)
{
input = output = delay_time = 0.0;
delay = index = 0;
ClipMin = ClipMax = new RealValue(0.0);
clip = cyclic_clip = false;
dt = fcs.GetChannelDeltaT();
propertyManager = fcs.GetPropertyManager();
if (element.LocalName.Equals("lag_filter"))
{
compType = "LAG_FILTER";
}
else if (element.LocalName.Equals("lead_lag_filter"))
{
compType = "LEAD_LAG_FILTER";
}
else if (element.LocalName.Equals("washout_filter"))
{
compType = "WASHOUT_FILTER";
}
else if (element.LocalName.Equals("second_order_filter"))
{
compType = "SECOND_ORDER_FILTER";
}
else if (element.LocalName.Equals("integrator"))
{
compType = "INTEGRATOR";
}
else if (element.LocalName.Equals("summer"))
{
compType = "SUMMER";
}
else if (element.LocalName.Equals("pure_gain"))
{
compType = "PURE_GAIN";
}
else if (element.LocalName.Equals("scheduled_gain"))
{
compType = "SCHEDULED_GAIN";
}
else if (element.LocalName.Equals("aerosurface_scale"))
{
compType = "AEROSURFACE_SCALE";
}
else if (element.LocalName.Equals("switch"))
{
compType = "SWITCH";
}
else if (element.LocalName.Equals("kinematic"))
{
compType = "KINEMATIC";
}
else if (element.LocalName.Equals("deadband"))
{
compType = "DEADBAND";
}
else if (element.LocalName.Equals("fcs_function"))
{
compType = "FCS_FUNCTION";
}
else if (element.LocalName.Equals("pid"))
{
compType = "PID";
}
else if (element.LocalName.Equals("sensor"))
{
compType = "SENSOR";
}
else if (element.LocalName.Equals("accelerometer"))
{
compType = "ACCELEROMETER";
}
else if (element.LocalName.Equals("magnetometer"))
{
compType = "MAGNETOMETER";
}
else if (element.LocalName.Equals("gyro"))
{
compType = "GYRO";
}
else if (element.LocalName.Equals("actuator"))
{
compType = "ACTUATOR";
}
else if (element.LocalName.Equals("waypoint_heading"))
{
compType = "WAYPOINT_HEADING";
}
else if (element.LocalName.Equals("waypoint_distance"))
{
compType = "WAYPOINT_DISTANCE";
}
else if (element.LocalName.Equals("angle"))
{
compType = "ANGLE";
}
else if (element.LocalName.Equals("distributor"))
{
compType = "DISTRIBUTOR";
}
else
{ // illegal component in this channel
compType = "UNKNOWN";
}
name = element.GetAttribute("name");
foreach (XmlNode currentNode in element.GetElementsByTagName("init"))
{
if (currentNode.NodeType == XmlNodeType.Element)
{
XmlElement init_element = (XmlElement)currentNode;
initNodes.Add(new PropertyValue(init_element.InnerText, propertyManager));
}
}
foreach (XmlNode currentNode in element.GetElementsByTagName("input"))
{
if (currentNode.NodeType == XmlNodeType.Element)
{
XmlElement input_element = (XmlElement)currentNode;
inputNodes.Add(new PropertyValue(input_element.InnerText, propertyManager));
}
}
foreach (XmlNode currentNode in element.GetElementsByTagName("output"))
{
if (currentNode.NodeType == XmlNodeType.Element)
{
XmlElement out_elem = (XmlElement)currentNode;
string output_node_name = out_elem.InnerText;
bool node_exists = propertyManager.HasNode(output_node_name);
PropertyNode OutputNode = propertyManager.GetNode(output_node_name, true);
if (OutputNode == null)
{
log.Error(" Unable to process property: " + output_node_name);
throw new Exception("Invalid output property name in flight control definition");
}
outputNodes.Add(OutputNode);
// If the node has just been created then it must be initialized to a
// sensible value since FGPropertyNode::GetNode() does not take care of
// that. If the node was already existing, its current value is kept
// unchanged.
if (!node_exists)
{
OutputNode.Set(output);
}
}
}
XmlElement delay_elem = element.FindElement("delay");
if (delay_elem != null)
{
delay_time = delay_elem.GetDataAsNumber();
string delayType = delay_elem.GetAttribute("type");
if (!string.IsNullOrEmpty(delayType))
{
if (delayType == "time")
{
delay = (int)(delay_time / dt);
}
else if (delayType == "frames")
{
delay = (int)delay_time;
}
else
{
log.Error("Unallowed delay type");
}
}
else
{
delay = (int)(delay_time / dt);
}
output_array.Resize(delay);
for (int i = 0; i < delay; i++)
{
output_array[i] = 0.0;
}
}
XmlElement clip_el = element.FindElement("clipto");
if (clip_el != null)
{
XmlElement el = clip_el.FindElement("min");
if (el == null)
{
log.Error("Element <min> is missing, <clipto> is ignored.");
return;
}
ClipMin = new ParameterValue(el, propertyManager);
el = clip_el.FindElement("max");
if (el == null)
{
log.Error("Element <max> is missing, <clipto> is ignored.");
ClipMin = null;
return;
}
ClipMax = new ParameterValue(el, propertyManager);
if (clip_el.GetAttribute("type") == "cyclic")
{
cyclic_clip = true;
}
clip = true;
}
Debug(0);
}
