/// <summary>
/// Start conversation with the TrackRoamer Proximity Board device.
/// </summary>
IEnumerator<ITask> StartProximityBoard(int timeout)
{
Tracer.Trace(string.Format("TrackRoamerBrickProximityBoardService::StartProximityBoard() timeout={0} ms", timeout));
_state.LinkState = "Initializing";
if (timeout > 0)
{
//
// caller asked us to wait <timeout> milliseconds until we start.
//
yield return Arbiter.Receive(false, TimeoutPort(timeout),
delegate(DateTime dt)
{
LogInfo(string.Format("StartProximityBoard() - Done Waiting {0} ms", timeout));
}
);
}
if (_pbCommanderTaskQueue == null)
{
//
// The internal services run on their own dispatcher, we need to create that (once)
//
AllocateExecutionResource allocExecRes = new AllocateExecutionResource(0, "TrackRoamerProximityBoard");
ResourceManagerPort.Post(allocExecRes);
yield return Arbiter.Choice(
allocExecRes.Result,
delegate(ExecutionAllocationResult result)
{
_pbCommanderTaskQueue = result.TaskQueue;
},
delegate(Exception e)
{
LogError(e);
}
);
}
_pbCommander = new ProximityBoardCcrServiceCommander(_pbCommanderTaskQueue ?? TaskQueue, _pbCommanderDataEventsPort, _state.VendorId, _state.ProductId);
_pbCommander.Parent = ServiceInfo.Service;
_pbCommander.Console = ConsoleOutputPort;
_state.IsConnected = false;
// Open is an empty operatiion, but we need to flush the internal queue - so let it be.
FlushPortSet(_pbCommanderDataEventsPort);
bool failed = false;
yield return (
Arbiter.Choice(
_pbCommander.Open(),
delegate(SuccessResult success)
{
_state.LinkState = "Initializing - link opened";
LogInfo("Opened link to Proximity Board");
},
delegate(Exception exception)
{
_state.LinkState = "Error Initializing - could not open link";
failed = true;
LogError(exception);
}
)
);
if (failed)
{
yield break;
}
//
// Set the servo sweep rate:
//
yield return (
Arbiter.Choice(
_pbCommander.SetServoSweepRate(),
delegate(SuccessResult success)
{
_state.LinkState = "Servo Sweep Rate Set";
LogInfo(_state.LinkState);
},
delegate(Exception exception)
{
_state.LinkState = "Error Initializing - could not set Servo Sweep Rate";
failed = true;
LogError(exception);
}
)
);
if (failed)
{
yield break;
}
//
// start continuous measurements.
//
yield return (
Arbiter.Choice(
_pbCommander.SetContinuous(),
delegate(SuccessResult success)
{
_state.LinkState = "Started Continuous Measurement";
_state.IsConnected = true;
LogInfo(_state.LinkState);
},
delegate(Exception failure)
{
_state.LinkState = "Error Initializing - could not start Continuous Measurement";
_pbCommanderDataEventsPort.Post(failure);
failed = true;
}
)
);
if (failed)
{
yield break;
}
// somehow the board skips commands on startup, send it twice after a wait:
yield return Arbiter.Receive(false, TimeoutPort(1000),
delegate(DateTime dt)
{
}
);
//
// start continuous measurements - try 2.
//
yield return (
Arbiter.Choice(
_pbCommander.SetContinuous(),
delegate(SuccessResult success)
{
_state.LinkState = "Started Continuous Measurement";
_state.IsConnected = true;
LogInfo(_state.LinkState);
},
delegate(Exception failure)
{
_state.LinkState = "Error Initializing - could not start Continuous Measurement";
_pbCommanderDataEventsPort.Post(failure);
failed = true;
}
)
);
if (failed)
{
yield break;
}
}
void DropHandler(DsspDefaultDrop drop)
{
Tracer.Trace("TrackRoamerBrickProximityBoardService::DropHandler()");
_state.LinkState = "Dropping service - Closing link to Proximity Board";
_state.IsConnected = false;
try
{
if (_pbCommander != null)
{
// release dispatcher queue resource
ResourceManagerPort.Post(new FreeExecutionResource(_pbCommander.TaskQueue));
_pbCommander.Close();
_pbCommander = null;
_state.IsConnected = false;
}
}
finally
{
base.DefaultDropHandler(drop);
}
}
IEnumerator<ITask> ExceptionHandler(Exception exception)
{
Tracer.Trace("TrackRoamerBrickProximityBoardService::ExceptionHandler() exception=" + exception);
LogError(exception);
//BadPacketException bpe = exception as BadPacketException;
//if (bpe != null && bpe.Count < 2)
//{
// yield break;
//}
_submgrPort.Post(new submgr.Submit(new ResetType(), DsspActions.SubmitRequest));
_state.LinkState = "Exception " + exception.Message + " - Closing link to Proximity Board";
_state.IsConnected = false;
LogInfo("Closing link to Proximity Board");
yield return
Arbiter.Choice(
_pbCommander.Close(),
delegate(SuccessResult success)
{
},
delegate(Exception except)
{
LogError(except);
}
);
_state.LinkState = "Exception " + exception.Message + " - Proximity Board link closed, waiting 5 seconds to reopen";
LogInfo(_state.LinkState);
_pbCommander = null;
SpawnIterator(openDelayMs, StartProximityBoard);
yield break;
}
IEnumerator<ITask> UsbDeviceDetached(ProximityBoardUsbDeviceDetached linkDetached)
{
Tracer.Trace("TrackRoamerBrickProximityBoardService::UsbDeviceDetached(): " + linkDetached.Description);
_submgrPort.Post(new submgr.Submit(new ResetType(), DsspActions.SubmitRequest));
_state.Description = linkDetached.Description;
_state.LinkState = "USB Device Detached - closing link to Proximity Board";
_state.IsConnected = false;
LogInfo("Closing link to Proximity Board");
yield return
Arbiter.Choice(
_pbCommander.Close(),
delegate(SuccessResult success)
{
_state.LinkState = "USB Device Detached - link to Proximity Board closed";
},
delegate(Exception except)
{
_state.LinkState = "USB Device Detached - Error closing link to Proximity Board";
LogError(except);
}
);
_state.LinkState = "USB Device Detached - Proximity Board link closed, waiting 5 seconds";
LogInfo(_state.LinkState);
_pbCommander = null;
SpawnIterator(openDelayMs, StartProximityBoard);
yield break;
}
