대한민국 육군 K239 천무 로켓 발사 반력 지지 유압 아웃리거 능동 레벨링 및 1,000Hz 0-Alloc C# 수평 안정화 제어 엔진

3. 1,000Hz 0-Alloc C# K239 천무 유압 아웃리거 능동 레벨링 제어 엔진

차체 2축 고정밀 틸트 센서 및 4채널 유압 로드셀 신호를 수신하여 4개 실린더 신축 밸브를 1ms 주기로 연산하는 고신뢰성 C# 코드입니다.

using System;
using System.Runtime.CompilerServices;

public ref struct ChunmooOutriggerLevelingEngine
{
    private readonly float _chassisLengthM;
    private readonly float _chassisWidthM;
    private readonly float _levelingToleranceDeg;
    private readonly float _cylinderGainKp;

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public ChunmooOutriggerLevelingEngine(
        float lengthM = 6.2f,
        float widthM = 2.4f,
        float toleranceDeg = 0.01f,
        float kp = 450.0f)
    {
        _chassisLengthM = lengthM;
        _chassisWidthM = widthM;
        _levelingToleranceDeg = toleranceDeg;
        _cylinderGainKp = kp;
    }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public void ComputeLevelingValves(
        float rollAngleDeg,
        float pitchAngleDeg,
        ReadOnlySpan padGroundForcesKn, // 4 pads: FL, FR, RL, RR
        Span valveFlowDemandsLpm,
        out bool chassisPerfectLevel,
        out bool softGroundSinkageWarning)
    {
        chassisPerfectLevel = (MathF.Abs(rollAngleDeg) <= _levelingToleranceDeg) && 
                              (MathF.Abs(pitchAngleDeg) <= _levelingToleranceDeg);

        softGroundSinkageWarning = false;

        // 지반 침하 또는 패드 이탈 검사 (접지력 5kN 이하 감지)
        for (int i = 0; i < 4; i++)
        {
            if (padGroundForcesKn[i] < 5.0f)
            {
                softGroundSinkageWarning = true;
                break;
            }
        }

        // 롤/피치 각도 오차에 따른 4점 유압 실린더 신축량 계산
        // FL (0): Pitch Down / Roll Left 보정
        // FR (1): Pitch Down / Roll Right 보정
        // RL (2): Pitch Up / Roll Left 보정
        // RR (3): Pitch Up / Roll Right 보정
        float rollCorrection = rollAngleDeg * _cylinderGainKp;
        float pitchCorrection = pitchAngleDeg * _cylinderGainKp;

        valveFlowDemandsLpm[0] = Math.Clamp(-pitchCorrection - rollCorrection, -60.0f, 60.0f);
        valveFlowDemandsLpm[1] = Math.Clamp(-pitchCorrection + rollCorrection, -60.0f, 60.0f);
        valveFlowDemandsLpm[2] = Math.Clamp(+pitchCorrection - rollCorrection, -60.0f, 60.0f);
        valveFlowDemandsLpm[3] = Math.Clamp(+pitchCorrection + rollCorrection, -60.0f, 60.0f);
    }
}