public TrackingData Process() { if (!firstCompleted) { TrackingData td = first.Process(); if (td.result == CompletionResult.Completed) { firstCompleted = true; } else { return(td); } } return(second.Process()); }
private void TrackingProc() { Trace.CorrelationManager.StartLogicalOperation("tracking loop"); OperationalTrace.ThreadTraceSource = TraceSource; double?lastSteeringCommand = null; using (MMWaitableTimer timer = new MMWaitableTimer((uint)(Settings.TrackingPeriod * 1000))) { // loop infinitely while (true) { if (Services.DebuggingService.StepMode) { Services.DebuggingService.WaitOnSequencer(typeof(TrackingManager)); } else { // wait for the timer timer.WaitEvent.WaitOne(); } Services.Dataset.MarkOperation("tracking rate", LocalCarTimeProvider.LocalNow); // check if there's a new command to switch in ITrackingCommand newCommand = Interlocked.Exchange(ref queuedCommand, null); if (newCommand != null) { TraceSource.TraceEvent(TraceEventType.Verbose, 0, "received new command: {0}", newCommand); // set the current result to working currentResult = CompletionResult.Working; // set the current command to the new command currentCommand = newCommand; } // check if we've completed or had a failure if (currentResult == CompletionResult.Working) { TrackingData td = new TrackingData(null, null, null, null, CompletionResult.Failed); try { // get the tracking data Trace.CorrelationManager.StartLogicalOperation("process"); td = currentCommand.Process(); } catch (Exception ex) { // log this somehow // if we get here, the completion result retains the failed label, so we won't output anything TraceSource.TraceEvent(TraceEventType.Error, 1, "exception tracking command: {0}", ex); } finally { Trace.CorrelationManager.StopLogicalOperation(); } // check if there is a failure if (td.result == CompletionResult.Failed) { td = new TrackingData(null, null, null, null, CompletionResult.Failed); } // output the command try { Trace.CorrelationManager.StartLogicalOperation("command output"); commandTransport.SetCommand(td.engineTorque, td.brakePressure, td.steeringAngle, td.gear); } catch (Exception ex) { TraceSource.TraceEvent(TraceEventType.Error, 0, "error setting command on transport: {0}", ex); } finally { Trace.CorrelationManager.StopLogicalOperation(); } // queue the command lock (deltaSteeringQueue) { if (lastSteeringCommand != null && td.steeringAngle != null) { double delta = td.steeringAngle.Value - lastSteeringCommand.Value; deltaSteeringQueue.Add(delta, LocalCarTimeProvider.LocalNow.ts); } Services.Dataset.ItemAs <double>("delta steering sigma").Add(DeltaSteeringStdDev, LocalCarTimeProvider.LocalNow); lastSteeringCommand = td.steeringAngle; } if (td.brakePressure.HasValue && td.brakePressure.Value > 90) { td.brakePressure = 90; } // output the tracking commands to the UI CarTimestamp now = Services.RelativePose.CurrentTimestamp; if (td.steeringAngle.HasValue) { Services.Dataset.ItemAs <double>("commanded steering").Add(td.steeringAngle.Value, now); } if (td.engineTorque.HasValue) { Services.Dataset.ItemAs <double>("commanded engine torque").Add(td.engineTorque.Value, now); } if (td.brakePressure.HasValue) { Services.Dataset.ItemAs <double>("commanded brake pressure").Add(td.brakePressure.Value, now); } if (td.result != CompletionResult.Working && trackingCompleted != null) { Trace.CorrelationManager.StartLogicalOperation("tracking completed"); TraceSource.TraceEvent(TraceEventType.Information, 0, "tracking completed, invoking event, result {0}", td.result); // raise the completed event asynchronously so we can go on processing try { trackingCompleted.BeginInvoke(this, new TrackingCompletedEventArgs(td.result, null, currentCommand), OnTrackingCompletedFinished, trackingCompleted); } catch (Exception ex) { TraceSource.TraceEvent(TraceEventType.Error, 0, "exception thrown in beging invoke of tracking complete event: {0}", ex); } finally { Trace.CorrelationManager.StopLogicalOperation(); } } // update the current result currentResult = td.result; } // flush the command transport every iteration commandTransport.Flush(); if (Services.DebuggingService.StepMode) { Services.DebuggingService.SetCompleted(typeof(TrackingManager)); } } } }