public void AddSample(IMessageCompassValue heading, IMessageCompassValue cog)
{
var segment = (int)heading.Value;
var deviation = (decimal)_context.ExecuteFunction(heading, cog, (value1, value2) => value1 - value2);
MeansByMeasuredDegree[segment].M = (deviation + MeansByMeasuredDegree[segment].C * MeansByMeasuredDegree[segment].M) / (MeansByMeasuredDegree[segment].C + 1);
MeansByMeasuredDegree[segment].C++;
}
public MagneticMessageCompassValue Magnetic(IMessageCompassValue value)
{
if (value is MagneticMessageCompassValue)
{
return((MagneticMessageCompassValue)value);
}
if (value is TrueMessageCompassValue)
{
return(new MagneticMessageCompassValue(value.Value + Declination + (Deviation ?? 0)));
}
return(null);
}
public TrueMessageCompassValue True(IMessageCompassValue value)
{
if (value is TrueMessageCompassValue)
{
return((TrueMessageCompassValue)value);
}
if (value is MagneticMessageCompassValue)
{
return(new TrueMessageCompassValue(value.Value - Declination - (Deviation ?? 0)));
}
return(null);
}
public IMessageCompassValue ExecuteFunction(IMessageCompassValue value1, Function op)
{
var value = op(value1.Value);
if (value1 is MagneticMessageCompassValue)
{
return(new MagneticMessageCompassValue(value));
}
if (value1 is TrueMessageCompassValue)
{
return(new TrueMessageCompassValue(value));
}
throw new InvalidOperationException();
}
public AutopilotControl(MagneticContext magneticContext, MessageSender messageSender)
{
_magneticContext = magneticContext;
_apb = new APB("SN");
_heading = new MagneticMessageCompassValue(0);
EnabledCommand = new DelegateCommand(() => Enabled = !Enabled);
RightCommand = new DelegateCommand(Right);
LeftCommand = new DelegateCommand(Left);
MagneticCommand = new DelegateCommand(() => HeadingMagnetic = true);
TrueCommand = new DelegateCommand(() => HeadingMagnetic = false);
SetToHeadingCommand = new DelegateCommand(() => CopyCurrentHeading = !CopyCurrentHeading);
_periodicalMessageSender = new RepeatingSender(messageSender, TimeSpan.FromMilliseconds(500), OnBeforeAPBSend)
{
Message = _apb
};
}
public double ExecuteFunction(IMessageCompassValue value1, IMessageCompassValue value2, Function2 op)
{
double value;
if (value1 is MagneticMessageCompassValue)
{
value = True(value2).Value;
}
else if (value1 is TrueMessageCompassValue)
{
value = Magnetic(value2).Value;
}
else
{
throw new InvalidOperationException();
}
return(op(value1.Value, value));
}
internal HDM(string talkedId, string[] parts) : base(talkedId)
{
Heading = MessageCompassValueFactory.FromMessageParts(parts[0], parts[1]);
}
protected abstract IMessageCompassValue Convert(IMessageCompassValue compassValue);
public IMessageCompassValue CorrectHeading(IMessageCompassValue heading)
{
var segment = (int)heading.Value;
return(_context.ExecuteFunction(heading, h => h - (double)MeansByMeasuredDegree[segment].M));
}
