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