AIDT 온디바이스 국방 MUM-T 지상 로봇(UGV) 암 내장형 능동 현수장치(ISU) 지형적응 서보 엔진

using System;
using System.Runtime.CompilerServices;

namespace ZeliDesk.Defense.MUMT.InArmSuspension
{
    public readonly struct IsuConfig
    {
        public readonly double TargetRideHeightMeters;
        public readonly double MaxActuatorForceNewtons;
        public readonly double SuspensionStiffnessNPerM;
        public readonly double Kp;
        public readonly double Kd;

        public IsuConfig(double targetRideHeightMeters, double maxActuatorForceNewtons,
                         double suspensionStiffnessNPerM, double kp, double kd)
        {
            TargetRideHeightMeters = targetRideHeightMeters;
            MaxActuatorForceNewtons = maxActuatorForceNewtons;
            SuspensionStiffnessNPerM = suspensionStiffnessNPerM;
            Kp = kp;
            Kd = kd;
        }
    }

    public ref struct InArmSuspensionServo
    {
        private readonly IsuConfig _config;
        private double _activeForceCommandNewtons;
        private double _lastDisplacementError;
        private double _monitoredHeightMeters;

        public InArmSuspensionServo(in IsuConfig config)
        {
            _config = config;
            _activeForceCommandNewtons = 0.0;
            _lastDisplacementError = 0.0;
            _monitoredHeightMeters = config.TargetRideHeightMeters;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void ProcessSuspensionTelemetry(ReadOnlySpan wheelDisplacementsMeters, ReadOnlySpan wheelVelocitiesMps, Span actuatorForceCommands)
        {
            double dt = 1.0e-4; // 10,000Hz
            for (int i = 0; i < wheelDisplacementsMeters.Length; i++)
            {
                double currentHeight = wheelDisplacementsMeters[i];
                double velocity = wheelVelocitiesMps[i];
                _monitoredHeightMeters = currentHeight;

                double heightError = _config.TargetRideHeightMeters - currentHeight;
                double errorDerivative = (heightError - _lastDisplacementError) / dt;
                _lastDisplacementError = heightError;

                double rawForce = (_config.Kp * heightError) + (_config.Kd * errorDerivative) - (velocity * 2500.0);
                _activeForceCommandNewtons = Math.Clamp(rawForce, -_config.MaxActuatorForceNewtons, _config.MaxActuatorForceNewtons);

                actuatorForceCommands[i] = _activeForceCommandNewtons;
            }
        }

        public readonly double CommandedForceN => _activeForceCommandNewtons;
        public readonly double CurrentRideHeightM => _monitoredHeightMeters;
    }
}