Line data    Source code

       1             : /**
       2             :  * @file safety_manager.c
       3             :  * @brief Safety Manager — Hill-Hold + Auto-Apply Logik.
       4             :  *
       5             :  * @arch SWA-001
       6             :  * @reqs SWE-007 SWE-008 SWE-009 SWE-010
       7             :  *
       8             :  * ASIL: D. Diese Komponente entscheidet, wann der Apply Controller eine
       9             :  * Apply-Anforderung erhaelt (Hill-Hold-Uebergabe, Auto-Apply bei Motor-Aus).
      10             :  * Aenderungen erfordern Technical Review mit 2 Approvals.
      11             :  */
      12             : #include <stddef.h>
      13             : 
      14             : #include "safety_manager.h"
      15             : 
      16             : typedef struct {
      17             :     SafetyState state;
      18             :     uint16_t    ticks_in_state;  /* 50ms-Ticks im aktuellen Zustand */
      19             :     bool        apply_requested;
      20             : } SafetyCtx;
      21             : 
      22             : static SafetyCtx s_ctx;
      23             : 
      24          34 : static void enter(SafetyState s)
      25             : {
      26          34 :     s_ctx.state          = s;
      27          34 :     s_ctx.ticks_in_state = 0U;
      28          34 : }
      29             : 
      30         218 : static bool standstill(const SafetyInputs* in)
      31             : {
      32         218 :     return in->vehicle_speed_kmh < SAFETY_STANDSTILL_KMH;
      33             : }
      34             : 
      35          24 : static bool grade_steep(const SafetyInputs* in)
      36             : {
      37             :     /* @reqs SWE-009: Hill-Hold ab |grade| > 5% */
      38          24 :     float g = in->grade_percent;
      39          24 :     if (g < 0.0f) {
      40           2 :         g = -g;
      41             :     }
      42          24 :     return g > SAFETY_HILLHOLD_GRADE_PCT;
      43             : }
      44             : 
      45          26 : EpbStatus safety_mgr_init(void)
      46             : {
      47          26 :     s_ctx.state           = SAFETY_IDLE;
      48          26 :     s_ctx.ticks_in_state  = 0U;
      49          26 :     s_ctx.apply_requested = false;
      50          26 :     return EPB_OK;
      51             : }
      52             : 
      53         234 : void safety_mgr_step_50ms(const SafetyInputs* in)
      54             : {
      55         234 :     if (in == NULL) {
      56           2 :         return;
      57             :     }
      58             : 
      59         232 :     if (s_ctx.ticks_in_state < UINT16_MAX) {
      60         232 :         ++s_ctx.ticks_in_state;
      61             :     }
      62             : 
      63             :     /* Default: no apply request unless explicitly set below. */
      64         232 :     s_ctx.apply_requested = false;
      65             : 
      66         232 :     switch (s_ctx.state) {
      67          22 :     case SAFETY_IDLE:
      68             :         /* @reqs SWE-009: Hill-Hold-Aktivierung */
      69          22 :         if (grade_steep(in) && standstill(in) && in->brake_pedal_pressed) {
      70          10 :             enter(SAFETY_HILL_HOLD_ARMED);
      71          10 :             break;
      72             :         }
      73             :         /* @reqs SWE-007: Motor-Aus-Bedingung erkennen */
      74          12 :         if (!in->engine_running && standstill(in)
      75          10 :             && in->current_state != EPB_STATE_APPLIED
      76           8 :             && in->current_state != EPB_STATE_APPLYING) {
      77           8 :             enter(SAFETY_AUTO_APPLY_ARMED);
      78             :         }
      79          12 :         break;
      80             : 
      81           6 :     case SAFETY_HILL_HOLD_ARMED:
      82             :         /* @reqs SWE-010: Beim Loslassen des Bremspedals Apply triggern */
      83           6 :         if (!in->brake_pedal_pressed) {
      84           4 :             s_ctx.apply_requested = true;
      85           4 :             enter(SAFETY_HILL_HOLD_ACTIVE);
      86           4 :             break;
      87             :         }
      88             :         /* Bedingung fuer Hill-Hold nicht mehr erfuellt? */
      89           2 :         if (!grade_steep(in) || !standstill(in)) {
      90           2 :             enter(SAFETY_IDLE);
      91             :         }
      92           2 :         break;
      93             : 
      94           2 :     case SAFETY_HILL_HOLD_ACTIVE:
      95             :         /* Beendet, wenn Fahrzeug rollt oder Bremse appliziert */
      96           2 :         if (in->vehicle_speed_kmh > SAFETY_RELEASE_KMH
      97           0 :             || in->current_state == EPB_STATE_APPLIED) {
      98           2 :             enter(SAFETY_IDLE);
      99             :         } else {
     100           0 :             s_ctx.apply_requested = true;
     101             :         }
     102           2 :         break;
     103             : 
     104         200 :     case SAFETY_AUTO_APPLY_ARMED:
     105             :         /* Bedingung muss durchgaengig erfuellt sein */
     106         200 :         if (in->engine_running || !standstill(in)) {
     107           2 :             enter(SAFETY_IDLE);
     108           2 :             break;
     109             :         }
     110             :         /* @reqs SWE-008: Auto-Apply nach 2 s (40 Ticks) Verzoegerung */
     111         198 :         if (s_ctx.ticks_in_state >= SAFETY_AUTO_APPLY_DELAY_50MS) {
     112           4 :             s_ctx.apply_requested = true;
     113           4 :             enter(SAFETY_AUTO_APPLY_TRIGGERED);
     114             :         }
     115         198 :         break;
     116             : 
     117           2 :     case SAFETY_AUTO_APPLY_TRIGGERED:
     118           2 :         if (in->current_state == EPB_STATE_APPLIED) {
     119           2 :             enter(SAFETY_IDLE);
     120             :         } else {
     121           0 :             s_ctx.apply_requested = true;
     122             :         }
     123           2 :         break;
     124             : 
     125           0 :     default:
     126           0 :         enter(SAFETY_IDLE);
     127           0 :         break;
     128             :     }
     129             : }
     130             : 
     131          14 : bool safety_mgr_apply_requested(void)
     132             : {
     133          14 :     return s_ctx.apply_requested;
     134             : }
     135             : 
     136         160 : SafetyState safety_mgr_get_state(void)
     137             : {
     138         160 :     return s_ctx.state;
     139             : }