/// <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; }