Line data    Source code

       1             : /**
       2             :  * @file test_safety_manager.c
       3             :  * @brief Unit tests for the Safety Manager (ASIL-D).
       4             :  *
       5             :  * @reqs SWE-007 SWE-008 SWE-009 SWE-010 SWE-011 SWE-012
       6             :  * @arch SWA-001
       7             :  */
       8             : #include "../unit_test_framework.h"
       9             : #include "../../src/safety_manager.h"
      10             : 
      11             : TestStats g_test_stats = {0, 0};
      12             : 
      13          22 : static SafetyInputs make_driving(void)
      14             : {
      15          22 :     SafetyInputs in = {0};
      16          22 :     in.engine_running       = true;
      17          22 :     in.brake_pedal_pressed  = false;
      18          22 :     in.vehicle_speed_kmh    = 50.0f;
      19          22 :     in.grade_percent        = 0.0f;
      20          22 :     in.current_state        = EPB_STATE_RELEASED;
      21          22 :     return in;
      22             : }
      23             : 
      24          12 : static SafetyInputs make_standstill(void)
      25             : {
      26          12 :     SafetyInputs in = make_driving();
      27          12 :     in.vehicle_speed_kmh = 0.0f;
      28          12 :     in.brake_pedal_pressed = true;
      29          12 :     return in;
      30             : }
      31             : 
      32           4 : static void step_n(SafetyInputs* in, int n)
      33             : {
      34          84 :     for (int i = 0; i < n; ++i) {
      35          80 :         safety_mgr_step_50ms(in);
      36             :     }
      37           4 : }
      38             : 
      39           2 : static void test_init(void)
      40             : {
      41           2 :     TEST_BEGIN("init -> IDLE, no apply request");
      42           2 :     ASSERT_EQ(safety_mgr_init(), EPB_OK);
      43           2 :     ASSERT_EQ(safety_mgr_get_state(), SAFETY_IDLE);
      44           2 :     ASSERT_TRUE(!safety_mgr_apply_requested());
      45           2 :     TEST_END();
      46           2 : }
      47             : 
      48           2 : static void test_null_input(void)
      49             : {
      50           2 :     TEST_BEGIN("NULL input is a no-op");
      51           2 :     (void)safety_mgr_init();
      52           2 :     safety_mgr_step_50ms(NULL);
      53           2 :     ASSERT_EQ(safety_mgr_get_state(), SAFETY_IDLE);
      54           2 :     TEST_END();
      55           2 : }
      56             : 
      57             : /* ---- Auto-Apply (SWE-007 + SWE-008) ---- */
      58             : 
      59           2 : static void test_auto_apply_armed_on_engine_off(void)
      60             : {
      61           2 :     TEST_BEGIN("SWE-007: engine off + standstill -> AUTO_APPLY_ARMED");
      62           2 :     (void)safety_mgr_init();
      63           2 :     SafetyInputs in = make_driving();
      64           2 :     in.engine_running    = false;
      65           2 :     in.vehicle_speed_kmh = 0.0f;
      66           2 :     safety_mgr_step_50ms(&in);
      67           2 :     ASSERT_EQ(safety_mgr_get_state(), SAFETY_AUTO_APPLY_ARMED);
      68           2 :     ASSERT_TRUE(!safety_mgr_apply_requested());
      69           2 :     TEST_END();
      70           2 : }
      71             : 
      72           2 : static void test_auto_apply_triggers_after_2s(void)
      73             : {
      74           2 :     TEST_BEGIN("SWE-008: after ~2s -> TRIGGERED + apply request");
      75           2 :     (void)safety_mgr_init();
      76           2 :     SafetyInputs in = make_driving();
      77           2 :     in.engine_running    = false;
      78           2 :     in.vehicle_speed_kmh = 0.0f;
      79             :     /* Halfway: still armed, no request */
      80           2 :     step_n(&in, 20);
      81           2 :     ASSERT_EQ(safety_mgr_get_state(), SAFETY_AUTO_APPLY_ARMED);
      82           2 :     ASSERT_TRUE(!safety_mgr_apply_requested());
      83             :     /* Run up to TRIGGERED within at most 25 more steps (~2 s leeway) */
      84          42 :     for (int i = 0; i < 25; ++i) {
      85          42 :         safety_mgr_step_50ms(&in);
      86          42 :         if (safety_mgr_get_state() == SAFETY_AUTO_APPLY_TRIGGERED) break;
      87             :     }
      88           2 :     ASSERT_EQ(safety_mgr_get_state(), SAFETY_AUTO_APPLY_TRIGGERED);
      89           2 :     ASSERT_TRUE(safety_mgr_apply_requested());
      90           2 :     TEST_END();
      91           2 : }
      92             : 
      93           2 : static void test_auto_apply_aborts_on_engine_on(void)
      94             : {
      95           2 :     TEST_BEGIN("Auto-Apply aborts when engine comes back on");
      96           2 :     (void)safety_mgr_init();
      97           2 :     SafetyInputs in = make_driving();
      98           2 :     in.engine_running    = false;
      99           2 :     in.vehicle_speed_kmh = 0.0f;
     100           2 :     step_n(&in, 20);
     101           2 :     ASSERT_EQ(safety_mgr_get_state(), SAFETY_AUTO_APPLY_ARMED);
     102           2 :     in.engine_running = true;
     103           2 :     safety_mgr_step_50ms(&in);
     104           2 :     ASSERT_EQ(safety_mgr_get_state(), SAFETY_IDLE);
     105           2 :     TEST_END();
     106           2 : }
     107             : 
     108           2 : static void test_auto_apply_returns_idle_when_applied(void)
     109             : {
     110           2 :     TEST_BEGIN("AUTO_APPLY_TRIGGERED -> IDLE when Apply Controller is done");
     111           2 :     (void)safety_mgr_init();
     112           2 :     SafetyInputs in = make_driving();
     113           2 :     in.engine_running    = false;
     114           2 :     in.vehicle_speed_kmh = 0.0f;
     115             :     /* Drive to TRIGGERED */
     116          82 :     for (int i = 0; i < 50; ++i) {
     117          82 :         safety_mgr_step_50ms(&in);
     118          82 :         if (safety_mgr_get_state() == SAFETY_AUTO_APPLY_TRIGGERED) break;
     119             :     }
     120           2 :     ASSERT_EQ(safety_mgr_get_state(), SAFETY_AUTO_APPLY_TRIGGERED);
     121           2 :     in.current_state = EPB_STATE_APPLIED;
     122           2 :     safety_mgr_step_50ms(&in);
     123           2 :     ASSERT_EQ(safety_mgr_get_state(), SAFETY_IDLE);
     124           2 :     ASSERT_TRUE(!safety_mgr_apply_requested());
     125           2 :     TEST_END();
     126           2 : }
     127             : 
     128             : /* ---- Hill-Hold (SWE-009 + SWE-010) ---- */
     129             : 
     130           2 : static void test_hillhold_arms_on_grade_brake_standstill(void)
     131             : {
     132           2 :     TEST_BEGIN("SWE-009: grade >5% + standstill + brake -> HILL_HOLD_ARMED");
     133           2 :     (void)safety_mgr_init();
     134           2 :     SafetyInputs in = make_standstill();
     135           2 :     in.grade_percent = 8.0f;
     136           2 :     safety_mgr_step_50ms(&in);
     137           2 :     ASSERT_EQ(safety_mgr_get_state(), SAFETY_HILL_HOLD_ARMED);
     138           2 :     ASSERT_TRUE(!safety_mgr_apply_requested());
     139           2 :     TEST_END();
     140           2 : }
     141             : 
     142           2 : static void test_hillhold_negative_grade_also_triggers(void)
     143             : {
     144           2 :     TEST_BEGIN("Hill-hold also on negative grade (downhill)");
     145           2 :     (void)safety_mgr_init();
     146           2 :     SafetyInputs in = make_standstill();
     147           2 :     in.grade_percent = -7.0f;
     148           2 :     safety_mgr_step_50ms(&in);
     149           2 :     ASSERT_EQ(safety_mgr_get_state(), SAFETY_HILL_HOLD_ARMED);
     150           2 :     TEST_END();
     151           2 : }
     152             : 
     153           2 : static void test_hillhold_below_threshold_no_arm(void)
     154             : {
     155           2 :     TEST_BEGIN("Grade < 5% -> no hill-hold");
     156           2 :     (void)safety_mgr_init();
     157           2 :     SafetyInputs in = make_standstill();
     158           2 :     in.grade_percent = 4.0f;
     159           2 :     safety_mgr_step_50ms(&in);
     160           2 :     ASSERT_EQ(safety_mgr_get_state(), SAFETY_IDLE);
     161           2 :     TEST_END();
     162           2 : }
     163             : 
     164           2 : static void test_hillhold_active_on_brake_release(void)
     165             : {
     166           2 :     TEST_BEGIN("SWE-010: brake released -> HILL_HOLD_ACTIVE + apply request");
     167           2 :     (void)safety_mgr_init();
     168           2 :     SafetyInputs in = make_standstill();
     169           2 :     in.grade_percent = 8.0f;
     170           2 :     safety_mgr_step_50ms(&in);  /* -> HILL_HOLD_ARMED */
     171           2 :     in.brake_pedal_pressed = false;
     172           2 :     safety_mgr_step_50ms(&in);
     173           2 :     ASSERT_EQ(safety_mgr_get_state(), SAFETY_HILL_HOLD_ACTIVE);
     174           2 :     ASSERT_TRUE(safety_mgr_apply_requested());
     175           2 :     TEST_END();
     176           2 : }
     177             : 
     178           2 : static void test_hillhold_active_ends_on_vehicle_rolling(void)
     179             : {
     180           2 :     TEST_BEGIN("HILL_HOLD_ACTIVE ends when vehicle rolls (v > 2 km/h)");
     181           2 :     (void)safety_mgr_init();
     182           2 :     SafetyInputs in = make_standstill();
     183           2 :     in.grade_percent = 8.0f;
     184           2 :     safety_mgr_step_50ms(&in);
     185           2 :     in.brake_pedal_pressed = false;
     186           2 :     safety_mgr_step_50ms(&in);
     187           2 :     ASSERT_EQ(safety_mgr_get_state(), SAFETY_HILL_HOLD_ACTIVE);
     188           2 :     in.vehicle_speed_kmh = 3.0f;
     189           2 :     safety_mgr_step_50ms(&in);
     190           2 :     ASSERT_EQ(safety_mgr_get_state(), SAFETY_IDLE);
     191           2 :     TEST_END();
     192           2 : }
     193             : 
     194           2 : static void test_hillhold_armed_to_idle_if_grade_drops(void)
     195             : {
     196           2 :     TEST_BEGIN("HILL_HOLD_ARMED -> IDLE when grade < 5%");
     197           2 :     (void)safety_mgr_init();
     198           2 :     SafetyInputs in = make_standstill();
     199           2 :     in.grade_percent = 8.0f;
     200           2 :     safety_mgr_step_50ms(&in);
     201           2 :     ASSERT_EQ(safety_mgr_get_state(), SAFETY_HILL_HOLD_ARMED);
     202           2 :     in.grade_percent = 1.0f;
     203           2 :     safety_mgr_step_50ms(&in);
     204           2 :     ASSERT_EQ(safety_mgr_get_state(), SAFETY_IDLE);
     205           2 :     TEST_END();
     206           2 : }
     207             : 
     208             : /* ---- Mutually exclusive: never in both modes at once ---- */
     209             : 
     210             : /* ---- Drive-Away-Assist (SWE-011 + SWE-012) ---- */
     211             : 
     212          10 : static SafetyInputs make_applied_at_rest(void)
     213             : {
     214          10 :     SafetyInputs in = {0};
     215          10 :     in.engine_running       = true;
     216          10 :     in.brake_pedal_pressed  = false;
     217          10 :     in.vehicle_speed_kmh    = 0.0f;
     218          10 :     in.grade_percent        = 0.0f;
     219          10 :     in.current_state        = EPB_STATE_APPLIED;
     220          10 :     in.gas_pedal_percent    = 0.0f;
     221          10 :     in.gear_in_drive        = false;
     222          10 :     in.door_closed          = true;
     223          10 :     in.seatbelt_fastened    = true;
     224          10 :     return in;
     225             : }
     226             : 
     227           2 : static void test_drive_away_armed_on_intent(void)
     228             : {
     229           2 :     TEST_BEGIN("SWE-011 + SWE-012: intent + safety -> DRIVE_AWAY + release request");
     230           2 :     (void)safety_mgr_init();
     231           2 :     SafetyInputs in = make_applied_at_rest();
     232           2 :     in.gas_pedal_percent = 25.0f;
     233           2 :     in.gear_in_drive     = true;
     234           2 :     safety_mgr_step_50ms(&in);
     235           2 :     ASSERT_EQ(safety_mgr_get_state(), SAFETY_DRIVE_AWAY);
     236           2 :     ASSERT_TRUE(safety_mgr_release_requested());
     237           2 :     TEST_END();
     238           2 : }
     239             : 
     240           2 : static void test_drive_away_blocked_without_safety(void)
     241             : {
     242           2 :     TEST_BEGIN("SWE-012: door open blocks drive-away");
     243           2 :     (void)safety_mgr_init();
     244           2 :     SafetyInputs in = make_applied_at_rest();
     245           2 :     in.gas_pedal_percent = 25.0f;
     246           2 :     in.gear_in_drive     = true;
     247           2 :     in.door_closed       = false;  /* door open */
     248           2 :     safety_mgr_step_50ms(&in);
     249           2 :     ASSERT_EQ(safety_mgr_get_state(), SAFETY_IDLE);
     250           2 :     ASSERT_TRUE(!safety_mgr_release_requested());
     251           2 :     TEST_END();
     252           2 : }
     253             : 
     254           2 : static void test_drive_away_blocked_without_seatbelt(void)
     255             : {
     256           2 :     TEST_BEGIN("SWE-012: seatbelt not fastened blocks drive-away");
     257           2 :     (void)safety_mgr_init();
     258           2 :     SafetyInputs in = make_applied_at_rest();
     259           2 :     in.gas_pedal_percent = 25.0f;
     260           2 :     in.gear_in_drive     = true;
     261           2 :     in.seatbelt_fastened = false;
     262           2 :     safety_mgr_step_50ms(&in);
     263           2 :     ASSERT_EQ(safety_mgr_get_state(), SAFETY_IDLE);
     264           2 :     TEST_END();
     265           2 : }
     266             : 
     267           2 : static void test_drive_away_blocked_below_gas_threshold(void)
     268             : {
     269           2 :     TEST_BEGIN("SWE-011: throttle < 10% does not trigger drive-away");
     270           2 :     (void)safety_mgr_init();
     271           2 :     SafetyInputs in = make_applied_at_rest();
     272           2 :     in.gas_pedal_percent = 5.0f;
     273           2 :     in.gear_in_drive     = true;
     274           2 :     safety_mgr_step_50ms(&in);
     275           2 :     ASSERT_EQ(safety_mgr_get_state(), SAFETY_IDLE);
     276           2 :     TEST_END();
     277           2 : }
     278             : 
     279           2 : static void test_drive_away_ends_when_released(void)
     280             : {
     281           2 :     TEST_BEGIN("DRIVE_AWAY -> IDLE when Apply Controller has released");
     282           2 :     (void)safety_mgr_init();
     283           2 :     SafetyInputs in = make_applied_at_rest();
     284           2 :     in.gas_pedal_percent = 25.0f;
     285           2 :     in.gear_in_drive     = true;
     286           2 :     safety_mgr_step_50ms(&in);  /* -> DRIVE_AWAY */
     287           2 :     in.current_state = EPB_STATE_RELEASED;
     288           2 :     safety_mgr_step_50ms(&in);
     289           2 :     ASSERT_EQ(safety_mgr_get_state(), SAFETY_IDLE);
     290           2 :     ASSERT_TRUE(!safety_mgr_release_requested());
     291           2 :     TEST_END();
     292           2 : }
     293             : 
     294           2 : static void test_already_applied_does_not_arm_auto_apply(void)
     295             : {
     296           2 :     TEST_BEGIN("Already applied: no auto-apply arming");
     297           2 :     (void)safety_mgr_init();
     298           2 :     SafetyInputs in = make_driving();
     299           2 :     in.engine_running    = false;
     300           2 :     in.vehicle_speed_kmh = 0.0f;
     301           2 :     in.current_state     = EPB_STATE_APPLIED;
     302           2 :     safety_mgr_step_50ms(&in);
     303           2 :     ASSERT_EQ(safety_mgr_get_state(), SAFETY_IDLE);
     304           2 :     TEST_END();
     305           2 : }
     306             : 
     307           2 : int main(void)
     308             : {
     309           2 :     printf("== test_safety_manager ==\n");
     310           2 :     test_init();
     311           2 :     test_null_input();
     312           2 :     test_auto_apply_armed_on_engine_off();
     313           2 :     test_auto_apply_triggers_after_2s();
     314           2 :     test_auto_apply_aborts_on_engine_on();
     315           2 :     test_auto_apply_returns_idle_when_applied();
     316           2 :     test_hillhold_arms_on_grade_brake_standstill();
     317           2 :     test_hillhold_negative_grade_also_triggers();
     318           2 :     test_hillhold_below_threshold_no_arm();
     319           2 :     test_hillhold_active_on_brake_release();
     320           2 :     test_hillhold_active_ends_on_vehicle_rolling();
     321           2 :     test_hillhold_armed_to_idle_if_grade_drops();
     322           2 :     test_drive_away_armed_on_intent();
     323           2 :     test_drive_away_blocked_without_safety();
     324           2 :     test_drive_away_blocked_without_seatbelt();
     325           2 :     test_drive_away_blocked_below_gas_threshold();
     326           2 :     test_drive_away_ends_when_released();
     327           2 :     test_already_applied_does_not_arm_auto_apply();
     328           2 :     TEST_SUMMARY();
     329             : }