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