Initial commit
This commit is contained in:
@@ -0,0 +1,141 @@
|
||||
namespace Sick.SafetyScanners.DataStructures;
|
||||
|
||||
/// <summary>
|
||||
/// Contains all data blocks from a UDP scan data packet
|
||||
/// </summary>
|
||||
public sealed class UdpScanData
|
||||
{
|
||||
/// <summary>
|
||||
/// Gets or sets the data header
|
||||
/// </summary>
|
||||
public DataHeader Header { get; init; } = new DataHeader();
|
||||
|
||||
/// <summary>
|
||||
/// Gets or sets the general system state
|
||||
/// </summary>
|
||||
public GeneralSystemState? GeneralSystemState { get; init; }
|
||||
|
||||
/// <summary>
|
||||
/// Gets or sets the derived values (configuration of data output)
|
||||
/// </summary>
|
||||
public DerivedValues? DerivedValues { get; init; }
|
||||
|
||||
/// <summary>
|
||||
/// Gets or sets the measurement data (scan points)
|
||||
/// </summary>
|
||||
public MeasurementData? MeasurementData { get; init; }
|
||||
|
||||
/// <summary>
|
||||
/// Gets or sets the intrusion data (field interruption)
|
||||
/// </summary>
|
||||
public IntrusionData? IntrusionData { get; init; }
|
||||
|
||||
/// <summary>
|
||||
/// Gets or sets the application data (inputs and outputs)
|
||||
/// </summary>
|
||||
public ApplicationData? ApplicationData { get; init; }
|
||||
|
||||
/// <summary>
|
||||
/// Gets the timestamp from the header as DateTime (if available)
|
||||
/// Returns UTC DateTime to ensure consistency with Cartographer timestamp handling
|
||||
///
|
||||
/// SICK Timestamp Format (according to official documentation):
|
||||
/// - TimestampDate (ushort, offset 24):
|
||||
/// * If UTC server is time master: Days since 1972-01-01
|
||||
/// * If Safety Designer device is time master: Number of full 24-hour cycles of time master
|
||||
/// * If no time sync: Number of full 24-hour cycles since device was switched on
|
||||
/// - TimestampTime (uint, offset 28): Milliseconds since midnight (or start of 24-hour cycle)
|
||||
///
|
||||
/// Since we cannot detect time sync status from UDP packet, we try both base dates:
|
||||
/// 1. Try 1972-01-01 (UTC server case) - most common in production
|
||||
/// 2. If result is unreasonable, try 1980-01-01 (legacy/fallback)
|
||||
/// 3. If TimestampDate = 0: Use current date as base
|
||||
/// </summary>
|
||||
public DateTime? Timestamp
|
||||
{
|
||||
get
|
||||
{
|
||||
if (Header.TimestampDate == 0 && Header.TimestampTime == 0)
|
||||
return null;
|
||||
|
||||
DateTime baseDate;
|
||||
string baseDateSource = "unknown";
|
||||
|
||||
if (Header.TimestampDate == 0)
|
||||
{
|
||||
// Date not set on device - use current date as base
|
||||
var now = DateTime.UtcNow;
|
||||
baseDate = new DateTime(now.Year, now.Month, now.Day, 0, 0, 0, DateTimeKind.Utc);
|
||||
baseDateSource = "current_date";
|
||||
}
|
||||
else
|
||||
{
|
||||
// Try 1972-01-01 first (UTC server case - most common according to documentation)
|
||||
var baseDate1972 = new DateTime(1972, 1, 1, 0, 0, 0, DateTimeKind.Utc);
|
||||
var time1972 = baseDate1972.AddDays(Header.TimestampDate).AddMilliseconds(Header.TimestampTime);
|
||||
|
||||
// Check if timestamp with 1972 base is reasonable (within 1 day of now)
|
||||
var currentTimeTicks = DateTime.UtcNow.Ticks;
|
||||
var time1972Ticks = time1972.Ticks;
|
||||
var diff1972Ms = Math.Abs(time1972Ticks - currentTimeTicks) / TimeSpan.TicksPerMillisecond;
|
||||
|
||||
if (diff1972Ms <= 86400000) // Within 1 day - reasonable
|
||||
{
|
||||
baseDate = baseDate1972;
|
||||
baseDateSource = "1972-01-01 (UTC server)";
|
||||
}
|
||||
else
|
||||
{
|
||||
// Try 1980-01-01 as fallback (legacy format or no UTC sync)
|
||||
var baseDate1980 = new DateTime(1980, 1, 1, 0, 0, 0, DateTimeKind.Utc);
|
||||
var time1980 = baseDate1980.AddDays(Header.TimestampDate).AddMilliseconds(Header.TimestampTime);
|
||||
var time1980Ticks = time1980.Ticks;
|
||||
var diff1980Ms = Math.Abs(time1980Ticks - currentTimeTicks) / TimeSpan.TicksPerMillisecond;
|
||||
|
||||
if (diff1980Ms <= diff1972Ms) // 1980 is closer to now
|
||||
{
|
||||
baseDate = baseDate1980;
|
||||
baseDateSource = "1980-01-01 (legacy/fallback)";
|
||||
}
|
||||
else
|
||||
{
|
||||
// 1972 is closer, use it even if not perfect
|
||||
baseDate = baseDate1972;
|
||||
baseDateSource = "1972-01-01 (best match)";
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Add days and milliseconds to base date
|
||||
var time = baseDate.AddDays(Header.TimestampDate).AddMilliseconds(Header.TimestampTime);
|
||||
|
||||
// Validate: timestamp should not be too far in the past or future
|
||||
var nowTicks = DateTime.UtcNow.Ticks;
|
||||
var timeTicks = time.Ticks;
|
||||
var diffMs = Math.Abs(timeTicks - nowTicks) / TimeSpan.TicksPerMillisecond;
|
||||
|
||||
// Debug logging for timestamp calculation
|
||||
if (diffMs > 3600000) // Log if more than 1 hour off
|
||||
{
|
||||
var timestampDateStr = Header.TimestampDate == 0 ? "0 (using current date)" : Header.TimestampDate.ToString();
|
||||
var timestampTimeStr = $"{Header.TimestampTime}ms ({Header.TimestampTime / 3600000.0:F2} hours)";
|
||||
var calculatedTime = time.ToString("yyyy-MM-dd HH:mm:ss.fff");
|
||||
System.Diagnostics.Debug.WriteLine(
|
||||
$"[SICK Timestamp Debug] TimestampDate={timestampDateStr}, TimestampTime={timestampTimeStr}, " +
|
||||
$"BaseDateSource={baseDateSource}, CalculatedTime={calculatedTime}, " +
|
||||
$"DiffFromNow={diffMs / 3600000.0:F2} hours");
|
||||
}
|
||||
|
||||
// If timestamp is more than 1 day off, it's likely incorrect
|
||||
// This can happen if device date is not set correctly or wrong base date
|
||||
if (diffMs > 86400000) // 1 day in milliseconds
|
||||
{
|
||||
// Use current time instead of potentially incorrect device timestamp
|
||||
return DateTime.UtcNow;
|
||||
}
|
||||
|
||||
// Ensure the result is marked as UTC
|
||||
return DateTime.SpecifyKind(time, DateTimeKind.Utc);
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user