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 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();
}