From 4c2ccd7fe5de1c1931c605857f571b2c499f9ed3 Mon Sep 17 00:00:00 2001 From: DevTekVE Date: Fri, 6 Dec 2024 01:39:36 +0100 Subject: [PATCH] Modular Assistive Driving System (MADS) (#40) * alternative experience * safety init * fix * more update * not really * misra * Add Custom MIT License (#38) * brake check was not handled * revert * alt -> lkas * explicit checks * support toyota and ford * rename * hyundai can-fd support * only allow lkas if enabled * hyundai: main button handling * revert * hyundai: main button heartbeat * add logging for controls allowed lateral * fix panda safety * ford btn * toyota btn * fca btn * honda btn * mads safety tests * more tests * safety misra * safety mutation * misra * mutation experiment * fix * ford test main button * ford test lkas button * more ford test * hyundai lkas and main * more ford * hyundai canfd * rename * rename * cleaner * more fixes * more hyundai tests * no longer needed * thanks for tests! * more tests for lat * more explicit * make sure to reset * try this out * probably needed * move * misra * not needed * move to safety_mads * not really needed * remove * MADS: Refactor MADS safety with improved state management (pull request #46) Refactor MADS safety with improved state management This commit introduces a major refactoring of the MADS safety module, improving state management and control flow. Key changes include: Core Changes: - Introduced a MADSState struct to centralize state management - Removed global state variables in favor of structured state - Implemented button transition handling with explicit state tracking (PRESSED/RELEASED/NO_CHANGE) - Added state flags for button availability detection - Simplified lateral control permission logic Button Handling: - Separated main button and LKAS button state tracking - Added independent engagement states for each button - Improved button press detection across multiple platforms - Added support for main and LKAS buttons on Hyundai platforms - Modified ACC main state handling Testing: - Added comprehensive test coverage for MADS state transitions - Added new MADS-specific test base class for consistent testing across platforms - Added mutation testing for state management - Extended timeout for mutation tests from 5 to 8 minutes - Added extensive button press validation tests - Enhanced debugging output in replay drive tests The refactored code provides a more organized implementation of MADS safety features while maintaining compatibility with existing safety checks. * adding note * adding ford (WIP) * adding honda (WIP) * adding toyota (WIP) * adding chrysler (WIP) * Standardize Button State Handling Across Platforms Refactor button state handling by replacing integer constants with an enumerated `ButtonState` type and updating logic to improve readability and maintainability. This change affects button press detection in Ford, Honda, Hyundai, and Toyota safety modules and aligns them with a unified MADS button state approach. Enums provide a clearer understanding of button states and transitions, facilitating easier maintenance and future enhancements. * Disable LKAS button press logic in Honda and Toyota safety. The code for processing LKAS button presses has been commented out in both Honda and Toyota safety implementations. This change aims to investigate or temporarily halt the button press effects without removing the logic altogether. It will be important to test for any impacts this may have on vehicle control functionality. * Remove commented out code in toyota_rx_hook function This commit cleans up the toyota_rx_hook function by removing unnecessary commented-out code that checks for LKAS button presses on bus 2. This helps improve code readability and maintainability without altering the existing functionality. * GM, mazda, nissan, subaru (global & preglobal) * Honda LKAS * Revert "Remove commented out code in toyota_rx_hook function" This reverts commit d6b012c01a08118d91fad56c9f6ac2f92b671968. * Toyota, Subaru Global LKAS * nissan fix * gm fix * use speed msg to force rx * im bored * misra * subaru/toyota/honda * nope * attempt * go through all buttons * try nissan * more nissan * nissan tests passed! * subaru lkas test (not sure why it's not passing 2 and 3 values) * Improved code organization in safety_subaru.h and test_subaru.py This commit includes a minor restructuring in safety_subaru.h and test_subaru.py for better readability and flow. The condition check in safety_subaru.h for lkas_hud now has explicit parentheses. With regard to test_subaru.py, an unnecessary import was removed, and the sequence of steps in the test was reordered - now enabling mads and cleaning up mads_states happens before each subtest. * Refactor tests to use _speed_msg instead of _user_brake_msg. Updated the MADS safety tests to utilize the _speed_msg(0) function call in place of _user_brake_msg(False). * Reworking the tests a little for clarity * disabling lkas again on toyota temporarily * fix mads condition to engage * hyundai and honda good with new tests * Redoing more tests * update for safety tick ensuring mads control is exited while lagging * Updating tests for toyota * cleaning up tests on hkg * commenting out temp_debug for future use * revert * constants * cleanup * format! * match yota * Apply suggestions from code review * force * explicit checks * revert --------- Co-authored-by: Jason Wen --- .github/workflows/test.yaml | 2 +- board/health.h | 1 + board/main_comms.h | 4 + board/safety.h | 18 +- board/safety/safety_chrysler.h | 1 + board/safety/safety_ford.h | 5 + board/safety/safety_gm.h | 4 + board/safety/safety_honda.h | 7 + board/safety/safety_hyundai.h | 10 + board/safety/safety_hyundai_canfd.h | 2 + board/safety/safety_hyundai_common.h | 2 + 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 | 246 +++++++++++++++++++++++++ python/__init__.py | 8 +- tests/libpanda/safety_helpers.h | 88 +++++++++ tests/libpanda/safety_helpers.py | 47 +++++ tests/safety/common.py | 5 +- tests/safety/mads_common.py | 238 ++++++++++++++++++++++++ tests/safety/test_chrysler.py | 3 + tests/safety/test_ford.py | 11 ++ tests/safety/test_gm.py | 3 + tests/safety/test_honda.py | 24 +++ tests/safety/test_hyundai.py | 42 +++++ tests/safety/test_hyundai_canfd.py | 26 +++ tests/safety/test_mazda.py | 3 + tests/safety/test_nissan.py | 29 ++- tests/safety/test_subaru.py | 15 ++ tests/safety/test_toyota.py | 16 ++ tests/safety/test_volkswagen_mqb.py | 2 + tests/safety/test_volkswagen_pq.py | 3 + tests/safety_replay/replay_drive.py | 8 +- 35 files changed, 899 insertions(+), 10 deletions(-) create mode 100644 board/sunnypilot/safety_mads.h create mode 100644 tests/safety/mads_common.py diff --git a/.github/workflows/test.yaml b/.github/workflows/test.yaml index 39559ea2..35d804e1 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: 8 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 74d822dc..cb827ce2 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_comms.h b/board/main_comms.h index e366b19a..7da190c3 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,9 @@ 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; + bool mads_enabled = (alternative_experience & ALT_EXP_ENABLE_MADS) != 0; + bool disengage_lateral_on_brake = !(alternative_experience & ALT_EXP_DISABLE_DISENGAGE_LATERAL_ON_BRAKE); + mads_set_system_state(mads_enabled, disengage_lateral_on_brake); } break; // **** 0xe0: uart read diff --git a/board/safety.h b/board/safety.h index 91310995..485f4d8b 100644 --- a/board/safety.h +++ b/board/safety.h @@ -3,6 +3,8 @@ #include "safety_declarations.h" #include "can.h" +#include "sunnypilot/safety_mads.h" + // include the safety policies. #include "safety/safety_defaults.h" #include "safety/safety_honda.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(); } 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, brake_pressed || regen_braking, cruise_engaged_prev); } 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 2bbc9427..d57bf4c7 100644 --- a/board/safety/safety_chrysler.h +++ b/board/safety/safety_chrysler.h @@ -78,6 +78,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 diff --git a/board/safety/safety_ford.h b/board/safety/safety_ford.h index 5b19dd9c..afa48720 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) { + lkas_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 f0902a92..2af436c5 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 60ccea1e..ab99decf 100644 --- a/board/safety/safety_honda.h +++ b/board/safety/safety_honda.h @@ -114,6 +114,13 @@ static void honda_rx_hook(const CANPacket_t *to_push) { // 0x1A6 for the ILX, 0x296 for the Civic Touring if (((addr == 0x1A6) || (addr == 0x296)) && (bus == pt_bus)) { int button = (GET_BYTE(to_push, 0) & 0xE0U) >> 5; + bool is_main = (button == HONDA_BTN_MAIN); + main_button_press = is_main ? MADS_BUTTON_PRESSED : MADS_BUTTON_NOT_PRESSED; + + int cruise_setting = (GET_BYTE(to_push, (addr == 0x296) ? 0U : 5U) & 0x0CU) >> 2U; + if (cruise_setting == 1) { + lkas_button_press = MADS_BUTTON_PRESSED; + } // enter controls on the falling edge of set or resume bool set = (button != HONDA_BTN_SET) && (cruise_button_prev == HONDA_BTN_SET); diff --git a/board/safety/safety_hyundai.h b/board/safety/safety_hyundai.h index 57043f67..cb09ae36 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) { + lkas_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; diff --git a/board/safety/safety_hyundai_canfd.h b/board/safety/safety_hyundai_canfd.h index b42889bb..55bce413 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); + lkas_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); + lkas_button_press = GET_BIT(to_push, 39U) ? MADS_BUTTON_PRESSED : MADS_BUTTON_NOT_PRESSED; } hyundai_common_cruise_buttons_check(cruise_button, main_button); } diff --git a/board/safety/safety_hyundai_common.h b/board/safety/safety_hyundai_common.h index af22b7d9..a92f8ae6 100644 --- a/board/safety/safety_hyundai_common.h +++ b/board/safety/safety_hyundai_common.h @@ -91,6 +91,8 @@ void hyundai_common_cruise_buttons_check(const int cruise_button, const bool mai hyundai_last_button_interaction = MIN(hyundai_last_button_interaction + 1U, HYUNDAI_PREV_BUTTON_SAMPLES); } + main_button_press = main_button ? MADS_BUTTON_PRESSED : MADS_BUTTON_NOT_PRESSED; + if (hyundai_longitudinal) { // enter controls on falling edge of resume or set bool set = (cruise_button != HYUNDAI_BTN_SET) && (cruise_button_prev == HYUNDAI_BTN_SET); diff --git a/board/safety/safety_mazda.h b/board/safety/safety_mazda.h index ed87451f..f49f9e63 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 fd47e094..f00df72f 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 29015065..0df3a4a7 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)) { + lkas_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 760840f3..40586b4d 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 7008bf84..69bede39 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)) { + lkas_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 00000000..328ac156 --- /dev/null +++ b/board/sunnypilot/safety_mads.h @@ -0,0 +1,246 @@ +#pragma once + +// Flags meant to be set by each specific safety_{make} +typedef enum { + MADS_BUTTON_UNAVAILABLE = -1, + MADS_BUTTON_NOT_PRESSED = 0, + MADS_BUTTON_PRESSED = 1 +} ButtonState; + +extern ButtonState main_button_press; +ButtonState main_button_press = MADS_BUTTON_UNAVAILABLE; + +extern ButtonState lkas_button_press; +ButtonState lkas_button_press = MADS_BUTTON_UNAVAILABLE; + +// extern int temp_debug; +// int temp_debug = 0; +// -- + +// Enable the ability to enable sunnypilot Automatic Lane Centering and ACC/SCC independently of each other. This +// will enable MADS and allow other features to be used. +// Enable the ability to re-engage sunnypilot Automatic Lane Centering only (NOT ACC/SCC) on brake release while MADS +// is enabled. +#define ALT_EXP_ENABLE_MADS 1024 +// Enable the ability to disable disengaging lateral control on brake press while MADS is enabled. +#define ALT_EXP_DISABLE_DISENGAGE_LATERAL_ON_BRAKE 2048 + +#define MISMATCH_DEFAULT_THRESHOLD 25 +#define MADS_STATE_FLAG_DEFAULT 0U +#define MADS_STATE_FLAG_RESERVED 1U +#define MADS_STATE_FLAG_MAIN_BUTTON_AVAILABLE 2U +#define MADS_STATE_FLAG_LKAS_BUTTON_AVAILABLE 4U + + +// Button transition types +typedef enum { + MADS_BUTTON_TRANSITION_NO_CHANGE, + MADS_BUTTON_TRANSITION_TO_PRESSED, + MADS_BUTTON_TRANSITION_TO_RELEASED +} ButtonTransition; + +// MADS System State Struct +typedef struct { + uint32_t state_flags; + + // Values from stock that we need + const bool *is_vehicle_moving_ptr; + + // System configuration flags + bool disengage_lateral_on_brake; + + // System-wide enable/disable + bool system_enabled; + + // Button states with last state tracking + struct { + const ButtonState *current; + ButtonState last; + ButtonTransition transition; + uint32_t press_timestamp; + bool is_engaged; + } main_button; + + struct { + const ButtonState *current; + ButtonState last; + ButtonTransition transition; + uint32_t press_timestamp; + bool is_engaged; + } lkas_button; // Rule 12.3: separate declarations + + // Vehicle condition states + bool is_braking; + bool cruise_engaged; + + // Lateral control permission states + bool controls_allowed_lat; + bool disengaged_from_brakes; + + // ACC main state tracking + struct { + const bool *current; + bool previous; + uint32_t mismatch_count; + uint32_t mismatch_threshold; + } acc_main; +} MADSState; + +// Global state instance +static MADSState _mads_state; // Rule 8.4: static for internal linkage + +// Determine button transition +static ButtonTransition _get_button_transition(bool current, bool last) { + ButtonTransition result = MADS_BUTTON_TRANSITION_NO_CHANGE; + + if (current && !last) { + result = MADS_BUTTON_TRANSITION_TO_PRESSED; + } else if (!current && last) { + result = MADS_BUTTON_TRANSITION_TO_RELEASED; + } else { + result = MADS_BUTTON_TRANSITION_NO_CHANGE; + } + + return result; +} + +// Initialize the MADS state +static void mads_state_init(void) { + _mads_state.is_vehicle_moving_ptr = NULL; + _mads_state.acc_main.current = NULL; + _mads_state.main_button.current = &main_button_press; + _mads_state.lkas_button.current = &lkas_button_press; + _mads_state.state_flags = MADS_STATE_FLAG_DEFAULT; + + _mads_state.system_enabled = false; + _mads_state.disengage_lateral_on_brake = true; + + // Button state initialization + _mads_state.main_button.last = MADS_BUTTON_UNAVAILABLE; + _mads_state.main_button.transition = MADS_BUTTON_TRANSITION_NO_CHANGE; + _mads_state.main_button.press_timestamp = 0; + _mads_state.main_button.is_engaged = false; + + _mads_state.lkas_button.last = MADS_BUTTON_UNAVAILABLE; + _mads_state.lkas_button.transition = MADS_BUTTON_TRANSITION_NO_CHANGE; + _mads_state.lkas_button.press_timestamp = 0; + _mads_state.lkas_button.is_engaged = false; + + // ACC main state initialization + _mads_state.acc_main.previous = false; + _mads_state.acc_main.mismatch_count = 0; + _mads_state.acc_main.mismatch_threshold = MISMATCH_DEFAULT_THRESHOLD; + + // Control states + _mads_state.is_braking = false; + _mads_state.cruise_engaged = false; + _mads_state.controls_allowed_lat = false; + _mads_state.disengaged_from_brakes = false; +} + +// Exit lateral controls +static void mads_exit_controls(void) { + if (_mads_state.controls_allowed_lat) { + _mads_state.disengaged_from_brakes = true; + _mads_state.controls_allowed_lat = false; + } +} + +// Resume lateral controls +static void _mads_resume_controls(void) { + if (_mads_state.disengaged_from_brakes) { + _mads_state.controls_allowed_lat = true; + _mads_state.disengaged_from_brakes = false; + } +} + +// Check braking condition +static void _mads_check_braking(bool is_braking) { + bool was_braking = _mads_state.is_braking; + if (is_braking && (!was_braking || *_mads_state.is_vehicle_moving_ptr) && _mads_state.disengage_lateral_on_brake) { + mads_exit_controls(); + } + + if (!is_braking && _mads_state.disengage_lateral_on_brake) { + _mads_resume_controls(); + } + _mads_state.is_braking = is_braking; +} + +// Update state based on input conditions +void mads_state_update(const bool *op_vehicle_moving, const bool *op_acc_main, bool is_braking, bool cruise_engaged) { + if (_mads_state.is_vehicle_moving_ptr == NULL) { + _mads_state.is_vehicle_moving_ptr = op_vehicle_moving; + } + + if (_mads_state.acc_main.current == NULL) { + _mads_state.acc_main.current = op_acc_main; + } + + // Update button states + if (main_button_press != MADS_BUTTON_UNAVAILABLE) { + _mads_state.state_flags |= MADS_STATE_FLAG_MAIN_BUTTON_AVAILABLE; + _mads_state.main_button.current = &main_button_press; + _mads_state.main_button.transition = _get_button_transition( + *_mads_state.main_button.current == MADS_BUTTON_PRESSED, + _mads_state.main_button.last == MADS_BUTTON_PRESSED + ); + + // Engage on press, disengage on press if already pressed + if (_mads_state.main_button.transition == MADS_BUTTON_TRANSITION_TO_PRESSED) { + if (_mads_state.main_button.is_engaged) { + _mads_state.main_button.is_engaged = false; // Disengage if already engaged + } else { + _mads_state.main_button.is_engaged = true; // Engage otherwise + } + } + + _mads_state.main_button.last = *_mads_state.main_button.current; + } + + // Same for LKAS button + if (lkas_button_press != MADS_BUTTON_UNAVAILABLE) { + _mads_state.state_flags |= MADS_STATE_FLAG_LKAS_BUTTON_AVAILABLE; + _mads_state.lkas_button.current = &lkas_button_press; + _mads_state.lkas_button.transition = _get_button_transition( + *_mads_state.lkas_button.current == MADS_BUTTON_PRESSED, + _mads_state.lkas_button.last == MADS_BUTTON_PRESSED + ); + + if (_mads_state.lkas_button.transition == MADS_BUTTON_TRANSITION_TO_PRESSED) { + if (_mads_state.lkas_button.is_engaged) { + _mads_state.lkas_button.is_engaged = false; + } else { + _mads_state.lkas_button.is_engaged = true; + } + } + + _mads_state.lkas_button.last = *_mads_state.lkas_button.current; + } + + // Update other states + _mads_state.cruise_engaged = cruise_engaged; + + //TODO-SP: theres a possibility of mismatching state if lat is engaged due to main button and disengaged due to lkas button. Need to validate if it's the case + // Use engagement state for lateral control + _mads_state.controls_allowed_lat = _mads_state.main_button.is_engaged || _mads_state.lkas_button.is_engaged || * + _mads_state.acc_main.current; + + // Check ACC main state and braking conditions + _mads_check_braking(is_braking); + + // Update ACC main state + _mads_state.acc_main.previous = *_mads_state.acc_main.current; +} + +// Global system enable/disable +void mads_set_system_state(bool enabled, bool disengage_lateral_on_brake) { + mads_state_init(); + _mads_state.system_enabled = enabled; + _mads_state.disengage_lateral_on_brake = disengage_lateral_on_brake; +} + +// Check if lateral control is currently allowed by MADS +bool mads_is_lateral_control_allowed_by_mads(void) { + return _mads_state.system_enabled && _mads_state.controls_allowed_lat; +} diff --git a/python/__init__.py b/python/__init__.py index 3748d1fb..2d4f590d 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 + DISABLE_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(" None: ... def get_controls_allowed(self) -> bool: ... + def set_controls_allowed_lat(self, c: bool) -> None: ... + def get_lat_active(self) -> bool: ... + def get_controls_allowed_lat(self) -> bool: ... + def get_main_button_press(self) -> int: ... + def set_mads_state_flags(self, flags: int) -> None: ... + def get_mads_state_flags(self) -> int: ... + def get_mads_main_button_transition(self) -> int: ... + def get_mads_main_button_current(self) -> int: ... + def get_mads_main_button_last(self) -> int: ... + def get_mads_acc_main(self) -> int: ... def get_longitudinal_allowed(self) -> bool: ... def set_alternative_experience(self, mode: int) -> None: ... def get_alternative_experience(self) -> int: ... @@ -66,6 +101,7 @@ class PandaSafety(Protocol): 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 +139,15 @@ class PandaSafety(Protocol): def set_honda_bosch_long(self, c: bool) -> None: ... def get_honda_hw(self) -> int: ... + def set_enable_mads(self, enable_mads: bool, disengage_lat_on_brake: bool) -> None: ... + def set_main_button_press(self, main_button_press: int) -> None: ... + def set_lkas_button_press(self, lkas_button_press: int) -> None: ... + def get_enable_mads(self) -> bool: ... + def get_disengage_lat_on_brake(self) -> bool: ... + def set_main_button_engaged(self, engaged: bool) -> None: ... + def get_main_button_engaged(self) -> bool: ... + def set_lkas_button_engaged(self, engaged: bool) -> None: ... + def get_lkas_button_engaged(self) -> bool: ... + def get_lkas_button_press(self) -> int: ... + # def get_temp_debug(self) -> int: ... diff --git a/tests/safety/common.py b/tests/safety/common.py index fdc54f6d..4d94aa56 100644 --- a/tests/safety/common.py +++ b/tests/safety/common.py @@ -8,6 +8,7 @@ from collections.abc import Callable 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 @@ class PandaSafetyTest(PandaSafetyTestBase): @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 @@ class PandaCarSafetyTest(PandaSafetyTest): 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/mads_common.py b/tests/safety/mads_common.py new file mode 100644 index 00000000..42bcb9b4 --- /dev/null +++ b/tests/safety/mads_common.py @@ -0,0 +1,238 @@ +import unittest +from abc import abstractmethod +from enum import IntFlag + + +class MadsStates(IntFlag): + DEFAULT = 0 + RESERVED = 1 + MAIN_BUTTON_AVAILABLE = 2 + LKAS_BUTTON_AVAILABLE = 4 + + +class MadsCommonBase(unittest.TestCase): + @abstractmethod + def _lkas_button_msg(self, enabled): + raise NotImplementedError + + @abstractmethod + def _main_cruise_button_msg(self, enabled): + try: + self._button_msg(enabled) + except (NotImplementedError, AttributeError): + raise unittest.SkipTest("Skipping test because _button_msg is not implemented for this car. If you know it, please implement it.") + + raise NotImplementedError("Since _button_msg is implemented, _main_cruise_button_msg should be implemented as well to signal the main cruise button press") + + @abstractmethod + def _acc_state_msg(self, enabled): + raise NotImplementedError + + def test_enable_control_from_cruise_button_press(self): + for enable_mads in (True, False): + with self.subTest("enable_mads", mads_enabled=enable_mads): + for cruise_button_press in [True, False]: + self.safety.set_enable_mads(enable_mads, False) + with self.subTest("cruise_button_press", cruise_button_press=cruise_button_press): + self._mads_states_cleanup() + self._rx(self._main_cruise_button_msg(cruise_button_press)) + self.assertEqual(enable_mads and cruise_button_press, self.safety.get_controls_allowed_lat()) + self._mads_states_cleanup() + + def test_enable_control_from_lkas_button_press(self): + try: + self._lkas_button_msg(False) + except NotImplementedError: + raise unittest.SkipTest("Skipping test because _lkas_button_msg is not implemented for this car") + + for enable_mads in (True, False): + with self.subTest("enable_mads", mads_enabled=enable_mads): + for lkas_button_press in [True, False]: + self.safety.set_enable_mads(enable_mads, False) + with self.subTest("lkas_button_press", button_state=lkas_button_press): + self._mads_states_cleanup() + self._rx(self._lkas_button_msg(lkas_button_press)) + self.assertEqual(enable_mads and lkas_button_press, self.safety.get_controls_allowed_lat()) + self._mads_states_cleanup() + + def _mads_states_cleanup(self): + self.safety.set_main_button_press(-1) + self.safety.set_lkas_button_press(-1) + self.safety.set_controls_allowed_lat(False) + self.safety.set_main_button_engaged(False) + self.safety.set_lkas_button_engaged(False) + self.safety.set_mads_state_flags(0) + self.safety.set_acc_main_on(False) + + def test_enable_control_from_setting_main_state_manually(self): + for enable_mads in (True, False): + with self.subTest("enable_mads", mads_enabled=enable_mads): + self.safety.set_enable_mads(enable_mads, False) + for main_button_press in (-1, 0, 1): + with self.subTest("main_button_press", button_state=main_button_press): + self._mads_states_cleanup() + self.safety.set_main_button_press(main_button_press) + self._rx(self._speed_msg(0)) + self.assertEqual(enable_mads and main_button_press == 1, self.safety.get_controls_allowed_lat()) + self._mads_states_cleanup() + + def test_enable_control_from_setting_lkas_state_manually(self): + for enable_mads in (True, False): + with self.subTest("enable_mads", mads_enabled=enable_mads): + self.safety.set_enable_mads(enable_mads, False) + for lkas_button_press in (-1, 0, 1): + with self.subTest("lkas_button_press", button_state=lkas_button_press): + self._mads_states_cleanup() + self.safety.set_lkas_button_press(lkas_button_press) + self._rx(self._speed_msg(0)) + self.assertEqual(enable_mads and lkas_button_press == 1, self.safety.get_controls_allowed_lat()) + self._mads_states_cleanup() + + def test_mads_state_flags(self): + for enable_mads in (True, False): + with self.subTest("enable_mads", mads_enabled=enable_mads): + self.safety.set_enable_mads(enable_mads, False) + self._mads_states_cleanup() + self.safety.set_main_button_press(0) # Meaning a message with those buttons was seen and the _prev inside is no longer -1 + self.safety.set_lkas_button_press(0) # Meaning a message with those buttons was seen and the _prev inside is no longer -1 + self._rx(self._speed_msg(0)) + self.assertTrue(self.safety.get_mads_state_flags() & MadsStates.MAIN_BUTTON_AVAILABLE) + self.assertTrue(self.safety.get_mads_state_flags() & MadsStates.LKAS_BUTTON_AVAILABLE) + self._mads_states_cleanup() + + def test_enable_control_from_acc_main_on(self): + """Test that lateral controls are allowed when ACC main is enabled""" + for enable_mads in (True, False): + with self.subTest("enable_mads", mads_enabled=enable_mads): + self.safety.set_enable_mads(enable_mads, False) + for acc_main_on in (True, False): + with self.subTest("acc_main_on", acc_main_on=acc_main_on): + self._mads_states_cleanup() + self.safety.set_acc_main_on(acc_main_on) + self._rx(self._speed_msg(0)) + self.assertEqual(enable_mads and acc_main_on, self.safety.get_controls_allowed_lat()) + self._mads_states_cleanup() + + def test_controls_allowed_must_always_enable_lat(self): + for mads_enabled in [True, False]: + with self.subTest("mads enabled", mads_enabled=mads_enabled): + self.safety.set_enable_mads(mads_enabled, False) + for controls_allowed in [True, False]: + with self.subTest("controls allowed", controls_allowed=controls_allowed): + self.safety.set_controls_allowed(controls_allowed) + self.assertEqual(self.safety.get_controls_allowed(), self.safety.get_lat_active()) + + def test_mads_disengage_lat_on_brake_setup(self): + for mads_enabled in [True, False]: + with self.subTest("mads enabled", mads_enabled=mads_enabled): + for disengage_on_brake in [True, False]: + with self.subTest("disengage on brake", disengage_on_brake=disengage_on_brake): + self.safety.set_enable_mads(mads_enabled, disengage_on_brake) + self.assertEqual(disengage_on_brake, self.safety.get_disengage_lat_on_brake()) + + def test_mads_state_flags_mutation(self): + """Test to catch mutations in bitwise operations for state flags. + Specifically targets the mutation of & to | in flag checking operations. + Tests both setting and clearing of flags to catch potential bitwise operation mutations.""" + + # Test both MADS enabled and disabled states + for enable_mads in (True, False): + with self.subTest("enable_mads", mads_enabled=enable_mads): + self.safety.set_enable_mads(enable_mads, False) + self._mads_states_cleanup() + + # Initial state - both flags should be unset + self._rx(self._speed_msg(0)) + initial_flags = self.safety.get_mads_state_flags() + self.assertEqual(initial_flags & MadsStates.MAIN_BUTTON_AVAILABLE, MadsStates.DEFAULT) # Main button flag + self.assertEqual(initial_flags & MadsStates.LKAS_BUTTON_AVAILABLE, MadsStates.DEFAULT) # LKAS button flag + + # Set only main button + self.safety.set_main_button_press(0) + self._rx(self._speed_msg(0)) + main_only_flags = self.safety.get_mads_state_flags() + self.assertEqual(main_only_flags & MadsStates.MAIN_BUTTON_AVAILABLE, MadsStates.MAIN_BUTTON_AVAILABLE) # Main button flag should be set + self.assertEqual(main_only_flags & MadsStates.LKAS_BUTTON_AVAILABLE, MadsStates.DEFAULT) # LKAS button flag should still be unset + + # Set LKAS button and verify both flags + self.safety.set_lkas_button_press(0) + self._rx(self._speed_msg(0)) + both_flags = self.safety.get_mads_state_flags() + self.assertEqual(both_flags & MadsStates.MAIN_BUTTON_AVAILABLE, MadsStates.MAIN_BUTTON_AVAILABLE) # Main button flag should remain set + self.assertEqual(both_flags & MadsStates.LKAS_BUTTON_AVAILABLE, MadsStates.LKAS_BUTTON_AVAILABLE) # LKAS button flag should be set + + # Verify that using | instead of & would give different results + self.assertNotEqual(both_flags & MadsStates.MAIN_BUTTON_AVAILABLE, both_flags | MadsStates.MAIN_BUTTON_AVAILABLE) + self.assertNotEqual(both_flags & MadsStates.LKAS_BUTTON_AVAILABLE, both_flags | MadsStates.LKAS_BUTTON_AVAILABLE) + + # Reset flags and verify they're cleared + self._mads_states_cleanup() + self._rx(self._speed_msg(0)) + cleared_flags = self.safety.get_mads_state_flags() + self.assertEqual(cleared_flags & MadsStates.MAIN_BUTTON_AVAILABLE, MadsStates.DEFAULT) + self.assertEqual(cleared_flags & MadsStates.LKAS_BUTTON_AVAILABLE, MadsStates.DEFAULT) + + def test_mads_state_flags_persistence(self): + """Test to verify that state flags remain set once buttons are seen""" + + for enable_mads in (True, False): + with self.subTest("enable_mads", mads_enabled=enable_mads): + self.safety.set_enable_mads(enable_mads, False) + self._mads_states_cleanup() + + # Set main button and verify flag + self.safety.set_main_button_press(0) + self._rx(self._speed_msg(0)) + self.assertEqual(self.safety.get_mads_state_flags() & MadsStates.MAIN_BUTTON_AVAILABLE, MadsStates.MAIN_BUTTON_AVAILABLE) + + # Reset main button to -1, flag should persist + self.safety.set_main_button_press(-1) + self._rx(self._speed_msg(0)) + self.assertEqual(self.safety.get_mads_state_flags() & MadsStates.MAIN_BUTTON_AVAILABLE, MadsStates.MAIN_BUTTON_AVAILABLE) + + # Set LKAS button and verify both flags + self.safety.set_lkas_button_press(0) + self._rx(self._speed_msg(0)) + flags = self.safety.get_mads_state_flags() + self.assertEqual(flags & MadsStates.MAIN_BUTTON_AVAILABLE, MadsStates.MAIN_BUTTON_AVAILABLE) + self.assertEqual(flags & MadsStates.LKAS_BUTTON_AVAILABLE, MadsStates.LKAS_BUTTON_AVAILABLE) + + def test_mads_state_flags_individual_control(self): + """Test the ability to individually control state flags. + Verifies that flags can be set and cleared independently.""" + + for enable_mads in (True, False): + with self.subTest("enable_mads", mads_enabled=enable_mads): + self.safety.set_enable_mads(enable_mads, False) + self._mads_states_cleanup() + + # Set main button flag only + self.safety.set_main_button_press(0) + self._rx(self._speed_msg(0)) + main_flags = self.safety.get_mads_state_flags() + self.assertEqual(main_flags & MadsStates.MAIN_BUTTON_AVAILABLE, MadsStates.MAIN_BUTTON_AVAILABLE) + self.assertEqual(main_flags & MadsStates.LKAS_BUTTON_AVAILABLE, MadsStates.DEFAULT) + + # Reset flags and set LKAS only + self._mads_states_cleanup() + self.safety.set_lkas_button_press(0) + self._rx(self._speed_msg(0)) + lkas_flags = self.safety.get_mads_state_flags() + self.assertEqual(lkas_flags & MadsStates.MAIN_BUTTON_AVAILABLE, MadsStates.DEFAULT) + self.assertEqual(lkas_flags & MadsStates.LKAS_BUTTON_AVAILABLE, MadsStates.LKAS_BUTTON_AVAILABLE) + + # Set both flags + self._mads_states_cleanup() + self.safety.set_main_button_press(0) + self.safety.set_lkas_button_press(0) + self._rx(self._speed_msg(0)) + both_flags = self.safety.get_mads_state_flags() + self.assertEqual(both_flags & MadsStates.MAIN_BUTTON_AVAILABLE, MadsStates.MAIN_BUTTON_AVAILABLE) + self.assertEqual(both_flags & MadsStates.LKAS_BUTTON_AVAILABLE, MadsStates.LKAS_BUTTON_AVAILABLE) + + # Clear all flags and verify + self._mads_states_cleanup() + self._rx(self._speed_msg(0)) + final_flags = self.safety.get_mads_state_flags() + self.assertEqual(final_flags & MadsStates.MAIN_BUTTON_AVAILABLE, MadsStates.DEFAULT) + self.assertEqual(final_flags & MadsStates.LKAS_BUTTON_AVAILABLE, MadsStates.DEFAULT) diff --git a/tests/safety/test_chrysler.py b/tests/safety/test_chrysler.py index 5bbb6dd1..271df14a 100755 --- a/tests/safety/test_chrysler.py +++ b/tests/safety/test_chrysler.py @@ -72,6 +72,9 @@ class TestChryslerSafety(common.PandaCarSafetyTest, common.MotorTorqueSteeringSa self.assertFalse(self._tx(self._button_msg(cancel=True, resume=True))) self.assertFalse(self._tx(self._button_msg(cancel=False, resume=False))) + def test_enable_control_from_cruise_button_press(self): + raise unittest.SkipTest("Chrysler does not enable controls from cruise button press, we only enable controls from the PCM status message") + class TestChryslerRamDTSafety(TestChryslerSafety): TX_MSGS = [[0xB1, 2], [0xA6, 0], [0xFA, 0]] diff --git a/tests/safety/test_ford.py b/tests/safety/test_ford.py index a97e2643..acdd2e6e 100755 --- a/tests/safety/test_ford.py +++ b/tests/safety/test_ford.py @@ -168,6 +168,17 @@ class TestFordSafetyBase(common.PandaCarSafetyTest): } return self.packer.make_can_msg_panda("Lane_Assist_Data1", 0, values) + def test_enable_control_from_main(self): + for enable_mads in (True, False): + with self.subTest("enable_mads", mads_enabled=enable_mads): + self.safety.set_enable_mads(enable_mads, False) + 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._rx(self._pcm_status_msg(main_button_msg_valid)) + self.assertEqual(enable_mads and main_button_msg_valid, self.safety.get_controls_allowed_lat()) + self._mads_states_cleanup() + # LCA command def _lat_ctl_msg(self, enabled: bool, path_offset: float, path_angle: float, curvature: float, curvature_rate: float): if self.STEER_MESSAGE == MSG_LateralMotionControl: diff --git a/tests/safety/test_gm.py b/tests/safety/test_gm.py index c6c5ac6b..fa481f13 100755 --- a/tests/safety/test_gm.py +++ b/tests/safety/test_gm.py @@ -140,6 +140,9 @@ class TestGmSafetyBase(common.PandaCarSafetyTest, common.DriverTorqueSteeringSaf values = {"ACCButtons": buttons} return self.packer.make_can_msg_panda("ASCMSteeringButton", self.BUTTONS_BUS, values) + def test_enable_control_from_cruise_button_press(self): + raise unittest.SkipTest("GM does not enable controls from cruise button press, we only enable controls from the PCM status message") + class TestGmAscmSafety(GmLongitudinalBase, TestGmSafetyBase): TX_MSGS = [[0x180, 0], [0x409, 0], [0x40A, 0], [0x2CB, 0], [0x370, 0], # pt bus diff --git a/tests/safety/test_honda.py b/tests/safety/test_honda.py index 082199c0..ebca5597 100755 --- a/tests/safety/test_honda.py +++ b/tests/safety/test_honda.py @@ -250,6 +250,25 @@ class HondaBase(common.PandaCarSafetyTest): self.assertTrue(self._tx(self._send_steer_msg(0x0000))) self.assertFalse(self._tx(self._send_steer_msg(0x1000))) + def _lkas_button_msg(self, setting_btn): + values = {"CRUISE_SETTING": 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 _main_cruise_button_msg(self, enabled): + return self._button_msg(Btn.MAIN if enabled else Btn.NONE, main_on=enabled) + + def test_enable_control_from_lkas_button_press(self): + for enable_mads in (True, False): + with self.subTest("enable_mads", mads_enabled=enable_mads): + self.safety.set_enable_mads(enable_mads, False) + for lkas_button_press in range(3): + with self.subTest("lkas_button_press", button_state=lkas_button_press): + self._mads_states_cleanup() + self._rx(self._lkas_button_msg(lkas_button_press)) + self.assertEqual(enable_mads and lkas_button_press == 1, self.safety.get_controls_allowed_lat()) + self._mads_states_cleanup() + # ********************* Honda Nidec ********************** @@ -371,6 +390,11 @@ class TestHondaNidecPcmAltSafety(TestHondaNidecPcmSafety): self.__class__.cnt_button += 1 return self.packer.make_can_msg_panda("SCM_BUTTONS", bus, values) + def _lkas_button_msg(self, setting_btn): + values = {"CRUISE_SETTING": 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) + # ********************* Honda Bosch ********************** diff --git a/tests/safety/test_hyundai.py b/tests/safety/test_hyundai.py index e90c4c4f..ba57ff54 100755 --- a/tests/safety/test_hyundai.py +++ b/tests/safety/test_hyundai.py @@ -112,6 +112,33 @@ class TestHyundaiSafety(HyundaiButtonBase, common.PandaCarSafetyTest, common.Dri 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_acc_main_state_from_stock_scc_message(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,21 @@ class TestHyundaiSafetyCameraSCC(TestHyundaiSafety): self.safety.set_safety_hooks(Panda.SAFETY_HYUNDAI, Panda.FLAG_HYUNDAI_CAMERA_SCC) self.safety.init_tests() + def test_acc_main_state_from_stock_scc_message(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 + """ + prior_safety_mode = self.safety.get_current_safety_mode() + prior_safety_param = self.safety.get_current_safety_param() + + 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()) + # Restore original safety mode and param + self.safety.set_safety_hooks(prior_safety_mode, prior_safety_param) + class TestHyundaiLegacySafety(TestHyundaiSafety): def setUp(self): diff --git a/tests/safety/test_hyundai_canfd.py b/tests/safety/test_hyundai_canfd.py index 7f280b63..da164d3a 100755 --- a/tests/safety/test_hyundai_canfd.py +++ b/tests/safety/test_hyundai_canfd.py @@ -78,6 +78,28 @@ class TestHyundaiCanfdBase(HyundaiButtonBase, common.PandaCarSafetyTest, common. } 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) + + def test_enable_control_from_lkas(self): + for enable_mads in (True, False): + with self.subTest("enable_mads", mads_enabled=enable_mads): + self.safety.set_enable_mads(enable_mads, False) + for lkas_button_msg_valid in (True, False): + with self.subTest("main_button_msg_valid", state_valid=lkas_button_msg_valid): + self._mads_states_cleanup() + self._rx(self._lkas_button_msg(lkas_button_msg_valid)) + self.assertEqual(enable_mads and lkas_button_msg_valid, self.safety.get_controls_allowed_lat()) + self._mads_states_cleanup() + class TestHyundaiCanfdHDA1Base(TestHyundaiCanfdBase): @@ -146,6 +168,10 @@ class TestHyundaiCanfdHDA1AltButtons(TestHyundaiCanfdHDA1Base): } 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. diff --git a/tests/safety/test_mazda.py b/tests/safety/test_mazda.py index 9d2fb898..f46432ec 100755 --- a/tests/safety/test_mazda.py +++ b/tests/safety/test_mazda.py @@ -81,6 +81,9 @@ class TestMazdaSafety(common.PandaCarSafetyTest, common.DriverTorqueSteeringSafe self.assertTrue(self._tx(self._button_msg(cancel=True))) self.assertTrue(self._tx(self._button_msg(resume=True))) + def test_enable_control_from_cruise_button_press(self): + raise unittest.SkipTest("Mazda does not enable controls from cruise button press, we only enable controls from the PCM status message") + if __name__ == "__main__": unittest.main() diff --git a/tests/safety/test_nissan.py b/tests/safety/test_nissan.py index 4c83ca32..10af73a2 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 @@ class TestNissanSafety(common.PandaCarSafetyTest, common.AngleSteeringSafetyTest 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, @@ -78,12 +83,26 @@ class TestNissanSafety(common.PandaCarSafetyTest, common.AngleSteeringSafetyTest tx = self._tx(self._acc_button_cmd(**args)) self.assertEqual(tx, should_tx) + def test_enable_control_from_acc_main_on(self): + """Test that lateral controls are allowed when ACC main is enabled""" + for enable_mads in (True, False): + with self.subTest("enable_mads", mads_enabled=enable_mads): + self.safety.set_enable_mads(enable_mads, False) + for acc_main_on in (True, False): + with self.subTest("acc_main_on", acc_main_on=acc_main_on): + self._mads_states_cleanup() + self._rx(self._acc_state_msg(acc_main_on)) + self._rx(self._speed_msg(0)) + self.assertEqual(enable_mads and acc_main_on, self.safety.get_controls_allowed_lat()) + self._mads_states_cleanup() + class TestNissanSafetyAltEpsBus(TestNissanSafety): """Altima uses different buses""" EPS_BUS = 1 CRUISE_BUS = 1 + ACC_MAIN_BUS = 2 def setUp(self): self.packer = CANPackerPanda("nissan_x_trail_2017_generated") @@ -91,13 +110,17 @@ class TestNissanSafetyAltEpsBus(TestNissanSafety): 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 +131,10 @@ class TestNissanLeafSafety(TestNissanSafety): 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 7d07f79a..75a5d8bd 100755 --- a/tests/safety/test_subaru.py +++ b/tests/safety/test_subaru.py @@ -106,6 +106,21 @@ class TestSubaruSafetyBase(common.PandaCarSafetyTest): values = {"Cruise_Activated": enable} return self.packer.make_can_msg_panda("CruiseControl", self.ALT_MAIN_BUS, values) + def _lkas_button_msg(self, lkas_hud): + values = {"LKAS_Dash_State": lkas_hud} + return self.packer.make_can_msg_panda("ES_LKAS_State", SUBARU_CAM_BUS, values) + + def test_enable_control_from_lkas_button_press(self): + for enable_mads in (True, False): + with self.subTest("enable_mads", mads_enabled=enable_mads): + for lkas_hud in range(4): + self.safety.set_enable_mads(enable_mads, False) + self._mads_states_cleanup() + with self.subTest("lkas_hud", button_state=lkas_hud): + self._rx(self._lkas_button_msg(lkas_hud)) + self.assertEqual(enable_mads and lkas_hud 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 e60b29c5..7ab0ae5a 100755 --- a/tests/safety/test_toyota.py +++ b/tests/safety/test_toyota.py @@ -81,6 +81,10 @@ class TestToyotaSafetyBase(common.PandaCarSafetyTest, common.LongitudinalAccelSa values = {"CRUISE_ACTIVE": enable} return self.packer.make_can_msg_panda("PCM_CRUISE", 0, values) + def _lkas_button_msg(self, lkas_hud): + values = {"LKAS_STATUS": 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,18 @@ class TestToyotaSafetyBase(common.PandaCarSafetyTest, common.LongitudinalAccelSa self.assertFalse(self._rx(to_push)) self.assertFalse(self.safety.get_controls_allowed()) + def test_enable_control_from_lkas_button_press(self): + for enable_mads in (True, False): + with self.subTest("enable_mads", mads_enabled=enable_mads): + for lkas_hud in range(4): + self.safety.set_enable_mads(enable_mads, False) + self._mads_states_cleanup() + with self.subTest("lkas_hud", button_state=lkas_hud): + self._rx(self._lkas_button_msg(lkas_hud)) + 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 lkas_hud 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/test_volkswagen_mqb.py b/tests/safety/test_volkswagen_mqb.py index 276ee6c2..a1f418c9 100755 --- a/tests/safety/test_volkswagen_mqb.py +++ b/tests/safety/test_volkswagen_mqb.py @@ -134,6 +134,8 @@ class TestVolkswagenMqbSafety(common.PandaCarSafetyTest, common.DriverTorqueStee self.assertEqual(0, self.safety.get_torque_driver_max()) self.assertEqual(0, self.safety.get_torque_driver_min()) + def test_enable_control_from_cruise_button_press(self): + raise unittest.SkipTest("Volkswagen does not enable controls from cruise button press, we only enable controls from the PCM status message") class TestVolkswagenMqbStockSafety(TestVolkswagenMqbSafety): TX_MSGS = [[MSG_HCA_01, 0], [MSG_LDW_02, 0], [MSG_LH_EPS_03, 2], [MSG_GRA_ACC_01, 0], [MSG_GRA_ACC_01, 2]] diff --git a/tests/safety/test_volkswagen_pq.py b/tests/safety/test_volkswagen_pq.py index f2bc3178..8e793def 100755 --- a/tests/safety/test_volkswagen_pq.py +++ b/tests/safety/test_volkswagen_pq.py @@ -115,6 +115,9 @@ class TestVolkswagenPqSafety(common.PandaCarSafetyTest, common.DriverTorqueSteer self.assertEqual(0, self.safety.get_torque_driver_max()) self.assertEqual(0, self.safety.get_torque_driver_min()) + def test_enable_control_from_cruise_button_press(self): + raise unittest.SkipTest("Volkswagen does not enable controls from cruise button press, we only enable controls from the PCM status message") + class TestVolkswagenPqStockSafety(TestVolkswagenPqSafety): # Transmit of GRA_Neu is allowed on bus 0 and 2 to keep compatibility with gateway and camera integration diff --git a/tests/safety_replay/replay_drive.py b/tests/safety_replay/replay_drive.py index df3e0554..cbf9e000 100755 --- a/tests/safety_replay/replay_drive.py +++ b/tests/safety_replay/replay_drive.py @@ -13,6 +13,7 @@ def replay_drive(lr, safety_mode, param, alternative_experience, segment=False): err = safety.set_safety_hooks(safety_mode, param) assert err == 0, "invalid safety mode: %d" % safety_mode safety.set_alternative_experience(alternative_experience) + safety.set_enable_mads(bool(alternative_experience & 1024), not bool(alternative_experience & 2048)) if segment: init_segment(safety, lr, safety_mode, param) @@ -44,7 +45,11 @@ def replay_drive(lr, safety_mode, param, alternative_experience, segment=False): 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)) + print(f"blocked bus {canmsg.src} msg {hex(canmsg.address)} at {(msg.logMonoTime - start_t) / 1e9} | " + + f"lat [{safety.get_lat_active()}] | alwd [{safety.get_controls_allowed_lat()}] | " + + f"main_on [{safety.get_acc_main_on()}] | " + + f"flags [{safety.get_mads_state_flags()}] | " + + f"mads_main [{safety.get_mads_acc_main()}] ") tx_controls += safety.get_controls_allowed() tx_tot += 1 elif msg.which() == 'can': @@ -68,6 +73,7 @@ def replay_drive(lr, safety_mode, param, alternative_experience, segment=False): print("blocked msgs:", tx_blocked) print("blocked with controls allowed:", tx_controls_blocked) print("blocked addrs:", blocked_addrs) + print("mads enabled:", safety.get_enable_mads()) return tx_controls_blocked == 0 and rx_invalid == 0 and not safety_tick_rx_invalid