internal virtual void Load(IProgressiveCommand progressiveCommand)
{
Gaz = FloatToInt32Converter.Convert(progressiveCommand.Gaz);
Yaw = FloatToInt32Converter.Convert(progressiveCommand.Yaw);
if (Math.Abs(progressiveCommand.Pitch) < Threshold && Math.Abs(progressiveCommand.Roll) < Threshold)
{
Pitch = 0;
Roll = 0;
Mode = 0;
}
else
{
Pitch = FloatToInt32Converter.Convert(progressiveCommand.Pitch);
Roll = FloatToInt32Converter.Convert(progressiveCommand.Roll);
Mode = Modes.EnableProgressiveCommands;
}
if (progressiveCommand.CombineYaw)
{
Mode |= Modes.CombineYaw;
}
if (progressiveCommand.AbsoluteControlMode)
{
float normalizedMagnetoPsi = NormalizeMagnetoPsiDegrees(progressiveCommand.ControllerHeading);
MagnetoPsi = FloatToInt32Converter.Convert(normalizedMagnetoPsi);
float normalizedMagnetoPsiAccuracy =
NormalizedMagnetoPsiAccuracy(progressiveCommand.ControllerHeadingAccuracy);
MagnetoPsiAccuracy = FloatToInt32Converter.Convert(normalizedMagnetoPsiAccuracy);
Mode |= Modes.AbsoluteControl;
}
else
{
MagnetoPsi = 0;
MagnetoPsiAccuracy = 0;
}
}
internal virtual void SendProgressiveCommand(IProgressiveCommand args)
{
string command;
ProgressiveCommandFormatter.Load(args);
if (args.AbsoluteControlMode)
{
string message = string.Format("{0},{1},{2},{3},{4},{5},{6}", (int)ProgressiveCommandFormatter.Mode,
ProgressiveCommandFormatter.Roll, ProgressiveCommandFormatter.Pitch, ProgressiveCommandFormatter.Gaz,
ProgressiveCommandFormatter.Yaw, ProgressiveCommandFormatter.MagnetoPsi,
ProgressiveCommandFormatter.MagnetoPsiAccuracy);
command = CommandFormatter.CreateCommand(PcmdMagCommand, message);
}
else
{
string message = string.Format("{0},{1},{2},{3},{4}", (int)ProgressiveCommandFormatter.Mode,
ProgressiveCommandFormatter.Roll, ProgressiveCommandFormatter.Pitch, ProgressiveCommandFormatter.Gaz,
ProgressiveCommandFormatter.Yaw);
command = CommandFormatter.CreateCommand(PcmdCommand, message);
}
CommandQueue.Enqueue(command);
}
