From 3ae376ecfef9f1db4cbea09fa336b22ba36a0da0 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Sun, 15 Dec 2024 18:05:31 -0500 Subject: [PATCH] Modular Assistive Driving System (MADS) (#57) * improvements on the state machine for exiting controls and improvements on replay drive, and optimizations * New tests on hyundai for now to see how they behave with lkas and main button pressed interchangeably * cleaning up hyundai tests and ensuring we always cleanup mads states at the end of the tests * Adjusting tests * Adjusting the tests a bit more to ensure clean states * Cleaning up and simplifying logic * ensuring all tests always cleanup * improving the state * make static happy * Refactor safety replay script for better debugging and update Honda safety code The commit performs a comprehensive revision of the safety replay script, specifically focusing on introducing debug variables and enhancing the logging capabilities for improved debugging. Furthermore, changes were made to the Honda safety code. The test helpers within libpanda were also expanded for inclusion of additional test conditions. * Introduce 'ACC_MAIN_OFF' as a new disengagement reason in Sunnypilot's 'safety_mads.h' The Sunnypilot's 'safety_mads.h' file has been updated to include 'ACC_MAIN_OFF' as a new cause for disconnection in the 'DisengageReason' enumeration. If an 'acc_main_off' signal is received, the 'mads_exit_controls' function halts all requests for lateral control engagement. Additionally, the status of 'controls_requested_lat' now mirrors 'controls_allowed_lat' after a button press. * simpler logic cleaner * reorder code for readability * tmp * Refactor state transitions and add event handlers Renamed StateTransition to EdgeTransition for clarity and updated related logic. Introduced event handlers for button presses and ACC state changes, reducing duplicated control flow code. Improved encapsulation and maintainability by restructuring state update functions. * reorder * Refactor MADS state handling logic Removed redundant event handler functions and unnecessary timestamp fields to streamline the code. Simplified button and binary state updates by integrating logic directly into transition checks. Commented out unused fields * adding some more tests * split init * format * update naming * Refine lateral control request logic in safety_mads.h The logic for setting the `controls_requested_lat` variable in safety_mads.h has been refined. Previously, it switched state based on the current value of `controls_allowed_lat`. Now, it also takes into account the current state of `acc_main`, ensuring a more nuanced control request mechanism that accounts for different operational scenarios. * Fix button state handling in mads_exit_controls logic. Refactor button state transitions to better handle lateral control requests when ACC is active. Ensure controls are correctly disengaged under specific conditions, by setting `controls_requested_lat` more reliably during state transitions. This change improves safety by preventing inadvertent disengagement when ACC is not active. * Add test for LKAS button press with ACC main on This commit introduces a new test to ensure that controls remain enabled when the LKAS/LFA button is pressed while ACC main is on. It checks that LKAS button operations don't interfere with control permissions in this specific configuration, improving test coverage and preventing potential safety issues. * Add mismatch detection and change mads_acc_main to bool Enhanced mismatch detection logic by tracking cases where 'controls_allowed' is true while 'controls_allowed_lat' is false, updating the script to print relevant debug information. Additionally, changed the data type of 'mads_acc_main' and 'mads_acc_main_prev' from int to bool for improved type accuracy and consistency. * update controls_allowed_lat_pkt on health pkt to actually follow is_lat_active() which has the final word on whether we can allow lat or not. * Can't perform this test on toyota as we never really process a button disengagement for toyota * wow, we forgot about pcm hyundai can-fd * nuke nuke nuke * Revert "nuke nuke nuke" This reverts commit 9bf0de640a3439ac43c27bcbc6568853966d370b. * update name * event driven update states * add get_pcm_main_cruise_available * split PCM and non-PCM main cruise tets * fix some * pcm main cruise availability mutation * toyota pass fake lkas btn pressed * more * make pcm acc main rising edge on init * only falling edge when actually 0 (need test for mutation) * misra * remove state flags, main button related * skip lkas related tests with toyota and subaru fake button * need for honda * static * mutation * misra * skip nidec pcm alt * engage mads if controls allowed rising * static * remove non pcm properties * fixup! engage mads if controls allowed rising * move back * fix static * move around * Hyundai openpilot longitudinal main cruise button state handling * main button unit test * acc_main_on mismatch unit tests * clean up old main cruise button unit tests * add more reasons * cleanup * rename * rearrange * Revert "rearrange" This reverts commit f07caaa5b98b74c23667b387430ac48ba95bf21c. * more rearrange * rename * more * too slow * Revert "too slow" This reverts commit 31a249aebfa9c985e37be050e525b6924ca9e83d. * too slow v2 * cleanup * rename * more cleanup * Parse more flags from alt exp, more tests, hyundai main cruise allowed * missed * mutation for controls allowed rising edge * ford mutation * Update tests/safety/test.sh Co-authored-by: DevTekVE * license * unused * remove * comment * Apply suggestions from code review Co-authored-by: DevTekVE * comment * refactor alternative experience handling with helper function * use always allowed mads button alt exp * rename * parenthesis * use alternative experience for unit tests inits * cleanup * rename * mutation tests for alternative experience flags * bump timer * test for disengage and no disengage lateral on brake * test allow MADS engage with brake pressed * rename * move around * button combo test * use acc_main_on directly from global * fix caught failures from last commit's fix * Revert "use acc_main_on directly from global" This reverts commit 346964f55d020a287a1a679e22691ad8873e2a64. * Properly fix lmao * Add support for LKAS button handling across Chrysler platforms Introduced LKAS button message parsing for multiple Chrysler platforms, including specific handling for center stack button messages. Updated tests and safety configurations to reflect these changes, ensuring compatibility with different vehicle variants. This enhances modularity and improves safety feature integration. * Dockerfile: point to sunnypilot/opendbc * Happy days :) * clean * testx * Revert "Happy days :)" This reverts commit 7ea27b53c8f3b1e37677c1ce9498229fceac9de6. * symlink prior building * comment * only parse mads lateral, not stock op's lateral * do not allow controls allowed if acc_main_on is off * expose system_enabled, do not allow controls allowed to steer if system_mads is off * fix hyundai tests with acc_main_on requirement * fix test with new controls allowed with system_mads off * fix replay drive * Change 'DISABLE_DISENGAGE_LATERAL_ON_BRAKE' to 'DISENGAGE_LATERAL_ON_BRAKE' The commit modifies the usage of the 'DISABLE_DISENGAGE_LATERAL_ON_BRAKE' variable globally and replaces it with 'DISENGAGE_LATERAL_ON_BRAKE'. This change promotes correct and clear semantics, since the variable now indicates a state rather than the negation of a state. * Adding some more debug printouts on replay drive * remove unified engagement mode in panda * treat MADS button as user entry * controls allow should be allowed at all times * squash! treat MADS button as user entry * heartbeat for mads * heartbeat mismatch exit control * remove always allow mads button from alt * move to safety_mads * check heartbeat directly in main * remove main cruise allowed from alt * uint * squash! check heartbeat directly in main * update tests * not needed * fix mads_exit_controls sometimes not assigning disengage reason * more disengage lateral on brake tests * extern * missesd * honda mutation test * again * rename * more dlob test * update name * fix tests * fix panda tests * Refactor MADS state management to simplify pointer usage. This change replaces many pointer-based state variables with direct ones, improving code readability and reducing complexity. It also standardizes the use of `const` for parameters and updates function implementations accordingly. These improvements enhance maintainability and reduce potential for pointer-related errors. * Simplify braking logic in m_mads_check_braking function Removed redundant conditions to streamline braking logic. This change maintains functionality while improving code readability and maintainability. Only necessary checks are now performed to determine disengagement. * Prevent lateral control engagement during braking Added a condition to disable lateral control engagement when braking with disengage-on-brake enabled. This change is marked as a demonstration and is not final for merging. Moved the disengage_reason to be set only when an actual disengagement occurred. * Refactor MADS state handling and fix type consistency Remove redundant `get_mads_state` inline definition and migrate it to a static function. Fix return type syntax in `get_mads_pending_disengage_reason`. Minor formatting adjustments improve readability and code clarity. Refactor disengagement logic with enhanced reason tracking Added distinction between active and pending disengagement reasons to improve system state tracking. Updated related enums, structs, and logic to ensure proper handling during control transitions. Added new safety tests to verify behavior under braking and ACC conditions. Refactoring lateral control permissions and brake checks in MADS This revision refactors the MADS safety code. The aim is to simplify and improve readability. Operations and checks for brake states and lateral control permissions have been consolidated into fewer methods. In addition, unused 'previous_disengage' state tracking has been removed from MADSState structure to avoid unnecessary state tracking. Moreover, the 'can_allow_controls_lat' function has been removed entirely and its functionality has been incorporated into other functions, reducing the function count and complexity of the code. The braking status is now tracked with BinaryStateTracking for consistency. These changes maintain the system's functionality while optimizing the code and improving maintainability. * Why MISRA, why!? WHY!???? I DIDNT EVEN TOUCH THIS FILE OR NOWHERE NEAR! * Some format * no more messing with misra * const * more generic names * revert to validate * are you srs * make gpio.h stock again and add to supression lists the check on gpio.h since we are not even touching it and we don't plan on ever doing so * hard code to skip heartbeat check * update comment * cleanup * Update tests/safety/test_honda.py --------- Co-authored-by: DevTekVE --- .github/workflows/test.yaml | 2 +- board/health.h | 1 + board/main.c | 2 + board/main_comms.h | 3 + board/safety.h | 18 +- board/safety/safety_chrysler.h | 28 +- board/safety/safety_ford.h | 5 + board/safety/safety_gm.h | 4 + board/safety/safety_honda.h | 8 + board/safety/safety_hyundai.h | 17 + board/safety/safety_hyundai_canfd.h | 8 + board/safety/safety_hyundai_common.h | 48 +++ board/safety/safety_mazda.h | 1 + board/safety/safety_nissan.h | 11 + board/safety/safety_subaru.h | 8 + board/safety/safety_subaru_preglobal.h | 1 + board/safety/safety_toyota.h | 15 + board/sunnypilot/safety_mads.h | 199 ++++++++++ board/sunnypilot/safety_mads_declarations.h | 121 ++++++ python/__init__.py | 13 +- tests/libpanda/safety_helpers.h | 77 ++++ tests/libpanda/safety_helpers.py | 37 ++ tests/misra/suppressions.txt | 1 + tests/safety/common.py | 5 +- tests/safety/hyundai_common.py | 140 +++++++ tests/safety/mads_common.py | 391 ++++++++++++++++++++ tests/safety/test_chrysler.py | 19 +- tests/safety/test_ford.py | 13 + tests/safety/test_honda.py | 43 +++ tests/safety/test_hyundai.py | 46 +++ tests/safety/test_hyundai_canfd.py | 23 ++ tests/safety/test_nissan.py | 16 +- tests/safety/test_subaru.py | 16 + tests/safety/test_toyota.py | 17 + tests/safety_replay/replay_drive.py | 68 +++- 35 files changed, 1407 insertions(+), 18 deletions(-) create mode 100644 board/sunnypilot/safety_mads.h create mode 100644 board/sunnypilot/safety_mads_declarations.h create mode 100644 tests/safety/mads_common.py diff --git a/.github/workflows/test.yaml b/.github/workflows/test.yaml index 39559ea246..27f145b521 100644 --- a/.github/workflows/test.yaml +++ b/.github/workflows/test.yaml @@ -128,7 +128,7 @@ jobs: - name: Build Docker image run: eval "$BUILD" - name: Mutation tests - timeout-minutes: 5 + timeout-minutes: 15 run: ${{ env.RUN }} "GIT_REF=${{ github.event_name == 'push' && (github.ref == 'refs/heads/master' || github.ref == 'refs/heads/master-new') && github.event.before || 'origin/master' }} cd tests/safety && ./mutation.sh" static_analysis: diff --git a/board/health.h b/board/health.h index 74d822dc6c..cb827ce252 100644 --- a/board/health.h +++ b/board/health.h @@ -13,6 +13,7 @@ struct __attribute__((packed)) health_t { uint8_t ignition_line_pkt; uint8_t ignition_can_pkt; uint8_t controls_allowed_pkt; + uint8_t controls_allowed_lat_pkt; uint8_t car_harness_status_pkt; uint8_t safety_mode_pkt; uint16_t safety_param_pkt; diff --git a/board/main.c b/board/main.c index aaea95d089..890345ab50 100644 --- a/board/main.c +++ b/board/main.c @@ -233,6 +233,8 @@ static void tick_handler(void) { heartbeat_engaged_mismatches = 0U; } + mads_heartbeat_engaged_check(); + if (!heartbeat_disabled) { // if the heartbeat has been gone for a while, go to SILENT safety mode and enter power save if (heartbeat_counter >= (check_started() ? HEARTBEAT_IGNITION_CNT_ON : HEARTBEAT_IGNITION_CNT_OFF)) { diff --git a/board/main_comms.h b/board/main_comms.h index e366b19a09..59dab59723 100644 --- a/board/main_comms.h +++ b/board/main_comms.h @@ -17,6 +17,7 @@ static int get_health_pkt(void *dat) { health->ignition_can_pkt = ignition_can; health->controls_allowed_pkt = controls_allowed; + health->controls_allowed_lat_pkt = mads_is_lateral_control_allowed_by_mads(); health->safety_tx_blocked_pkt = safety_tx_blocked; health->safety_rx_invalid_pkt = safety_rx_invalid; health->tx_buffer_overflow_pkt = tx_buffer_overflow; @@ -247,6 +248,7 @@ int comms_control_handler(ControlPacket_t *req, uint8_t *resp) { // you can only set this if you are in a non car safety mode if (!is_car_safety_mode(current_safety_mode)) { alternative_experience = req->param1; + mads_set_alternative_experience(&alternative_experience); } break; // **** 0xe0: uart read @@ -350,6 +352,7 @@ int comms_control_handler(ControlPacket_t *req, uint8_t *resp) { heartbeat_lost = false; heartbeat_disabled = false; heartbeat_engaged = (req->param1 == 1U); + heartbeat_engaged_mads = true; // FIXME-SP: Implement proper heartbeat check from sunnypilot break; } // **** 0xf6: set siren enabled diff --git a/board/safety.h b/board/safety.h index 913109957c..5470e7b364 100644 --- a/board/safety.h +++ b/board/safety.h @@ -1,5 +1,7 @@ #pragma once +#include "sunnypilot/safety_mads.h" + #include "safety_declarations.h" #include "can.h" @@ -111,6 +113,10 @@ uint16_t current_safety_param = 0; static const safety_hooks *current_hooks = &nooutput_hooks; safety_config current_safety_config; +static bool is_lat_active(void) { + return controls_allowed || mads_is_lateral_control_allowed_by_mads(); +} + static bool is_msg_valid(RxCheck addr_list[], int index) { bool valid = true; if (index != -1) { @@ -301,6 +307,7 @@ void safety_tick(const safety_config *cfg) { cfg->rx_checks[i].status.lagging = lagging; if (lagging) { controls_allowed = false; + mads_exit_controls(MADS_DISENGAGE_REASON_LAG); } if (lagging || !is_msg_valid(cfg->rx_checks, i)) { @@ -343,6 +350,7 @@ void generic_rx_checks(bool stock_ecu_detected) { if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && stock_ecu_detected) { relay_malfunction_set(); } + mads_state_update(vehicle_moving, acc_main_on, controls_allowed, brake_pressed || regen_braking); } static void relay_malfunction_reset(void) { @@ -601,7 +609,7 @@ bool steer_torque_cmd_checks(int desired_torque, int steer_req, const SteeringLi bool violation = false; uint32_t ts = microsecond_timer_get(); - if (controls_allowed) { + if (is_lat_active()) { // *** global torque limit check *** violation |= max_limit_check(desired_torque, limits.max_steer, -limits.max_steer); @@ -628,7 +636,7 @@ bool steer_torque_cmd_checks(int desired_torque, int steer_req, const SteeringLi } // no torque if controls is not allowed - if (!controls_allowed && (desired_torque != 0)) { + if (!is_lat_active() && (desired_torque != 0)) { violation = true; } @@ -670,7 +678,7 @@ bool steer_torque_cmd_checks(int desired_torque, int steer_req, const SteeringLi } // reset to 0 if either controls is not allowed or there's a violation - if (violation || !controls_allowed) { + if (violation || !is_lat_active()) { valid_steer_req_count = 0; invalid_steer_req_count = 0; desired_torque_last = 0; @@ -686,7 +694,7 @@ bool steer_torque_cmd_checks(int desired_torque, int steer_req, const SteeringLi bool steer_angle_cmd_checks(int desired_angle, bool steer_control_enabled, const SteeringLimits limits) { bool violation = false; - if (controls_allowed && steer_control_enabled) { + if (is_lat_active() && steer_control_enabled) { // convert floating point angle rate limits to integers in the scale of the desired angle on CAN, // add 1 to not false trigger the violation. also fudge the speed by 1 m/s so rate limits are // always slightly above openpilot's in case we read an updated speed in between angle commands @@ -729,7 +737,7 @@ bool steer_angle_cmd_checks(int desired_angle, bool steer_control_enabled, const } // No angle control allowed when controls are not allowed - violation |= !controls_allowed && steer_control_enabled; + violation |= !is_lat_active() && steer_control_enabled; return violation; } diff --git a/board/safety/safety_chrysler.h b/board/safety/safety_chrysler.h index 2bbc942715..566ce8b81d 100644 --- a/board/safety/safety_chrysler.h +++ b/board/safety/safety_chrysler.h @@ -11,6 +11,10 @@ typedef struct { const int DAS_6; const int LKAS_COMMAND; const int CRUISE_BUTTONS; + const int LKAS_HEARTBIT; + const int Center_Stack_1; + const int Center_Stack_2; + const int TRACTION_BUTTON; } ChryslerAddrs; typedef enum { @@ -78,6 +82,7 @@ static void chrysler_rx_hook(const CANPacket_t *to_push) { if ((bus == das_3_bus) && (addr == chrysler_addrs->DAS_3)) { bool cruise_engaged = GET_BIT(to_push, 21U); pcm_cruise_check(cruise_engaged); + acc_main_on = GET_BIT(to_push, 20U); } // TODO: use the same message for both @@ -101,6 +106,19 @@ static void chrysler_rx_hook(const CANPacket_t *to_push) { brake_pressed = ((GET_BYTE(to_push, 0U) & 0xFU) >> 2U) == 1U; } + if ((chrysler_platform == CHRYSLER_PACIFICA) && (bus == 0) && (addr == chrysler_addrs->TRACTION_BUTTON)) { + mads_button_press = GET_BIT(to_push, 53U) ? MADS_BUTTON_PRESSED : MADS_BUTTON_NOT_PRESSED; + } + + if ((chrysler_platform != CHRYSLER_PACIFICA) && (bus == 0)) { + if (addr == chrysler_addrs->Center_Stack_1) { + mads_button_press = GET_BIT(to_push, 53U) ? MADS_BUTTON_PRESSED : MADS_BUTTON_NOT_PRESSED; + } + if (addr == chrysler_addrs->Center_Stack_2) { + mads_button_press = GET_BIT(to_push, 57U) ? MADS_BUTTON_PRESSED : MADS_BUTTON_NOT_PRESSED; + } + } + generic_rx_checks((bus == 0) && (addr == chrysler_addrs->LKAS_COMMAND)); } @@ -175,7 +193,8 @@ static int chrysler_fwd_hook(int bus_num, int addr) { } // forward all messages from camera except LKAS messages - const bool is_lkas = ((addr == chrysler_addrs->LKAS_COMMAND) || (addr == chrysler_addrs->DAS_6)); + const bool is_lkas_heartbit = (addr == chrysler_addrs->LKAS_HEARTBIT) && (chrysler_platform == CHRYSLER_PACIFICA); + const bool is_lkas = ((addr == chrysler_addrs->LKAS_COMMAND) || (addr == chrysler_addrs->DAS_6) || is_lkas_heartbit); if ((bus_num == 2) && !is_lkas){ bus_fwd = 0; } @@ -197,6 +216,8 @@ static safety_config chrysler_init(uint16_t param) { .DAS_6 = 0x2A6, // LKAS HUD and auto headlight control from DASM .LKAS_COMMAND = 0x292, // LKAS controls from DASM .CRUISE_BUTTONS = 0x23B, // Cruise control buttons + .LKAS_HEARTBIT = 0x2D9, // LKAS HEARTBIT from DASM + .TRACTION_BUTTON = 0x330, // Traction control button }; // CAN messages for the 5th gen RAM DT platform @@ -209,6 +230,8 @@ static safety_config chrysler_init(uint16_t param) { .DAS_6 = 0xFA, // LKAS HUD and auto headlight control from DASM .LKAS_COMMAND = 0xA6, // LKAS controls from DASM .CRUISE_BUTTONS = 0xB1, // Cruise control buttons + .Center_Stack_1 = 0xDD, // Center stack buttons1 + .Center_Stack_2 = 0x28A, // Center stack buttons2 }; static RxCheck chrysler_ram_dt_rx_checks[] = { @@ -232,6 +255,7 @@ static safety_config chrysler_init(uint16_t param) { {CHRYSLER_ADDRS.CRUISE_BUTTONS, 0, 3}, {CHRYSLER_ADDRS.LKAS_COMMAND, 0, 6}, {CHRYSLER_ADDRS.DAS_6, 0, 8}, + {CHRYSLER_ADDRS.LKAS_HEARTBIT, 0, 5}, }; static const CanMsg CHRYSLER_RAM_DT_TX_MSGS[] = { @@ -251,6 +275,8 @@ static safety_config chrysler_init(uint16_t param) { .DAS_6 = 0x275, // LKAS HUD and auto headlight control from DASM .LKAS_COMMAND = 0x276, // LKAS controls from DASM .CRUISE_BUTTONS = 0x23A, // Cruise control buttons + .Center_Stack_1 = 0x330, // Center stack buttons1 + .Center_Stack_2 = 0x28A, // Center stack buttons2 }; static RxCheck chrysler_ram_hd_rx_checks[] = { diff --git a/board/safety/safety_ford.h b/board/safety/safety_ford.h index 5b19dd9ca5..29354b3dbd 100644 --- a/board/safety/safety_ford.h +++ b/board/safety/safety_ford.h @@ -180,6 +180,11 @@ static void ford_rx_hook(const CANPacket_t *to_push) { unsigned int cruise_state = GET_BYTE(to_push, 1) & 0x07U; bool cruise_engaged = (cruise_state == 4U) || (cruise_state == 5U); pcm_cruise_check(cruise_engaged); + acc_main_on = (cruise_state == 3U) || cruise_engaged; + } + + if (addr == FORD_Steering_Data_FD1) { + mads_button_press = GET_BIT(to_push, 40U) ? MADS_BUTTON_PRESSED : MADS_BUTTON_NOT_PRESSED; } // If steering controls messages are received on the destination bus, it's an indication diff --git a/board/safety/safety_gm.h b/board/safety/safety_gm.h index f0902a921a..2af436c56e 100644 --- a/board/safety/safety_gm.h +++ b/board/safety/safety_gm.h @@ -85,6 +85,10 @@ static void gm_rx_hook(const CANPacket_t *to_push) { regen_braking = (GET_BYTE(to_push, 0) >> 4) != 0U; } + if (addr == 0xC9) { + acc_main_on = GET_BIT(to_push, 29U); + } + bool stock_ecu_detected = (addr == 0x180); // ASCMLKASteeringCmd // Check ASCMGasRegenCmd only if we're blocking it diff --git a/board/safety/safety_honda.h b/board/safety/safety_honda.h index 60ccea1e16..fba93c45d6 100644 --- a/board/safety/safety_honda.h +++ b/board/safety/safety_honda.h @@ -115,6 +115,14 @@ static void honda_rx_hook(const CANPacket_t *to_push) { if (((addr == 0x1A6) || (addr == 0x296)) && (bus == pt_bus)) { int button = (GET_BYTE(to_push, 0) & 0xE0U) >> 5; + int cruise_setting = (GET_BYTE(to_push, (addr == 0x296) ? 0U : 5U) & 0x0CU) >> 2U; + if (cruise_setting == 1) { + mads_button_press = MADS_BUTTON_PRESSED; + } else if (cruise_setting == 0) { + mads_button_press = MADS_BUTTON_NOT_PRESSED; + } else { + } + // enter controls on the falling edge of set or resume bool set = (button != HONDA_BTN_SET) && (cruise_button_prev == HONDA_BTN_SET); bool res = (button != HONDA_BTN_RESUME) && (cruise_button_prev == HONDA_BTN_RESUME); diff --git a/board/safety/safety_hyundai.h b/board/safety/safety_hyundai.h index 57043f67bc..8d5742db03 100644 --- a/board/safety/safety_hyundai.h +++ b/board/safety/safety_hyundai.h @@ -126,6 +126,12 @@ static void hyundai_rx_hook(const CANPacket_t *to_push) { hyundai_common_cruise_state_check(cruise_engaged); } + if ((addr == 0x420) && (((bus == 0) && !hyundai_camera_scc) || ((bus == 2) && hyundai_camera_scc))) { + if (!hyundai_longitudinal) { + acc_main_on = GET_BIT(to_push, 0U); + } + } + if (bus == 0) { if (addr == 0x251) { int torque_driver_new = (GET_BYTES(to_push, 0, 2) & 0x7ffU) - 1024U; @@ -133,6 +139,10 @@ static void hyundai_rx_hook(const CANPacket_t *to_push) { update_sample(&torque_driver, torque_driver_new); } + if (addr == 0x391) { + mads_button_press = GET_BIT(to_push, 4U) ? MADS_BUTTON_PRESSED : MADS_BUTTON_NOT_PRESSED; + } + // ACC steering wheel buttons if (addr == 0x4F1) { int cruise_button = GET_BYTE(to_push, 0) & 0x7U; @@ -170,6 +180,8 @@ static void hyundai_rx_hook(const CANPacket_t *to_push) { } generic_rx_checks(stock_ecu_detected); } + + hyundai_common_reset_acc_main_on_mismatches(); } static bool hyundai_tx_hook(const CANPacket_t *to_send) { @@ -190,6 +202,11 @@ static bool hyundai_tx_hook(const CANPacket_t *to_send) { } } + if (addr == 0x420) { + acc_main_on_tx = GET_BIT(to_send, 0U); + hyundai_common_acc_main_on_sync(); + } + // ACCEL: safety check if (addr == 0x421) { int desired_accel_raw = (((GET_BYTE(to_send, 4) & 0x7U) << 8) | GET_BYTE(to_send, 3)) - 1023U; diff --git a/board/safety/safety_hyundai_canfd.h b/board/safety/safety_hyundai_canfd.h index b42889bb0e..5ae8f82549 100644 --- a/board/safety/safety_hyundai_canfd.h +++ b/board/safety/safety_hyundai_canfd.h @@ -68,9 +68,11 @@ static void hyundai_canfd_rx_hook(const CANPacket_t *to_push) { if (addr == 0x1cf) { cruise_button = GET_BYTE(to_push, 2) & 0x7U; main_button = GET_BIT(to_push, 19U); + mads_button_press = GET_BIT(to_push, 23U) ? MADS_BUTTON_PRESSED : MADS_BUTTON_NOT_PRESSED; } else { cruise_button = (GET_BYTE(to_push, 4) >> 4) & 0x7U; main_button = GET_BIT(to_push, 34U); + mads_button_press = GET_BIT(to_push, 39U) ? MADS_BUTTON_PRESSED : MADS_BUTTON_NOT_PRESSED; } hyundai_common_cruise_buttons_check(cruise_button, main_button); } @@ -105,6 +107,7 @@ static void hyundai_canfd_rx_hook(const CANPacket_t *to_push) { int cruise_status = ((GET_BYTE(to_push, 8) >> 4) & 0x7U); bool cruise_engaged = (cruise_status == 1) || (cruise_status == 2); hyundai_common_cruise_state_check(cruise_engaged); + acc_main_on = GET_BIT(to_push, 66U); } } @@ -118,6 +121,8 @@ static void hyundai_canfd_rx_hook(const CANPacket_t *to_push) { } generic_rx_checks(stock_ecu_detected); + hyundai_common_reset_acc_main_on_mismatches(); + } static bool hyundai_canfd_tx_hook(const CANPacket_t *to_send) { @@ -192,6 +197,9 @@ static bool hyundai_canfd_tx_hook(const CANPacket_t *to_send) { if (violation) { tx = false; } + + acc_main_on_tx = GET_BIT(to_send, 66U); + hyundai_common_acc_main_on_sync(); } return tx; diff --git a/board/safety/safety_hyundai_common.h b/board/safety/safety_hyundai_common.h index af22b7d942..80b51e722c 100644 --- a/board/safety/safety_hyundai_common.h +++ b/board/safety/safety_hyundai_common.h @@ -40,8 +40,16 @@ bool hyundai_alt_limits = false; extern bool hyundai_escc; bool hyundai_escc = false; +extern bool hyundai_longitudinal_main_cruise_toggleable; +bool hyundai_longitudinal_main_cruise_toggleable = false; + static uint8_t hyundai_last_button_interaction; // button messages since the user pressed an enable button +static bool main_button_prev; +static bool acc_main_on_prev; +static bool acc_main_on_tx; +static uint32_t acc_main_on_mismatches; + void hyundai_common_init(uint16_t param) { const int HYUNDAI_PARAM_EV_GAS = 1; const int HYUNDAI_PARAM_HYBRID_GAS = 2; @@ -49,6 +57,7 @@ void hyundai_common_init(uint16_t param) { const int HYUNDAI_PARAM_CANFD_HDA2 = 16; const int HYUNDAI_PARAM_ALT_LIMITS = 64; // TODO: shift this down with the rest of the common flags const int HYUNDAI_PARAM_ESCC = 512; + const int HYUNDAI_PARAM_LONGITUDINAL_MAIN_CRUISE_TOGGLEABLE = 1024; hyundai_ev_gas_signal = GET_FLAG(param, HYUNDAI_PARAM_EV_GAS); hyundai_hybrid_gas_signal = !hyundai_ev_gas_signal && GET_FLAG(param, HYUNDAI_PARAM_HYBRID_GAS); @@ -56,9 +65,15 @@ void hyundai_common_init(uint16_t param) { hyundai_canfd_hda2 = GET_FLAG(param, HYUNDAI_PARAM_CANFD_HDA2); hyundai_alt_limits = GET_FLAG(param, HYUNDAI_PARAM_ALT_LIMITS); hyundai_escc = GET_FLAG(param, HYUNDAI_PARAM_ESCC); + hyundai_longitudinal_main_cruise_toggleable = GET_FLAG(param, HYUNDAI_PARAM_LONGITUDINAL_MAIN_CRUISE_TOGGLEABLE); hyundai_last_button_interaction = HYUNDAI_PREV_BUTTON_SAMPLES; + main_button_prev = false; + acc_main_on_prev = false; + acc_main_on_tx = false; + acc_main_on_mismatches = 0U; + #ifdef ALLOW_DEBUG const int HYUNDAI_PARAM_LONGITUDINAL = 4; hyundai_longitudinal = GET_FLAG(param, HYUNDAI_PARAM_LONGITUDINAL); @@ -104,7 +119,13 @@ void hyundai_common_cruise_buttons_check(const int cruise_button, const bool mai controls_allowed = false; } + // toggle main cruise state on rising edge of main cruise button + if (main_button && !main_button_prev && hyundai_longitudinal_main_cruise_toggleable) { + acc_main_on = !acc_main_on; + } + cruise_button_prev = cruise_button; + main_button_prev = main_button; } } @@ -132,3 +153,30 @@ uint32_t hyundai_common_canfd_compute_checksum(const CANPacket_t *to_push) { return crc; } + +// reset mismatches on rising edge of acc_main_on to avoid rare race conditions when using non-PCM main cruise state +void hyundai_common_reset_acc_main_on_mismatches(void) { + if (acc_main_on && !acc_main_on_prev) { + acc_main_on_mismatches = 0U; + } + + acc_main_on_prev = acc_main_on; +} + +// exit lateral controls allowed if sunnypilot and panda main cruise states are desynced +void hyundai_common_acc_main_on_sync(void) { + if (acc_main_on && !acc_main_on_tx) { + acc_main_on_mismatches += 1U; + + if (acc_main_on_mismatches >= 3U) { // desync by 3 frames + acc_main_on = false; + mads_exit_controls(MADS_DISENGAGE_REASON_NON_PCM_ACC_MAIN_DESYNC); + } + } else { + acc_main_on_mismatches = 0U; + } +} + +uint32_t get_acc_main_on_mismatches(void) { + return acc_main_on_mismatches; +} diff --git a/board/safety/safety_mazda.h b/board/safety/safety_mazda.h index ed87451f77..f49f9e6358 100644 --- a/board/safety/safety_mazda.h +++ b/board/safety/safety_mazda.h @@ -36,6 +36,7 @@ static void mazda_rx_hook(const CANPacket_t *to_push) { if (addr == MAZDA_CRZ_CTRL) { bool cruise_engaged = GET_BYTE(to_push, 0) & 0x8U; pcm_cruise_check(cruise_engaged); + acc_main_on = GET_BIT(to_push, 17U); } if (addr == MAZDA_ENGINE_DATA) { diff --git a/board/safety/safety_nissan.h b/board/safety/safety_nissan.h index fd47e09448..f00df72f07 100644 --- a/board/safety/safety_nissan.h +++ b/board/safety/safety_nissan.h @@ -3,6 +3,7 @@ #include "safety_declarations.h" static bool nissan_alt_eps = false; +static bool nissan_leaf = false; static void nissan_rx_hook(const CANPacket_t *to_push) { int bus = GET_BUS(to_push); @@ -53,6 +54,14 @@ static void nissan_rx_hook(const CANPacket_t *to_push) { pcm_cruise_check(cruise_engaged); } + if ((addr == 0x239) && (bus == 0) && nissan_leaf) { + acc_main_on = GET_BIT(to_push, 17U); + } + + if ((addr == 0x1B6) && (bus == (nissan_alt_eps ? 2 : 1))) { + acc_main_on = GET_BIT(to_push, 36U); + } + generic_rx_checks((addr == 0x169) && (bus == 0)); } @@ -150,8 +159,10 @@ static safety_config nissan_init(uint16_t param) { // EPS Location. false = V-CAN, true = C-CAN const int NISSAN_PARAM_ALT_EPS_BUS = 1; + const int NISSAN_PARAM_LEAF = 512; nissan_alt_eps = GET_FLAG(param, NISSAN_PARAM_ALT_EPS_BUS); + nissan_leaf = GET_FLAG(param, NISSAN_PARAM_LEAF); return BUILD_SAFETY_CFG(nissan_rx_checks, NISSAN_TX_MSGS); } diff --git a/board/safety/safety_subaru.h b/board/safety/safety_subaru.h index 290150657e..728366f5c6 100644 --- a/board/safety/safety_subaru.h +++ b/board/safety/safety_subaru.h @@ -106,10 +106,18 @@ static void subaru_rx_hook(const CANPacket_t *to_push) { update_sample(&angle_meas, angle_meas_new); } + if ((addr == MSG_SUBARU_ES_LKAS_State) && (bus == SUBARU_CAM_BUS)) { + int lkas_hud = (GET_BYTE(to_push, 2U) & 0x0CU) >> 2U; + if ((lkas_hud >= 1) && (lkas_hud <= 3)) { + mads_button_press = MADS_BUTTON_PRESSED; + } + } + // enter controls on rising edge of ACC, exit controls on ACC off if ((addr == MSG_SUBARU_CruiseControl) && (bus == alt_main_bus)) { bool cruise_engaged = GET_BIT(to_push, 41U); pcm_cruise_check(cruise_engaged); + acc_main_on = GET_BIT(to_push, 40U); } // update vehicle moving with any non-zero wheel speed diff --git a/board/safety/safety_subaru_preglobal.h b/board/safety/safety_subaru_preglobal.h index 760840f333..40586b4d9a 100644 --- a/board/safety/safety_subaru_preglobal.h +++ b/board/safety/safety_subaru_preglobal.h @@ -36,6 +36,7 @@ static void subaru_preglobal_rx_hook(const CANPacket_t *to_push) { if (addr == MSG_SUBARU_PG_CruiseControl) { bool cruise_engaged = GET_BIT(to_push, 49U); pcm_cruise_check(cruise_engaged); + acc_main_on = GET_BIT(to_push, 48U); } // update vehicle moving with any non-zero wheel speed diff --git a/board/safety/safety_toyota.h b/board/safety/safety_toyota.h index 7008bf8419..48b86adcab 100644 --- a/board/safety/safety_toyota.h +++ b/board/safety/safety_toyota.h @@ -64,6 +64,17 @@ static bool toyota_get_quality_flag_valid(const CANPacket_t *to_push) { static void toyota_rx_hook(const CANPacket_t *to_push) { const int TOYOTA_LTA_MAX_ANGLE = 1657; // EPS only accepts up to 94.9461 + if (GET_BUS(to_push) == 2U) { + int addr = GET_ADDR(to_push); + + if (addr == 0x412) { + int lkas_hud = (GET_BYTE(to_push, 0U) & 0xC0U) >> 6U; + if ((lkas_hud >= 1) && (lkas_hud <= 3)) { + mads_button_press = MADS_BUTTON_PRESSED; + } + } + } + if (GET_BUS(to_push) == 0U) { int addr = GET_ADDR(to_push); @@ -139,6 +150,10 @@ static void toyota_rx_hook(const CANPacket_t *to_push) { UPDATE_VEHICLE_SPEED(speed / 4.0 * 0.01 / 3.6); } + if (addr == 0x1D3) { + acc_main_on = GET_BIT(to_push, 15U); + } + bool stock_ecu_detected = addr == 0x2E4; // STEERING_LKA if (!toyota_stock_longitudinal && (addr == 0x343)) { stock_ecu_detected = true; // ACC_CONTROL diff --git a/board/sunnypilot/safety_mads.h b/board/sunnypilot/safety_mads.h new file mode 100644 index 0000000000..4ba6748eac --- /dev/null +++ b/board/sunnypilot/safety_mads.h @@ -0,0 +1,199 @@ +/** + * The MIT License + * + * Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + * + * Last updated: July 29, 2024 + */ + +#pragma once + +#include "sunnypilot/safety_mads_declarations.h" + +// =============================== +// Global Variables +// =============================== + +ButtonState mads_button_press = MADS_BUTTON_UNAVAILABLE; +MADSState m_mads_state; + +// state for mads controls_allowed_lat timeout logic +bool heartbeat_engaged_mads = false; // MADS enabled, passed in heartbeat USB command +uint32_t heartbeat_engaged_mads_mismatches = 0U; // count of mismatches between heartbeat_engaged_mads and controls_allowed_lat + +// =============================== +// State Update Helpers +// =============================== + +static EdgeTransition m_get_edge_transition(const bool current, const bool last) { + EdgeTransition state; + + if (current && !last) { + state = MADS_EDGE_RISING; + } else if (!current && last) { + state = MADS_EDGE_FALLING; + } else { + state = MADS_EDGE_NO_CHANGE; + } + + return state; +} + +static void m_mads_state_init(void) { + m_mads_state.is_vehicle_moving = NULL; + m_mads_state.acc_main.current = NULL; + m_mads_state.mads_button.current = MADS_BUTTON_UNAVAILABLE; + + m_mads_state.system_enabled = false; + m_mads_state.disengage_lateral_on_brake = false; + + m_mads_state.acc_main.previous = false; + m_mads_state.acc_main.transition = MADS_EDGE_NO_CHANGE; + + m_mads_state.mads_button.last = MADS_BUTTON_UNAVAILABLE; + m_mads_state.mads_button.transition = MADS_EDGE_NO_CHANGE; + + + m_mads_state.current_disengage.active_reason = MADS_DISENGAGE_REASON_NONE; + m_mads_state.current_disengage.pending_reasons = MADS_DISENGAGE_REASON_NONE; + + m_mads_state.controls_requested_lat = false; + m_mads_state.controls_allowed_lat = false; +} + +static void m_update_button_state(ButtonStateTracking *button_state) { + if (button_state->current != MADS_BUTTON_UNAVAILABLE) { + button_state->transition = m_get_edge_transition( + button_state->current == MADS_BUTTON_PRESSED, + button_state->last == MADS_BUTTON_PRESSED + ); + + button_state->last = button_state->current; + } +} + +static void m_update_binary_state(BinaryStateTracking *state) { + state->transition = m_get_edge_transition(state->current, state->previous); + state->previous = state->current; +} + +/** + * @brief Updates the MADS control state based on current system conditions + * + * @return void + */ +static void m_update_control_state(void) { + bool allowed = true; + + // Initial control requests from button or ACC transitions + if ((m_mads_state.acc_main.transition == MADS_EDGE_RISING) || (m_mads_state.mads_button.transition == MADS_EDGE_RISING)) { + m_mads_state.controls_requested_lat = true; + } + + // Primary control blockers - these prevent any further control processing + if (m_mads_state.acc_main.transition == MADS_EDGE_FALLING) { + mads_exit_controls(MADS_DISENGAGE_REASON_ACC_MAIN_OFF); + allowed = false; // No matter what, no further control processing on this cycle + } + + // Secondary control conditions - only checked if primary conditions don't block further control processing + if (allowed && m_mads_state.disengage_lateral_on_brake) { + // Brake rising edge immediately blocks controls + // Brake release might request controls if brake was the ONLY reason for disengagement + if (m_mads_state.braking.transition == MADS_EDGE_RISING) { + mads_exit_controls(MADS_DISENGAGE_REASON_BRAKE); + allowed = false; + } else if ((m_mads_state.braking.transition == MADS_EDGE_FALLING) && + (m_mads_state.current_disengage.active_reason == MADS_DISENGAGE_REASON_BRAKE) && + (m_mads_state.current_disengage.pending_reasons == MADS_DISENGAGE_REASON_BRAKE)) { + m_mads_state.controls_requested_lat = true; + } else if (m_mads_state.braking.current) { + allowed = false; + } else { + } + } + + // Process control request if conditions allow + if (allowed && m_mads_state.controls_requested_lat && !m_mads_state.controls_allowed_lat) { + m_mads_state.controls_requested_lat = false; + m_mads_state.controls_allowed_lat = true; + m_mads_state.current_disengage.active_reason = MADS_DISENGAGE_REASON_NONE; + m_mads_state.current_disengage.pending_reasons = MADS_DISENGAGE_REASON_NONE; + } +} + +inline void mads_heartbeat_engaged_check(void) { + if (m_mads_state.controls_allowed_lat && !heartbeat_engaged_mads) { + heartbeat_engaged_mads_mismatches += 1U; + if (heartbeat_engaged_mads_mismatches >= 3U) { + mads_exit_controls(MADS_DISENGAGE_REASON_HEARTBEAT_ENGAGED_MISMATCH); + } + } else { + heartbeat_engaged_mads_mismatches = 0U; + } +} + +// =============================== +// Function Implementations +// =============================== + +inline void mads_set_alternative_experience(const int *mode) { + const bool mads_enabled = (*mode & ALT_EXP_ENABLE_MADS) != 0; + const bool disengage_lateral_on_brake = (*mode & ALT_EXP_DISENGAGE_LATERAL_ON_BRAKE) != 0; + + mads_set_system_state(mads_enabled, disengage_lateral_on_brake); +} + +inline void mads_set_system_state(const bool enabled, const bool disengage_lateral_on_brake) { + m_mads_state_init(); + m_mads_state.system_enabled = enabled; + m_mads_state.disengage_lateral_on_brake = disengage_lateral_on_brake; +} + +inline void mads_exit_controls(const DisengageReason reason) { + // Always track this as a pending reason + m_mads_state.current_disengage.pending_reasons |= reason; + + if (m_mads_state.controls_allowed_lat) { + m_mads_state.current_disengage.active_reason = reason; + m_mads_state.controls_requested_lat = false; + m_mads_state.controls_allowed_lat = false; + } +} + +inline bool mads_is_lateral_control_allowed_by_mads(void) { + return m_mads_state.system_enabled && m_mads_state.controls_allowed_lat; +} + +inline void mads_state_update(const bool op_vehicle_moving, const bool op_acc_main, const bool op_allowed, const bool is_braking) { + m_mads_state.is_vehicle_moving = op_vehicle_moving; + m_mads_state.acc_main.current = op_acc_main; + m_mads_state.op_controls_allowed.current = op_allowed; + m_mads_state.mads_button.current = mads_button_press; + m_mads_state.braking.current = is_braking; + + m_update_binary_state(&m_mads_state.acc_main); + m_update_binary_state(&m_mads_state.op_controls_allowed); + m_update_binary_state(&m_mads_state.braking); + m_update_button_state(&m_mads_state.mads_button); + + m_update_control_state(); +} diff --git a/board/sunnypilot/safety_mads_declarations.h b/board/sunnypilot/safety_mads_declarations.h new file mode 100644 index 0000000000..a44f18498c --- /dev/null +++ b/board/sunnypilot/safety_mads_declarations.h @@ -0,0 +1,121 @@ +/** + * The MIT License + * + * Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + * + * Last updated: July 29, 2024 + */ + +#pragma once + +// =============================== +// Type Definitions and Enums +// =============================== + +typedef enum __attribute__((packed)) { + MADS_BUTTON_UNAVAILABLE = -1, ///< Button state cannot be determined + MADS_BUTTON_NOT_PRESSED = 0, ///< Button is not pressed + MADS_BUTTON_PRESSED = 1 ///< Button is pressed +} ButtonState; + +typedef enum __attribute__((packed)) { + MADS_EDGE_NO_CHANGE = 0, ///< No state change detected + MADS_EDGE_RISING = 1, ///< State changed from false to true + MADS_EDGE_FALLING = 2 ///< State changed from true to false +} EdgeTransition; + +typedef enum __attribute__((packed)) { + MADS_DISENGAGE_REASON_NONE = 0, ///< No disengagement + MADS_DISENGAGE_REASON_BRAKE = 1, ///< Brake pedal pressed + MADS_DISENGAGE_REASON_LAG = 2, ///< System lag detected + MADS_DISENGAGE_REASON_BUTTON = 4, ///< User button press + MADS_DISENGAGE_REASON_ACC_MAIN_OFF = 8, ///< ACC system turned off + MADS_DISENGAGE_REASON_NON_PCM_ACC_MAIN_DESYNC = 16, ///< ACC sync error + MADS_DISENGAGE_REASON_HEARTBEAT_ENGAGED_MISMATCH = 32, ///< Heartbeat mismatch +} DisengageReason; + +// =============================== +// Constants and Defines +// =============================== + +#define ALT_EXP_ENABLE_MADS 1024 +#define ALT_EXP_DISENGAGE_LATERAL_ON_BRAKE 2048 + +#define MISMATCH_DEFAULT_THRESHOLD 25 + +// =============================== +// Data Structures +// =============================== + +typedef struct { + DisengageReason active_reason; // The reason that actually disengaged controls + DisengageReason pending_reasons; // All conditions that would've prevented engagement while controls were disengaged +} DisengageState; + +typedef struct { + ButtonState current; + ButtonState last; + EdgeTransition transition; +} ButtonStateTracking; + +typedef struct { + EdgeTransition transition; + bool current : 1; + bool previous : 1; +} BinaryStateTracking; + +typedef struct { + bool is_vehicle_moving : 1; + + ButtonStateTracking mads_button; + BinaryStateTracking acc_main; + BinaryStateTracking op_controls_allowed; + BinaryStateTracking braking; + + DisengageState current_disengage; + + bool system_enabled : 1; + bool disengage_lateral_on_brake : 1; + bool controls_requested_lat : 1; + bool controls_allowed_lat : 1; +} MADSState; + +// =============================== +// Global Variables +// =============================== + +extern ButtonState mads_button_press; +extern MADSState m_mads_state; + +// state for mads controls_allowed_lat timeout logic +extern bool heartbeat_engaged_mads; +extern uint32_t heartbeat_engaged_mads_mismatches; + +// =============================== +// External Function Declarations (kept as needed) +// =============================== + +extern void mads_set_system_state(bool enabled, bool disengage_lateral_on_brake); +extern void mads_set_alternative_experience(const int *mode); +extern void mads_state_update(bool op_vehicle_moving, bool op_acc_main, bool op_allowed, bool is_braking); +extern void mads_exit_controls(DisengageReason reason); +extern bool mads_is_lateral_control_allowed_by_mads(void); +extern void mads_heartbeat_engaged_check(void); diff --git a/python/__init__.py b/python/__init__.py index 3748d1fbbd..2e714a0f97 100644 --- a/python/__init__.py +++ b/python/__init__.py @@ -108,6 +108,10 @@ class ALTERNATIVE_EXPERIENCE: RAISE_LONGITUDINAL_LIMITS_TO_ISO_MAX = 8 ALLOW_AEB = 16 + # sunnypilot + ENABLE_MADS = 2 ** 10 + DISENGAGE_LATERAL_ON_BRAKE = 2 ** 11 + class Panda: # matches cereal.car.CarParams.SafetyModel @@ -163,7 +167,7 @@ class Panda: CAN_PACKET_VERSION = 4 HEALTH_PACKET_VERSION = 16 CAN_HEALTH_PACKET_VERSION = 5 - HEALTH_STRUCT = struct.Struct("controls_requested_lat; +} + +bool get_enable_mads(void){ + return get_mads_state()->system_enabled; +} + +bool get_disengage_lateral_on_brake(void){ + return get_mads_state()->disengage_lateral_on_brake; +} + int get_alternative_experience(void){ return alternative_experience; } @@ -75,6 +100,10 @@ bool get_acc_main_on(void){ return acc_main_on; } +void set_acc_main_on(bool c){ + acc_main_on = c; +} + int get_vehicle_speed_min(void){ return vehicle_speed.min; } @@ -181,6 +210,54 @@ bool get_honda_fwd_brake(void){ return honda_fwd_brake; } +void set_mads_button_press(int c){ + mads_button_press = c; +} + +int get_mads_button_press(void){ + return mads_button_press; +} + +void set_controls_allowed_lat(bool c){ + m_mads_state.controls_allowed_lat = c; +} + +bool get_mads_acc_main(void){ + return m_mads_state.acc_main.current; +} + +int mads_get_current_disengage_reason(void) { + return get_mads_state()->current_disengage.active_reason; +} + +void mads_set_current_disengage_reason(int reason) { + m_mads_state.current_disengage.active_reason = reason; +} + +void set_controls_requested_lat(bool c){ + m_mads_state.controls_requested_lat = c; +} + +void set_mads_params(bool enable_mads, bool disengage_lateral_on_brake){ + alternative_experience = 0; + if (enable_mads) { + alternative_experience |= ALT_EXP_ENABLE_MADS; + + if (disengage_lateral_on_brake) + alternative_experience |= ALT_EXP_DISENGAGE_LATERAL_ON_BRAKE; + } + + mads_set_alternative_experience(&alternative_experience); +} + +void set_heartbeat_engaged_mads(bool c){ + heartbeat_engaged_mads = c; +} + +//int get_temp_debug(void){ +// return temp_debug; +//} + void init_tests(void){ // get HW_TYPE from env variable set in test.sh if (getenv("HW_TYPE")) { diff --git a/tests/libpanda/safety_helpers.py b/tests/libpanda/safety_helpers.py index 6b510da6a8..2d72b4a86a 100644 --- a/tests/libpanda/safety_helpers.py +++ b/tests/libpanda/safety_helpers.py @@ -5,6 +5,9 @@ def setup_safety_helpers(ffi): ffi.cdef(""" void set_controls_allowed(bool c); bool get_controls_allowed(void); + bool get_lat_active(void); + bool get_controls_allowed_lat(void); + bool get_controls_requested_lat(void); bool get_longitudinal_allowed(void); void set_alternative_experience(int mode); int get_alternative_experience(void); @@ -51,11 +54,33 @@ def setup_safety_helpers(ffi): void set_honda_alt_brake_msg(bool c); void set_honda_bosch_long(bool c); int get_honda_hw(void); + + bool get_enable_mads(void); + bool get_disengage_lateral_on_brake(void); + void set_mads_button_press(int mads_button_press); + void set_controls_allowed_lat(bool c); + void set_controls_requested_lat(bool c); + + bool get_mads_acc_main(void); + void set_acc_main_on(bool c); + int get_mads_button_press(void); + void mads_set_current_disengage_reason(int reason); + int mads_get_current_disengage_reason(void); + int get_temp_debug(void); + uint32_t get_acc_main_on_mismatches(void); + void set_mads_params(bool enable_mads, bool disengage_lat_on_brake); + void set_heartbeat_engaged_mads(bool c); """) class PandaSafety(Protocol): def set_controls_allowed(self, c: bool) -> None: ... def get_controls_allowed(self) -> bool: ... + def set_controls_allowed_lat(self, c: bool) -> None: ... + def set_controls_requested_lat(self, c: bool) -> None: ... + def get_lat_active(self) -> bool: ... + def get_controls_allowed_lat(self) -> bool: ... + def get_controls_requested_lat(self) -> bool: ... + def get_mads_acc_main(self) -> bool: ... def get_longitudinal_allowed(self) -> bool: ... def set_alternative_experience(self, mode: int) -> None: ... def get_alternative_experience(self) -> int: ... @@ -66,6 +91,7 @@ def set_gas_pressed_prev(self, c: bool) -> None: ... def get_brake_pressed_prev(self) -> bool: ... def get_regen_braking_prev(self) -> bool: ... def get_acc_main_on(self) -> bool: ... + def set_acc_main_on(self, c: bool) -> None: ... def get_vehicle_speed_min(self) -> int: ... def get_vehicle_speed_max(self) -> int: ... def get_vehicle_speed_last(self) -> int: ... @@ -103,4 +129,15 @@ def set_honda_alt_brake_msg(self, c: bool) -> None: ... def set_honda_bosch_long(self, c: bool) -> None: ... def get_honda_hw(self) -> int: ... + def set_mads_button_press(self, mads_button_press: int) -> None: ... + def get_enable_mads(self) -> bool: ... + def get_disengage_lateral_on_brake(self) -> bool: ... + + def get_mads_button_press(self) -> int: ... + def mads_set_current_disengage_reason(self, reason: int) -> None: ... + def mads_get_current_disengage_reason(self) -> int: ... + def get_acc_main_on_mismatches(self) -> int: ... + def set_mads_params(self, enable_mads: bool, disengage_lat_on_brake: bool) -> None: ... + def set_heartbeat_engaged_mads(self, c: bool) -> None: ... + # def get_temp_debug(self) -> int: ... diff --git a/tests/misra/suppressions.txt b/tests/misra/suppressions.txt index 4800a270bc..0fb0377dca 100644 --- a/tests/misra/suppressions.txt +++ b/tests/misra/suppressions.txt @@ -19,3 +19,4 @@ unusedFunction:*/interrupt_handlers*.h # cppcheck from 2.5 -> 2.13. they are listed here to separate the update from # fixing the violations and all are intended to be removed soon after misra-c2012-2.5 # unused macros. a few legit, rest aren't common between F4/H7 builds. should we do this in the unusedFunction pass? +misra-c2012-12.2:*/gpio.h # Suppress 12.2 for any file named gpio.h diff --git a/tests/safety/common.py b/tests/safety/common.py index fdc54f6d03..4d94aa5635 100644 --- a/tests/safety/common.py +++ b/tests/safety/common.py @@ -8,6 +8,7 @@ from opendbc.can.packer import CANPacker # pylint: disable=import-error from panda import ALTERNATIVE_EXPERIENCE from panda.tests.libpanda import libpanda_py +from panda.tests.safety.mads_common import MadsCommonBase MAX_WRONG_COUNTERS = 5 MAX_SAMPLE_VALS = 6 @@ -813,7 +814,7 @@ def test_tx_hook_on_wrong_safety_mode(self): @add_regen_tests -class PandaCarSafetyTest(PandaSafetyTest): +class PandaCarSafetyTest(PandaSafetyTest, MadsCommonBase): STANDSTILL_THRESHOLD: float | None = None GAS_PRESSED_THRESHOLD = 0 RELAY_MALFUNCTION_ADDRS: dict[int, tuple[int, ...]] | None = None @@ -985,6 +986,8 @@ def test_vehicle_moving(self): def test_safety_tick(self): self.safety.set_timer(int(2e6)) self.safety.set_controls_allowed(True) + self.safety.set_controls_allowed_lat(True) self.safety.safety_tick_current_safety_config() self.assertFalse(self.safety.get_controls_allowed()) + self.assertFalse(self.safety.get_controls_allowed_lat()) self.assertFalse(self.safety.safety_config_valid()) diff --git a/tests/safety/hyundai_common.py b/tests/safety/hyundai_common.py index da18671af5..929ad1a65f 100644 --- a/tests/safety/hyundai_common.py +++ b/tests/safety/hyundai_common.py @@ -3,6 +3,7 @@ import panda.tests.safety.common as common from panda.tests.libpanda import libpanda_py from panda.tests.safety.common import make_msg +from panda import Panda class Buttons: @@ -107,6 +108,12 @@ def _pcm_status_msg(self, enable): def _accel_msg(self, accel, aeb_req=False, aeb_decel=0): raise NotImplementedError + def _acc_state_msg(self, enable): + raise NotImplementedError + + def test_pcm_main_cruise_state_availability(self): + pass + def test_set_resume_buttons(self): """ SET and RESUME enter controls allowed on their falling edge. @@ -132,6 +139,139 @@ def test_cancel_button(self): self._rx(self._button_msg(Buttons.CANCEL)) self.assertFalse(self.safety.get_controls_allowed()) + def test_main_cruise_button(self): + """Test that main cruise button correctly toggles acc_main_on state""" + default_safety_mode = self.safety.get_current_safety_mode() + default_safety_param = self.safety.get_current_safety_param() + + for enable_mads in (True, False): + with self.subTest("enable_mads", mads_enabled=enable_mads): + for main_cruise_toggleable in (True, False): + with self.subTest("main_cruise_toggleable", main_cruise_toggleable=main_cruise_toggleable): + main_cruise_toggleable_flag = Panda.FLAG_HYUNDAI_LONG_MAIN_CRUISE_TOGGLEABLE if main_cruise_toggleable else 0 + self.safety.set_safety_hooks(default_safety_mode, default_safety_param | main_cruise_toggleable_flag) + + # Test initial state + self._mads_states_cleanup() + self.safety.set_mads_params(enable_mads, False) + + self.assertFalse(self.safety.get_acc_main_on()) + + self._rx(self._main_cruise_button_msg(0)) + self._rx(self._main_cruise_button_msg(1)) + self.assertEqual(enable_mads and main_cruise_toggleable, self.safety.get_controls_allowed_lat()) + + self._rx(self._main_cruise_button_msg(0)) + self.assertEqual(enable_mads and main_cruise_toggleable, self.safety.get_controls_allowed_lat()) + + self._rx(self._main_cruise_button_msg(1)) + self.assertFalse(self.safety.get_controls_allowed_lat()) + + for _ in range(10): + self._rx(self._main_cruise_button_msg(1)) + self.assertFalse(self.safety.get_controls_allowed_lat()) + self._mads_states_cleanup() + self.safety.set_safety_hooks(default_safety_mode, default_safety_param) + + def test_acc_main_sync_mismatches_reset(self): + """Test that acc_main_on_mismatches resets properly on rising edge of main button""" + default_safety_mode = self.safety.get_current_safety_mode() + default_safety_param = self.safety.get_current_safety_param() + + for enable_mads in (True, False): + with self.subTest("enable_mads", mads_enabled=enable_mads): + main_cruise_toggleable_flag = Panda.FLAG_HYUNDAI_LONG_MAIN_CRUISE_TOGGLEABLE + self.safety.set_safety_hooks(default_safety_mode, default_safety_param | main_cruise_toggleable_flag) + + self._mads_states_cleanup() + self.safety.set_mads_params(enable_mads, False) + + # Initial state + self._rx(self._main_cruise_button_msg(0)) + self.assertFalse(self.safety.get_acc_main_on()) + + # Set up mismatch condition + self._rx(self._main_cruise_button_msg(1)) # Press button - acc_main_on = True + self._rx(self._main_cruise_button_msg(0)) # Release button + self._tx(self._tx_acc_state_msg(False)) # acc_main_on_tx = False + self.assertTrue(self.safety.get_acc_main_on()) + self.assertEqual(1, self.safety.get_acc_main_on_mismatches()) + + # Rising edge of acc_main_on should reset + self._rx(self._main_cruise_button_msg(1)) # Press button again + self._rx(self._main_cruise_button_msg(0)) # Release button + self._tx(self._tx_acc_state_msg(False)) # acc_main_on_tx = False + self.assertFalse(self.safety.get_acc_main_on()) + self.assertEqual(0, self.safety.get_acc_main_on_mismatches()) + self._mads_states_cleanup() + self.safety.set_safety_hooks(default_safety_mode, default_safety_param) + + def test_acc_main_sync_mismatch_counter(self): + """Test mismatch counter behavior and disengagement""" + default_safety_mode = self.safety.get_current_safety_mode() + default_safety_param = self.safety.get_current_safety_param() + + for enable_mads in (True, False): + with self.subTest("enable_mads", mads_enabled=enable_mads): + main_cruise_toggleable_flag = Panda.FLAG_HYUNDAI_LONG_MAIN_CRUISE_TOGGLEABLE + self.safety.set_safety_hooks(default_safety_mode, default_safety_param | main_cruise_toggleable_flag) + + self._mads_states_cleanup() + self.safety.set_mads_params(enable_mads, False) + self.safety.set_controls_allowed_lat(True) + + # Start with matched states + self._rx(self._main_cruise_button_msg(0)) + self._tx(self._tx_acc_state_msg(False)) + self.assertEqual(0, self.safety.get_acc_main_on_mismatches()) + + # Create mismatch by enabling acc_main_on + self._rx(self._main_cruise_button_msg(1)) # Press button + self._rx(self._main_cruise_button_msg(0)) # Release button + self._tx(self._tx_acc_state_msg(False)) # acc_main_on_tx stays false + self.assertTrue(self.safety.get_acc_main_on()) + self.assertEqual(1, self.safety.get_acc_main_on_mismatches()) + + # Second mismatch + self._tx(self._tx_acc_state_msg(False)) + self.assertTrue(self.safety.get_acc_main_on()) + self.assertEqual(2, self.safety.get_acc_main_on_mismatches()) + + # Third mismatch should trigger disengagement + self._tx(self._tx_acc_state_msg(False)) + self.assertFalse(self.safety.get_acc_main_on()) + self.assertFalse(self.safety.get_controls_allowed_lat()) + # Counter should reset after disengagement + self._tx(self._tx_acc_state_msg(False)) + self.assertEqual(0, self.safety.get_acc_main_on_mismatches()) + self._mads_states_cleanup() + self.safety.set_safety_hooks(default_safety_mode, default_safety_param) + + def test_acc_main_sync_mismatch_recovery(self): + default_safety_mode = self.safety.get_current_safety_mode() + default_safety_param = self.safety.get_current_safety_param() + + """Test that mismatch counter resets when states resync""" + for enable_mads in (True, False): + with self.subTest("enable_mads", mads_enabled=enable_mads): + main_cruise_toggleable_flag = Panda.FLAG_HYUNDAI_LONG_MAIN_CRUISE_TOGGLEABLE + self.safety.set_safety_hooks(default_safety_mode, default_safety_param | main_cruise_toggleable_flag) + + self._mads_states_cleanup() + self.safety.set_mads_params(enable_mads, False) + + # Create initial mismatch + self._rx(self._main_cruise_button_msg(1)) # Press button + self._rx(self._main_cruise_button_msg(0)) # Release button + self._tx(self._tx_acc_state_msg(False)) # acc_main_on_tx = False + self.assertEqual(1, self.safety.get_acc_main_on_mismatches()) + + # Sync states + self._tx(self._tx_acc_state_msg(True)) # Match acc_main_on_tx to acc_main_on + self.assertEqual(0, self.safety.get_acc_main_on_mismatches()) + self._mads_states_cleanup() + self.safety.set_safety_hooks(default_safety_mode, default_safety_param) + def test_tester_present_allowed(self): """ Ensure tester present diagnostic message is allowed to keep ECU knocked out diff --git a/tests/safety/mads_common.py b/tests/safety/mads_common.py new file mode 100644 index 0000000000..a262383fb1 --- /dev/null +++ b/tests/safety/mads_common.py @@ -0,0 +1,391 @@ +import unittest +from abc import abstractmethod + + +class MadsCommonBase(unittest.TestCase): + @abstractmethod + def _lkas_button_msg(self, enabled): + raise NotImplementedError + + @abstractmethod + def _acc_state_msg(self, enabled): + raise NotImplementedError + + def _mads_states_cleanup(self): + self.safety.set_mads_button_press(-1) + self.safety.set_controls_allowed_lat(False) + self.safety.set_controls_requested_lat(False) + self.safety.set_acc_main_on(False) + self.safety.set_mads_params(False, False) + self.safety.set_heartbeat_engaged_mads(True) + + def test_enable_control_allowed_with_mads_button(self): + """Toggle MADS with MADS button""" + try: + self._lkas_button_msg(False) + except NotImplementedError: + raise unittest.SkipTest("Skipping test because MADS button is not supported") + + try: + for enable_mads in (True, False): + with self.subTest("enable_mads", mads_enabled=enable_mads): + self._mads_states_cleanup() + self.safety.set_mads_params(enable_mads, False) + self.assertEqual(enable_mads, self.safety.get_enable_mads()) + + self._rx(self._lkas_button_msg(True)) + self._rx(self._speed_msg(0)) + self._rx(self._lkas_button_msg(False)) + self._rx(self._speed_msg(0)) + self.assertEqual(enable_mads, self.safety.get_controls_allowed_lat()) + finally: + self._mads_states_cleanup() + + def test_enable_control_allowed_with_manual_acc_main_on_state(self): + try: + self._acc_state_msg(False) + except NotImplementedError: + self._mads_states_cleanup() + raise unittest.SkipTest("Skipping test because _acc_state_msg is not implemented for this car") + + try: + for enable_mads in (True, False): + with self.subTest("enable_mads", mads_enabled=enable_mads): + self._mads_states_cleanup() + self.safety.set_mads_params(enable_mads, False) + self._rx(self._acc_state_msg(True)) + self._rx(self._speed_msg(0)) + self.assertEqual(enable_mads, self.safety.get_controls_allowed_lat()) + finally: + self._mads_states_cleanup() + + def test_enable_control_allowed_with_manual_mads_button_state(self): + try: + for enable_mads in (True, False): + with self.subTest("enable_mads", mads_enabled=enable_mads): + for mads_button_press in (-1, 0, 1): + with self.subTest("mads_button_press", button_state=mads_button_press): + self._mads_states_cleanup() + self.safety.set_mads_params(enable_mads, False) + + self.safety.set_mads_button_press(mads_button_press) + self._rx(self._speed_msg(0)) + self.assertEqual(enable_mads and mads_button_press == 1, self.safety.get_controls_allowed_lat()) + finally: + self._mads_states_cleanup() + + def test_enable_control_allowed_from_acc_main_on(self): + """Test that lateral controls are allowed when ACC main is enabled and disabled when ACC main is disabled""" + try: + for enable_mads in (True, False): + with self.subTest("enable_mads", mads_enabled=enable_mads): + for acc_main_on in (True, False): + with self.subTest("initial_acc_main", initial_acc_main=acc_main_on): + self._mads_states_cleanup() + self.safety.set_mads_params(enable_mads, False) + + # Set initial state + self.safety.set_acc_main_on(acc_main_on) + self._rx(self._speed_msg(0)) + expected_lat = enable_mads and acc_main_on + self.assertEqual(expected_lat, self.safety.get_controls_allowed_lat(), + f"Expected lat: [{expected_lat}] when acc_main_on goes to [{acc_main_on}]") + + # Test transition to opposite state + self.safety.set_acc_main_on(not acc_main_on) + self._rx(self._speed_msg(0)) + expected_lat = enable_mads and not acc_main_on + self.assertEqual(expected_lat, self.safety.get_controls_allowed_lat(), + f"Expected lat: [{expected_lat}] when acc_main_on goes from [{acc_main_on}] to [{not acc_main_on}]") + + # Test transition back to initial state + self.safety.set_acc_main_on(acc_main_on) + self._rx(self._speed_msg(0)) + expected_lat = enable_mads and acc_main_on + self.assertEqual(expected_lat, self.safety.get_controls_allowed_lat(), + f"Expected lat: [{expected_lat}] when acc_main_on goes from [{not acc_main_on}] to [{acc_main_on}]") + finally: + self._mads_states_cleanup() + + def test_mads_with_acc_main_on(self): + try: + for enable_mads in (True, False): + with self.subTest("enable_mads", mads_enabled=enable_mads): + self._mads_states_cleanup() + self.safety.set_mads_params(enable_mads, False) + + self.safety.set_acc_main_on(True) + self._rx(self._speed_msg(0)) + self.assertEqual(enable_mads, self.safety.get_controls_allowed_lat()) + + self.safety.set_acc_main_on(False) + self._rx(self._speed_msg(0)) + self.assertFalse(self.safety.get_controls_allowed_lat()) + finally: + self._mads_states_cleanup() + + def test_disengage_lateral_on_brake_setup(self): + try: + for enable_mads in (True, False): + with self.subTest("enable_mads", enable_mads=enable_mads): + for disengage_on_brake in (True, False): + with self.subTest("disengage on brake", disengage_on_brake=disengage_on_brake): + self._mads_states_cleanup() + self.safety.set_mads_params(enable_mads, disengage_on_brake) + self.assertEqual(enable_mads and disengage_on_brake, self.safety.get_disengage_lateral_on_brake()) + finally: + self._mads_states_cleanup() + + def test_disengage_lateral_on_brake(self): + try: + self._mads_states_cleanup() + self.safety.set_mads_params(True, True) + + self._rx(self._user_brake_msg(False)) + self.safety.set_controls_requested_lat(True) + self.safety.set_controls_allowed_lat(True) + + self._rx(self._user_brake_msg(True)) + # Test we pause lateral + self.assertFalse(self.safety.get_controls_allowed_lat()) + # Make sure we can re-gain lateral actuation + self._rx(self._user_brake_msg(False)) + self.assertTrue(self.safety.get_controls_allowed_lat()) + finally: + self._mads_states_cleanup() + + def test_no_disengage_lateral_on_brake(self): + try: + self._mads_states_cleanup() + self.safety.set_mads_params(True, False) + + self._rx(self._user_brake_msg(False)) + self.safety.set_controls_requested_lat(True) + self.safety.set_controls_allowed_lat(True) + + self._rx(self._user_brake_msg(True)) + self.assertTrue(self.safety.get_controls_allowed_lat()) + finally: + self._mads_states_cleanup() + + def test_engage_with_brake_pressed(self): + try: + self._lkas_button_msg(False) + except NotImplementedError: + raise unittest.SkipTest("Skipping test because MADS button is not supported") + + try: + for enable_mads in (True, False): + with self.subTest("enable_mads", enable_mads=enable_mads): + for disengage_lateral_on_brake in (True, False): + with self.subTest("disengage_lateral_on_brake", disengage_lateral_on_brake=disengage_lateral_on_brake): + with self.subTest("mads_button"): + self._mads_states_cleanup() + self.safety.set_mads_params(enable_mads, disengage_lateral_on_brake) + + # Brake press rising edge + self._rx(self._user_brake_msg(True)) + self._rx(self._lkas_button_msg(True)) + self._rx(self._speed_msg(0)) + self.assertEqual(enable_mads and not disengage_lateral_on_brake, self.safety.get_controls_allowed_lat()) + + # Continuous braking after the first frame of brake press rising edge + self.assertEqual(enable_mads and not disengage_lateral_on_brake, self.safety.get_controls_allowed_lat()) + for _ in range(400): + self._rx(self._user_brake_msg(True)) + self.assertEqual(enable_mads and not disengage_lateral_on_brake, self.safety.get_controls_allowed_lat()) + + with self.subTest("acc_main_on"): + self._mads_states_cleanup() + self.safety.set_mads_params(enable_mads, disengage_lateral_on_brake) + + # Brake press rising edge + self._rx(self._user_brake_msg(True)) + self.safety.set_acc_main_on(True) + self._rx(self._speed_msg(0)) + self.assertEqual(enable_mads and not disengage_lateral_on_brake, self.safety.get_controls_allowed_lat()) + + # Continuous braking after the first frame of brake press rising edge + self.assertEqual(enable_mads and not disengage_lateral_on_brake, self.safety.get_controls_allowed_lat()) + for _ in range(400): + self._rx(self._user_brake_msg(True)) + self.assertEqual(enable_mads and not disengage_lateral_on_brake, self.safety.get_controls_allowed_lat()) + finally: + self._mads_states_cleanup() + + def test_disengage_lateral_on_brake_with_pressed_and_released(self): + try: + for enable_mads in (True, False): + with self.subTest("enable_mads", enable_mads=enable_mads): + for disengage_lateral_on_brake in (True, False): + with self.subTest("disengage_lateral_on_brake", disengage_lateral_on_brake=disengage_lateral_on_brake): + self._mads_states_cleanup() + self.safety.set_mads_params(enable_mads, disengage_lateral_on_brake) + + # Set controls_allowed_lat rising edge + self.safety.set_controls_requested_lat(True) + self._rx(self._speed_msg(0)) + self.assertEqual(enable_mads, self.safety.get_controls_allowed_lat()) + + # User brake press, validate controls_allowed_lat is false + self._rx(self._user_brake_msg(True)) + self._rx(self._speed_msg(0)) + self.assertEqual(enable_mads and not disengage_lateral_on_brake, self.safety.get_controls_allowed_lat()) + + # User brake release, validate controls_allowed_lat is true + self._rx(self._user_brake_msg(False)) + self._rx(self._speed_msg(0)) + self.assertEqual(enable_mads, self.safety.get_controls_allowed_lat()) + finally: + self._mads_states_cleanup() + + def test_disengage_lateral_on_brake_persistent_control_allowed_off(self): + try: + self._mads_states_cleanup() + self.safety.set_mads_params(True, True) + + self.safety.set_controls_requested_lat(True) + + # Vehicle moving, validate controls_allowed_lat is true + for _ in range(10): + self._rx(self._speed_msg(10)) + self.assertTrue(self.safety.get_controls_allowed_lat()) + + # User braked, vehicle slowed down in 10 frames, then stopped for 10 frames + # Validate controls_allowed_lat is false + self._rx(self._user_brake_msg(True)) + for _ in range(10): + self._rx(self._speed_msg(5)) + self.assertFalse(self.safety.get_controls_allowed_lat()) + for _ in range(10): + self._rx(self._speed_msg(0)) + self.assertFalse(self.safety.get_controls_allowed_lat()) + finally: + self._mads_states_cleanup() + + def test_enable_lateral_control_with_controls_allowed_rising_edge(self): + try: + for enable_mads in (True, False): + with self.subTest("enable_mads", enable_mads=enable_mads): + self._mads_states_cleanup() + self.safety.set_mads_params(enable_mads, False) + + self.safety.set_controls_allowed(False) + self._rx(self._speed_msg(0)) + self.safety.set_controls_allowed(True) + self._rx(self._speed_msg(0)) + self.assertTrue(self.safety.get_controls_allowed()) + finally: + self._mads_states_cleanup() + + def test_enable_control_allowed_with_mads_button_and_disable_with_main_cruise(self): + """Tests main cruise and MADS button state transitions. + + Sequence: + 1. Main cruise off -> on + 2. MADS button engage + 3. Main cruise off + + """ + try: + self._lkas_button_msg(False) + except NotImplementedError: + raise unittest.SkipTest("Skipping test because MADS button is not supported") + + try: + self._acc_state_msg(False) + except NotImplementedError: + raise unittest.SkipTest("Skipping test because _acc_state_msg is not implemented for this car") + + try: + for enable_mads in (True, False): + with self.subTest("enable_mads", enable_mads=enable_mads): + self._mads_states_cleanup() + self.safety.set_mads_params(enable_mads, False) + + self._rx(self._lkas_button_msg(True)) + self._rx(self._speed_msg(0)) + self._rx(self._lkas_button_msg(False)) + self._rx(self._speed_msg(0)) + self.assertEqual(enable_mads, self.safety.get_controls_allowed_lat()) + + self._rx(self._acc_state_msg(True)) + self._rx(self._speed_msg(0)) + self.assertEqual(enable_mads, self.safety.get_controls_allowed_lat()) + + self._rx(self._acc_state_msg(False)) + self._rx(self._speed_msg(0)) + self.assertFalse(self.safety.get_controls_allowed_lat()) + finally: + self._mads_states_cleanup() + + def test_brake_disengage_with_control_request(self): + """Tests behavior when controls are requested while brake is engaged + + Sequence: + 1. Enable MADS with disengage on brake + 2. Brake to disengage lateral control + 3. Set control request while braking + 4. Release brake + 5. Verify controls become allowed + """ + try: + self._mads_states_cleanup() + self.safety.set_mads_params(True, True) # enable MADS with disengage on brake + + # Initial state + self.safety.set_controls_allowed_lat(True) + self._rx(self._speed_msg(0)) + self.assertTrue(self.safety.get_controls_allowed_lat()) + + # Brake press disengages lateral + self._rx(self._user_brake_msg(True)) + self.assertFalse(self.safety.get_controls_allowed_lat()) + + # Request controls while braking + self.safety.set_controls_requested_lat(True) + self.assertFalse(self.safety.get_controls_allowed_lat()) + + # Release brake - should enable since controls were requested + self._rx(self._user_brake_msg(False)) + self._rx(self._speed_msg(0)) + self.assertTrue(self.safety.get_controls_allowed_lat()) + + finally: + self._mads_states_cleanup() + + def test_brake_disengage_with_acc_main_off(self): + """Tests behavior when ACC main is turned off while brake is engaged + + Sequence: + 1. Enable MADS with disengage on brake + 2. Brake to disengage lateral control + 3. Turn ACC main off while braking + 4. Release brake + 5. Verify controls remain disengaged + """ + try: + self._mads_states_cleanup() + self.safety.set_mads_params(True, True) # enable MADS with disengage on brake + + # Initial state - enable with ACC main + self.safety.set_acc_main_on(True) + self._rx(self._speed_msg(0)) + self.assertTrue(self.safety.get_controls_allowed_lat()) + + # Brake press disengages lateral + self._rx(self._user_brake_msg(True)) + self.assertFalse(self.safety.get_controls_allowed_lat()) + + # Turn ACC main off while braking + self.safety.set_acc_main_on(False) + self._rx(self._speed_msg(0)) + self.assertFalse(self.safety.get_controls_allowed_lat()) + + # Release brake - should remain disabled since ACC main is off + self._rx(self._user_brake_msg(False)) + self._rx(self._speed_msg(0)) + self.assertFalse(self.safety.get_controls_allowed_lat()) + + finally: + self._mads_states_cleanup() diff --git a/tests/safety/test_chrysler.py b/tests/safety/test_chrysler.py index 5bbb6dd103..e6941b9781 100755 --- a/tests/safety/test_chrysler.py +++ b/tests/safety/test_chrysler.py @@ -7,10 +7,10 @@ class TestChryslerSafety(common.PandaCarSafetyTest, common.MotorTorqueSteeringSafetyTest): - TX_MSGS = [[0x23B, 0], [0x292, 0], [0x2A6, 0]] + TX_MSGS = [[0x23B, 0], [0x292, 0], [0x2A6, 0], [0x2D9, 0]] STANDSTILL_THRESHOLD = 0 RELAY_MALFUNCTION_ADDRS = {0: (0x292,)} - FWD_BLACKLISTED_ADDRS = {2: [0x292, 0x2A6]} + FWD_BLACKLISTED_ADDRS = {2: [0x292, 0x2A6, 0x2D9]} FWD_BUS_LOOKUP = {0: 2, 2: 0} MAX_RATE_UP = 3 @@ -72,6 +72,14 @@ def test_buttons(self): self.assertFalse(self._tx(self._button_msg(cancel=True, resume=True))) self.assertFalse(self._tx(self._button_msg(cancel=False, resume=False))) + def _lkas_button_msg_center_stack(self, enabled, is_center_stack_1=True): + values = {"LKAS_Button": enabled} + return self.packer.make_can_msg_panda("Center_Stack_1" if is_center_stack_1 else "Center_Stack_2", 0, values) + + def _lkas_button_msg(self, enabled): + values = {"TOGGLE_LKAS": enabled} + return self.packer.make_can_msg_panda("TRACTION_BUTTON", 0, values) + class TestChryslerRamDTSafety(TestChryslerSafety): TX_MSGS = [[0xB1, 2], [0xA6, 0], [0xFA, 0]] @@ -96,6 +104,10 @@ def _speed_msg(self, speed): values = {"Vehicle_Speed": speed} return self.packer.make_can_msg_panda("ESP_8", 0, values) + def _lkas_button_msg(self, enabled): + return self._lkas_button_msg_center_stack(enabled) + + class TestChryslerRamHDSafety(TestChryslerSafety): TX_MSGS = [[0x275, 0], [0x276, 0], [0x23A, 2]] RELAY_MALFUNCTION_ADDRS = {0: (0x276,)} @@ -120,6 +132,9 @@ def _speed_msg(self, speed): values = {"Vehicle_Speed": speed} return self.packer.make_can_msg_panda("ESP_8", 0, values) + def _lkas_button_msg(self, enabled): + return self._lkas_button_msg_center_stack(enabled, is_center_stack_1=False) + if __name__ == "__main__": unittest.main() diff --git a/tests/safety/test_ford.py b/tests/safety/test_ford.py index a97e26430b..907e350da4 100755 --- a/tests/safety/test_ford.py +++ b/tests/safety/test_ford.py @@ -354,6 +354,19 @@ def test_acc_buttons(self): for bus in (0, 2): self.assertEqual(enabled, self._tx(self._acc_button_msg(Buttons.CANCEL, bus))) + def test_enable_control_allowed_from_acc_main_on(self): + try: + for enable_mads in (True, False): + with self.subTest("enable_mads", mads_enabled=enable_mads): + for main_button_msg_valid in (True, False): + with self.subTest("main_button_msg_valid", state_valid=main_button_msg_valid): + self._mads_states_cleanup() + self.safety.set_mads_params(enable_mads, False) + self._rx(self._pcm_status_msg(main_button_msg_valid)) + self.assertEqual(enable_mads and main_button_msg_valid, self.safety.get_controls_allowed_lat()) + finally: + self._mads_states_cleanup() + class TestFordStockSafety(TestFordSafetyBase): STEER_MESSAGE = MSG_LateralMotionControl diff --git a/tests/safety/test_honda.py b/tests/safety/test_honda.py index 082199c02b..b66ca5720c 100755 --- a/tests/safety/test_honda.py +++ b/tests/safety/test_honda.py @@ -250,6 +250,49 @@ def test_steer_safety_check(self): self.assertTrue(self._tx(self._send_steer_msg(0x0000))) self.assertFalse(self._tx(self._send_steer_msg(0x1000))) + def _lkas_button_msg(self, lkas_button=False, setting_btn=0): + values = {"CRUISE_SETTING": 1 if lkas_button else setting_btn, "COUNTER": self.cnt_button % 4} + self.__class__.cnt_button += 1 + return self.packer.make_can_msg_panda("SCM_BUTTONS", self.PT_BUS, values) + + def test_enable_control_allowed_with_mads_button(self): + """Tests MADS button state transitions and internal button press state.""" + try: + for enable_mads in (True, False): + with self.subTest("enable_mads", mads_enabled=enable_mads): + self._mads_states_cleanup() + self.safety.set_mads_params(enable_mads, False) + + # Verify initial state + self._rx(self._lkas_button_msg(False, 0)) + self.assertEqual(0, self.safety.get_mads_button_press()) # NOT_PRESSED + self.assertFalse(self.safety.get_controls_allowed_lat()) + + # Verify press sets correct internal state + self._rx(self._lkas_button_msg(False, 1)) + self.assertEqual(1, self.safety.get_mads_button_press()) # PRESSED + self.assertEqual(enable_mads, self.safety.get_controls_allowed_lat()) + + # Verify release sets correct internal state + self._rx(self._lkas_button_msg(False, 0)) + self.assertEqual(0, self.safety.get_mads_button_press()) # NOT_PRESSED + self.assertEqual(enable_mads, self.safety.get_controls_allowed_lat()) + + # Test invalid values - should not change button press state + for invalid_setting in (2, 3): + self._rx(self._lkas_button_msg(False, invalid_setting)) + self.assertEqual(0, self.safety.get_mads_button_press()) # Should remain NOT_PRESSED + self.assertEqual(enable_mads, self.safety.get_controls_allowed_lat()) + + # Verify we can still transition after invalid values + self._rx(self._lkas_button_msg(False, 1)) + self.assertEqual(1, self.safety.get_mads_button_press()) + self._rx(self._lkas_button_msg(False, 0)) + self.assertEqual(0, self.safety.get_mads_button_press()) + + finally: + self._mads_states_cleanup() + # ********************* Honda Nidec ********************** diff --git a/tests/safety/test_hyundai.py b/tests/safety/test_hyundai.py index e90c4c4f23..4d58cf750c 100755 --- a/tests/safety/test_hyundai.py +++ b/tests/safety/test_hyundai.py @@ -112,6 +112,33 @@ def _torque_cmd_msg(self, torque, steer_req=1): values = {"CR_Lkas_StrToqReq": torque, "CF_Lkas_ActToi": steer_req} return self.packer.make_can_msg_panda("LKAS11", 0, values) + def _acc_state_msg(self, enable): + values = {"MainMode_ACC": enable, "AliveCounterACC": self.cnt_cruise % 16} + self.__class__.cnt_cruise += 1 + return self.packer.make_can_msg_panda("SCC11", self.SCC_BUS, values) + + def _lkas_button_msg(self, enabled): + values = {"LFA_Pressed": enabled} + return self.packer.make_can_msg_panda("BCM_PO_11", 0, values) + + def _main_cruise_button_msg(self, enabled): + return self._button_msg(0, enabled) + + def test_pcm_main_cruise_state_availability(self): + """Test that ACC main state is correctly set when receiving 0x420 message, toggling HYUNDAI_LONG flag""" + prior_safety_mode = self.safety.get_current_safety_mode() + prior_safety_param = self.safety.get_current_safety_param() + + for hyundai_longitudinal in (True, False): + with self.subTest("hyundai_longitudinal", hyundai_longitudinal=hyundai_longitudinal): + self.safety.set_safety_hooks(Panda.SAFETY_HYUNDAI, 0 if hyundai_longitudinal else Panda.FLAG_HYUNDAI_LONG) + for should_turn_acc_main_on in (True, False): + with self.subTest("acc_main_on", should_turn_acc_main_on=should_turn_acc_main_on): + self._rx(self._acc_state_msg(should_turn_acc_main_on)) # Send the ACC state message + expected_acc_main = should_turn_acc_main_on and hyundai_longitudinal # ACC main should only be set if hyundai_longitudinal is True + self.assertEqual(expected_acc_main, self.safety.get_acc_main_on()) + self.safety.set_safety_hooks(prior_safety_mode, prior_safety_param) + class TestHyundaiSafetyAltLimits(TestHyundaiSafety): MAX_RATE_UP = 2 @@ -135,6 +162,17 @@ def setUp(self): self.safety.set_safety_hooks(Panda.SAFETY_HYUNDAI, Panda.FLAG_HYUNDAI_CAMERA_SCC) self.safety.init_tests() + def test_pcm_main_cruise_state_availability(self): + """ + Test that ACC main state is correctly set when receiving 0x420 message. + For camera SCC, ACC main should always be on when receiving 0x420 message + """ + + for should_turn_acc_main_on in (True, False): + with self.subTest("acc_main_on", should_turn_acc_main_on=should_turn_acc_main_on): + self._rx(self._acc_state_msg(should_turn_acc_main_on)) + self.assertEqual(should_turn_acc_main_on, self.safety.get_acc_main_on()) + class TestHyundaiLegacySafety(TestHyundaiSafety): def setUp(self): @@ -200,6 +238,10 @@ def _fca11_msg(self, idx=0, vsm_aeb_req=False, fca_aeb_req=False, aeb_decel=0): } return self.packer.make_can_msg_panda("FCA11", 0, values) + def _tx_acc_state_msg(self, enable): + values = {"MainMode_ACC": enable} + return self.packer.make_can_msg_panda("SCC11", 0, values) + def test_no_aeb_fca11(self): self.assertTrue(self._tx(self._fca11_msg())) self.assertFalse(self._tx(self._fca11_msg(vsm_aeb_req=True))) @@ -230,6 +272,10 @@ def _accel_msg(self, accel, aeb_req=False, aeb_decel=0): } return self.packer.make_can_msg_panda("SCC12", self.SCC_BUS, values) + def _tx_acc_state_msg(self, enable): + values = {"MainMode_ACC": enable} + return self.packer.make_can_msg_panda("SCC11", 0, values) + def test_tester_present_allowed(self): pass diff --git a/tests/safety/test_hyundai_canfd.py b/tests/safety/test_hyundai_canfd.py index 7f280b6319..3fc9db84d0 100755 --- a/tests/safety/test_hyundai_canfd.py +++ b/tests/safety/test_hyundai_canfd.py @@ -78,6 +78,17 @@ def _button_msg(self, buttons, main_button=0, bus=None): } return self.packer.make_can_msg_panda("CRUISE_BUTTONS", bus, values) + def _acc_state_msg(self, enable): + values = {"MainMode_ACC": enable} + return self.packer.make_can_msg_panda("SCC_CONTROL", self.SCC_BUS, values) + + def _lkas_button_msg(self, enabled): + values = {"LKAS_BTN": enabled} + return self.packer.make_can_msg_panda("CRUISE_BUTTONS", self.PT_BUS, values) + + def _main_cruise_button_msg(self, enabled): + return self._button_msg(0, enabled) + class TestHyundaiCanfdHDA1Base(TestHyundaiCanfdBase): @@ -146,6 +157,10 @@ def _button_msg(self, buttons, main_button=0, bus=1): } return self.packer.make_can_msg_panda("CRUISE_BUTTONS_ALT", self.PT_BUS, values) + def _lkas_button_msg(self, enabled): + values = {"LFA_BTN": enabled} + return self.packer.make_can_msg_panda("CRUISE_BUTTONS_ALT", self.PT_BUS, values) + def test_button_sends(self): """ No button send allowed with alt buttons. @@ -223,6 +238,10 @@ def _accel_msg(self, accel, aeb_req=False, aeb_decel=0): } return self.packer.make_can_msg_panda("SCC_CONTROL", 1, values) + def _tx_acc_state_msg(self, enable): + values = {"MainMode_ACC": enable} + return self.packer.make_can_msg_panda("SCC_CONTROL", 0, values) + # Tests HDA1 longitudinal for ICE, hybrid, EV @parameterized_class([ @@ -263,6 +282,10 @@ def _accel_msg(self, accel, aeb_req=False, aeb_decel=0): } return self.packer.make_can_msg_panda("SCC_CONTROL", 0, values) + def _tx_acc_state_msg(self, enable): + values = {"MainMode_ACC": enable} + return self.packer.make_can_msg_panda("SCC_CONTROL", 0, values) + # no knockout def test_tester_present_allowed(self): pass diff --git a/tests/safety/test_nissan.py b/tests/safety/test_nissan.py index 4c83ca329e..29432a144b 100755 --- a/tests/safety/test_nissan.py +++ b/tests/safety/test_nissan.py @@ -17,6 +17,7 @@ class TestNissanSafety(common.PandaCarSafetyTest, common.AngleSteeringSafetyTest EPS_BUS = 0 CRUISE_BUS = 2 + ACC_MAIN_BUS = 1 # Angle control limits DEG_TO_CAN = 100 @@ -55,6 +56,10 @@ def _user_gas_msg(self, gas): values = {"GAS_PEDAL": gas} return self.packer.make_can_msg_panda("GAS_PEDAL", self.EPS_BUS, values) + def _acc_state_msg(self, main_on): + values = {"CRUISE_ON": main_on} + return self.packer.make_can_msg_panda("PRO_PILOT", self.ACC_MAIN_BUS, values) + def _acc_button_cmd(self, cancel=0, propilot=0, flw_dist=0, _set=0, res=0): no_button = not any([cancel, propilot, flw_dist, _set, res]) values = {"CANCEL_BUTTON": cancel, "PROPILOT_BUTTON": propilot, @@ -84,6 +89,7 @@ class TestNissanSafetyAltEpsBus(TestNissanSafety): EPS_BUS = 1 CRUISE_BUS = 1 + ACC_MAIN_BUS = 2 def setUp(self): self.packer = CANPackerPanda("nissan_x_trail_2017_generated") @@ -91,13 +97,17 @@ def setUp(self): self.safety.set_safety_hooks(Panda.SAFETY_NISSAN, Panda.FLAG_NISSAN_ALT_EPS_BUS) self.safety.init_tests() + def _acc_state_msg(self, main_on): + values = {"CRUISE_ON": main_on} + return self.packer.make_can_msg_panda("PRO_PILOT", self.ACC_MAIN_BUS, values) + class TestNissanLeafSafety(TestNissanSafety): def setUp(self): self.packer = CANPackerPanda("nissan_leaf_2018_generated") self.safety = libpanda_py.libpanda - self.safety.set_safety_hooks(Panda.SAFETY_NISSAN, 0) + self.safety.set_safety_hooks(Panda.SAFETY_NISSAN, Panda.FLAG_NISSAN_LEAF) self.safety.init_tests() def _user_brake_msg(self, brake): @@ -108,6 +118,10 @@ def _user_gas_msg(self, gas): values = {"GAS_PEDAL": gas} return self.packer.make_can_msg_panda("CRUISE_THROTTLE", 0, values) + def _acc_state_msg(self, main_on): + values = {"CRUISE_AVAILABLE": main_on} + return self.packer.make_can_msg_panda("CRUISE_THROTTLE", 0, values) + # TODO: leaf should use its own safety param def test_acc_buttons(self): pass diff --git a/tests/safety/test_subaru.py b/tests/safety/test_subaru.py index 7d07f79aaf..de00a8fbb5 100755 --- a/tests/safety/test_subaru.py +++ b/tests/safety/test_subaru.py @@ -106,6 +106,22 @@ def _pcm_status_msg(self, enable): values = {"Cruise_Activated": enable} return self.packer.make_can_msg_panda("CruiseControl", self.ALT_MAIN_BUS, values) + def _lkas_button_msg(self, lkas_pressed=False, lkas_hud=0): + values = {"LKAS_Dash_State": 2 if lkas_pressed else lkas_hud} + return self.packer.make_can_msg_panda("ES_LKAS_State", SUBARU_CAM_BUS, values) + + def test_enable_control_allowed_with_mads_button(self): + for enable_mads in (True, False): + with self.subTest("enable_mads", mads_enabled=enable_mads): + for mads_button_press in range(4): + with self.subTest("mads_button_press", button_state=mads_button_press): + self._mads_states_cleanup() + self.safety.set_mads_params(enable_mads, False) + + self._rx(self._lkas_button_msg(False, mads_button_press)) + self.assertEqual(enable_mads and mads_button_press in range(1, 4), + self.safety.get_controls_allowed_lat()) + self._mads_states_cleanup() class TestSubaruStockLongitudinalSafetyBase(TestSubaruSafetyBase): def _cancel_msg(self, cancel, cruise_throttle=0): diff --git a/tests/safety/test_toyota.py b/tests/safety/test_toyota.py index e60b29c5c2..030e52eb88 100755 --- a/tests/safety/test_toyota.py +++ b/tests/safety/test_toyota.py @@ -81,6 +81,10 @@ def _pcm_status_msg(self, enable): values = {"CRUISE_ACTIVE": enable} return self.packer.make_can_msg_panda("PCM_CRUISE", 0, values) + def _lkas_button_msg(self, lkas_pressed=False, lkas_hud=0): + values = {"LKAS_STATUS": 2 if lkas_pressed else lkas_hud} + return self.packer.make_can_msg_panda("LKAS_HUD", 2, values) + def test_diagnostics(self, stock_longitudinal: bool = False): for should_tx, msg in ((False, b"\x6D\x02\x3E\x00\x00\x00\x00\x00"), # fwdCamera tester present (False, b"\x0F\x03\xAA\xAA\x00\x00\x00\x00"), # non-tester present @@ -127,6 +131,19 @@ def test_rx_hook(self): self.assertFalse(self._rx(to_push)) self.assertFalse(self.safety.get_controls_allowed()) + def test_enable_control_allowed_with_mads_button(self): + for enable_mads in (True, False): + with self.subTest("enable_mads", mads_enabled=enable_mads): + for mads_button_press in range(4): + with self.subTest("mads_button_press", button_state=mads_button_press): + self._mads_states_cleanup() + self.safety.set_mads_params(enable_mads, False) + + self._rx(self._lkas_button_msg(False, mads_button_press)) + self._rx(self._speed_msg(0)) # Only for Toyota, we must send a msg to bus 0 because generic_rx_checks happen only there. + self.assertEqual(enable_mads and mads_button_press in range(1, 4), + self.safety.get_controls_allowed_lat()) + self._mads_states_cleanup() class TestToyotaSafetyTorque(TestToyotaSafetyBase, common.MotorTorqueSteeringSafetyTest, common.SteerRequestCutSafetyTest): diff --git a/tests/safety_replay/replay_drive.py b/tests/safety_replay/replay_drive.py index df3e0554f6..3ba27fea1c 100755 --- a/tests/safety_replay/replay_drive.py +++ b/tests/safety_replay/replay_drive.py @@ -1,11 +1,24 @@ #!/usr/bin/env python3 import argparse import os -from collections import Counter +from collections import Counter, defaultdict +from panda.python import ALTERNATIVE_EXPERIENCE from panda.tests.libpanda import libpanda_py from panda.tests.safety_replay.helpers import package_can_msg, init_segment +# Define debug variables and their getter methods +DEBUG_VARS = { + 'lat_active': lambda safety: safety.get_lat_active(), + 'controls_allowed': lambda safety: safety.get_controls_allowed(), + 'controls_requested_lat': lambda safety: safety.get_controls_requested_lat(), + 'controls_allowed_lat': lambda safety: safety.get_controls_allowed_lat(), + 'current_disengage_reason': lambda safety: safety.mads_get_current_disengage_reason(), + 'stock_acc_main': lambda safety: safety.get_acc_main_on(), + 'mads_acc_main': lambda safety: safety.get_mads_acc_main(), +} + + # replay a drive to check for safety violations def replay_drive(lr, safety_mode, param, alternative_experience, segment=False): safety = libpanda_py.libpanda @@ -14,15 +27,28 @@ def replay_drive(lr, safety_mode, param, alternative_experience, segment=False): assert err == 0, "invalid safety mode: %d" % safety_mode safety.set_alternative_experience(alternative_experience) + _enable_mads = bool(alternative_experience & ALTERNATIVE_EXPERIENCE.ENABLE_MADS) + _disengage_lateral_on_brake = bool(alternative_experience & ALTERNATIVE_EXPERIENCE.DISENGAGE_LATERAL_ON_BRAKE) + safety.set_mads_params(_enable_mads, _disengage_lateral_on_brake) + print("alternative experience:") + print(f" enable mads: {_enable_mads}") + print(f" disengage lateral on brake: {_disengage_lateral_on_brake}") + if segment: init_segment(safety, lr, safety_mode, param) lr.reset() - rx_tot, rx_invalid, tx_tot, tx_blocked, tx_controls, tx_controls_blocked = 0, 0, 0, 0, 0, 0 + rx_tot, rx_invalid, tx_tot, tx_blocked, tx_controls, tx_controls_lat, tx_controls_blocked, tx_controls_lat_blocked, mads_mismatch = 0, 0, 0, 0, 0, 0, 0, 0, 0 safety_tick_rx_invalid = False blocked_addrs = Counter() invalid_addrs = set() + # Track last good state for each address + last_good_states = defaultdict(lambda: { + 'timestamp': None, + **{var: None for var in DEBUG_VARS} + }) + can_msgs = [m for m in lr if m.which() in ('can', 'sendcan')] start_t = can_msgs[0].logMonoTime end_t = can_msgs[-1].logMonoTime @@ -38,14 +64,44 @@ def replay_drive(lr, safety_mode, param, alternative_experience, segment=False): for canmsg in msg.sendcan: to_send = package_can_msg(canmsg) sent = safety.safety_tx_hook(to_send) + + # mismatched + if (safety.get_controls_allowed() and not safety.get_controls_allowed_lat()): + mads_mismatch += 1 + print(f"controls allowed but not controls allowed lat [{mads_mismatch}]") + print(f"msg:{canmsg.address} ({hex(canmsg.address)})") + for var, getter in DEBUG_VARS.items(): + print(f" {var}: {getter(safety)}") + if not sent: tx_blocked += 1 tx_controls_blocked += safety.get_controls_allowed() + tx_controls_lat_blocked += safety.get_controls_allowed_lat() blocked_addrs[canmsg.address] += 1 if "DEBUG" in os.environ: - print("blocked bus %d msg %d at %f" % (canmsg.src, canmsg.address, (msg.logMonoTime - start_t) / 1e9)) + last_good = last_good_states[canmsg.address] + print(f"\nBlocked message at {(msg.logMonoTime - start_t) / 1e9:.3f}s:") + print(f"Address: {hex(canmsg.address)} (bus {canmsg.src})") + print("Current state:") + for var, getter in DEBUG_VARS.items(): + print(f" {var}: {getter(safety)}") + + if last_good['timestamp'] is not None: + print(f"\nLast good state ({last_good['timestamp']:.3f}s):") + for var in DEBUG_VARS: + print(f" {var}: {last_good[var]}") + else: + print("\nNo previous good state found for this address") + print("-" * 80) + else: # Update last good state if message is allowed + last_good_states[canmsg.address].update({ + 'timestamp': (msg.logMonoTime - start_t) / 1e9, + **{var: getter(safety) for var, getter in DEBUG_VARS.items()} + }) + tx_controls += safety.get_controls_allowed() + tx_controls_lat += safety.get_controls_allowed_lat() tx_tot += 1 elif msg.which() == 'can': # ignore msgs we sent @@ -65,11 +121,15 @@ def replay_drive(lr, safety_mode, param, alternative_experience, segment=False): print("\nTX") print("total openpilot msgs:", tx_tot) print("total msgs with controls allowed:", tx_controls) + print("total msgs with controls_lat allowed:", tx_controls_lat) print("blocked msgs:", tx_blocked) print("blocked with controls allowed:", tx_controls_blocked) + print("blocked with controls_lat allowed:", tx_controls_lat_blocked) print("blocked addrs:", blocked_addrs) + print("mads enabled:", safety.get_enable_mads()) + + return tx_controls_blocked == 0 and tx_controls_lat_blocked == 0 and rx_invalid == 0 and not safety_tick_rx_invalid - return tx_controls_blocked == 0 and rx_invalid == 0 and not safety_tick_rx_invalid if __name__ == "__main__": from openpilot.tools.lib.logreader import LogReader