private IEnumerator<ITask> SetChannelHandlerCore(DateTime dt, SetChannel setChannel)
{
if (_usc == null)
{
setChannel.ResponsePort.Post(Fault.FromException(new Exception("Device not initialized")));
yield break;
}
var i = (byte)setChannel.Body.ServoIndex;
try
{
LogVerbose("SetChannel received for servo " + i);
// drain newer update commands and get to the latest
var pending = _setServoPorts[i].ItemCount;
if (pending > 0) LogInfo(string.Format("SetChannel #{0}: Draining {1} pending request", i, pending));
while (pending > 0)
{
SetChannel newerSetServo;
if (_setServoPorts[i].Test(out newerSetServo))
{
/*
if (newerSetServo.ResponsePort != null)
newerSetServo.ResponsePort.Post(DefaultUpdateResponseType.Instance);
*/
setChannel = newerSetServo;
}
pending--;
}
var servoState = setChannel.Body;
// set servo methods
if (servoState.Acceleration != null)
_usc.setSpeed(i, (ushort)servoState.Acceleration);
if (servoState.Speed != null)
_usc.setSpeed(i, (ushort)servoState.Speed);
if (servoState.Target != null)
_usc.setTarget(i, (ushort)servoState.Target);
// notify on completion
setChannel.ResponsePort.Post(DefaultUpdateResponseType.Instance);
// trigger update state
_servoPollingPort.Post(DateTime.Now);
}
catch (Exception ex)
{
LogError("Error updating servo state");
setChannel.ResponsePort.Post(Fault.FromException(ex));
}
finally
{
// send next throtling message
RegisterSetChannelReceiver(i);
}
}
private void SetChannelHandler(SetChannel setChannel)
{
_setServoPorts[setChannel.Body.ServoIndex].Post(setChannel);
}