safety_manager.c

gehe zur Dokumentation dieser Datei

/**
 * @file safety_manager.c
 * @brief Safety Manager — Hill-Hold + Auto-Apply Logik.
 *
 * @arch SWA-001
 * @reqs SWE-007 SWE-008 SWE-009 SWE-010
 *
 * ASIL: D. Diese Komponente entscheidet, wann der Apply Controller eine
 * Apply-Anforderung erhaelt (Hill-Hold-Uebergabe, Auto-Apply bei Motor-Aus).
 * Aenderungen erfordern Technical Review mit 2 Approvals.
 */
#include <stddef.h>
 
#include "safety_manager.h"
 
typedef struct {
    SafetyState state;
    uint16_t    ticks_in_state;  /* 50ms-Ticks im aktuellen Zustand */
    bool        apply_requested;
} SafetyCtx;
 
static SafetyCtx s_ctx;
 
static void enter(SafetyState s)
{
    s_ctx.state          = s;
    s_ctx.ticks_in_state = 0U;
}
 
static bool standstill(const SafetyInputs* in)
{
    return in->vehicle_speed_kmh < SAFETY_STANDSTILL_KMH;
}
 
static bool grade_steep(const SafetyInputs* in)
{
    /* @reqs SWE-009: Hill-Hold ab |grade| > 5% */
    float g = in->grade_percent;
    if (g < 0.0f) {
        g = -g;
    }
    return g > SAFETY_HILLHOLD_GRADE_PCT;
}
 
EpbStatus safety_mgr_init(void)
{
    s_ctx.state           = SAFETY_IDLE;
    s_ctx.ticks_in_state  = 0U;
    s_ctx.apply_requested = false;
    return EPB_OK;
}
 
void safety_mgr_step_50ms(const SafetyInputs* in)
{
    if (in == NULL) {
        return;
    }
 
    if (s_ctx.ticks_in_state < UINT16_MAX) {
        ++s_ctx.ticks_in_state;
    }
 
    /* Default: no apply request unless explicitly set below. */
    s_ctx.apply_requested = false;
 
    switch (s_ctx.state) {
    case SAFETY_IDLE:
        /* @reqs SWE-009: Hill-Hold-Aktivierung */
        if (grade_steep(in) && standstill(in) && in->brake_pedal_pressed) {
            enter(SAFETY_HILL_HOLD_ARMED);
            break;
        }
        /* @reqs SWE-007: Motor-Aus-Bedingung erkennen */
        if (!in->engine_running && standstill(in)
            && in->current_state != EPB_STATE_APPLIED
            && in->current_state != EPB_STATE_APPLYING) {
            enter(SAFETY_AUTO_APPLY_ARMED);
        }
        break;
 
    case SAFETY_HILL_HOLD_ARMED:
        /* @reqs SWE-010: Beim Loslassen des Bremspedals Apply triggern */
        if (!in->brake_pedal_pressed) {
            s_ctx.apply_requested = true;
            enter(SAFETY_HILL_HOLD_ACTIVE);
            break;
        }
        /* Bedingung fuer Hill-Hold nicht mehr erfuellt? */
        if (!grade_steep(in) || !standstill(in)) {
            enter(SAFETY_IDLE);
        }
        break;
 
    case SAFETY_HILL_HOLD_ACTIVE:
        /* Beendet, wenn Fahrzeug rollt oder Bremse appliziert */
        if (in->vehicle_speed_kmh > SAFETY_RELEASE_KMH
            || in->current_state == EPB_STATE_APPLIED) {
            enter(SAFETY_IDLE);
        } else {
            s_ctx.apply_requested = true;
        }
        break;
 
    case SAFETY_AUTO_APPLY_ARMED:
        /* Bedingung muss durchgaengig erfuellt sein */
        if (in->engine_running || !standstill(in)) {
            enter(SAFETY_IDLE);
            break;
        }
        /* @reqs SWE-008: Auto-Apply nach 2 s (40 Ticks) Verzoegerung */
        if (s_ctx.ticks_in_state >= SAFETY_AUTO_APPLY_DELAY_50MS) {
            s_ctx.apply_requested = true;
            enter(SAFETY_AUTO_APPLY_TRIGGERED);
        }
        break;
 
    case SAFETY_AUTO_APPLY_TRIGGERED:
        if (in->current_state == EPB_STATE_APPLIED) {
            enter(SAFETY_IDLE);
        } else {
            s_ctx.apply_requested = true;
        }
        break;
 
    default:
        enter(SAFETY_IDLE);
        break;
    }
}
 
bool safety_mgr_apply_requested(void)
{
    return s_ctx.apply_requested;
}
 
SafetyState safety_mgr_get_state(void)
{
    return s_ctx.state;
}