private void ReceiveSensorDataTask(RobotSpecification specRobot)
{
while (true)
{
if (m_tsCancel.IsCancellationRequested)
{
m_LogEntries.AddEntry(new ActivityLogEntry("Sensor Task stopped"));
break;
}
else
{
if (m_RunSensors)
{
try
{
// Time telemetry request/reception
System.Diagnostics.Stopwatch sw = System.Diagnostics.Stopwatch.StartNew();
// Request and receive remote sensor data via telemetry channel
SensorRepository sr = new SensorRepository(specRobot);
m_Channels[m_TelemetryChannel].Send("T1");
string data = m_Channels[m_TelemetryChannel].ReadLine();
sw.Stop();
// Parse received sensor data
sr.ParseSensorData(data, specRobot.SensorSpecs.Count);
if (sr.IsValid)
{
// Add sensor repository to behavior/arbitrate queue
if (m_SensorReadings.TryAdd(sr, 1, m_tsCancel.Token))
{
sr.AcquisitionTime = sw.ElapsedMilliseconds;
sr.ID = m_SensorRepID;
m_SensorRepID++;
}
else
{
// Behaviors have not consumed previous sensor data so remove it and get newer
m_LogEntries.AddEntry(new ActivityLogEntry("Sensor data not used."));
if (m_SensorReadings.TryTake(out sr, 1, m_tsCancel.Token))
m_LogEntries.AddEntry(new ActivityLogEntry("Old sensor data removed!"));
}
}
else
{
// Lost communications with the robot
m_LogEntries.AddEntry(new ActivityLogEntry(ActivityLogEntry.LogEntryType.Error, "No communications with the robot!", null));
m_LogEntries.AddEntry(new ActivityLogEntry("RX Data = " + data));
}
m_LogEntries.AddEntry(new ActivityLogEntry("Sensor Acquisition Time", sw.ElapsedMilliseconds));
}
catch (Exception ex)
{
m_LogEntries.AddEntry(new ActivityLogEntry("Exception in Sensor Task: " + ex.Message));
}
}
}
}
}
