303 lines
14 KiB
C#
303 lines
14 KiB
C#
using System.Collections.Concurrent;
|
|
using System.Text.Json;
|
|
using AutoForge.Core.ExecutionTargets;
|
|
using AutoForge.Core.Simulation;
|
|
using AutoForge.Core.Vehicles;
|
|
using AutoForge.Diagnostics;
|
|
|
|
namespace AutoForge.Simulation;
|
|
|
|
public sealed class DynamicsDiagnosticTransportFactory : IDynamicsDiagnosticTransportFactory
|
|
{
|
|
public const string ProviderIdentifier = "autoforge.simulation.dynamics";
|
|
|
|
private readonly Dictionary<(string ProfileIdentifier, int Revision), SimulationDefinition> _definitions;
|
|
private readonly IDynamicsPacer _pacer;
|
|
private readonly ConcurrentDictionary<string, DynamicsDiagnosticTransport> _active =
|
|
new(StringComparer.Ordinal);
|
|
|
|
public DynamicsDiagnosticTransportFactory(IEnumerable<SimulationDefinition> definitions, IDynamicsPacer? pacer = null)
|
|
{
|
|
ArgumentNullException.ThrowIfNull(definitions);
|
|
_pacer = pacer ?? new TaskDelayDynamicsPacer();
|
|
_definitions = definitions.ToDictionary(
|
|
definition => (definition.ProfileIdentifier.ToUpperInvariant(), definition.ProfileRevision));
|
|
Descriptor = new DiagnosticProviderDescriptor(
|
|
new ExecutionTargetDescriptor(ProviderIdentifier, ExecutionTargetKind.Simulation),
|
|
"Synthetic fixed-step dynamics provider");
|
|
}
|
|
|
|
public DiagnosticProviderDescriptor Descriptor { get; }
|
|
|
|
public IDiagnosticTransport OpenTransport(VehicleProfile profile, DiagnosticSessionIdentity identity) =>
|
|
throw new InvalidOperationException("Dynamics provider sessions must be explicitly opened with a dynamics configuration.");
|
|
|
|
public DynamicsSessionBinding OpenDynamicsTransport(
|
|
VehicleProfile profile,
|
|
DiagnosticSessionIdentity identity,
|
|
DynamicsConfiguration configuration)
|
|
{
|
|
ArgumentNullException.ThrowIfNull(profile);
|
|
ArgumentNullException.ThrowIfNull(identity);
|
|
ArgumentNullException.ThrowIfNull(configuration);
|
|
if (identity.Target.Kind != ExecutionTargetKind.Simulation ||
|
|
!StringComparer.OrdinalIgnoreCase.Equals(identity.Target.ProviderIdentifier, ProviderIdentifier) ||
|
|
!StringComparer.OrdinalIgnoreCase.Equals(identity.ProfileIdentifier, profile.ProfileIdentifier) ||
|
|
identity.ProfileRevision != profile.Revision ||
|
|
!StringComparer.OrdinalIgnoreCase.Equals(identity.VehicleIdentifier, profile.Vehicle.VehicleIdentifier) ||
|
|
!StringComparer.Ordinal.Equals(identity.DynamicsConfigurationIdentifier, configuration.ConfigurationIdentifier) ||
|
|
identity.DynamicsRunIdentifier is null)
|
|
{
|
|
throw new ArgumentException("Dynamics session identity does not bind to the exact selected profile, provider and configuration.", nameof(identity));
|
|
}
|
|
|
|
configuration.ValidateAgainst(profile);
|
|
if (!_definitions.TryGetValue((profile.ProfileIdentifier.ToUpperInvariant(), profile.Revision), out var definition))
|
|
{
|
|
throw new DiagnosticProviderException(
|
|
DiagnosticProviderFailure.ConfigurationNotFound,
|
|
$"No dynamics endpoint mapping exists for profile '{profile.ProfileIdentifier}' revision {profile.Revision}.");
|
|
}
|
|
|
|
definition.ValidateAgainst(profile);
|
|
var engineModule = definition.Modules.FirstOrDefault(module =>
|
|
StringComparer.OrdinalIgnoreCase.Equals(module.ModuleIdentifier, configuration.EngineModuleIdentifier));
|
|
var transmissionModule = definition.Modules.FirstOrDefault(module =>
|
|
StringComparer.OrdinalIgnoreCase.Equals(module.ModuleIdentifier, configuration.TransmissionModuleIdentifier));
|
|
if (engineModule is null || transmissionModule is null)
|
|
{
|
|
throw new ArgumentException("The selected configuration modules must have explicit diagnostic endpoint mappings.", nameof(configuration));
|
|
}
|
|
|
|
var engine = new DynamicsEngine(configuration, identity.DynamicsRunIdentifier);
|
|
var control = new DynamicsRunControl(engine, _pacer);
|
|
var transport = new DynamicsDiagnosticTransport(
|
|
profile,
|
|
identity,
|
|
configuration,
|
|
engine,
|
|
definition.Modules
|
|
.Where(module => StringComparer.OrdinalIgnoreCase.Equals(module.ModuleIdentifier, engineModule.ModuleIdentifier) ||
|
|
StringComparer.OrdinalIgnoreCase.Equals(module.ModuleIdentifier, transmissionModule.ModuleIdentifier))
|
|
.ToArray(),
|
|
closed => _active.TryRemove(closed, out _));
|
|
if (!_active.TryAdd(identity.SessionIdentifier, transport))
|
|
{
|
|
_ = control.DisposeAsync();
|
|
throw new DiagnosticProviderException(
|
|
DiagnosticProviderFailure.SessionConflict,
|
|
$"Dynamics session '{identity.SessionIdentifier}' is already active.");
|
|
}
|
|
|
|
return new DynamicsSessionBinding(transport, control);
|
|
}
|
|
}
|
|
|
|
internal sealed class DynamicsDiagnosticTransport : IDiagnosticTransport
|
|
{
|
|
private readonly VehicleProfile _profile;
|
|
private readonly DiagnosticSessionIdentity _identity;
|
|
private readonly DynamicsConfiguration _configuration;
|
|
private readonly DynamicsEngine _engine;
|
|
private readonly IReadOnlyDictionary<string, VirtualModuleDefinition> _modules;
|
|
private readonly Action<string> _onClosed;
|
|
private int _closed;
|
|
|
|
public DynamicsDiagnosticTransport(
|
|
VehicleProfile profile,
|
|
DiagnosticSessionIdentity identity,
|
|
DynamicsConfiguration configuration,
|
|
DynamicsEngine engine,
|
|
IReadOnlyList<VirtualModuleDefinition> modules,
|
|
Action<string> onClosed)
|
|
{
|
|
_profile = profile;
|
|
_identity = identity;
|
|
_configuration = configuration;
|
|
_engine = engine;
|
|
_modules = modules.ToDictionary(module => module.Address, StringComparer.OrdinalIgnoreCase);
|
|
_onClosed = onClosed;
|
|
ExpectedEndpoints = Array.AsReadOnly(modules
|
|
.Select(module => new DiagnosticEndpoint(module.ModuleIdentifier, module.Address))
|
|
.ToArray());
|
|
}
|
|
|
|
public IReadOnlyList<DiagnosticEndpoint> ExpectedEndpoints { get; }
|
|
|
|
public ValueTask<TransportReply> SendAsync(ReadOnlyMemory<byte> payload, CancellationToken cancellationToken)
|
|
{
|
|
cancellationToken.ThrowIfCancellationRequested();
|
|
if (Volatile.Read(ref _closed) != 0)
|
|
{
|
|
return ValueTask.FromResult(new TransportReply(
|
|
TransportOutcome.Cancelled,
|
|
ReadOnlyMemory<byte>.Empty,
|
|
0,
|
|
"Dynamics diagnostic transport is closed."));
|
|
}
|
|
|
|
if (!SyntheticProtocolV1.TryDecodeRequest(payload, out var request) || request is null)
|
|
{
|
|
return ValueTask.FromResult(new TransportReply(
|
|
TransportOutcome.NoResponse,
|
|
ReadOnlyMemory<byte>.Empty,
|
|
0,
|
|
"Request frame was malformed."));
|
|
}
|
|
|
|
if (request.Version != 2)
|
|
{
|
|
return Reply(request, false, "UNSUPPORTED_VERSION", "Dynamics protocol version 2 is required.");
|
|
}
|
|
|
|
if (!StringComparer.Ordinal.Equals(request.SessionIdentifier, _identity.SessionIdentifier) ||
|
|
!StringComparer.Ordinal.Equals(request.ConfigurationIdentifier, _identity.ConfigurationIdentifier) ||
|
|
request.ProfileRevision != _identity.ProfileRevision ||
|
|
!StringComparer.Ordinal.Equals(request.ProfileIdentifier, _identity.ProfileIdentifier))
|
|
{
|
|
return Reply(request, false, "SESSION_MISMATCH", "Request session/profile identity is not bound to this dynamics transport.");
|
|
}
|
|
|
|
if (!_modules.TryGetValue(request.TargetAddress, out var module))
|
|
{
|
|
return Reply(request, false, "UNKNOWN_ADDRESS", "No dynamics endpoint is mapped to this address.");
|
|
}
|
|
|
|
if (!module.ImplementedServices.Contains(request.Service, StringComparer.Ordinal))
|
|
{
|
|
return Reply(request, false, "UNSUPPORTED_SERVICE", $"Service '{request.Service}' is not implemented by this virtual endpoint.");
|
|
}
|
|
|
|
cancellationToken.ThrowIfCancellationRequested();
|
|
object data;
|
|
try
|
|
{
|
|
data = request.Service switch
|
|
{
|
|
DiagnosticServices.Probe => new
|
|
{
|
|
moduleIdentifier = module.ModuleIdentifier,
|
|
implementedServices = module.ImplementedServices
|
|
},
|
|
DiagnosticServices.ReadIdentity => IdentityData(module),
|
|
DiagnosticServices.ReadTroubleCodes => new { moduleIdentifier = module.ModuleIdentifier, codes = Array.Empty<object>() },
|
|
DiagnosticServices.ClearTroubleCodes => new { moduleIdentifier = module.ModuleIdentifier, clearedCount = 0 },
|
|
DiagnosticServices.ReadSignals => ReadDynamicSignals(module, request),
|
|
_ => throw new DiagnosticProtocolException("UNSUPPORTED_SERVICE", $"Service '{request.Service}' is not defined by dynamics protocol v2.")
|
|
};
|
|
}
|
|
catch (DiagnosticProtocolException exception)
|
|
{
|
|
return Reply(request, false, exception.FailureCode, exception.Message);
|
|
}
|
|
|
|
return ValueTask.FromResult(new TransportReply(
|
|
TransportOutcome.Response,
|
|
SyntheticProtocolV2.EncodePositiveResponse(request, data),
|
|
1));
|
|
}
|
|
|
|
public ValueTask DisposeAsync()
|
|
{
|
|
if (Interlocked.Exchange(ref _closed, 1) == 0)
|
|
{
|
|
_onClosed(_identity.SessionIdentifier);
|
|
}
|
|
|
|
return ValueTask.CompletedTask;
|
|
}
|
|
|
|
private object IdentityData(VirtualModuleDefinition module)
|
|
{
|
|
var identity = module.Identity;
|
|
var profileModule = _profile.GetModule(module.ModuleIdentifier);
|
|
if (!StringComparer.OrdinalIgnoreCase.Equals(identity.Role, profileModule.Role) ||
|
|
!StringComparer.OrdinalIgnoreCase.Equals(identity.Name, profileModule.Name) ||
|
|
!StringComparer.OrdinalIgnoreCase.Equals(identity.CalibrationIdentifier, profileModule.CalibrationIdentifier))
|
|
{
|
|
throw new DiagnosticProtocolException("INVALID_MAPPING", "Dynamics endpoint identity does not match the selected profile.");
|
|
}
|
|
|
|
return new
|
|
{
|
|
moduleIdentifier = module.ModuleIdentifier,
|
|
role = identity.Role,
|
|
name = identity.Name,
|
|
hardwareIdentifier = identity.HardwareIdentifier,
|
|
softwareIdentifier = identity.SoftwareIdentifier,
|
|
firmwareIdentifier = identity.FirmwareIdentifier,
|
|
calibrationIdentifier = identity.CalibrationIdentifier
|
|
};
|
|
}
|
|
|
|
private object ReadDynamicSignals(VirtualModuleDefinition module, SyntheticDiagnosticRequest request)
|
|
{
|
|
lock (_engine.SyncRoot)
|
|
{
|
|
var status = _engine.Status;
|
|
if (status.State == DynamicsLifecycleState.Faulted)
|
|
{
|
|
throw new DiagnosticProtocolException(
|
|
"DYNAMICS_RUN_FAULTED",
|
|
status.Failure ?? "Dynamics run failed; telemetry is unavailable.");
|
|
}
|
|
|
|
var state = _engine.ReadTelemetry(module.ModuleIdentifier);
|
|
var signals = StringComparer.OrdinalIgnoreCase.Equals(module.ModuleIdentifier, _configuration.EngineModuleIdentifier)
|
|
? EngineSignals(state)
|
|
: TransmissionSignals(state);
|
|
return new
|
|
{
|
|
moduleIdentifier = module.ModuleIdentifier,
|
|
configurationIdentifier = _configuration.ConfigurationIdentifier,
|
|
runIdentifier = _identity.DynamicsRunIdentifier,
|
|
modelStepIndex = state.StepIndex,
|
|
modelTimeSeconds = state.ModelTimeSeconds,
|
|
origin = "SYNTHETIC_DYNAMIC",
|
|
captureLogicalTimeTicks = request.LogicalTimeTicks,
|
|
signals
|
|
};
|
|
}
|
|
}
|
|
|
|
private static object[] EngineSignals(DynamicsTelemetryState state) =>
|
|
[
|
|
Signal("engine.running", state.EngineRunning, "boolean"),
|
|
Signal("engine.speed", state.EngineSpeedRpm, "rpm"),
|
|
Signal("vehicle.speed", state.VehicleSpeedMetersPerSecond, "m/s"),
|
|
Signal("engine.throttle", state.Throttle, "ratio"),
|
|
Signal("engine.load", state.EngineLoad, "ratio"),
|
|
Signal("engine.torque", state.EngineTorqueNewtonMeters, "N*m"),
|
|
Signal("engine.fuel-rate", state.FuelRateKilogramsPerSecond, "kg/s"),
|
|
Signal("engine.fuel-consumed", state.FuelConsumedKilograms, "kg"),
|
|
Signal("engine.temperature", state.EngineTemperatureCelsius, "degC"),
|
|
Signal("vehicle.distance", state.DistanceMeters, "m"),
|
|
Signal("vehicle.acceleration", state.AccelerationMetersPerSecondSquared, "m/s^2"),
|
|
Signal("driver.brake", state.Brake, "ratio")
|
|
];
|
|
|
|
private static object[] TransmissionSignals(DynamicsTelemetryState state) =>
|
|
[
|
|
Signal("transmission.gear", state.Gear, "gear"),
|
|
Signal("transmission.shift-count", state.ShiftCount, "count"),
|
|
Signal("transmission.coupling-slip", state.CouplingSlipFraction, "ratio"),
|
|
Signal("vehicle.speed", state.VehicleSpeedMetersPerSecond, "m/s"),
|
|
Signal("engine.speed", state.EngineSpeedRpm, "rpm")
|
|
];
|
|
|
|
private static object Signal(string identifier, object value, string unit) =>
|
|
new { signalIdentifier = identifier, value, unit, quality = "Good" };
|
|
|
|
private static ValueTask<TransportReply> Reply(
|
|
SyntheticDiagnosticRequest request,
|
|
bool success,
|
|
string code,
|
|
string message) =>
|
|
ValueTask.FromResult(new TransportReply(
|
|
TransportOutcome.Response,
|
|
success
|
|
? SyntheticProtocolV2.EncodePositiveResponse(request, new { })
|
|
: SyntheticProtocolV2.EncodeNegativeResponse(request, code, message),
|
|
1));
|
|
}
|