Line data    Source code

       1             : /**
       2             :  * @file safety_manager.c
       3             :  * @brief Safety Manager — Hill-Hold, Auto-Apply, Drive-Away Assist.
       4             :  *
       5             :  * @arch SWA-001
       6             :  * @reqs SWE-007 SWE-008 SWE-009 SWE-010 SWE-011 SWE-012
       7             :  *
       8             :  * ASIL: D. This component decides, when the Apply Controller receives an
       9             :  * apply or release request (hill-hold handover, auto-apply on
      10             :  * engine-off, drive-away assist).
      11             :  * Changes require a technical review with 2 approvals.
      12             :  */
      13             : #include <stddef.h>
      14             : 
      15             : #include "safety_manager.h"
      16             : 
      17             : typedef struct {
      18             :     SafetyState state;
      19             :     uint16_t    ticks_in_state;  /* 50ms ticks in the current state */
      20             :     bool        apply_requested;
      21             :     bool        release_requested;
      22             : } SafetyCtx;
      23             : 
      24             : static SafetyCtx s_ctx;
      25             : 
      26          40 : static void enter(SafetyState s)
      27             : {
      28          40 :     s_ctx.state          = s;
      29          40 :     s_ctx.ticks_in_state = 0U;
      30          40 : }
      31             : 
      32         218 : static bool standstill(const SafetyInputs* in)
      33             : {
      34         218 :     return in->vehicle_speed_kmh < SAFETY_STANDSTILL_KMH;
      35             : }
      36             : 
      37          34 : static bool grade_steep(const SafetyInputs* in)
      38             : {
      39             :     /* @reqs SWE-009: Hill-hold above |grade| > 5% */
      40          34 :     float g = in->grade_percent;
      41          34 :     if (g < 0.0f) {
      42           2 :         g = -g;
      43             :     }
      44          34 :     return g > SAFETY_HILLHOLD_GRADE_PCT;
      45             : }
      46             : 
      47             : /**
      48             :  * @reqs SWE-011 (Detect drive-away intent)
      49             :  */
      50          12 : static bool drive_intent(const SafetyInputs* in)
      51             : {
      52          12 :     return (in->gas_pedal_percent > SAFETY_DRIVE_INTENT_GAS_PCT)
      53           8 :         && in->gear_in_drive
      54          20 :         && in->engine_running;
      55             : }
      56             : 
      57             : /**
      58             :  * @reqs SWE-012 (Safety check before auto-release)
      59             :  */
      60           8 : static bool drive_away_safety_ok(const SafetyInputs* in)
      61             : {
      62           8 :     return in->door_closed && in->seatbelt_fastened;
      63             : }
      64             : 
      65          36 : EpbStatus safety_mgr_init(void)
      66             : {
      67          36 :     s_ctx.state             = SAFETY_IDLE;
      68          36 :     s_ctx.ticks_in_state    = 0U;
      69          36 :     s_ctx.apply_requested   = false;
      70          36 :     s_ctx.release_requested = false;
      71          36 :     return EPB_OK;
      72             : }
      73             : 
      74         246 : void safety_mgr_step_50ms(const SafetyInputs* in)
      75             : {
      76         246 :     if (in == NULL) {
      77           2 :         return;
      78             :     }
      79             : 
      80         244 :     if (s_ctx.ticks_in_state < UINT16_MAX) {
      81         244 :         ++s_ctx.ticks_in_state;
      82             :     }
      83             : 
      84             :     /* Default: no apply/release request unless explicitly set below. */
      85         244 :     s_ctx.apply_requested   = false;
      86         244 :     s_ctx.release_requested = false;
      87             : 
      88         244 :     switch (s_ctx.state) {
      89          32 :     case SAFETY_IDLE:
      90             :         /* @reqs SWE-009: Hill-hold activation */
      91          32 :         if (grade_steep(in) && standstill(in) && in->brake_pedal_pressed) {
      92          10 :             enter(SAFETY_HILL_HOLD_ARMED);
      93          10 :             break;
      94             :         }
      95             :         /* @reqs SWE-007: Detect engine-off condition */
      96          22 :         if (!in->engine_running && standstill(in)
      97          10 :             && in->current_state != EPB_STATE_APPLIED
      98           8 :             && in->current_state != EPB_STATE_APPLYING) {
      99           8 :             enter(SAFETY_AUTO_APPLY_ARMED);
     100           8 :             break;
     101             :         }
     102             :         /* @reqs SWE-011 + SWE-012: Drive-Away Assist */
     103          14 :         if (in->current_state == EPB_STATE_APPLIED
     104          12 :             && drive_intent(in) && drive_away_safety_ok(in)) {
     105           4 :             s_ctx.release_requested = true;
     106           4 :             enter(SAFETY_DRIVE_AWAY);
     107             :         }
     108          14 :         break;
     109             : 
     110           6 :     case SAFETY_HILL_HOLD_ARMED:
     111             :         /* @reqs SWE-010: Trigger apply on brake release */
     112           6 :         if (!in->brake_pedal_pressed) {
     113           4 :             s_ctx.apply_requested = true;
     114           4 :             enter(SAFETY_HILL_HOLD_ACTIVE);
     115           4 :             break;
     116             :         }
     117             :         /* Hill-hold condition no longer met? */
     118           2 :         if (!grade_steep(in) || !standstill(in)) {
     119           2 :             enter(SAFETY_IDLE);
     120             :         }
     121           2 :         break;
     122             : 
     123           2 :     case SAFETY_HILL_HOLD_ACTIVE:
     124             :         /* Ends when vehicle rolls or brake is applied */
     125           2 :         if (in->vehicle_speed_kmh > SAFETY_RELEASE_KMH
     126           0 :             || in->current_state == EPB_STATE_APPLIED) {
     127           2 :             enter(SAFETY_IDLE);
     128             :         } else {
     129           0 :             s_ctx.apply_requested = true;
     130             :         }
     131           2 :         break;
     132             : 
     133         200 :     case SAFETY_AUTO_APPLY_ARMED:
     134             :         /* Condition must remain continuously satisfied */
     135         200 :         if (in->engine_running || !standstill(in)) {
     136           2 :             enter(SAFETY_IDLE);
     137           2 :             break;
     138             :         }
     139             :         /* @reqs SWE-008: Auto-apply after 2 s (40 ticks) delay */
     140         198 :         if (s_ctx.ticks_in_state >= SAFETY_AUTO_APPLY_DELAY_50MS) {
     141           4 :             s_ctx.apply_requested = true;
     142           4 :             enter(SAFETY_AUTO_APPLY_TRIGGERED);
     143             :         }
     144         198 :         break;
     145             : 
     146           2 :     case SAFETY_AUTO_APPLY_TRIGGERED:
     147           2 :         if (in->current_state == EPB_STATE_APPLIED) {
     148           2 :             enter(SAFETY_IDLE);
     149             :         } else {
     150           0 :             s_ctx.apply_requested = true;
     151             :         }
     152           2 :         break;
     153             : 
     154           2 :     case SAFETY_DRIVE_AWAY:
     155             :         /* Ends when the brake has been released or preconditions are no longer ok. */
     156           2 :         if (in->current_state == EPB_STATE_RELEASED
     157           0 :             || in->current_state == EPB_STATE_RELEASING) {
     158           2 :             enter(SAFETY_IDLE);
     159           0 :         } else if (!drive_intent(in) || !drive_away_safety_ok(in)) {
     160           0 :             enter(SAFETY_IDLE);
     161             :         } else {
     162           0 :             s_ctx.release_requested = true;
     163             :         }
     164           2 :         break;
     165             : 
     166           0 :     default:
     167           0 :         enter(SAFETY_IDLE);
     168           0 :         break;
     169             :     }
     170             : }
     171             : 
     172          14 : bool safety_mgr_apply_requested(void)
     173             : {
     174          14 :     return s_ctx.apply_requested;
     175             : }
     176             : 
     177           6 : bool safety_mgr_release_requested(void)
     178             : {
     179           6 :     return s_ctx.release_requested;
     180             : }
     181             : 
     182         170 : SafetyState safety_mgr_get_state(void)
     183             : {
     184         170 :     return s_ctx.state;
     185             : }