diff --git a/opendbc/safety/modes/body.h b/opendbc/safety/modes/body.h index e3d5c212ca0..5fe9d15a5da 100644 --- a/opendbc/safety/modes/body.h +++ b/opendbc/safety/modes/body.h @@ -3,9 +3,8 @@ #include "opendbc/safety/declarations.h" static void body_rx_hook(const CANPacket_t *msg) { - if (msg->addr == 0x201U) { - controls_allowed = true; - } + SAFETY_UNUSED(msg); + controls_allowed = true; } static bool body_tx_hook(const CANPacket_t *msg) { diff --git a/opendbc/safety/modes/chrysler.h b/opendbc/safety/modes/chrysler.h index 8c2aa0742fb..9572efbbd80 100644 --- a/opendbc/safety/modes/chrysler.h +++ b/opendbc/safety/modes/chrysler.h @@ -67,7 +67,7 @@ static void chrysler_rx_hook(const CANPacket_t *msg) { if ((chrysler_platform != CHRYSLER_PACIFICA) && (msg->bus == 0U) && (msg->addr == CHRYSLER_ADDR(ESP_8))) { vehicle_moving = ((msg->data[4] << 8) + msg->data[5]) != 0U; } - if ((chrysler_platform == CHRYSLER_PACIFICA) && (msg->bus == 0U) && (msg->addr == 514U)) { + if ((chrysler_platform == CHRYSLER_PACIFICA) && (msg->addr == 514U)) { int speed_l = (msg->data[0] << 4) + (msg->data[1] >> 4); int speed_r = (msg->data[2] << 4) + (msg->data[3] >> 4); vehicle_moving = (speed_l != 0) || (speed_r != 0); diff --git a/opendbc/safety/modes/chrysler_cusw.h b/opendbc/safety/modes/chrysler_cusw.h index a9fb94601e1..96283ea13a4 100644 --- a/opendbc/safety/modes/chrysler_cusw.h +++ b/opendbc/safety/modes/chrysler_cusw.h @@ -22,33 +22,32 @@ static safety_config chrysler_cusw_init(uint16_t param) { } static void chrysler_cusw_rx_hook(const CANPacket_t *msg) { - if (msg->bus == 0U) { - if (msg->addr == 0x1ECU) { - // Signal: EPS_STATUS.TORQUE_MOTOR - int torque_meas_new = ((msg->data[3] & 0xFU) << 8) + msg->data[4] - 2048U; - update_sample(&torque_meas, torque_meas_new); - } + // The RX checks already validate the address, bus, and length. + if (msg->addr == 0x1ECU) { + // Signal: EPS_STATUS.TORQUE_MOTOR + int torque_meas_new = ((msg->data[3] & 0xFU) << 8) + msg->data[4] - 2048U; + update_sample(&torque_meas, torque_meas_new); + } - if (msg->addr == 0x2ECU) { - // Signal: ACC_CONTROL.ACC_ACTIVE - bool cruise_engaged = GET_BIT(msg, 7U); - pcm_cruise_check(cruise_engaged); - } + if (msg->addr == 0x2ECU) { + // Signal: ACC_CONTROL.ACC_ACTIVE + bool cruise_engaged = GET_BIT(msg, 7U); + pcm_cruise_check(cruise_engaged); + } - if (msg->addr == 0x1E4U) { - // Signal: BRAKE_1.VEHICLE_SPEED - vehicle_moving = (((msg->data[4] & 0x7U) << 8) + msg->data[5]) != 0U; - } + if (msg->addr == 0x1E4U) { + // Signal: BRAKE_1.VEHICLE_SPEED + vehicle_moving = (((msg->data[4] & 0x7U) << 8) + msg->data[5]) != 0U; + } - if (msg->addr == 0x1FEU) { - // Signal: ACCEL_GAS.GAS_HUMAN - gas_pressed = msg->data[1] != 0U; - } + if (msg->addr == 0x1FEU) { + // Signal: ACCEL_GAS.GAS_HUMAN + gas_pressed = msg->data[1] != 0U; + } - if (msg->addr == 0x1E8U) { - // Signal: BRAKE_3.DRIVER_BRAKE_SWITCH - brake_pressed = GET_BIT(msg, 18U); - } + if (msg->addr == 0x1E8U) { + // Signal: BRAKE_3.DRIVER_BRAKE_SWITCH + brake_pressed = GET_BIT(msg, 18U); } } diff --git a/opendbc/safety/modes/ford.h b/opendbc/safety/modes/ford.h index bfcb5866404..7f8bb11dad9 100644 --- a/opendbc/safety/modes/ford.h +++ b/opendbc/safety/modes/ford.h @@ -26,10 +26,9 @@ static uint8_t ford_get_counter(const CANPacket_t *msg) { if (msg->addr == FORD_BrakeSysFeatures) { // Signal: VehVActlBrk_No_Cnt cnt = (msg->data[2] >> 2) & 0xFU; - } else if (msg->addr == FORD_Yaw_Data_FD1) { + } else { // msg->addr == FORD_Yaw_Data_FD1 // Signal: VehRollYaw_No_Cnt cnt = msg->data[5]; - } else { } return cnt; } @@ -39,10 +38,9 @@ static uint32_t ford_get_checksum(const CANPacket_t *msg) { if (msg->addr == FORD_BrakeSysFeatures) { // Signal: VehVActlBrk_No_Cs chksum = msg->data[3]; - } else if (msg->addr == FORD_Yaw_Data_FD1) { + } else { // msg->addr == FORD_Yaw_Data_FD1 // Signal: VehRollYawW_No_Cs chksum = msg->data[4]; - } else { } return chksum; } @@ -54,14 +52,13 @@ static uint32_t ford_compute_checksum(const CANPacket_t *msg) { chksum += msg->data[2] >> 6; // VehVActlBrk_D_Qf chksum += (msg->data[2] >> 2) & 0xFU; // VehVActlBrk_No_Cnt chksum = 0xFFU - chksum; - } else if (msg->addr == FORD_Yaw_Data_FD1) { + } else { // msg->addr == FORD_Yaw_Data_FD1 chksum += msg->data[0] + msg->data[1]; // VehRol_W_Actl chksum += msg->data[2] + msg->data[3]; // VehYaw_W_Actl chksum += msg->data[5]; // VehRollYaw_No_Cnt chksum += msg->data[6] >> 6; // VehRolWActl_D_Qf chksum += (msg->data[6] >> 4) & 0x3U; // VehYawWActl_D_Qf chksum = 0xFFU - chksum; - } else { } return chksum; } @@ -72,9 +69,8 @@ static bool ford_get_quality_flag_valid(const CANPacket_t *msg) { valid = (msg->data[2] >> 6) == 0x3U; // VehVActlBrk_D_Qf } else if (msg->addr == FORD_EngVehicleSpThrottle2) { valid = ((msg->data[4] >> 5) & 0x3U) == 0x3U; // VehVActlEng_D_Qf - } else if (msg->addr == FORD_Yaw_Data_FD1) { + } else { // msg->addr == FORD_Yaw_Data_FD1 valid = ((msg->data[6] >> 4) & 0x3U) == 0x3U; // VehYawWActl_D_Qf - } else { } return valid; } @@ -96,55 +92,54 @@ static const CurvatureSteeringLimits FORD_STEERING_LIMITS = { }; static void ford_rx_hook(const CANPacket_t *msg) { - if (msg->bus == FORD_MAIN_BUS) { - // Update in motion state from standstill signal - if (msg->addr == FORD_DesiredTorqBrk) { - // Signal: VehStop_D_Stat - vehicle_moving = ((msg->data[3] >> 3) & 0x3U) != 1U; - } + // The RX checks already validate the address, bus, and length. + // Update in motion state from standstill signal + if (msg->addr == FORD_DesiredTorqBrk) { + // Signal: VehStop_D_Stat + vehicle_moving = ((msg->data[3] >> 3) & 0x3U) != 1U; + } - // Update vehicle speed - if (msg->addr == FORD_BrakeSysFeatures) { - // Signal: Veh_V_ActlBrk - UPDATE_VEHICLE_SPEED(((msg->data[0] << 8) | msg->data[1]) * 0.01 * KPH_TO_MS); - } + // Update vehicle speed + if (msg->addr == FORD_BrakeSysFeatures) { + // Signal: Veh_V_ActlBrk + UPDATE_VEHICLE_SPEED(((msg->data[0] << 8) | msg->data[1]) * 0.01 * KPH_TO_MS); + } - // Check vehicle speed against a second source - if (msg->addr == FORD_EngVehicleSpThrottle2) { - // Disable controls if speeds from ABS and PCM ECUs are too far apart. - // Signal: Veh_V_ActlEng - float filtered_pcm_speed = ((msg->data[6] << 8) | msg->data[7]) * 0.01 * KPH_TO_MS; - UPDATE_VEHICLE_SPEED_2(filtered_pcm_speed); - } + // Check vehicle speed against a second source + if (msg->addr == FORD_EngVehicleSpThrottle2) { + // Disable controls if speeds from ABS and PCM ECUs are too far apart. + // Signal: Veh_V_ActlEng + float filtered_pcm_speed = ((msg->data[6] << 8) | msg->data[7]) * 0.01 * KPH_TO_MS; + UPDATE_VEHICLE_SPEED_2(filtered_pcm_speed); + } - // Update vehicle yaw rate - if (msg->addr == FORD_Yaw_Data_FD1) { - // FIXME: safety can receive yaw before new vehicle speed, it should recompute meas on either received - // Signal: VehYaw_W_Actl - // TODO: we should use the speed which results in the closest angle measurement to the desired angle - float ford_yaw_rate = (((msg->data[2] << 8U) | msg->data[3]) * 0.0002) - 6.5; - float current_curvature = ford_yaw_rate / SAFETY_MAX(vehicle_speed.values[0] / VEHICLE_SPEED_FACTOR, 0.1); - // convert current curvature into units on CAN for comparison with desired curvature - update_sample(&curvature_state.meas, ROUND(current_curvature * FORD_STEERING_LIMITS.curvature_to_can)); - } + // Update vehicle yaw rate + if (msg->addr == FORD_Yaw_Data_FD1) { + // FIXME: safety can receive yaw before new vehicle speed, it should recompute meas on either received + // Signal: VehYaw_W_Actl + // TODO: we should use the speed which results in the closest angle measurement to the desired angle + float ford_yaw_rate = (((msg->data[2] << 8U) | msg->data[3]) * 0.0002) - 6.5; + float current_curvature = ford_yaw_rate / SAFETY_MAX(vehicle_speed.values[0] / VEHICLE_SPEED_FACTOR, 0.1); + // convert current curvature into units on CAN for comparison with desired curvature + update_sample(&curvature_state.meas, ROUND(current_curvature * FORD_STEERING_LIMITS.curvature_to_can)); + } - // Update gas pedal - if (msg->addr == FORD_EngVehicleSpThrottle) { - // Pedal position: (0.1 * val) in percent - // Signal: ApedPos_Pc_ActlArb - gas_pressed = (((msg->data[0] & 0x03U) << 8) | msg->data[1]) > 0U; - } + // Update gas pedal + if (msg->addr == FORD_EngVehicleSpThrottle) { + // Pedal position: (0.1 * val) in percent + // Signal: ApedPos_Pc_ActlArb + gas_pressed = (((msg->data[0] & 0x03U) << 8) | msg->data[1]) > 0U; + } - // Update brake pedal and cruise state - if (msg->addr == FORD_EngBrakeData) { - // Signal: BpedDrvAppl_D_Actl - brake_pressed = ((msg->data[0] >> 4) & 0x3U) == 2U; + // Update brake pedal and cruise state + if (msg->addr == FORD_EngBrakeData) { + // Signal: BpedDrvAppl_D_Actl + brake_pressed = ((msg->data[0] >> 4) & 0x3U) == 2U; - // Signal: CcStat_D_Actl - unsigned int cruise_state = msg->data[1] & 0x07U; - bool cruise_engaged = (cruise_state == 4U) || (cruise_state == 5U); - pcm_cruise_check(cruise_engaged); - } + // Signal: CcStat_D_Actl + unsigned int cruise_state = msg->data[1] & 0x07U; + bool cruise_engaged = (cruise_state == 4U) || (cruise_state == 5U); + pcm_cruise_check(cruise_engaged); } } diff --git a/opendbc/safety/modes/gm.h b/opendbc/safety/modes/gm.h index 2cf806f2614..64a6c514bdc 100644 --- a/opendbc/safety/modes/gm.h +++ b/opendbc/safety/modes/gm.h @@ -32,64 +32,63 @@ static bool gm_pcm_cruise = false; static void gm_rx_hook(const CANPacket_t *msg) { const int GM_STANDSTILL_THRSLD = 10; // 0.311kph - if (msg->bus == 0U) { - if (msg->addr == 0x184U) { - int torque_driver_new = ((msg->data[6] & 0x7U) << 8) | msg->data[7]; - torque_driver_new = to_signed(torque_driver_new, 11); - // update array of samples - update_sample(&torque_driver, torque_driver_new); - } - - // sample rear wheel speeds - if (msg->addr == 0x34AU) { - int left_rear_speed = (msg->data[0] << 8) | msg->data[1]; - int right_rear_speed = (msg->data[2] << 8) | msg->data[3]; - vehicle_moving = (left_rear_speed > GM_STANDSTILL_THRSLD) || (right_rear_speed > GM_STANDSTILL_THRSLD); - } - - // ACC steering wheel buttons (GM_CAM is tied to the PCM) - if ((msg->addr == 0x1E1U) && !gm_pcm_cruise) { - int button = (msg->data[5] & 0x70U) >> 4; + // The RX checks already validate the address, bus, and length. + if (msg->addr == 0x184U) { + int torque_driver_new = ((msg->data[6] & 0x7U) << 8) | msg->data[7]; + torque_driver_new = to_signed(torque_driver_new, 11); + // update array of samples + update_sample(&torque_driver, torque_driver_new); + } - // enter controls on falling edge of set or rising edge of resume (avoids fault) - bool set = (button != GM_BTN_SET) && (cruise_button_prev == GM_BTN_SET); - bool res = (button == GM_BTN_RESUME) && (cruise_button_prev != GM_BTN_RESUME); - if (set || res) { - controls_allowed = true; - } + // sample rear wheel speeds + if (msg->addr == 0x34AU) { + int left_rear_speed = (msg->data[0] << 8) | msg->data[1]; + int right_rear_speed = (msg->data[2] << 8) | msg->data[3]; + vehicle_moving = (left_rear_speed > GM_STANDSTILL_THRSLD) || (right_rear_speed > GM_STANDSTILL_THRSLD); + } - // exit controls on cancel press - if (button == GM_BTN_CANCEL) { - controls_allowed = false; - } + // ACC steering wheel buttons (GM_CAM is tied to the PCM) + if ((msg->addr == 0x1E1U) && !gm_pcm_cruise) { + int button = (msg->data[5] & 0x70U) >> 4; - cruise_button_prev = button; + // enter controls on falling edge of set or rising edge of resume (avoids fault) + bool set = (button != GM_BTN_SET) && (cruise_button_prev == GM_BTN_SET); + bool res = (button == GM_BTN_RESUME) && (cruise_button_prev != GM_BTN_RESUME); + if (set || res) { + controls_allowed = true; } - // Reference for brake pressed signals: - // https://github.com/commaai/openpilot/blob/master/selfdrive/car/gm/carstate.py - if ((msg->addr == 0xBEU) && (gm_hw == GM_ASCM)) { - brake_pressed = msg->data[1] >= 8U; + // exit controls on cancel press + if (button == GM_BTN_CANCEL) { + controls_allowed = false; } - if ((msg->addr == 0xC9U) && (gm_hw == GM_CAM)) { - brake_pressed = GET_BIT(msg, 40U); - } + cruise_button_prev = button; + } - if (msg->addr == 0x1C4U) { - gas_pressed = msg->data[5] != 0U; + // Reference for brake pressed signals: + // https://github.com/commaai/openpilot/blob/master/selfdrive/car/gm/carstate.py + if ((msg->addr == 0xBEU) && (gm_hw == GM_ASCM)) { + brake_pressed = msg->data[1] >= 8U; + } - // enter controls on rising edge of ACC, exit controls when ACC off - if (gm_pcm_cruise) { - bool cruise_engaged = (msg->data[1] >> 5) != 0U; - pcm_cruise_check(cruise_engaged); - } - } + if ((msg->addr == 0xC9U) && (gm_hw == GM_CAM)) { + brake_pressed = GET_BIT(msg, 40U); + } - if (msg->addr == 0xBDU) { - regen_braking = (msg->data[0] >> 4) != 0U; + if (msg->addr == 0x1C4U) { + gas_pressed = msg->data[5] != 0U; + + // enter controls on rising edge of ACC, exit controls when ACC off + if (gm_pcm_cruise) { + bool cruise_engaged = (msg->data[1] >> 5) != 0U; + pcm_cruise_check(cruise_engaged); } } + + if (msg->addr == 0xBDU) { + regen_braking = (msg->data[0] >> 4) != 0U; + } } static bool gm_tx_hook(const CANPacket_t *msg) { @@ -143,7 +142,7 @@ static bool gm_tx_hook(const CANPacket_t *msg) { } // BUTTONS: used for resume spamming and cruise cancellation with stock longitudinal - if ((msg->addr == 0x1E1U) && gm_pcm_cruise) { + if (msg->addr == 0x1E1U) { int button = (msg->data[5] >> 4) & 0x7U; bool allowed_cancel = (button == 6) && cruise_engaged_prev; diff --git a/opendbc/safety/modes/honda.h b/opendbc/safety/modes/honda.h index c5d2498e55e..bbfc524917f 100644 --- a/opendbc/safety/modes/honda.h +++ b/opendbc/safety/modes/honda.h @@ -36,10 +36,6 @@ typedef enum {HONDA_NIDEC, HONDA_BOSCH} HondaHw; static HondaHw honda_hw = HONDA_NIDEC; -static unsigned int honda_get_pt_bus(void) { - return ((honda_hw == HONDA_BOSCH) && !honda_bosch_radarless && !honda_bosch_canfd) ? 1U : 0U; -} - static uint32_t honda_get_checksum(const CANPacket_t *msg) { int checksum_byte = GET_LEN(msg) - 1U; return (uint8_t)(msg->data[checksum_byte]) & 0xFU; @@ -69,7 +65,6 @@ static uint8_t honda_get_counter(const CANPacket_t *msg) { static void honda_rx_hook(const CANPacket_t *msg) { const bool pcm_cruise = ((honda_hw == HONDA_BOSCH) && !honda_bosch_long) || (honda_hw == HONDA_NIDEC); - unsigned int pt_bus = honda_get_pt_bus(); // sample speed if (msg->addr == 0x158U) { @@ -103,7 +98,7 @@ static void honda_rx_hook(const CANPacket_t *msg) { // state machine to enter and exit controls for button enabling // 0x1A6 for the ILX, 0x296 for the Civic Touring - if (((msg->addr == 0x1A6U) || (msg->addr == 0x296U)) && (msg->bus == pt_bus)) { + if ((msg->addr == 0x1A6U) || (msg->addr == 0x296U)) { int button = (msg->data[0] & 0xE0U) >> 5; // enter controls on the falling edge of set or resume @@ -145,7 +140,7 @@ static void honda_rx_hook(const CANPacket_t *msg) { // disable stock Honda AEB in alternative experience if (!(alternative_experience & ALT_EXP_DISABLE_STOCK_AEB)) { - if ((msg->bus == 2U) && (msg->addr == 0x1FAU)) { + if (msg->addr == 0x1FAU) { bool honda_stock_aeb = GET_BIT(msg, 29U); int honda_stock_brake = (msg->data[0] << 2) | (msg->data[1] >> 6); @@ -181,11 +176,8 @@ static bool honda_tx_hook(const CANPacket_t *msg) { bool tx = true; - unsigned int bus_pt = honda_get_pt_bus(); - unsigned int bus_buttons = (honda_bosch_radarless) ? 2U : bus_pt; // the camera controls ACC on radarless Bosch cars - // ACC_HUD: safety check (nidec w/o pedal) - if ((msg->addr == 0x30CU) && (msg->bus == bus_pt)) { + if (msg->addr == 0x30CU) { int pcm_speed = (msg->data[0] << 8) | msg->data[1]; int pcm_gas = msg->data[2]; @@ -198,7 +190,7 @@ static bool honda_tx_hook(const CANPacket_t *msg) { } // BRAKE: safety check (nidec) - if ((msg->addr == 0x1FAU) && (msg->bus == bus_pt)) { + if (msg->addr == 0x1FAU) { honda_brake = (msg->data[0] << 2) + ((msg->data[1] >> 6) & 0x3U); if (longitudinal_brake_checks(honda_brake, HONDA_NIDEC_LONG_LIMITS)) { tx = false; @@ -209,7 +201,7 @@ static bool honda_tx_hook(const CANPacket_t *msg) { } // BRAKE/GAS: safety check (bosch) - if ((msg->addr == 0x1DFU) && (msg->bus == bus_pt)) { + if (msg->addr == 0x1DFU) { int accel = (msg->data[3] << 3) | ((msg->data[4] >> 5) & 0x7U); accel = to_signed(accel, 11); @@ -225,7 +217,7 @@ static bool honda_tx_hook(const CANPacket_t *msg) { } // ACCEL: safety check (radarless) - if ((msg->addr == 0x1C8U) && (msg->bus == bus_pt)) { + if (msg->addr == 0x1C8U) { int accel = (msg->data[0] << 4) | (msg->data[1] >> 4); accel = to_signed(accel, 12); @@ -256,7 +248,7 @@ static bool honda_tx_hook(const CANPacket_t *msg) { // FORCE CANCEL: safety check only relevant when spamming the cancel button in Bosch HW // ensuring that only the cancel button press is sent (VAL 2) when controls are off. // This avoids unintended engagements while still allowing resume spam - if ((msg->addr == 0x296U) && !controls_allowed && (msg->bus == bus_buttons)) { + if ((msg->addr == 0x296U) && !controls_allowed) { if (((msg->data[0] >> 5) & 0x7U) != 2U) { tx = false; } diff --git a/opendbc/safety/modes/hyundai.h b/opendbc/safety/modes/hyundai.h index b867f1850a0..3d919868ab8 100644 --- a/opendbc/safety/modes/hyundai.h +++ b/opendbc/safety/modes/hyundai.h @@ -69,9 +69,8 @@ static uint8_t hyundai_get_counter(const CANPacket_t *msg) { cnt = (msg->data[1] >> 5) & 0x7U; } else if (msg->addr == 0x421U) { cnt = msg->data[7] & 0xFU; - } else if (msg->addr == 0x4F1U) { + } else { // msg->addr == 0x4F1U cnt = (msg->data[3] >> 4) & 0xFU; - } else { } return cnt; } @@ -85,9 +84,8 @@ static uint32_t hyundai_get_checksum(const CANPacket_t *msg) { chksum = ((msg->data[7] >> 6) << 2) | (msg->data[5] >> 6); } else if (msg->addr == 0x394U) { chksum = msg->data[6] & 0xFU; - } else if (msg->addr == 0x421U) { + } else { // SCC12: the remaining checksum-checked message chksum = msg->data[7] >> 4; - } else { } return chksum; } @@ -101,7 +99,7 @@ static uint32_t hyundai_compute_checksum(const CANPacket_t *msg) { for (int j = 0; j < 8; j++) { uint8_t bit = 0; // exclude checksum and counter - if (((i != 1) || (j < 6)) && ((i != 3) || (j < 6)) && ((i != 5) || (j < 6)) && ((i != 7) || (j < 6))) { + if (((i % 2) == 0) || (j < 6)) { bit = (b >> (uint8_t)j) & 1U; } chksum += bit; @@ -130,11 +128,9 @@ static void hyundai_rx_hook(const CANPacket_t *msg) { // SCC12 is on bus 2 for camera-based SCC cars, bus 0 on all others if (msg->addr == 0x421U) { - if (((msg->bus == 0U) && !hyundai_camera_scc) || ((msg->bus == 2U) && hyundai_camera_scc)) { - // 2 bits: 13-14 - int cruise_engaged = (GET_BYTES(msg, 0, 4) >> 13) & 0x3U; - hyundai_common_cruise_state_check(cruise_engaged); - } + // 2 bits: 13-14 + int cruise_engaged = (GET_BYTES(msg, 0, 4) >> 13) & 0x3U; + hyundai_common_cruise_state_check(cruise_engaged); } if (msg->bus == 0U) { @@ -156,7 +152,7 @@ static void hyundai_rx_hook(const CANPacket_t *msg) { gas_pressed = (((msg->data[4] & 0x7FU) << 1) | (msg->data[3] >> 7)) != 0U; } else if ((msg->addr == 0x371U) && hyundai_hybrid_gas_signal) { gas_pressed = msg->data[7] != 0U; - } else if ((msg->addr == 0x91U) && hyundai_fcev_gas_signal) { + } else if (msg->addr == 0x91U) { gas_pressed = msg->data[6] != 0U; } else if ((msg->addr == 0x260U) && !hyundai_ev_gas_signal && !hyundai_hybrid_gas_signal) { gas_pressed = (msg->data[7] >> 6) != 0U; diff --git a/opendbc/safety/modes/hyundai_canfd.h b/opendbc/safety/modes/hyundai_canfd.h index bf08fb173bb..560335e4b98 100644 --- a/opendbc/safety/modes/hyundai_canfd.h +++ b/opendbc/safety/modes/hyundai_canfd.h @@ -126,7 +126,7 @@ static void hyundai_canfd_rx_hook(const CANPacket_t *msg) { if (msg->bus == scc_bus) { // cruise state - if ((msg->addr == 0x1a0U) && !hyundai_longitudinal) { + if (msg->addr == 0x1a0U) { // 1=enabled, 2=driver override int cruise_status = ((msg->data[8] >> 4) & 0x7U); bool cruise_engaged = (cruise_status == 1) || (cruise_status == 2); @@ -179,7 +179,7 @@ static bool hyundai_canfd_tx_hook(const CANPacket_t *msg) { } // UDS: only tester present ("\x02\x3E\x80\x00\x00\x00\x00\x00") allowed on diagnostics address - if (((msg->addr == 0x730U) && hyundai_canfd_lka_steer_msg) || ((msg->addr == 0x7D0U) && !hyundai_camera_scc)) { + if ((msg->addr == 0x730U) || (msg->addr == 0x7D0U)) { if ((GET_BYTES(msg, 0, 4) != 0x00803E02U) || (GET_BYTES(msg, 4, 4) != 0x0U)) { tx = false; } diff --git a/opendbc/safety/modes/hyundai_common.h b/opendbc/safety/modes/hyundai_common.h index 6797ae74c44..375cad49d55 100644 --- a/opendbc/safety/modes/hyundai_common.h +++ b/opendbc/safety/modes/hyundai_common.h @@ -76,16 +76,14 @@ void hyundai_common_cruise_state_check(const bool cruise_engaged) { // so keep track of user button presses to deny engagement if no interaction // enter controls on rising edge of ACC and recent user button press, exit controls when ACC off - if (!hyundai_longitudinal) { - if (cruise_engaged && !cruise_engaged_prev && (hyundai_last_button_interaction < HYUNDAI_PREV_BUTTON_SAMPLES)) { - controls_allowed = true; - } + if (cruise_engaged && !cruise_engaged_prev && (hyundai_last_button_interaction < HYUNDAI_PREV_BUTTON_SAMPLES)) { + controls_allowed = true; + } - if (!cruise_engaged) { - controls_allowed = false; - } - cruise_engaged_prev = cruise_engaged; + if (!cruise_engaged) { + controls_allowed = false; } + cruise_engaged_prev = cruise_engaged; } void hyundai_common_cruise_buttons_check(const int cruise_button, const bool main_button) { @@ -128,10 +126,8 @@ uint32_t hyundai_common_canfd_compute_checksum(const CANPacket_t *msg) { if (len == 24) { crc ^= 0x819dU; - } else if (len == 32) { + } else { // 32-byte checksum-checked messages crc ^= 0x9f5bU; - } else { - } return crc; diff --git a/opendbc/safety/modes/mazda.h b/opendbc/safety/modes/mazda.h index f95cfaf878f..fb6df2683b9 100644 --- a/opendbc/safety/modes/mazda.h +++ b/opendbc/safety/modes/mazda.h @@ -17,32 +17,31 @@ // track msgs coming from OP so that we know what CAM msgs to drop and what to forward static void mazda_rx_hook(const CANPacket_t *msg) { - if ((int)msg->bus == MAZDA_MAIN) { - if (msg->addr == MAZDA_ENGINE_DATA) { - // sample speed: scale by 0.01 to get kph - int speed = (msg->data[2] << 8) | msg->data[3]; - vehicle_moving = speed > 10; // moving when speed > 0.1 kph - } + // The RX checks already validate the address, bus, and length. + if (msg->addr == MAZDA_ENGINE_DATA) { + // sample speed: scale by 0.01 to get kph + int speed = (msg->data[2] << 8) | msg->data[3]; + vehicle_moving = speed > 10; // moving when speed > 0.1 kph + } - if (msg->addr == MAZDA_STEER_TORQUE) { - int torque_driver_new = msg->data[0] - 127U; - // update array of samples - update_sample(&torque_driver, torque_driver_new); - } + if (msg->addr == MAZDA_STEER_TORQUE) { + int torque_driver_new = msg->data[0] - 127U; + // update array of samples + update_sample(&torque_driver, torque_driver_new); + } - // enter controls on rising edge of ACC, exit controls on ACC off - if (msg->addr == MAZDA_CRZ_CTRL) { - bool cruise_engaged = msg->data[0] & 0x8U; - pcm_cruise_check(cruise_engaged); - } + // enter controls on rising edge of ACC, exit controls on ACC off + if (msg->addr == MAZDA_CRZ_CTRL) { + bool cruise_engaged = msg->data[0] & 0x8U; + pcm_cruise_check(cruise_engaged); + } - if (msg->addr == MAZDA_ENGINE_DATA) { - gas_pressed = (msg->data[4] || (msg->data[5] & 0xF0U)); - } + if (msg->addr == MAZDA_ENGINE_DATA) { + gas_pressed = (msg->data[4] || (msg->data[5] & 0xF0U)); + } - if (msg->addr == MAZDA_PEDALS) { - brake_pressed = (msg->data[0] & 0x10U); - } + if (msg->addr == MAZDA_PEDALS) { + brake_pressed = (msg->data[0] & 0x10U); } } @@ -58,25 +57,22 @@ static bool mazda_tx_hook(const CANPacket_t *msg) { }; bool tx = true; - // Check if msg is sent on the main BUS - if (msg->bus == (unsigned char)MAZDA_MAIN) { - // steer cmd checks - if (msg->addr == MAZDA_LKAS) { - int desired_torque = (((msg->data[0] & 0x0FU) << 8) | msg->data[1]) - 2048U; - - if (steer_torque_cmd_checks(desired_torque, -1, MAZDA_STEERING_LIMITS)) { - tx = false; - } + // steer cmd checks + if (msg->addr == MAZDA_LKAS) { + int desired_torque = (((msg->data[0] & 0x0FU) << 8) | msg->data[1]) - 2048U; + + if (steer_torque_cmd_checks(desired_torque, -1, MAZDA_STEERING_LIMITS)) { + tx = false; } + } - // cruise buttons check - if (msg->addr == MAZDA_CRZ_BTNS) { - // allow resume spamming while controls allowed, but - // only allow cancel while controls not allowed - bool cancel_cmd = (msg->data[0] == 0x1U); - if (!controls_allowed && !cancel_cmd) { - tx = false; - } + // cruise buttons check + if (msg->addr == MAZDA_CRZ_BTNS) { + // allow resume spamming while controls allowed, but + // only allow cancel while controls not allowed + bool cancel_cmd = (msg->data[0] == 0x1U); + if (!controls_allowed && !cancel_cmd) { + tx = false; } } diff --git a/opendbc/safety/modes/mg.h b/opendbc/safety/modes/mg.h index a41bfb7a088..0f0c7b02c40 100644 --- a/opendbc/safety/modes/mg.h +++ b/opendbc/safety/modes/mg.h @@ -25,46 +25,43 @@ static uint8_t mg_get_counter(const CANPacket_t *msg) { counter = (msg->data[0] >> 4) & 0xFU; } else if (msg->addr == 0x242U) { counter = (msg->data[0] >> 3) & 0xFU; - } else if (msg->addr == 0x1b6U) { + } else { // EHBS_HSC2_FrP00: the remaining counter-checked message counter = msg->data[6] & 0xFU; - } else { - // No counter for this message } return counter; } static void mg_rx_hook(const CANPacket_t *msg) { - if (msg->bus == 0U) { - // Vehicle speed - if (msg->addr == 0x23cU) { - float speed = (((msg->data[2] & 0x7FU) << 8) | msg->data[3]) * 0.015625; - vehicle_moving = speed > 0.0; - UPDATE_VEHICLE_SPEED(speed * KPH_TO_MS); - } - - // Gas pressed - if (msg->addr == 0xafU) { - gas_pressed = msg->data[0] != 0U; - } - - // Driver torque - if (msg->addr == 0x1ecU) { - int torque_driver_new = (((msg->data[4] & 0x7U) << 8) | msg->data[5]) - 1024U; - update_sample(&torque_driver, torque_driver_new); - } - - // Brake pressed - if (msg->addr == 0x1b6U) { - brake_pressed = GET_BIT(msg, 10U); - } - - // Cruise state - if (msg->addr == 0x242U) { - int cruise_state = (msg->data[5] & 0x38U) >> 3; - bool cruise_engaged = (cruise_state == 2) || // Active - (cruise_state == 3); // Override - pcm_cruise_check(cruise_engaged); - } + // The RX checks already validate the address, bus, and length. + // Vehicle speed + if (msg->addr == 0x23cU) { + float speed = (((msg->data[2] & 0x7FU) << 8) | msg->data[3]) * 0.015625; + vehicle_moving = speed > 0.0; + UPDATE_VEHICLE_SPEED(speed * KPH_TO_MS); + } + + // Gas pressed + if (msg->addr == 0xafU) { + gas_pressed = msg->data[0] != 0U; + } + + // Driver torque + if (msg->addr == 0x1ecU) { + int torque_driver_new = (((msg->data[4] & 0x7U) << 8) | msg->data[5]) - 1024U; + update_sample(&torque_driver, torque_driver_new); + } + + // Brake pressed + if (msg->addr == 0x1b6U) { + brake_pressed = GET_BIT(msg, 10U); + } + + // Cruise state + if (msg->addr == 0x242U) { + int cruise_state = (msg->data[5] & 0x38U) >> 3; + bool cruise_engaged = (cruise_state == 2) || // Active + (cruise_state == 3); // Override + pcm_cruise_check(cruise_engaged); } } @@ -83,12 +80,10 @@ static bool mg_tx_hook(const CANPacket_t *msg) { bool violation = false; // Steering control - if (msg->addr == 0x1fdU) { - int desired_torque = (((msg->data[0] & 0x7U) << 8) | msg->data[1]) - 1024U; - bool steer_req = GET_BIT(msg, 35U); + int desired_torque = (((msg->data[0] & 0x7U) << 8) | msg->data[1]) - 1024U; + bool steer_req = GET_BIT(msg, 35U); - violation |= steer_torque_cmd_checks(desired_torque, steer_req, MG_STEERING_LIMITS); - } + violation |= steer_torque_cmd_checks(desired_torque, steer_req, MG_STEERING_LIMITS); if (violation) { tx = false; diff --git a/opendbc/safety/modes/psa.h b/opendbc/safety/modes/psa.h index 63026c3e1ea..b6cddccd226 100644 --- a/opendbc/safety/modes/psa.h +++ b/opendbc/safety/modes/psa.h @@ -19,9 +19,8 @@ static uint8_t psa_get_counter(const CANPacket_t *msg) { uint8_t cnt = 0; if (msg->addr == PSA_HS2_DAT_MDD_CMD_452) { cnt = (msg->data[3] >> 4) & 0xFU; - } else if (msg->addr == PSA_HS2_DYN_ABR_38D) { + } else { // msg->addr == PSA_HS2_DYN_ABR_38D cnt = (msg->data[5] >> 4) & 0xFU; - } else { } return cnt; } @@ -50,9 +49,8 @@ static uint32_t psa_compute_checksum(const CANPacket_t *msg) { uint8_t chk = 0; if (msg->addr == PSA_HS2_DAT_MDD_CMD_452) { chk = _psa_compute_checksum(msg, 0x4, 5); - } else if (msg->addr == PSA_HS2_DYN_ABR_38D) { + } else { // msg->addr == PSA_HS2_DYN_ABR_38D chk = _psa_compute_checksum(msg, 0x7, 5); - } else { } return chk; } @@ -74,16 +72,12 @@ static void psa_rx_hook(const CANPacket_t *msg) { } if (msg->bus == PSA_ADAS_BUS) { - if (msg->addr == PSA_HS2_DAT_MDD_CMD_452) { - pcm_cruise_check((msg->data[2U] >> 7U) & 1U); // RVV_ACC_ACTIVATION_REQ - } + pcm_cruise_check((msg->data[2U] >> 7U) & 1U); // RVV_ACC_ACTIVATION_REQ } if (msg->bus == PSA_CAM_BUS) { - if (msg->addr == PSA_DAT_BSI) { - brake_pressed = (msg->data[0U] >> 5U) & 1U; // P013_MainBrake - } + brake_pressed = (msg->data[0U] >> 5U) & 1U; // P013_MainBrake } } @@ -103,15 +97,13 @@ static bool psa_tx_hook(const CANPacket_t *msg) { }; // Safety check for LKA - if (msg->addr == PSA_LANE_KEEP_ASSIST) { - // SET_ANGLE - int desired_angle = to_signed((msg->data[6] << 6) | ((msg->data[7] & 0xFCU) >> 2), 14); - // TORQUE_FACTOR - bool lka_active = ((msg->data[5] & 0xFEU) >> 1) == 100U; - - if (steer_angle_cmd_checks(desired_angle, lka_active, PSA_STEERING_LIMITS)) { - tx = false; - } + // SET_ANGLE + int desired_angle = to_signed((msg->data[6] << 6) | ((msg->data[7] & 0xFCU) >> 2), 14); + // TORQUE_FACTOR + bool lka_active = ((msg->data[5] & 0xFEU) >> 1) == 100U; + + if (steer_angle_cmd_checks(desired_angle, lka_active, PSA_STEERING_LIMITS)) { + tx = false; } return tx; } diff --git a/opendbc/safety/modes/rivian.h b/opendbc/safety/modes/rivian.h index 1209937b462..287846bdc51 100644 --- a/opendbc/safety/modes/rivian.h +++ b/opendbc/safety/modes/rivian.h @@ -34,9 +34,8 @@ static uint32_t rivian_compute_checksum(const CANPacket_t *msg) { uint8_t chksum = 0; if (msg->addr == 0x208U) { chksum = _rivian_compute_checksum(msg, 0x1D, 0xB1); - } else if (msg->addr == 0x150U) { + } else { // msg->addr == 0x150U chksum = _rivian_compute_checksum(msg, 0x1D, 0x9A); - } else { } return chksum; } @@ -45,9 +44,8 @@ static bool rivian_get_quality_flag_valid(const CANPacket_t *msg) { bool valid = false; if (msg->addr == 0x208U) { valid = ((msg->data[3] >> 3) & 0x3U) == 0x1U; // ESP_Vehicle_Speed_Q - } else if (msg->addr == 0x150U) { + } else { // msg->addr == 0x150U valid = (msg->data[1] >> 6) == 0x1U; // VDM_VehicleSpeedQ - } else { } return valid; } @@ -85,10 +83,8 @@ static void rivian_rx_hook(const CANPacket_t *msg) { if (msg->bus == 2U) { // Cruise state - if (msg->addr == 0x100U) { - const int feature_status = msg->data[2] >> 5U; - pcm_cruise_check(feature_status == 1); - } + const int feature_status = msg->data[2] >> 5U; + pcm_cruise_check(feature_status == 1); } } diff --git a/opendbc/safety/modes/subaru.h b/opendbc/safety/modes/subaru.h index c128278607c..ec63592a0d4 100644 --- a/opendbc/safety/modes/subaru.h +++ b/opendbc/safety/modes/subaru.h @@ -71,9 +71,8 @@ static uint32_t subaru_compute_checksum(const CANPacket_t *msg) { } static void subaru_rx_hook(const CANPacket_t *msg) { - const unsigned int alt_main_bus = subaru_gen2 ? SUBARU_ALT_BUS : SUBARU_MAIN_BUS; - - if ((msg->addr == MSG_SUBARU_Steering_Torque) && (msg->bus == SUBARU_MAIN_BUS)) { + // The RX checks select the bus for each address, including Gen2 variants. + if (msg->addr == MSG_SUBARU_Steering_Torque) { int torque_driver_new; torque_driver_new = ((GET_BYTES(msg, 0, 4) >> 16) & 0x7FFU); torque_driver_new = -1 * to_signed(torque_driver_new, 11); @@ -81,13 +80,13 @@ static void subaru_rx_hook(const CANPacket_t *msg) { } // enter controls on rising edge of ACC, exit controls on ACC off - if ((msg->addr == MSG_SUBARU_CruiseControl) && (msg->bus == alt_main_bus)) { + if (msg->addr == MSG_SUBARU_CruiseControl) { bool cruise_engaged = (msg->data[5] >> 1) & 1U; pcm_cruise_check(cruise_engaged); } // update vehicle moving with any non-zero wheel speed - if ((msg->addr == MSG_SUBARU_Wheel_Speeds) && (msg->bus == alt_main_bus)) { + if (msg->addr == MSG_SUBARU_Wheel_Speeds) { uint32_t fr = (GET_BYTES(msg, 1, 3) >> 4) & 0x1FFFU; uint32_t rr = (GET_BYTES(msg, 3, 3) >> 1) & 0x1FFFU; uint32_t rl = (GET_BYTES(msg, 4, 3) >> 6) & 0x1FFFU; @@ -98,11 +97,11 @@ static void subaru_rx_hook(const CANPacket_t *msg) { UPDATE_VEHICLE_SPEED((fr + rr + rl + fl) / 4.0 * 0.057 * KPH_TO_MS); } - if ((msg->addr == MSG_SUBARU_Brake_Status) && (msg->bus == alt_main_bus)) { + if (msg->addr == MSG_SUBARU_Brake_Status) { brake_pressed = (msg->data[7] >> 6) & 1U; } - if ((msg->addr == MSG_SUBARU_Throttle) && (msg->bus == SUBARU_MAIN_BUS)) { + if (msg->addr == MSG_SUBARU_Throttle) { gas_pressed = msg->data[4] != 0U; } } diff --git a/opendbc/safety/modes/subaru_preglobal.h b/opendbc/safety/modes/subaru_preglobal.h index 4ddcdc10e70..cf96adb9540 100644 --- a/opendbc/safety/modes/subaru_preglobal.h +++ b/opendbc/safety/modes/subaru_preglobal.h @@ -20,33 +20,32 @@ static bool subaru_pg_reversed_driver_torque = false; static void subaru_preglobal_rx_hook(const CANPacket_t *msg) { - if (msg->bus == SUBARU_PG_MAIN_BUS) { - if (msg->addr == MSG_SUBARU_PG_Steering_Torque) { - int torque_driver_new; - torque_driver_new = (msg->data[3] >> 5) + (msg->data[4] << 3); - torque_driver_new = to_signed(torque_driver_new, 11); - torque_driver_new = subaru_pg_reversed_driver_torque ? -torque_driver_new : torque_driver_new; - update_sample(&torque_driver, torque_driver_new); - } + // The RX checks already validate the address, bus, and length. + if (msg->addr == MSG_SUBARU_PG_Steering_Torque) { + int torque_driver_new; + torque_driver_new = (msg->data[3] >> 5) + (msg->data[4] << 3); + torque_driver_new = to_signed(torque_driver_new, 11); + torque_driver_new = subaru_pg_reversed_driver_torque ? -torque_driver_new : torque_driver_new; + update_sample(&torque_driver, torque_driver_new); + } - // enter controls on rising edge of ACC, exit controls on ACC off - if (msg->addr == MSG_SUBARU_PG_CruiseControl) { - bool cruise_engaged = (msg->data[6] >> 1) & 1U; - pcm_cruise_check(cruise_engaged); - } + // enter controls on rising edge of ACC, exit controls on ACC off + if (msg->addr == MSG_SUBARU_PG_CruiseControl) { + bool cruise_engaged = (msg->data[6] >> 1) & 1U; + pcm_cruise_check(cruise_engaged); + } - // update vehicle moving with any non-zero wheel speed - if (msg->addr == MSG_SUBARU_PG_Wheel_Speeds) { - vehicle_moving = ((GET_BYTES(msg, 0, 4) >> 12) != 0U) || (GET_BYTES(msg, 4, 4) != 0U); - } + // update vehicle moving with any non-zero wheel speed + if (msg->addr == MSG_SUBARU_PG_Wheel_Speeds) { + vehicle_moving = ((GET_BYTES(msg, 0, 4) >> 12) != 0U) || (GET_BYTES(msg, 4, 4) != 0U); + } - if (msg->addr == MSG_SUBARU_PG_Brake_Pedal) { - brake_pressed = ((GET_BYTES(msg, 0, 4) >> 16) & 0xFFU) > 0U; - } + if (msg->addr == MSG_SUBARU_PG_Brake_Pedal) { + brake_pressed = ((GET_BYTES(msg, 0, 4) >> 16) & 0xFFU) > 0U; + } - if (msg->addr == MSG_SUBARU_PG_Throttle) { - gas_pressed = msg->data[0] != 0U; - } + if (msg->addr == MSG_SUBARU_PG_Throttle) { + gas_pressed = msg->data[0] != 0U; } } diff --git a/opendbc/safety/modes/tesla.h b/opendbc/safety/modes/tesla.h index a137f5e4b31..dc74381e906 100644 --- a/opendbc/safety/modes/tesla.h +++ b/opendbc/safety/modes/tesla.h @@ -24,9 +24,6 @@ static uint8_t tesla_get_counter(const CANPacket_t *msg) { } else if (msg->addr == 0x488U) { // Signal: DAS_steeringControlCounter cnt = msg->data[2] & 0x0FU; - } else if ((msg->addr == 0x257U) || (msg->addr == 0x118U) || (msg->addr == 0x145U) || (msg->addr == 0x286U) || (msg->addr == 0x311U)) { - // Signal: DI_speedCounter, DI_systemStatusCounter, ESP_statusCounter, DI_locStatusCounter, UI_warningCounter - cnt = msg->data[1] & 0x0FU; } else if (msg->addr == 0x155U) { // Signal: ESP_wheelRotationCounter cnt = msg->data[6] >> 4; @@ -34,46 +31,40 @@ static uint8_t tesla_get_counter(const CANPacket_t *msg) { // Signal: EPAS3S_sysStatusCounter cnt = msg->data[6] & 0x0FU; } else { + // Remaining counter-checked messages: DI_speed, DI_systemStatus, + // ESP_status, DI_state, and UI_warning. + cnt = msg->data[1] & 0x0FU; } return cnt; } static int _tesla_get_checksum_byte(const int addr) { - int checksum_byte = -1; + int checksum_byte = 0; // Remaining checksum-checked messages use byte 0. if ((addr == 0x370) || (addr == 0x2b9) || (addr == 0x155)) { // Signal: EPAS3S_sysStatusChecksum, DAS_controlChecksum, ESP_wheelRotationChecksum checksum_byte = 7; } else if (addr == 0x488) { // Signal: DAS_steeringControlChecksum checksum_byte = 3; - } else if ((addr == 0x257) || (addr == 0x118) || (addr == 0x145) || (addr == 0x286) || (addr == 0x311)) { - // Signal: DI_speedChecksum, DI_systemStatusChecksum, ESP_statusChecksum, DI_locStatusChecksum, UI_warningChecksum - checksum_byte = 0; } else { } return checksum_byte; } static uint32_t tesla_get_checksum(const CANPacket_t *msg) { - uint8_t chksum = 0; int checksum_byte = _tesla_get_checksum_byte(msg->addr); - if (checksum_byte != -1) { - chksum = msg->data[checksum_byte]; - } - return chksum; + return msg->data[checksum_byte]; } static uint32_t tesla_compute_checksum(const CANPacket_t *msg) { uint8_t chksum = 0; int checksum_byte = _tesla_get_checksum_byte(msg->addr); - if (checksum_byte != -1) { - chksum = (uint8_t)((msg->addr & 0xFFU) + ((msg->addr >> 8) & 0xFFU)); - int len = GET_LEN(msg); - for (int i = 0; i < len; i++) { - if (i != checksum_byte) { - chksum += msg->data[i]; - } + chksum = (uint8_t)((msg->addr & 0xFFU) + ((msg->addr >> 8) & 0xFFU)); + int len = GET_LEN(msg); + for (int i = 0; i < len; i++) { + if (i != checksum_byte) { + chksum += msg->data[i]; } } return chksum; @@ -84,28 +75,13 @@ static bool tesla_get_quality_flag_valid(const CANPacket_t *msg) { bool valid = false; if (msg->addr == 0x155U) { valid = (msg->data[5] & 0x1U) == 0x1U; // ESP_wheelSpeedsQF - } else if (msg->addr == 0x145U) { + } else { // ESP_status: the remaining quality-checked message int user_brake_status = (msg->data[3] >> 5) & 0x03U; valid = (user_brake_status != 0) && (user_brake_status != 3); // ESP_driverBrakeApply=NotInit_orOff, Faulty_SNA - } else { } return valid; } -static int tesla_get_steer_ctrl_type(const int ctrl_type) { - // Returns ANGLE_CONTROL-equivalent control type for FSD 14 - int steer_ctrl_type = ctrl_type; - if (tesla_fsd_14) { - if (ctrl_type == 1) { - steer_ctrl_type = 2; - } else if (ctrl_type == 2) { - steer_ctrl_type = 1; - } else { - } - } - return steer_ctrl_type; -} - static void tesla_rx_hook(const CANPacket_t *msg) { if (msg->bus == 0U) { @@ -191,7 +167,8 @@ static void tesla_rx_hook(const CANPacket_t *msg) { // DAS_steeringControl if (msg->addr == 0x488U) { int steering_control_type = msg->data[2] >> 6; - bool tesla_stock_lkas_now = steering_control_type == tesla_get_steer_ctrl_type(2); // "LANE_KEEP_ASSIST" + const int lkas_ctrl_type = tesla_fsd_14 ? 1 : 2; // LANE_KEEP_ASSIST + bool tesla_stock_lkas_now = steering_control_type == lkas_ctrl_type; // Only consider rising edges while controls are not allowed if (tesla_stock_lkas_now && !tesla_stock_lkas_prev && !controls_allowed) { @@ -240,7 +217,7 @@ static bool tesla_tx_hook(const CANPacket_t *msg) { int raw_angle_can = ((msg->data[0] & 0x7FU) << 8) | msg->data[1]; int desired_angle = raw_angle_can - 16384; int steer_control_type = msg->data[2] >> 6; - const int angle_ctrl_type = tesla_get_steer_ctrl_type(1); + const int angle_ctrl_type = tesla_fsd_14 ? 2 : 1; // ANGLE_CONTROL bool steer_control_enabled = steer_control_type == angle_ctrl_type; // ANGLE_CONTROL if (steer_angle_cmd_checks_vm(desired_angle, steer_control_enabled, TESLA_STEERING_LIMITS, TESLA_STEERING_PARAMS)) { diff --git a/opendbc/safety/modes/toyota.h b/opendbc/safety/modes/toyota.h index e98abf519f5..152cd0b34e6 100644 --- a/opendbc/safety/modes/toyota.h +++ b/opendbc/safety/modes/toyota.h @@ -80,7 +80,7 @@ static bool toyota_get_quality_flag_valid(const CANPacket_t *msg) { bool valid = false; if (msg->addr == 0x260U) { valid = !GET_BIT(msg, 3U); // STEER_TORQUE_SENSOR.STEER_ANGLE_INITIALIZING - } else if (msg->addr == 0xaaU) { // WHEEL_SPEEDS + } else { // WHEEL_SPEEDS // each wheel speed is 1-bit fault + 15-bit speed valid = true; for (uint8_t i = 0U; i < 4U; i += 1U) { @@ -89,85 +89,83 @@ static bool toyota_get_quality_flag_valid(const CANPacket_t *msg) { break; } } - } else { } return valid; } static void toyota_rx_hook(const CANPacket_t *msg) { - if (msg->bus == 0U) { + // The RX checks already validate the address, bus, and length. - // get eps motor torque (0.66 factor in dbc) - if (msg->addr == 0x260U) { - int torque_meas_new = (msg->data[5] << 8) | msg->data[6]; - torque_meas_new = to_signed(torque_meas_new, 16); - - // scale by dbc_factor - torque_meas_new = (torque_meas_new * toyota_dbc_eps_torque_factor) / 100; - - // update array of sample - update_sample(&torque_meas, torque_meas_new); - - // increase torque_meas by 1 to be conservative on rounding - torque_meas.min--; - torque_meas.max++; - - // driver torque for angle limiting - int torque_driver_new = (msg->data[1] << 8) | msg->data[2]; - torque_driver_new = to_signed(torque_driver_new, 16); - update_sample(&torque_driver, torque_driver_new); - - // LTA request angle should match current angle while inactive, clipped to max accepted angle. - // note that angle can be relative to init angle on some TSS2 platforms, LTA has the same offset - bool steer_angle_initializing = GET_BIT(msg, 3U); - if (!steer_angle_initializing) { - int angle_meas_new = (msg->data[3] << 8U) | msg->data[4]; - angle_meas_new = to_signed(angle_meas_new, 16); - update_sample(&angle_meas, angle_meas_new); - } + // get eps motor torque (0.66 factor in dbc) + if (msg->addr == 0x260U) { + int torque_meas_new = (msg->data[5] << 8) | msg->data[6]; + torque_meas_new = to_signed(torque_meas_new, 16); + + // scale by dbc_factor + torque_meas_new = (torque_meas_new * toyota_dbc_eps_torque_factor) / 100; + + // update array of sample + update_sample(&torque_meas, torque_meas_new); + + // increase torque_meas by 1 to be conservative on rounding + torque_meas.min--; + torque_meas.max++; + + // driver torque for angle limiting + int torque_driver_new = (msg->data[1] << 8) | msg->data[2]; + torque_driver_new = to_signed(torque_driver_new, 16); + update_sample(&torque_driver, torque_driver_new); + + // LTA request angle should match current angle while inactive, clipped to max accepted angle. + // note that angle can be relative to init angle on some TSS2 platforms, LTA has the same offset + bool steer_angle_initializing = GET_BIT(msg, 3U); + if (!steer_angle_initializing) { + int angle_meas_new = (msg->data[3] << 8U) | msg->data[4]; + angle_meas_new = to_signed(angle_meas_new, 16); + update_sample(&angle_meas, angle_meas_new); } + } - // enter controls on rising edge of ACC, exit controls on ACC off - // exit controls on rising edge of gas press, if not alternative experience - // exit controls on rising edge of brake press - if (toyota_secoc) { - if (msg->addr == 0x176U) { - bool cruise_engaged = GET_BIT(msg, 5U); // PCM_CRUISE.CRUISE_ACTIVE - pcm_cruise_check(cruise_engaged); - } - if (msg->addr == 0x116U) { - gas_pressed = msg->data[1] != 0U; // GAS_PEDAL.GAS_PEDAL_USER - } - if (msg->addr == 0x101U) { - brake_pressed = GET_BIT(msg, 3U); // BRAKE_MODULE.BRAKE_PRESSED (toyota_rav4_prime_generated.dbc) - } - } else { - if (msg->addr == 0x1D2U) { - bool cruise_engaged = GET_BIT(msg, 5U); // PCM_CRUISE.CRUISE_ACTIVE - pcm_cruise_check(cruise_engaged); - gas_pressed = !GET_BIT(msg, 4U); // PCM_CRUISE.GAS_RELEASED - } - if (!toyota_alt_brake && (msg->addr == 0x226U)) { - brake_pressed = GET_BIT(msg, 37U); // BRAKE_MODULE.BRAKE_PRESSED (toyota_nodsu_pt_generated.dbc) - } - if (toyota_alt_brake && (msg->addr == 0x224U)) { - brake_pressed = GET_BIT(msg, 5U); // BRAKE_MODULE.BRAKE_PRESSED (toyota_new_mc_pt_generated.dbc) - } + // enter controls on rising edge of ACC, exit controls on ACC off + // exit controls on rising edge of gas press, if not alternative experience + // exit controls on rising edge of brake press + if (toyota_secoc) { + if (msg->addr == 0x176U) { + bool cruise_engaged = GET_BIT(msg, 5U); // PCM_CRUISE.CRUISE_ACTIVE + pcm_cruise_check(cruise_engaged); } + if (msg->addr == 0x116U) { + gas_pressed = msg->data[1] != 0U; // GAS_PEDAL.GAS_PEDAL_USER + } + if (msg->addr == 0x101U) { + brake_pressed = GET_BIT(msg, 3U); // BRAKE_MODULE.BRAKE_PRESSED (toyota_rav4_prime_generated.dbc) + } + } else { + if (msg->addr == 0x1D2U) { + bool cruise_engaged = GET_BIT(msg, 5U); // PCM_CRUISE.CRUISE_ACTIVE + pcm_cruise_check(cruise_engaged); + gas_pressed = !GET_BIT(msg, 4U); // PCM_CRUISE.GAS_RELEASED + } + if (!toyota_alt_brake && (msg->addr == 0x226U)) { + brake_pressed = GET_BIT(msg, 37U); // BRAKE_MODULE.BRAKE_PRESSED (toyota_nodsu_pt_generated.dbc) + } + if (toyota_alt_brake && (msg->addr == 0x224U)) { + brake_pressed = GET_BIT(msg, 5U); // BRAKE_MODULE.BRAKE_PRESSED (toyota_new_mc_pt_generated.dbc) + } + } - // sample speed - if (msg->addr == 0xaaU) { - int speed = 0; - // sum 4 wheel speeds. conversion: raw * 0.01 - 67.67 - for (uint8_t i = 0U; i < 8U; i += 2U) { - int wheel_speed = ((msg->data[i] & 0x7FU) << 8U) | msg->data[(i + 1U)]; - speed += wheel_speed - 6767; - } - // check that all wheel speeds are at zero value - vehicle_moving = speed != 0; - - UPDATE_VEHICLE_SPEED(speed / 4.0 * 0.01 * KPH_TO_MS); + // sample speed + if (msg->addr == 0xaaU) { + int speed = 0; + // sum 4 wheel speeds. conversion: raw * 0.01 - 67.67 + for (uint8_t i = 0U; i < 8U; i += 2U) { + int wheel_speed = ((msg->data[i] & 0x7FU) << 8U) | msg->data[(i + 1U)]; + speed += wheel_speed - 6767; } + // check that all wheel speeds are at zero value + vehicle_moving = speed != 0; + + UPDATE_VEHICLE_SPEED(speed / 4.0 * 0.01 * KPH_TO_MS); } } diff --git a/opendbc/safety/modes/volkswagen_common.h b/opendbc/safety/modes/volkswagen_common.h index fc5516b3c95..f061d91508b 100644 --- a/opendbc/safety/modes/volkswagen_common.h +++ b/opendbc/safety/modes/volkswagen_common.h @@ -78,10 +78,8 @@ static uint32_t volkswagen_mqb_meb_compute_crc(const CANPacket_t *msg) { crc ^= (uint8_t[]){0xC4, 0xE2, 0x4F, 0xE4, 0xF8, 0x2F, 0x56, 0x81, 0x9F, 0xE5, 0x83, 0x44, 0x05, 0x3F, 0x97, 0xDF}[counter]; } else if (msg->addr == MSG_MOTOR_20) { crc ^= (uint8_t[]){0xE9, 0x65, 0xAE, 0x6B, 0x7B, 0x35, 0xE5, 0x5F, 0x4E, 0xC7, 0x86, 0xA2, 0xBB, 0xDD, 0xEB, 0xB4}[counter]; - } else if (msg->addr == MSG_GRA_ACC_01) { + } else { // GRA_ACC_01: the remaining checksum-checked message crc ^= (uint8_t[]){0x6A, 0x38, 0xB4, 0x27, 0x22, 0xEF, 0xE1, 0xBB, 0xF8, 0x80, 0x84, 0x49, 0xC7, 0x9E, 0x1E, 0x2B}[counter]; - } else { - // Undefined CAN message, CRC check expected to fail } crc = volkswagen_crc8_lut_8h2f[crc]; diff --git a/opendbc/safety/modes/volkswagen_meb.h b/opendbc/safety/modes/volkswagen_meb.h index adaef46f443..e10c0e36421 100644 --- a/opendbc/safety/modes/volkswagen_meb.h +++ b/opendbc/safety/modes/volkswagen_meb.h @@ -54,10 +54,8 @@ static uint32_t volkswagen_meb_compute_crc(const CANPacket_t *msg) { crc ^= (uint8_t[]){0x77, 0x5C, 0xA0, 0x89, 0x4B, 0x7C, 0xBB, 0xD6, 0x1F, 0x6C, 0x4F, 0xF6, 0x20, 0x2B, 0x43, 0xDD}[counter]; } else if (msg->addr == MSG_ESP_21) { crc ^= (uint8_t[]){0xB4, 0xEF, 0xF8, 0x49, 0x1E, 0xE5, 0xC2, 0xC0, 0x97, 0x19, 0x3C, 0xC9, 0xF1, 0x98, 0xD6, 0x61}[counter]; - } else if (msg->addr == MSG_Motor_51) { + } else { // Motor_51 crc ^= (uint8_t[]){0x77, 0x5C, 0xA0, 0x89, 0x4B, 0x7C, 0xBB, 0xD6, 0x1F, 0x6C, 0x4F, 0xF6, 0x20, 0x2B, 0x43, 0xDD}[counter]; - } else { - // Undefined CAN message, CRC check expected to fail } crc = volkswagen_crc8_lut_8h2f[crc]; @@ -91,10 +89,8 @@ static uint32_t volkswagen_meb_alt_crc_compute(const CANPacket_t *msg) { crc ^= (uint8_t[]){0x18, 0x71, 0x10, 0x8D, 0xD7, 0xAA, 0xB0, 0x78, 0xAC, 0x12, 0xAE, 0x0C, 0xDD, 0xF1, 0x85, 0x68}[counter]; } else if (msg->addr == MSG_ESC_51) { crc ^= (uint8_t[]){0x69, 0xDC, 0xF9, 0x64, 0x6A, 0xCE, 0x55, 0x2C, 0xC4, 0x38, 0x8F, 0xD1, 0xC6, 0x43, 0xB4, 0xB1}[counter]; - } else if (msg->addr == MSG_Motor_51) { + } else { // Motor_51, selected by len > 0 above crc ^= (uint8_t[]){0x2C, 0xB1, 0x1A, 0x75, 0xBB, 0x65, 0x79, 0x47, 0x81, 0x2B, 0xCC, 0x96, 0x17, 0xDB, 0xC0, 0x94}[counter]; - } else { - // Undefined CAN message, CRC check expected to fail } crc = (uint8_t)(volkswagen_crc8_lut_8h2f[crc] ^ 0xFFU); @@ -142,69 +138,68 @@ static safety_config volkswagen_meb_init(uint16_t param) { } static void volkswagen_meb_rx_hook(const CANPacket_t *msg) { - if (msg->bus == 0U) { - // Update in-motion state by sampling wheel speeds - if (msg->addr == MSG_ESC_51) { - uint32_t fl = msg->data[8] | (msg->data[9] << 8); - uint32_t fr = msg->data[10] | (msg->data[11] << 8); - uint32_t rl = msg->data[12] | (msg->data[13] << 8); - uint32_t rr = msg->data[14] | (msg->data[15] << 8); - vehicle_moving = (fr > 0U) || (rr > 0U) || (rl > 0U) || (fl > 0U); - UPDATE_VEHICLE_SPEED((fr + rr + rl + fl) / 4.0 * 0.0075 * KPH_TO_MS); - } - - // Check vehicle speed with redundant source - if (msg->addr == MSG_ESP_21) { - // Signal: ESP_v_Signal - float esp_speed = ((msg->data[5] << 8) | msg->data[4]) * 0.01 * KPH_TO_MS; - UPDATE_VEHICLE_SPEED_2(esp_speed); - } + // The RX checks already validate the address, bus, and length. + // Update in-motion state by sampling wheel speeds + if (msg->addr == MSG_ESC_51) { + uint32_t fl = msg->data[8] | (msg->data[9] << 8); + uint32_t fr = msg->data[10] | (msg->data[11] << 8); + uint32_t rl = msg->data[12] | (msg->data[13] << 8); + uint32_t rr = msg->data[14] | (msg->data[15] << 8); + vehicle_moving = (fr > 0U) || (rr > 0U) || (rl > 0U) || (fl > 0U); + UPDATE_VEHICLE_SPEED((fr + rr + rl + fl) / 4.0 * 0.0075 * KPH_TO_MS); + } - if (msg->addr == MSG_QFK_01) { - int current_curvature = ((msg->data[6] & 0x7FU) << 8) | msg->data[5]; - current_curvature *= GET_BIT(msg, 55U) ? 1 : -1; - update_sample(&curvature_state.meas, current_curvature); - } + // Check vehicle speed with redundant source + if (msg->addr == MSG_ESP_21) { + // Signal: ESP_v_Signal + float esp_speed = ((msg->data[5] << 8) | msg->data[4]) * 0.01 * KPH_TO_MS; + UPDATE_VEHICLE_SPEED_2(esp_speed); + } - if (msg->addr == MSG_LH_EPS_03) { - update_sample(&torque_driver, volkswagen_mlb_mqb_driver_input_torque(msg)); - } + if (msg->addr == MSG_QFK_01) { + int current_curvature = ((msg->data[6] & 0x7FU) << 8) | msg->data[5]; + current_curvature *= GET_BIT(msg, 55U) ? 1 : -1; + update_sample(&curvature_state.meas, current_curvature); + } - if (msg->addr == MSG_Motor_51) { - int acc_status = (msg->data[11] & 0x07U); - bool cruise_engaged = (acc_status == 3) || (acc_status == 4) || (acc_status == 5); - acc_main_on = cruise_engaged || (acc_status == 2); + if (msg->addr == MSG_LH_EPS_03) { + update_sample(&torque_driver, volkswagen_mlb_mqb_driver_input_torque(msg)); + } - if (!acc_main_on) { - controls_allowed = false; - } + if (msg->addr == MSG_Motor_51) { + int acc_status = (msg->data[11] & 0x07U); + bool cruise_engaged = (acc_status == 3) || (acc_status == 4) || (acc_status == 5); + acc_main_on = cruise_engaged || (acc_status == 2); - int accel_pedal_value = ((msg->data[1] >> 4) & 0x0FU) | ((msg->data[2] & 0x1FU) << 4); - gas_pressed = accel_pedal_value > 0; + if (!acc_main_on) { + controls_allowed = false; } - if (msg->addr == MSG_GRA_ACC_01) { - // Enter controls on falling edge of Set or Resume with main switch on - // Signal: GRA_ACC_01.GRA_Tip_Setzen - // Signal: GRA_ACC_01.GRA_Tip_Wiederaufnahme - bool set_button = GET_BIT(msg, 16U); - bool resume_button = GET_BIT(msg, 19U); - if ((volkswagen_set_button_prev && !set_button) || (volkswagen_resume_button_prev && !resume_button)) { - controls_allowed = acc_main_on; - } - volkswagen_set_button_prev = set_button; - volkswagen_resume_button_prev = resume_button; - - // Always exit controls on rising edge of Cancel - if (GET_BIT(msg, 13U)) { - controls_allowed = false; - } + int accel_pedal_value = ((msg->data[1] >> 4) & 0x0FU) | ((msg->data[2] & 0x1FU) << 4); + gas_pressed = accel_pedal_value > 0; + } + + if (msg->addr == MSG_GRA_ACC_01) { + // Enter controls on falling edge of Set or Resume with main switch on + // Signal: GRA_ACC_01.GRA_Tip_Setzen + // Signal: GRA_ACC_01.GRA_Tip_Wiederaufnahme + bool set_button = GET_BIT(msg, 16U); + bool resume_button = GET_BIT(msg, 19U); + if ((volkswagen_set_button_prev && !set_button) || (volkswagen_resume_button_prev && !resume_button)) { + controls_allowed = acc_main_on; } + volkswagen_set_button_prev = set_button; + volkswagen_resume_button_prev = resume_button; - if (msg->addr == MSG_MOTOR_14) { - brake_pressed = GET_BIT(msg, 28U); + // Always exit controls on rising edge of Cancel + if (GET_BIT(msg, 13U)) { + controls_allowed = false; } } + + if (msg->addr == MSG_MOTOR_14) { + brake_pressed = GET_BIT(msg, 28U); + } } static bool volkswagen_meb_tx_hook(const CANPacket_t *msg) { diff --git a/opendbc/safety/modes/volkswagen_mlb.h b/opendbc/safety/modes/volkswagen_mlb.h index 1390ea7e529..efbf261805d 100644 --- a/opendbc/safety/modes/volkswagen_mlb.h +++ b/opendbc/safety/modes/volkswagen_mlb.h @@ -67,14 +67,12 @@ static void volkswagen_mlb_rx_hook(const CANPacket_t *msg) { } if (msg->bus == 1U) { - if (msg->addr == MSG_TSK_04) { - // When using stock ACC, enter controls on rising edge of stock ACC engage, exit on disengage - // Signal: TSK_04.TSK_Status_GRA_ACC_02 - int acc_status = (msg->data[7] & 0xC0U) >> 6; - bool cruise_engaged = (acc_status == 1) || (acc_status == 2); + // When using stock ACC, enter controls on rising edge of stock ACC engage, exit on disengage + // Signal: TSK_04.TSK_Status_GRA_ACC_02 + int acc_status = (msg->data[7] & 0xC0U) >> 6; + bool cruise_engaged = (acc_status == 1) || (acc_status == 2); - pcm_cruise_check(cruise_engaged); - } + pcm_cruise_check(cruise_engaged); } } diff --git a/opendbc/safety/modes/volkswagen_mqb.h b/opendbc/safety/modes/volkswagen_mqb.h index bf7274648ac..416b19961d1 100644 --- a/opendbc/safety/modes/volkswagen_mqb.h +++ b/opendbc/safety/modes/volkswagen_mqb.h @@ -35,80 +35,79 @@ static safety_config volkswagen_mqb_init(uint16_t param) { } static void volkswagen_mqb_rx_hook(const CANPacket_t *msg) { - if (msg->bus == 0U) { - // Update in-motion state by sampling wheel speeds - if (msg->addr == MSG_ESP_19) { - // sum 4 wheel speeds - int speed = 0; - for (uint8_t i = 0U; i < 8U; i += 2U) { - int wheel_speed = msg->data[i] | (msg->data[i + 1U] << 8); - speed += wheel_speed; - } - // Check all wheel speeds for any movement - vehicle_moving = speed > 0; + // The RX checks already validate the address, bus, and length. + // Update in-motion state by sampling wheel speeds + if (msg->addr == MSG_ESP_19) { + // sum 4 wheel speeds + int speed = 0; + for (uint8_t i = 0U; i < 8U; i += 2U) { + int wheel_speed = msg->data[i] | (msg->data[i + 1U] << 8); + speed += wheel_speed; } + // Check all wheel speeds for any movement + vehicle_moving = speed > 0; + } - // Update driver input torque samples - // Signal: LH_EPS_03.EPS_Lenkmoment (absolute torque) - // Signal: LH_EPS_03.EPS_VZ_Lenkmoment (direction) - if (msg->addr == MSG_LH_EPS_03) { - update_sample(&torque_driver, volkswagen_mlb_mqb_driver_input_torque(msg)); - } + // Update driver input torque samples + // Signal: LH_EPS_03.EPS_Lenkmoment (absolute torque) + // Signal: LH_EPS_03.EPS_VZ_Lenkmoment (direction) + if (msg->addr == MSG_LH_EPS_03) { + update_sample(&torque_driver, volkswagen_mlb_mqb_driver_input_torque(msg)); + } - if (msg->addr == MSG_TSK_06) { - // When using stock ACC, enter controls on rising edge of stock ACC engage, exit on disengage - // Always exit controls on main switch off - // Signal: TSK_06.TSK_Status - int acc_status = (msg->data[3] & 0x7U); - bool cruise_engaged = (acc_status == 3) || (acc_status == 4) || (acc_status == 5); - acc_main_on = cruise_engaged || (acc_status == 2); + if (msg->addr == MSG_TSK_06) { + // When using stock ACC, enter controls on rising edge of stock ACC engage, exit on disengage + // Always exit controls on main switch off + // Signal: TSK_06.TSK_Status + int acc_status = (msg->data[3] & 0x7U); + bool cruise_engaged = (acc_status == 3) || (acc_status == 4) || (acc_status == 5); + acc_main_on = cruise_engaged || (acc_status == 2); - if (!volkswagen_longitudinal) { - pcm_cruise_check(cruise_engaged); - } + if (!volkswagen_longitudinal) { + pcm_cruise_check(cruise_engaged); + } - if (!acc_main_on) { - controls_allowed = false; - } + if (!acc_main_on) { + controls_allowed = false; } + } - if (msg->addr == MSG_GRA_ACC_01) { - // If using openpilot longitudinal, enter controls on falling edge of Set or Resume with main switch on - // Signal: GRA_ACC_01.GRA_Tip_Setzen - // Signal: GRA_ACC_01.GRA_Tip_Wiederaufnahme - if (volkswagen_longitudinal) { - bool set_button = GET_BIT(msg, 16U); - bool resume_button = GET_BIT(msg, 19U); - if ((volkswagen_set_button_prev && !set_button) || (volkswagen_resume_button_prev && !resume_button)) { - controls_allowed = acc_main_on; - } - volkswagen_set_button_prev = set_button; - volkswagen_resume_button_prev = resume_button; - } - // Always exit controls on rising edge of Cancel - // Signal: GRA_ACC_01.GRA_Abbrechen - if (GET_BIT(msg, 13U)) { - controls_allowed = false; + if (msg->addr == MSG_GRA_ACC_01) { + // If using openpilot longitudinal, enter controls on falling edge of Set or Resume with main switch on + // Signal: GRA_ACC_01.GRA_Tip_Setzen + // Signal: GRA_ACC_01.GRA_Tip_Wiederaufnahme + if (volkswagen_longitudinal) { + bool set_button = GET_BIT(msg, 16U); + bool resume_button = GET_BIT(msg, 19U); + if ((volkswagen_set_button_prev && !set_button) || (volkswagen_resume_button_prev && !resume_button)) { + controls_allowed = acc_main_on; } + volkswagen_set_button_prev = set_button; + volkswagen_resume_button_prev = resume_button; } - - // Signal: Motor_20.MO_Fahrpedalrohwert_01 - if (msg->addr == MSG_MOTOR_20) { - gas_pressed = ((GET_BYTES(msg, 0, 4) >> 12) & 0xFFU) != 0U; + // Always exit controls on rising edge of Cancel + // Signal: GRA_ACC_01.GRA_Abbrechen + if (GET_BIT(msg, 13U)) { + controls_allowed = false; } + } - // Signal: Motor_14.MO_Fahrer_bremst (ECU detected brake pedal switch F63) - if (msg->addr == MSG_MOTOR_14) { - volkswagen_brake_pedal_switch = GET_BIT(msg, 28U); - } + // Signal: Motor_20.MO_Fahrpedalrohwert_01 + if (msg->addr == MSG_MOTOR_20) { + gas_pressed = ((GET_BYTES(msg, 0, 4) >> 12) & 0xFFU) != 0U; + } - // Signal: ESP_05.ESP_Fahrer_bremst (ESP detected driver brake pressure above platform specified threshold) - if (msg->addr == MSG_ESP_05) { - volkswagen_brake_pressure_detected = GET_BIT(msg, 26U); - } + // Signal: Motor_14.MO_Fahrer_bremst (ECU detected brake pedal switch F63) + if (msg->addr == MSG_MOTOR_14) { + volkswagen_brake_pedal_switch = GET_BIT(msg, 28U); + } - brake_pressed = volkswagen_brake_pedal_switch || volkswagen_brake_pressure_detected; + // Signal: ESP_05.ESP_Fahrer_bremst (ESP detected driver brake pressure above platform specified threshold) + if (msg->addr == MSG_ESP_05) { + volkswagen_brake_pressure_detected = GET_BIT(msg, 26U); } + + brake_pressed = volkswagen_brake_pedal_switch || volkswagen_brake_pressure_detected; } static bool volkswagen_mqb_tx_hook(const CANPacket_t *msg) { diff --git a/opendbc/safety/modes/volkswagen_pq.h b/opendbc/safety/modes/volkswagen_pq.h index c3b2184bdbd..d77290fbaa7 100644 --- a/opendbc/safety/modes/volkswagen_pq.h +++ b/opendbc/safety/modes/volkswagen_pq.h @@ -23,9 +23,8 @@ static uint8_t volkswagen_pq_get_counter(const CANPacket_t *msg) { if (msg->addr == MSG_LENKHILFE_3) { counter = (uint8_t)(msg->data[1] & 0xF0U) >> 4; - } else if (msg->addr == MSG_GRA_NEU) { + } else { // GRA_Neu: the remaining counter-checked message counter = (uint8_t)(msg->data[2] & 0xF0U) >> 4; - } else { } return counter; @@ -75,73 +74,72 @@ static safety_config volkswagen_pq_init(uint16_t param) { } static void volkswagen_pq_rx_hook(const CANPacket_t *msg) { - if (msg->bus == 0U) { - // Update in-motion state from speed value. - // Signal: Bremse_1.BR1_Rad_kmh - if (msg->addr == MSG_BREMSE_1) { - int speed = ((msg->data[2] & 0xFEU) >> 1) | (msg->data[3] << 7); - vehicle_moving = speed > 0; - } + // The RX checks already validate the address, bus, and length. + // Update in-motion state from speed value. + // Signal: Bremse_1.BR1_Rad_kmh + if (msg->addr == MSG_BREMSE_1) { + int speed = ((msg->data[2] & 0xFEU) >> 1) | (msg->data[3] << 7); + vehicle_moving = speed > 0; + } - // Update driver input torque samples - // Signal: Lenkhilfe_3.LH3_LM (absolute torque) - // Signal: Lenkhilfe_3.LH3_LMSign (direction) - if (msg->addr == MSG_LENKHILFE_3) { - int torque_driver_new = msg->data[2] | ((msg->data[3] & 0x3U) << 8); - int sign = (msg->data[3] & 0x4U) >> 2; - if (sign == 1) { - torque_driver_new *= -1; - } - update_sample(&torque_driver, torque_driver_new); + // Update driver input torque samples + // Signal: Lenkhilfe_3.LH3_LM (absolute torque) + // Signal: Lenkhilfe_3.LH3_LMSign (direction) + if (msg->addr == MSG_LENKHILFE_3) { + int torque_driver_new = msg->data[2] | ((msg->data[3] & 0x3U) << 8); + int sign = (msg->data[3] & 0x4U) >> 2; + if (sign == 1) { + torque_driver_new *= -1; } + update_sample(&torque_driver, torque_driver_new); + } - if (volkswagen_longitudinal) { - if (msg->addr == MSG_MOTOR_5) { - // ACC main switch on is a prerequisite to enter controls, exit controls immediately on main switch off - // Signal: Motor_5.MO5_GRA_Hauptsch - acc_main_on = GET_BIT(msg, 50U); - if (!acc_main_on) { - controls_allowed = false; - } + if (volkswagen_longitudinal) { + if (msg->addr == MSG_MOTOR_5) { + // ACC main switch on is a prerequisite to enter controls, exit controls immediately on main switch off + // Signal: Motor_5.MO5_GRA_Hauptsch + acc_main_on = GET_BIT(msg, 50U); + if (!acc_main_on) { + controls_allowed = false; } + } - if (msg->addr == MSG_GRA_NEU) { - // If ACC main switch is on, enter controls on falling edge of Set or Resume - // Signal: GRA_Neu.GRA_Neu_Setzen - // Signal: GRA_Neu.GRA_Neu_Recall - bool set_button = GET_BIT(msg, 16U); - bool resume_button = GET_BIT(msg, 17U); - if ((volkswagen_set_button_prev && !set_button) || (volkswagen_resume_button_prev && !resume_button)) { - controls_allowed = acc_main_on; - } - volkswagen_set_button_prev = set_button; - volkswagen_resume_button_prev = resume_button; - // Exit controls on rising edge of Cancel, override Set/Resume if present simultaneously - // Signal: GRA_ACC_01.GRA_Abbrechen - if (GET_BIT(msg, 9U)) { - controls_allowed = false; - } + if (msg->addr == MSG_GRA_NEU) { + // If ACC main switch is on, enter controls on falling edge of Set or Resume + // Signal: GRA_Neu.GRA_Neu_Setzen + // Signal: GRA_Neu.GRA_Neu_Recall + bool set_button = GET_BIT(msg, 16U); + bool resume_button = GET_BIT(msg, 17U); + if ((volkswagen_set_button_prev && !set_button) || (volkswagen_resume_button_prev && !resume_button)) { + controls_allowed = acc_main_on; } - } else { - if (msg->addr == MSG_MOTOR_2) { - // Enter controls on rising edge of stock ACC, exit controls if stock ACC disengages - // Signal: Motor_2.MO2_Sta_GRA - int acc_status = (msg->data[2] & 0xC0U) >> 6; - bool cruise_engaged = (acc_status == 1) || (acc_status == 2); - pcm_cruise_check(cruise_engaged); + volkswagen_set_button_prev = set_button; + volkswagen_resume_button_prev = resume_button; + // Exit controls on rising edge of Cancel, override Set/Resume if present simultaneously + // Signal: GRA_ACC_01.GRA_Abbrechen + if (GET_BIT(msg, 9U)) { + controls_allowed = false; } } - - // Signal: Motor_3.MO3_Pedalwert - if (msg->addr == MSG_MOTOR_3) { - gas_pressed = (msg->data[2]); - } - - // Signal: Motor_2.MO2_BLS + } else { if (msg->addr == MSG_MOTOR_2) { - brake_pressed = (msg->data[2] & 0x1U); + // Enter controls on rising edge of stock ACC, exit controls if stock ACC disengages + // Signal: Motor_2.MO2_Sta_GRA + int acc_status = (msg->data[2] & 0xC0U) >> 6; + bool cruise_engaged = (acc_status == 1) || (acc_status == 2); + pcm_cruise_check(cruise_engaged); } } + + // Signal: Motor_3.MO3_Pedalwert + if (msg->addr == MSG_MOTOR_3) { + gas_pressed = (msg->data[2]); + } + + // Signal: Motor_2.MO2_BLS + if (msg->addr == MSG_MOTOR_2) { + brake_pressed = (msg->data[2] & 0x1U); + } } static bool volkswagen_pq_tx_hook(const CANPacket_t *msg) { diff --git a/opendbc/safety/safety.h b/opendbc/safety/safety.h index 4e47109875f..31411d4e4fd 100644 --- a/opendbc/safety/safety.h +++ b/opendbc/safety/safety.h @@ -147,12 +147,10 @@ static void update_addr_timestamp(RxCheck addr_list[], int index) { } static void update_counter(RxCheck addr_list[], int index, uint8_t counter) { - if (index != -1) { - uint8_t expected_counter = (addr_list[index].status.last_counter + 1U) % (addr_list[index].msg[addr_list[index].status.index].max_counter + 1U); - addr_list[index].status.wrong_counters += (expected_counter == counter) ? -1 : 1; - addr_list[index].status.wrong_counters = SAFETY_CLAMP(addr_list[index].status.wrong_counters, 0, MAX_WRONG_COUNTERS); - addr_list[index].status.last_counter = counter; - } + uint8_t expected_counter = (addr_list[index].status.last_counter + 1U) % (addr_list[index].msg[addr_list[index].status.index].max_counter + 1U); + addr_list[index].status.wrong_counters += (expected_counter == counter) ? -1 : 1; + addr_list[index].status.wrong_counters = SAFETY_CLAMP(addr_list[index].status.wrong_counters, 0, MAX_WRONG_COUNTERS); + addr_list[index].status.last_counter = counter; } static bool rx_msg_safety_check(const CANPacket_t *msg, @@ -484,7 +482,7 @@ int set_safety_hooks(uint16_t mode, uint16_t param) { set_status = 0; // set } } - if ((set_status == 0) && (current_hooks->init != NULL)) { + if (set_status == 0) { safety_config cfg = current_hooks->init(param); current_safety_config.rx_checks = cfg.rx_checks; current_safety_config.rx_checks_len = cfg.rx_checks_len; diff --git a/opendbc/safety/tests/hyundai_common.py b/opendbc/safety/tests/hyundai_common.py index 5d79155aaf1..56dbc947456 100644 --- a/opendbc/safety/tests/hyundai_common.py +++ b/opendbc/safety/tests/hyundai_common.py @@ -71,6 +71,15 @@ def test_sampling_cruise_buttons(self): self.assertEqual(controls_allowed, self.safety.get_controls_allowed()) self._rx(self._button_msg(Buttons.NONE)) + def test_active_cruise_does_not_reenable(self): + self.assertTrue(self._rx(self._pcm_status_msg(False))) + self.assertTrue(self._rx(self._button_msg(Buttons.SET))) + self.assertTrue(self._rx(self._pcm_status_msg(True))) + self.assertTrue(self.safety.get_controls_allowed()) + self.safety.set_controls_allowed(False) + self.assertTrue(self._rx(self._pcm_status_msg(True))) + self.assertFalse(self.safety.get_controls_allowed()) + class HyundaiLongitudinalBase(common.LongitudinalAccelSafetyTest): @@ -96,6 +105,9 @@ def test_sampling_cruise_buttons(self): def test_cruise_engaged_prev(self): pass + def test_active_cruise_does_not_reenable(self): + pass + def test_button_sends(self): pass @@ -138,6 +150,7 @@ def test_tester_present_allowed(self, ecu_disable: bool = True): addr, bus = self.DISABLED_ECU_UDS_MSG for should_tx, msg in ((True, b"\x02\x3E\x80\x00\x00\x00\x00\x00"), + (False, b"\x02\x3E\x80\x00\x00\x00\x00\x01"), (False, b"\x03\xAA\xAA\x00\x00\x00\x00\x00")): tester_present = libsafety_py.make_CANPacket(addr, bus, msg) self.assertEqual(should_tx and ecu_disable, self._tx(tester_present)) diff --git a/opendbc/safety/tests/libsafety/libsafety_py.py b/opendbc/safety/tests/libsafety/libsafety_py.py index 23c9dccc260..e8824c1fddf 100644 --- a/opendbc/safety/tests/libsafety/libsafety_py.py +++ b/opendbc/safety/tests/libsafety/libsafety_py.py @@ -60,6 +60,7 @@ class CANPacket: bool safety_tx_hook(CANPacket_t *msg); int safety_fwd_hook(int bus_num, int addr); int set_safety_hooks(uint16_t mode, uint16_t param); +int to_signed(int d, int bits); void set_controls_allowed(bool c); bool get_controls_allowed(void); @@ -105,6 +106,13 @@ class CANPacket: void safety_tick_current_safety_config(); bool safety_config_valid(); +void safety_test_configure_rx(uint32_t frequency, bool ignore_checksum, bool ignore_counter, + bool ignore_quality_flag, uint8_t max_counter, uint8_t callbacks); +unsigned int safety_test_get_rx_count(void); +bool get_safety_rx_checks_invalid(void); +void safety_test_tick_null(void); +float safety_test_interpolate(float x, float midpoint); +bool safety_test_dynamic_torque_limit(float torque); void init_tests(void); diff --git a/opendbc/safety/tests/libsafety/safety.c b/opendbc/safety/tests/libsafety/safety.c index 64f981c8981..97b024e42a8 100644 --- a/opendbc/safety/tests/libsafety/safety.c +++ b/opendbc/safety/tests/libsafety/safety.c @@ -1,6 +1,7 @@ #include #include #include +#include // TODO: time should just be passed into the hooks we expose uint32_t timer_cnt = 0; @@ -13,6 +14,87 @@ uint32_t microsecond_timer_get(void) { #include "opendbc/safety/safety.h" #include "opendbc/safety/ignition.h" +static RxCheck *test_rx_checks; +static safety_hooks test_hooks; +static unsigned int test_rx_count; + +static void test_rx_hook(const CANPacket_t *msg) { + steering_disengage = (msg->data[4] & 1U) != 0U; + test_rx_count++; +} + +static uint32_t test_get_checksum(const CANPacket_t *msg) { + return msg->data[0]; +} + +static uint32_t test_compute_checksum(const CANPacket_t *msg) { + return msg->data[1]; +} + +static uint8_t test_get_counter(const CANPacket_t *msg) { + return msg->data[2]; +} + +static bool test_get_quality_flag_valid(const CANPacket_t *msg) { + return msg->data[3] == 1U; +} + +// Build configurations that production modes intentionally avoid, so the common +// safety checks' fail-closed behavior can be tested independently of any car. +void safety_test_configure_rx(uint32_t frequency, bool ignore_checksum, bool ignore_counter, + bool ignore_quality_flag, uint8_t max_counter, uint8_t callbacks) { + set_safety_hooks(SAFETY_NOOUTPUT, 0); + const RxCheck checks[] = { + {.msg = {{0x123, 0, 8, frequency, .ignore_checksum = ignore_checksum, .ignore_counter = ignore_counter, + .max_counter = max_counter, .ignore_quality_flag = ignore_quality_flag}, {0}, {0}}}, + }; + free(test_rx_checks); + // Fresh storage initializes the const message descriptors without modifying + // the const subobjects of a previously declared RxCheck. + test_rx_checks = malloc(sizeof(checks)); + if (test_rx_checks == NULL) { + abort(); + } + memcpy(test_rx_checks, checks, sizeof(checks)); + current_safety_config.rx_checks = test_rx_checks; + current_safety_config.rx_checks_len = 1; + test_hooks = (safety_hooks){ + .rx = test_rx_hook, + .get_checksum = (callbacks & 1U) ? test_get_checksum : NULL, + .compute_checksum = (callbacks & 2U) ? test_compute_checksum : NULL, + .get_counter = (callbacks & 4U) ? test_get_counter : NULL, + .get_quality_flag_valid = (callbacks & 8U) ? test_get_quality_flag_valid : NULL, + }; + current_hooks = &test_hooks; + test_rx_count = 0; +} + +unsigned int safety_test_get_rx_count(void) { + return test_rx_count; +} + +bool get_safety_rx_checks_invalid(void) { + return safety_rx_checks_invalid; +} + +void safety_test_tick_null(void) { + safety_tick(NULL); +} + +float safety_test_interpolate(float x, float midpoint) { + const struct lookup_t table = {{0., midpoint, 1.}, {0., 1., 2.}}; + return safety_interpolate(table, x); +} + +bool safety_test_dynamic_torque_limit(float torque) { + const TorqueSteeringLimits limits = { + .max_torque = 300, .max_rate_up = 10, .max_rate_down = 10, + .dynamic_max_torque = true, .max_torque_lookup = {{0., 1., 2.}, {torque, torque, torque}}, + .type = TorqueDriverLimited, + }; + return steer_torque_cmd_checks(0, 0, limits); +} + void safety_tick_current_safety_config() { safety_tick(¤t_safety_config); } diff --git a/opendbc/safety/tests/test.sh b/opendbc/safety/tests/test.sh index 491b9e3f383..f918b580349 100755 --- a/opendbc/safety/tests/test.sh +++ b/opendbc/safety/tests/test.sh @@ -29,10 +29,10 @@ if [ "$1" == "--report" ]; then fi # test coverage -GCOV="gcovr -r $DIR/../ --gcov-executable \"$GCOV_EXEC\" -d --fail-under-line=100 -e ^libsafety" +GCOV="gcovr -r $DIR/../ --gcov-executable \"$GCOV_EXEC\" -d --fail-under-line=100 --fail-under-branch=100 --txt-metric branch -e ^libsafety" if ! GCOV_OUTPUT="$(eval $GCOV)"; then echo -e "FAILED:\n$GCOV_OUTPUT" exit 1 else - echo "SUCCESS: All checked files have 100% coverage!" + echo "SUCCESS: All checked files have 100% line and branch coverage!" fi diff --git a/opendbc/safety/tests/test_body.py b/opendbc/safety/tests/test_body.py index 8512221df9a..df521a0e978 100755 --- a/opendbc/safety/tests/test_body.py +++ b/opendbc/safety/tests/test_body.py @@ -45,9 +45,12 @@ def test_can_flasher(self): # CAN flasher always allowed self.safety.set_controls_allowed(False) self.assertTrue(self._tx(common.make_msg(0, 0x1, 8))) + self.assertTrue(self._tx(common.make_msg(0, 0x1, dat=b'\xce\xfa\xad\xde\x1e\x0b\xb0\x0a'))) # 0xdeadfaceU allowed for CAN flashing mode self.assertTrue(self._tx(common.make_msg(0, 0x250, dat=b'\xce\xfa\xad\xde\x1e\x0b\xb0\x0a'))) + self.assertFalse(self._tx(common.make_msg(0, 0x250, dat=b'\xce\xfa\xad\xde\x1e\x0b\xb0\x00'))) # wrong second word + self.assertFalse(self._tx(common.make_msg(0, 0x250, dat=b'\xce\xfa\xad\xde\x1e\x0b'))) # valid torque length, not flash length self.assertFalse(self._tx(common.make_msg(0, 0x250, dat=b'\xce\xfa\xad\xde\x1e\x0b\xb0'))) # not correct data/len self.assertFalse(self._tx(common.make_msg(0, 0x251, dat=b'\xce\xfa\xad\xde\x1e\x0b\xb0\x0a'))) # wrong address diff --git a/opendbc/safety/tests/test_chrysler.py b/opendbc/safety/tests/test_chrysler.py index daa585abf2d..64fdc6822fe 100755 --- a/opendbc/safety/tests/test_chrysler.py +++ b/opendbc/safety/tests/test_chrysler.py @@ -41,6 +41,19 @@ def _speed_msg(self, speed): values = {"SPEED_LEFT": speed, "SPEED_RIGHT": speed} return self.packer.make_can_msg_safety("SPEED_1", 0, values) + def test_vehicle_moving_single_wheel(self): + if self.DAS_BUS != 0: + self.skipTest("RAM reports a single vehicle speed") + for wheel in ("LEFT", "RIGHT"): + with self.subTest(wheel=wheel): + self.assertTrue(self._rx(self._speed_msg(0))) + self.assertFalse(self.safety.get_vehicle_moving()) + msg = self.packer.make_can_msg_safety("SPEED_1", 0, {f"SPEED_{wheel}": 1}) + self.assertTrue(self._rx(msg)) + self.assertTrue(self.safety.get_vehicle_moving()) + self.assertTrue(self._rx(self._speed_msg(0))) + self.assertFalse(self.safety.get_vehicle_moving()) + def _user_gas_msg(self, gas): values = {"Accelerator_Position": gas} return self.packer.make_can_msg_safety("ECM_5", 0, values) diff --git a/opendbc/safety/tests/test_chrysler_cusw.py b/opendbc/safety/tests/test_chrysler_cusw.py index 3fe78c625f0..8307bf52ec5 100644 --- a/opendbc/safety/tests/test_chrysler_cusw.py +++ b/opendbc/safety/tests/test_chrysler_cusw.py @@ -62,6 +62,9 @@ def test_buttons(self): # can always cancel self.assertTrue(self._tx(self._button_msg(cancel=True))) + # A frame with no requested button is not a cancel or resume command. + self.assertFalse(self._tx(self._button_msg())) + def test_rx_hook(self): for count in range(20): self.assertTrue(self._rx(self._speed_msg(0)), f"{count=}") diff --git a/opendbc/safety/tests/test_ford.py b/opendbc/safety/tests/test_ford.py index 0ea658a876f..fc27db6c0ab 100755 --- a/opendbc/safety/tests/test_ford.py +++ b/opendbc/safety/tests/test_ford.py @@ -169,6 +169,15 @@ def _pcm_status_msg(self, enable: bool): } return self.packer.make_can_msg_safety("EngBrakeData", 0, values) + def test_cruise_states(self): + for state in range(8): + with self.subTest(state=state): + self.assertTrue(self._rx(self._pcm_status_msg(False))) + values = {"BpedDrvAppl_D_Actl": 1, "CcStat_D_Actl": state} + self.assertTrue(self._rx(self.packer.make_can_msg_safety("EngBrakeData", 0, values))) + self.assertEqual(state in (4, 5), self.safety.get_controls_allowed()) + self.assertEqual(state in (4, 5), self.safety.get_cruise_engaged_prev()) + # LKAS command def _lkas_command_msg(self, action: int): values = { @@ -530,6 +539,17 @@ def test_brake_safety_check(self): should_tx = should_tx and (controls_allowed or not brake_actuation) self.assertEqual(should_tx, self._tx(self._acc_command_msg(self.INACTIVE_GAS, brake, brake_actuation))) + def test_brake_request_bits(self): + for controls_allowed in (False, True): + for precharge in (False, True): + for decel in (False, True): + with self.subTest(controls_allowed=controls_allowed, precharge=precharge, decel=decel): + self.safety.set_controls_allowed(controls_allowed) + values = {"AccPrpl_A_Rq": self.INACTIVE_GAS, "AccPrpl_A_Pred": self.INACTIVE_GAS, + "AccBrkTot_A_Rq": self.INACTIVE_ACCEL, "AccBrkPrchg_B_Rq": precharge, "AccBrkDecel_B_Rq": decel} + msg = self.packer.make_can_msg_safety("ACCDATA", 0, values) + self.assertEqual(controls_allowed or not (precharge or decel), self._tx(msg)) + class TestFordLongitudinalSafety(TestFordLongitudinalSafetyBase): STEER_MESSAGE = MSG_LateralMotionControl diff --git a/opendbc/safety/tests/test_gm.py b/opendbc/safety/tests/test_gm.py index ad9b9789209..f537f75e597 100755 --- a/opendbc/safety/tests/test_gm.py +++ b/opendbc/safety/tests/test_gm.py @@ -31,10 +31,17 @@ def _send_brake_msg(self, brake): values = {"FrictionBrakeCmd": -brake} return self.packer_chassis.make_can_msg_safety("EBCMFrictionBrakeCmd", self.BRAKE_BUS, values) - def _send_gas_msg(self, gas): - values = {"GasRegenCmd": gas} + def _send_gas_msg(self, gas, apply=False): + values = {"GasRegenCmd": gas, "GasRegenCmdActive": apply} return self.packer.make_can_msg_safety("ASCMGasRegenCmd", 0, values) + def test_gas_apply_requires_controls(self): + for controls_allowed in (False, True): + for apply in (False, True): + with self.subTest(controls_allowed=controls_allowed, apply=apply): + self.safety.set_controls_allowed(controls_allowed) + self.assertEqual(controls_allowed or not apply, self._tx(self._send_gas_msg(self.INACTIVE_GAS, apply))) + # override these tests from CarSafetyTest, GM longitudinal uses button enable def _pcm_status_msg(self, enable): raise NotImplementedError @@ -103,10 +110,20 @@ def _pcm_status_msg(self, enable): else: raise NotImplementedError - def _speed_msg(self, speed): - values = {"%sWheelSpd" % s: speed for s in ["RL", "RR"]} + def _speed_msg(self, speed, **wheel_speeds): + values = {"%sWheelSpd" % s: wheel_speeds.get(s, speed) for s in ["RL", "RR"]} return self.packer.make_can_msg_safety("EBCMWheelSpdRear", 0, values) + def test_vehicle_moving_single_wheel(self): + for wheel in ("RL", "RR"): + with self.subTest(wheel=wheel): + self.assertTrue(self._rx(self._speed_msg(0))) + self.assertFalse(self.safety.get_vehicle_moving()) + self.assertTrue(self._rx(self._speed_msg(0, **{wheel: self.STANDSTILL_THRESHOLD + 1}))) + self.assertTrue(self.safety.get_vehicle_moving()) + self.assertTrue(self._rx(self._speed_msg(0))) + self.assertFalse(self.safety.get_vehicle_moving()) + def _user_brake_msg(self, brake): # GM safety has a brake threshold of 8 values = {"BrakePedalPos": 8 if brake else 0} @@ -179,6 +196,15 @@ class TestGmCameraSafety(TestGmCameraSafetyBase): FWD_BLACKLISTED_ADDRS = {2: [0x180], 0: [0x184]} # block LKAS message and PSCMStatus BUTTONS_BUS = 2 # tx only + def test_rx_buttons_do_not_change_pcm_controls(self): + for controls_allowed in (False, True): + for button in (Buttons.RES_ACCEL, Buttons.DECEL_SET, Buttons.UNPRESS, Buttons.CANCEL): + with self.subTest(controls_allowed=controls_allowed, button=button): + self.safety.set_controls_allowed(controls_allowed) + msg = self.packer.make_can_msg_safety("ASCMSteeringButton", 0, {"ACCButtons": button}) + self.assertTrue(self._rx(msg)) + self.assertEqual(controls_allowed, self.safety.get_controls_allowed()) + def setUp(self): self.packer = CANPackerSafety("gm_global_a_powertrain_generated") self.packer_chassis = CANPackerSafety("gm_global_a_chassis") diff --git a/opendbc/safety/tests/test_honda.py b/opendbc/safety/tests/test_honda.py index 44220095aa9..e26b78fe181 100755 --- a/opendbc/safety/tests/test_honda.py +++ b/opendbc/safety/tests/test_honda.py @@ -3,6 +3,7 @@ import numpy as np from opendbc.car.honda.values import HondaSafetyFlags +from opendbc.safety import ALTERNATIVE_EXPERIENCE from opendbc.safety.tests.libsafety import libsafety_py import opendbc.safety.tests.common as common from opendbc.car.structs import CarParams @@ -175,7 +176,7 @@ class HondaBase(common.CarSafetyTest): cnt_powertrain_data = 0 cnt_acc_state = 0 - def _powertrain_data_msg(self, cruise_on=None, brake_pressed=None, gas_pressed=None): + def _powertrain_data_msg(self, cruise_on=None, brake_pressed=None, gas_pressed=None, brake_switch=False): # preserve the state if cruise_on is None: # or'd with controls allowed since the tests use it to "enable" cruise @@ -188,6 +189,7 @@ def _powertrain_data_msg(self, cruise_on=None, brake_pressed=None, gas_pressed=N values = { "ACC_STATUS": cruise_on, "BRAKE_PRESSED": brake_pressed, + "BRAKE_SWITCH": brake_switch, "PEDAL_GAS": gas_pressed, "COUNTER": self.cnt_powertrain_data % 4 } @@ -232,10 +234,31 @@ def test_disengage_on_brake(self): self._rx(self._user_brake_msg(1)) self.assertFalse(self.safety.get_controls_allowed()) + def test_brake_switch_debounce(self): + if self.safety.get_current_safety_param() & HondaSafetyFlags.ALT_BRAKE: + self.skipTest("This configuration uses the separate driver brake message") + self.assertTrue(self._rx(self._speed_msg(1))) + # A one-frame switch pulse is ignored; consecutive frames detect braking + # before the delayed BRAKE_PRESSED signal. A release resets the debounce. + for switch, pressed in ((False, False), (True, False), (False, False), + (True, False), (True, True), (True, True), (False, False)): + self.safety.set_controls_allowed(True) + self.assertTrue(self._rx(self._powertrain_data_msg(brake_pressed=False, brake_switch=switch))) + self.assertEqual(pressed, self.safety.get_brake_pressed_prev()) + self.assertEqual(not pressed, self.safety.get_controls_allowed()) + def test_steer_safety_check(self): - self.safety.set_controls_allowed(0) - self.assertTrue(self._tx(self._send_steer_msg(0x0000))) - self.assertFalse(self._tx(self._send_steer_msg(0x1000))) + # Both steering addresses are admitted on Nidec. Nonzero bytes must be + # blocked when disabled, regardless of which byte carries the torque. + for addr, length in ((0xE4, 5), (0x194, 4)): + if [addr, self.STEER_BUS] not in self.TX_MSGS: + continue + for enabled in (False, True): + self.safety.set_controls_allowed(enabled) + for torque in (0, 1, 0x100, 0x1000): + with self.subTest(addr=addr, enabled=enabled, torque=torque): + data = torque.to_bytes(2, "big") + bytes(length - 2) + self.assertEqual(enabled or torque == 0, self._tx(libsafety_py.make_CANPacket(addr, self.STEER_BUS, data))) # ********************* Honda Nidec ********************** @@ -313,6 +336,17 @@ def test_honda_fwd_brake_latching(self): self.assertTrue(self._rx(self._rx_brake_msg(0, aeb_req=0))) self.assertFalse(self.safety.get_honda_fwd_brake()) + def test_disable_stock_aeb(self): + for disable_aeb in (False, True): + self.setUp() + self.safety.set_alternative_experience(ALTERNATIVE_EXPERIENCE.DISABLE_STOCK_AEB if disable_aeb else 0) + self.safety.set_controls_allowed(True) + self.assertTrue(self._tx(self._send_brake_msg(10))) + self.assertTrue(self._rx(self._rx_brake_msg(20, aeb_req=1))) + self.assertEqual(not disable_aeb, self.safety.get_honda_fwd_brake()) + self.assertEqual(-1 if disable_aeb else 0, self.safety.safety_fwd_hook(2, 0x1FA)) + self.assertEqual(disable_aeb, self._tx(self._send_brake_msg(10))) + def test_brake_safety_check(self): for fwd_brake in [False, True]: self.safety.set_honda_fwd_brake(fwd_brake) @@ -436,6 +470,18 @@ def setUp(self): self.safety.set_safety_hooks(CarParams.SafetyModel.hondaBosch, 0) self.safety.init_tests() + def test_supplemental_control_payload(self): + valid = b"\x04\x00\x80\x10\x00\x00\x00\x00" + self.assertTrue(self._tx(libsafety_py.make_CANPacket(0xE5, self.STEER_BUS, valid))) + for byte in range(7): + with self.subTest(byte=byte): + malformed = bytearray(valid) + malformed[byte] ^= 1 + self.assertFalse(self._tx(libsafety_py.make_CANPacket(0xE5, self.STEER_BUS, malformed))) + # The final byte carries the counter/checksum, not actuation fields. + for counter_checksum in (0, 0x55, 0xFF): + self.assertTrue(self._tx(libsafety_py.make_CANPacket(0xE5, self.STEER_BUS, valid[:7] + bytes([counter_checksum])))) + class TestHondaBoschAltBrakeSafety(HondaPcmEnableBase, TestHondaBoschAltBrakeSafetyBase): """ @@ -482,6 +528,12 @@ def test_diagnostics(self): not_tester_present = libsafety_py.make_CANPacket(0x18DAB0F1, self.PT_BUS, b"\x03\xAA\xAA\x00\x00\x00\x00\x00") self.assertFalse(self._tx(not_tester_present)) + # Reject mutations to every byte, including padding after the UDS request. + for byte in range(8): + malformed = bytearray(b"\x02\x3E\x80\x00\x00\x00\x00\x00") + malformed[byte] ^= 1 + self.assertFalse(self._tx(libsafety_py.make_CANPacket(0x18DAB0F1, self.PT_BUS, malformed))) + def test_gas_safety_check(self): for controls_allowed in [True, False]: for gas in np.arange(self.NO_GAS, self.MAX_GAS + 2000, 100): diff --git a/opendbc/safety/tests/test_hyundai.py b/opendbc/safety/tests/test_hyundai.py index e53a448f37b..6036f948baf 100755 --- a/opendbc/safety/tests/test_hyundai.py +++ b/opendbc/safety/tests/test_hyundai.py @@ -7,7 +7,7 @@ from opendbc.safety.tests.libsafety import libsafety_py import opendbc.safety.tests.common as common from opendbc.safety.tests.common import CANPackerSafety -from opendbc.safety.tests.hyundai_common import HyundaiButtonBase, HyundaiLongitudinalBase +from opendbc.safety.tests.hyundai_common import Buttons, HyundaiButtonBase, HyundaiLongitudinalBase # 4 bit checkusm used in some hyundai messages @@ -90,14 +90,25 @@ def _user_brake_msg(self, brake): self.__class__.cnt_brake += 1 return self.packer.make_can_msg_safety("TCS13", 0, values, fix_checksum=checksum) - def _speed_msg(self, speed): + def _speed_msg(self, speed, **wheel_speeds): # safety doesn't scale, so undo the scaling - values = {"WHL_SPD_%s" % s: speed * 0.03125 for s in ["FL", "FR", "RL", "RR"]} + values = {"WHL_SPD_%s" % s: wheel_speeds.get(s, speed) * 0.03125 for s in ["FL", "FR", "RL", "RR"]} values["WHL_SPD_AliveCounter_LSB"] = (self.cnt_speed % 16) & 0x3 values["WHL_SPD_AliveCounter_MSB"] = (self.cnt_speed % 16) >> 2 self.__class__.cnt_speed += 1 return self.packer.make_can_msg_safety("WHL_SPD11", 0, values, fix_checksum=checksum) + def test_vehicle_moving_single_wheel(self): + # Both wheel-speed inputs used for standstill must independently detect motion. + for wheel in ("FL", "RR"): + with self.subTest(wheel=wheel): + self.assertTrue(self._rx(self._speed_msg(0))) + self.assertFalse(self.safety.get_vehicle_moving()) + self.assertTrue(self._rx(self._speed_msg(0, **{wheel: self.STANDSTILL_THRESHOLD + 1}))) + self.assertTrue(self.safety.get_vehicle_moving()) + self.assertTrue(self._rx(self._speed_msg(0))) + self.assertFalse(self.safety.get_vehicle_moving()) + def _pcm_status_msg(self, enable): values = {"ACCMode": enable, "CR_VSM_Alive": self.cnt_cruise % 16} self.__class__.cnt_cruise += 1 @@ -201,6 +212,14 @@ class TestHyundaiLongitudinalSafety(HyundaiLongitudinalBase, TestHyundaiSafety): DISABLED_ECU_UDS_MSG = (0x7D0, 0) DISABLED_ECU_ACTUATION_MSG = (0x421, 0) + def test_button_sends(self): + # With openpilot longitudinal, CLU11 is forwarded without the stock SCC restrictions. + for controls_allowed in (False, True): + for button in (Buttons.NONE, Buttons.RESUME, Buttons.SET, Buttons.CANCEL): + with self.subTest(controls_allowed=controls_allowed, button=button): + self.safety.set_controls_allowed(controls_allowed) + self.assertTrue(self._tx(self._button_msg(button, bus=self.BUTTONS_TX_BUS))) + def setUp(self): self.packer = CANPackerSafety("hyundai_can_generated") self.safety = libsafety_py.libsafety @@ -271,6 +290,23 @@ def test_disabled_ecu_alive(self): pass +class TestHyundaiGasConfiguration(unittest.TestCase): + def test_hybrid_signal_does_not_act_as_ice_gas(self): + safety = libsafety_py.libsafety + packer = CANPackerSafety("hyundai_can_generated") + safety.set_safety_hooks(CarParams.SafetyModel.hyundai, 0) + safety.init_tests() + # The first accelerator packet selects the shared RX descriptor. Even though + # this alternative is admitted, an ICE configuration must ignore its pedal. + for pedal in (0, 1, 255, 0): + with self.subTest(pedal=pedal): + msg = packer.make_can_msg_safety("E_EMS11", 0, {"CR_Vcu_AccPedDep_Pos": pedal}) + safety.set_controls_allowed(True) + self.assertTrue(safety.safety_rx_hook(msg)) + self.assertFalse(safety.get_gas_pressed_prev()) + self.assertTrue(safety.get_controls_allowed()) + + class TestHyundaiSafetyFCEVLong(TestHyundaiLongitudinalSafety, TestHyundaiSafetyFCEV): def setUp(self): self.packer = CANPackerSafety("hyundai_can_generated") diff --git a/opendbc/safety/tests/test_hyundai_canfd.py b/opendbc/safety/tests/test_hyundai_canfd.py index 6cec7376ece..52deefd9856 100755 --- a/opendbc/safety/tests/test_hyundai_canfd.py +++ b/opendbc/safety/tests/test_hyundai_canfd.py @@ -7,7 +7,7 @@ from opendbc.safety.tests.libsafety import libsafety_py import opendbc.safety.tests.common as common from opendbc.safety.tests.common import CANPackerSafety -from opendbc.safety.tests.hyundai_common import HyundaiButtonBase, HyundaiLongitudinalBase +from opendbc.safety.tests.hyundai_common import Buttons, HyundaiButtonBase, HyundaiLongitudinalBase # All combinations of radar/camera-SCC and gas/hybrid/EV cars ALL_GAS_EV_HYBRID_COMBOS = [ @@ -56,10 +56,53 @@ def _torque_cmd_msg(self, torque, steer_req=1): values = {"StrTqReqVal": torque, "ActToiSta": steer_req} return self.packer.make_can_msg_safety(self.STEER_MSG, self.STEER_BUS, values) - def _speed_msg(self, speed): - values = {f"WHL_Spd{pos}Val": speed * 0.03125 for pos in ["FL", "FR", "RL", "RR"]} + def _speed_msg(self, speed, **wheel_speeds): + values = {f"WHL_Spd{pos}Val": wheel_speeds.get(pos, speed) * 0.03125 for pos in ["FL", "FR", "RL", "RR"]} return self.packer.make_can_msg_safety("WHEEL_SPEEDS", self.PT_BUS, values) + def test_vehicle_moving_single_wheel(self): + for wheel in ("FL", "FR", "RL", "RR"): + with self.subTest(wheel=wheel): + self.assertTrue(self._rx(self._speed_msg(0))) + self.assertFalse(self.safety.get_vehicle_moving()) + self.assertTrue(self._rx(self._speed_msg(0, **{wheel: self.STANDSTILL_THRESHOLD + 1}))) + self.assertTrue(self.safety.get_vehicle_moving()) + self.assertTrue(self._rx(self._speed_msg(0))) + self.assertFalse(self.safety.get_vehicle_moving()) + + def test_gas_powertrain_isolation(self): + # The RX table admits all three accelerator addresses. Only the configured + # powertrain's signal may change pedal state. Gas override preserves controls. + gas_messages = ( + ("ACCELERATOR_BRAKE_ALT", "ACCELERATOR_PEDAL_PRESSED", (0, 1)), + ("ACCELERATOR", "ACCELERATOR_PEDAL", (0, 1)), + ("ACCELERATOR_ALT", "ACCELERATOR_PEDAL", (0, 0.25, 0.5, 128)), + ) + for name, signal, pedal_values in gas_messages: + for pedal in pedal_values: + with self.subTest(message=name, pedal=pedal): + # Reset the RX alternative selection before testing each message. + self.setUp() + self.safety.set_controls_allowed(True) + msg = self.packer.make_can_msg_safety(name, self.PT_BUS, {signal: pedal}) + self.assertTrue(self._rx(msg)) + pressed = name == self.GAS_MSG[0] and pedal > 0 + self.assertEqual(pressed, self.safety.get_gas_pressed_prev()) + self.assertTrue(self.safety.get_controls_allowed()) + + def test_cruise_states(self): + if isinstance(self, HyundaiLongitudinalBase): + self.skipTest("Longitudinal mode enables from cruise buttons") + # ACC remains engaged during driver override, but not in cancel/fault states. + for state in range(8): + with self.subTest(state=state): + self.assertTrue(self._rx(self._pcm_status_msg(False))) + self.assertTrue(self._rx(self._button_msg(Buttons.SET))) + msg = self.packer.make_can_msg_safety("SCC_CONTROL", self.SCC_BUS, {"ACCMode": state}) + self.assertTrue(self._rx(msg)) + self.assertEqual(state in (1, 2), self.safety.get_controls_allowed()) + self.assertEqual(state in (1, 2), self.safety.get_cruise_engaged_prev()) + def _user_brake_msg(self, brake): values = {"DriverBraking": brake} return self.packer.make_can_msg_safety("TCS", self.PT_BUS, values) @@ -129,8 +172,8 @@ def _button_msg(self, buttons, main_button=0, bus=1): } return self.packer.make_can_msg_safety("CRUISE_BUTTONS_ALT", self.PT_BUS, values) - def _acc_cancel_msg(self, cancel, accel=0): - values = {"ACCMode": 4 if cancel else 0, "aReqRaw": accel, "aReqValue": accel} + def _acc_cancel_msg(self, cancel, accel=0, accel_raw=None): + values = {"ACCMode": 4 if cancel else 0, "aReqRaw": accel if accel_raw is None else accel_raw, "aReqValue": accel} return self.packer.make_can_msg_safety("SCC_CONTROL", self.PT_BUS, values) def test_button_sends(self): @@ -148,6 +191,8 @@ def test_acc_cancel(self): self.safety.set_controls_allowed(enabled) self.assertTrue(self._tx(self._acc_cancel_msg(True))) self.assertFalse(self._tx(self._acc_cancel_msg(True, accel=1))) + for raw, value in ((0, 1), (1, 0), (0, -1), (-1, 0)): + self.assertFalse(self._tx(self._acc_cancel_msg(True, accel=value, accel_raw=raw))) self.assertFalse(self._tx(self._acc_cancel_msg(False))) diff --git a/opendbc/safety/tests/test_mazda.py b/opendbc/safety/tests/test_mazda.py index 8fb77444a7c..d963b7b736a 100755 --- a/opendbc/safety/tests/test_mazda.py +++ b/opendbc/safety/tests/test_mazda.py @@ -56,6 +56,13 @@ def _user_gas_msg(self, gas): values = {"PEDAL_GAS": gas} return self.packer.make_can_msg_safety("ENGINE_DATA", 0, values) + def test_gas_signal_bytes(self): + # The pedal spans two bytes; activity in either byte must be detected. + for gas in (0, 1, 16, 0): + with self.subTest(gas=gas): + self.assertTrue(self._rx(self._user_gas_msg(gas))) + self.assertEqual(gas != 0, self.safety.get_gas_pressed_prev()) + def _pcm_status_msg(self, enable): values = {"CRZ_ACTIVE": enable} return self.packer.make_can_msg_safety("CRZ_CTRL", 0, values) diff --git a/opendbc/safety/tests/test_mg.py b/opendbc/safety/tests/test_mg.py index 06b85e01b67..07acab1790d 100755 --- a/opendbc/safety/tests/test_mg.py +++ b/opendbc/safety/tests/test_mg.py @@ -72,6 +72,15 @@ def _pcm_status_msg(self, enable): values = {"ACCSysSts_RadarHSC2": 2 if enable else 1, "ACCSysAlvRlngCtr_SCSHSC2": self._counter(0x242)} return self.packer.make_can_msg_safety("RADAR_HSC2_FrP00", 0, values, fix_checksum=checksum) + def test_cruise_states(self): + for state in range(8): + with self.subTest(state=state): + self.assertTrue(self._rx(self._pcm_status_msg(False))) + values = {"ACCSysSts_RadarHSC2": state, "ACCSysAlvRlngCtr_SCSHSC2": self._counter(0x242)} + self.assertTrue(self._rx(self.packer.make_can_msg_safety("RADAR_HSC2_FrP00", 0, values, fix_checksum=checksum))) + self.assertEqual(state in (2, 3), self.safety.get_controls_allowed()) + self.assertEqual(state in (2, 3), self.safety.get_cruise_engaged_prev()) + if __name__ == "__main__": unittest.main() diff --git a/opendbc/safety/tests/test_nissan.py b/opendbc/safety/tests/test_nissan.py index 7b5e6781462..19f53894757 100755 --- a/opendbc/safety/tests/test_nissan.py +++ b/opendbc/safety/tests/test_nissan.py @@ -44,6 +44,20 @@ def _pcm_status_msg(self, enable): values = {"CRUISE_ENABLED": enable} return self.packer.make_can_msg_safety("CRUISE_STATE", self.CRUISE_BUS, values) + def test_cruise_wrong_variant_bus_first_packet(self): + # Both buses are RX alternatives, so the variant guard must also protect the + # first packet, before the RX check has selected a bus. + for enabled in (False, True): + with self.subTest(enabled=enabled): + self.setUp() + self.safety.set_controls_allowed(not enabled) + self.safety.set_cruise_engaged_prev(not enabled) + msg = self._pcm_status_msg(enabled) + msg[0].bus = 1 if self.CRUISE_BUS == 2 else 2 + self.assertTrue(self._rx(msg)) + self.assertEqual(self.safety.get_controls_allowed(), not enabled) + self.assertEqual(self.safety.get_cruise_engaged_prev(), not enabled) + def _speed_msg(self, speed): values = {"WHEEL_SPEED_%s" % s: speed * 3.6 for s in ["RR", "RL"]} return self.packer.make_can_msg_safety("WHEEL_SPEEDS_REAR", self.EPS_BUS, values) diff --git a/opendbc/safety/tests/test_safety_config.py b/opendbc/safety/tests/test_safety_config.py new file mode 100644 index 00000000000..bd6da3dee16 --- /dev/null +++ b/opendbc/safety/tests/test_safety_config.py @@ -0,0 +1,161 @@ +import itertools +import unittest + +from opendbc.car.structs import CarParams +from opendbc.safety.tests.libsafety import libsafety_py + + +class TestSafetyConfig(unittest.TestCase): + def setUp(self): + self.safety = libsafety_py.libsafety + self.safety.init_tests() + + def tearDown(self): + self.safety.set_safety_hooks(CarParams.SafetyModel.noOutput, 0) + + def configure(self, frequency=100, ignore_checksum=True, ignore_counter=True, ignore_quality_flag=True, max_counter=0, callbacks=0): + self.safety.safety_test_configure_rx(frequency, ignore_checksum, ignore_counter, ignore_quality_flag, max_counter, callbacks) + self.safety.set_timer(0) + + def message(self, address=0x123, bus=0, length=8, checksum=1, payload=1, counter=1, quality=1, steering_disengage=False): + return libsafety_py.make_CANPacket(address, bus, (bytes((checksum, payload, counter, quality, steering_disengage)) + bytes(59))[:length]) + + def test_rx_whitelist(self): + # Both before and after choosing an RX descriptor, a wrong bus, address or + # length must never reach the car hook or update its liveness timestamp. + for seen, (address, bus, length) in itertools.product((False, True), ((0x124, 0, 8), (0x123, 1, 8), (0x123, 0, 7))): + with self.subTest(seen=seen, address=address, bus=bus, length=length): + self.configure() + if seen: + self.assertTrue(self.safety.safety_rx_hook(self.message())) + self.safety.set_timer(1_000_001) + self.assertTrue(self.safety.safety_rx_hook(self.message(address, bus, length))) + self.assertEqual(self.safety.safety_test_get_rx_count(), int(seen)) + self.safety.set_controls_allowed(True) + self.safety.safety_tick_current_safety_config() + self.assertFalse(self.safety.get_controls_allowed()) + self.assertTrue(self.safety.get_safety_rx_checks_invalid()) + + def test_missing_checksum_callbacks(self): + for callbacks, ignored in itertools.product(range(4), (False, True)): + with self.subTest(callbacks=callbacks, ignored=ignored): + self.configure(ignore_checksum=ignored, callbacks=callbacks) + self.safety.set_controls_allowed(True) + expected = ignored or callbacks == 3 + self.assertEqual(self.safety.safety_rx_hook(self.message()), expected) + self.assertEqual(self.safety.get_controls_allowed(), expected) + self.assertEqual(self.safety.safety_test_get_rx_count(), int(expected)) + + def test_missing_counter_configuration(self): + for callbacks, max_counter, ignored in itertools.product((0, 4), (0, 3), (False, True)): + with self.subTest(callbacks=callbacks, max_counter=max_counter, ignored=ignored): + self.configure(ignore_counter=ignored, max_counter=max_counter, callbacks=callbacks) + self.safety.set_controls_allowed(True) + expected = ignored or bool(callbacks and max_counter) + self.assertEqual(self.safety.safety_rx_hook(self.message()), expected) + self.assertEqual(self.safety.get_controls_allowed(), expected) + + def test_missing_quality_callback(self): + for callbacks, ignored in itertools.product((0, 8), (False, True)): + with self.subTest(callbacks=callbacks, ignored=ignored): + self.configure(ignore_quality_flag=ignored, callbacks=callbacks) + self.safety.set_controls_allowed(True) + expected = ignored or bool(callbacks) + self.assertEqual(self.safety.safety_rx_hook(self.message()), expected) + self.assertEqual(self.safety.get_controls_allowed(), expected) + + def test_tick_lag_boundary_and_minimum_frequency(self): + for frequency in (5, 9, 10, 100): + threshold = max(10 * (1_000_000 // frequency), 1_000_000) + for elapsed in (threshold - 1, threshold, threshold + 1): + with self.subTest(frequency=frequency, elapsed=elapsed): + self.configure(frequency=frequency) + self.assertTrue(self.safety.safety_rx_hook(self.message())) + self.safety.set_controls_allowed(True) + self.safety.set_timer(elapsed) + self.safety.safety_tick_current_safety_config() + invalid = elapsed > threshold or frequency < 10 + self.assertEqual(self.safety.get_safety_rx_checks_invalid(), invalid) + self.assertEqual(self.safety.get_controls_allowed(), not invalid) + + def test_tick_checks_message_validity(self): + self.configure(ignore_checksum=False, callbacks=3) + self.assertFalse(self.safety.safety_rx_hook(self.message(checksum=0))) + self.safety.set_controls_allowed(True) + self.safety.safety_tick_current_safety_config() + self.assertTrue(self.safety.get_safety_rx_checks_invalid()) + self.assertFalse(self.safety.get_controls_allowed()) + + def test_tick_without_config(self): + self.safety.set_controls_allowed(True) + self.safety.safety_test_tick_null() + self.assertFalse(self.safety.get_safety_rx_checks_invalid()) + self.assertTrue(self.safety.get_controls_allowed()) + + def test_unknown_safety_mode(self): + self.safety.set_controls_allowed(True) + self.assertEqual(self.safety.set_safety_hooks(0xFFFF, 0), -1) + self.assertFalse(self.safety.get_controls_allowed()) + + def test_steering_override_edges(self): + self.configure() + for override, should_disengage in ((True, True), (True, False), (False, False), (True, True)): + with self.subTest(override=override, should_disengage=should_disengage): + self.safety.set_controls_allowed(True) + self.assertTrue(self.safety.safety_rx_hook(self.message(steering_disengage=override))) + self.assertEqual(self.safety.get_controls_allowed(), not should_disengage) + + def test_ignition_rejects_wrong_bus_or_length(self): + for address, length, initial in itertools.product((0x1F1, 0x152, 0x221, 0x9E, 0x3C0), (0, 1, 7, 12), (False, True)): + # Ignition parsing accepts bus 0 and exactly 8 bytes (4 for MEB). + for bus, actual_length in ((0, length), (1, 4 if address == 0x3C0 else 8)): + with self.subTest(address=address, bus=bus, length=actual_length, initial=initial): + self.safety.set_ignition_can(initial) + for counter in range(3): + data = bytearray(12) + if address == 0x1F1: + data[0] = 0 if initial else 2 + elif address == 0x152: + data[1], data[7] = counter, 0 if initial else 0x10 + elif address == 0x221: + data[6], data[0] = counter << 4, 0 if initial else 0x60 + elif address == 0x9E: + data[0] = 0 if initial else 0xC0 + else: + data[1], data[2] = counter, 0 if initial else 2 + self.safety.ignition_can_hook(libsafety_py.make_CANPacket(address, bus, data[:actual_length])) + self.assertEqual(self.safety.get_ignition_can(), initial) + + def test_interpolation_small_interval(self): + # Preserve the small-denominator guard, including inputs below its floor. + for midpoint in (0.00005, 0.0001, 0.1): + with self.subTest(midpoint=midpoint): + self.assertEqual(self.safety.safety_test_interpolate(-1, midpoint), 0) + self.assertEqual(self.safety.safety_test_interpolate(2, midpoint), 2) + expected = (midpoint / 2) / max(midpoint, 0.0001) + self.assertAlmostEqual(self.safety.safety_test_interpolate(midpoint / 2, midpoint), expected, places=6) + + def test_invalid_dynamic_torque_lookup_fails_closed(self): + for torque in (-1000, 0, 1000): + with self.subTest(torque=torque): + self.safety.set_safety_hooks(CarParams.SafetyModel.noOutput, 0) + self.safety.set_controls_allowed(True) + violation = self.safety.safety_test_dynamic_torque_limit(torque) + self.assertEqual(violation, torque < 0) + + def test_signed_conversion_and_shift_floor(self): + for bits in (1, 2, 8, 16, 30): + sign = 1 << (bits - 1) + for value, expected in ((0, 0), (sign - 1, sign - 1), (sign, -sign), ((1 << bits) - 1, -1)): + with self.subTest(bits=bits, value=value): + self.assertEqual(self.safety.to_signed(value, bits), expected) + # Preserve the defensive shift floor for nonpositive widths without + # evaluating a negative C shift (the test library also enables UBSan). + for bits in (-1, 0): + for value, expected in ((-1, -1), (0, 0), (1, 0), (2, 1)): + with self.subTest(bits=bits, value=value): + self.assertEqual(self.safety.to_signed(value, bits), expected) + + +if __name__ == "__main__": + unittest.main() diff --git a/opendbc/safety/tests/test_subaru.py b/opendbc/safety/tests/test_subaru.py index 8c3d69dda6d..f6931e75fbc 100755 --- a/opendbc/safety/tests/test_subaru.py +++ b/opendbc/safety/tests/test_subaru.py @@ -73,10 +73,20 @@ def _torque_driver_msg(self, torque): values = {"Steer_Torque_Sensor": torque} return self.packer.make_can_msg_safety("Steering_Torque", 0, values) - def _speed_msg(self, speed): - values = {s: speed for s in ["FR", "FL", "RR", "RL"]} + def _speed_msg(self, speed, **wheel_speeds): + values = {s: wheel_speeds.get(s, speed) for s in ["FR", "FL", "RR", "RL"]} return self.packer.make_can_msg_safety("Wheel_Speeds", self.ALT_MAIN_BUS, values) + def test_vehicle_moving_single_wheel(self): + for wheel in ("FR", "FL", "RR", "RL"): + with self.subTest(wheel=wheel): + self.assertTrue(self._rx(self._speed_msg(0))) + self.assertFalse(self.safety.get_vehicle_moving()) + self.assertTrue(self._rx(self._speed_msg(0, **{wheel: 1}))) + self.assertTrue(self.safety.get_vehicle_moving()) + self.assertTrue(self._rx(self._speed_msg(0))) + self.assertFalse(self.safety.get_vehicle_moving()) + def _user_brake_msg(self, brake): values = {"Brake": brake} return self.packer.make_can_msg_safety("Brake_Status", self.ALT_MAIN_BUS, values) diff --git a/opendbc/safety/tests/test_subaru_preglobal.py b/opendbc/safety/tests/test_subaru_preglobal.py index fb96de24898..00d21221667 100755 --- a/opendbc/safety/tests/test_subaru_preglobal.py +++ b/opendbc/safety/tests/test_subaru_preglobal.py @@ -38,11 +38,22 @@ def _torque_driver_msg(self, torque): values = {"Steer_Torque_Sensor": torque} return self.packer.make_can_msg_safety("Steering_Torque", 0, values) - def _speed_msg(self, speed): + def _speed_msg(self, speed, **wheel_speeds): # subaru safety doesn't use the scaled value, so undo the scaling - values = {s: speed*0.0592 for s in ["FR", "FL", "RR", "RL"]} + values = {s: wheel_speeds.get(s, speed) * 0.0592 for s in ["FR", "FL", "RR", "RL"]} return self.packer.make_can_msg_safety("Wheel_Speeds", 0, values) + def test_vehicle_moving_rear_or_right_front_wheel(self): + # Verify each wheel currently decoded independently by this safety mode. + for wheel in ("FR", "RR", "RL"): + with self.subTest(wheel=wheel): + self.assertTrue(self._rx(self._speed_msg(0))) + self.assertFalse(self.safety.get_vehicle_moving()) + self.assertTrue(self._rx(self._speed_msg(0, **{wheel: 1}))) + self.assertTrue(self.safety.get_vehicle_moving()) + self.assertTrue(self._rx(self._speed_msg(0))) + self.assertFalse(self.safety.get_vehicle_moving()) + def _user_brake_msg(self, brake): values = {"Brake_Pedal": brake} return self.packer.make_can_msg_safety("Brake_Pedal", 0, values) diff --git a/opendbc/safety/tests/test_tesla.py b/opendbc/safety/tests/test_tesla.py index c4e42884f34..67e27197331 100755 --- a/opendbc/safety/tests/test_tesla.py +++ b/opendbc/safety/tests/test_tesla.py @@ -94,6 +94,15 @@ def _angle_meas_msg(self, angle: float, hands_on_level: int = 0, eac_status: int self.__class__.cnt_epas += 1 return self.packer.make_can_msg_safety("EPAS3S_sysStatus", 0, values) + def test_inactive_angle_reset_clips_measurement(self): + for angle in (-self.STEER_ANGLE_MAX - 1, 0, self.STEER_ANGLE_MAX + 1): + with self.subTest(angle=angle): + self.safety.set_controls_allowed(False) + self._reset_angle_measurement(angle) + clipped_angle = min(max(angle, -self.STEER_ANGLE_MAX), self.STEER_ANGLE_MAX) + self.assertTrue(self._tx(self._angle_cmd_msg(clipped_angle, False))) + self.assertEqual(round(clipped_angle * self.DEG_TO_CAN), self.safety.get_desired_angle_last()) + def _user_brake_msg(self, brake, quality_flag: bool = True): values = {"ESP_driverBrakeApply": 2 if brake else 1} if not quality_flag: @@ -124,6 +133,15 @@ def _pcm_status_msg(self, enable, autopark_state=0): } return self.packer.make_can_msg_safety("DI_state", 0, values) + def test_cruise_states(self): + for state in range(8): + with self.subTest(state=state): + self.assertTrue(self._rx(self._pcm_status_msg(False))) + msg = self.packer.make_can_msg_safety("DI_state", 0, {"DI_cruiseState": state}) + self.assertTrue(self._rx(msg)) + self.assertEqual(state in (2, 3, 4, 6, 7), self.safety.get_controls_allowed()) + self.assertEqual(state in (2, 3, 4, 6, 7), self.safety.get_cruise_engaged_prev()) + def _long_control_msg(self, set_speed, acc_state=0, jerk_limits=(0, 0), accel_limits=(0, 0), aeb_event=0, bus=0): values = { "DAS_setSpeed": set_speed, @@ -293,6 +311,44 @@ def test_stock_lkas_passthrough(self): self.assertEqual(0, self.safety.safety_fwd_hook(2, lkas_msg_cam.addr)) self.assertFalse(self._tx(no_lkas_msg)) + def test_stock_lkas_rising_edge_while_disengaged(self): + def receive_lkas(active): + state = self.steer_control_types["LANE_KEEP_ASSIST"] if active else self.steer_control_types["NONE"] + self.assertTrue(self._rx(self._angle_cmd_msg(0, state=state, bus=2))) + + self.assertTrue(self._rx(self._pcm_status_msg(True))) + receive_lkas(False) + receive_lkas(True) + self.assertEqual(-1, self.safety.safety_fwd_hook(2, MSG_DAS_steeringControl)) + self.assertTrue(self._tx(self._angle_cmd_msg(0, True))) + + # Disengaging while stock LKAS is already active must not latch passthrough. + self.assertTrue(self._rx(self._pcm_status_msg(False))) + receive_lkas(True) + self.assertEqual(-1, self.safety.safety_fwd_hook(2, MSG_DAS_steeringControl)) + self.assertTrue(self._tx(self._angle_cmd_msg(0, False))) + + # A fresh activation while disengaged does latch it until stock LKAS exits. + receive_lkas(False) + for _ in range(2): + receive_lkas(True) + self.assertEqual(0, self.safety.safety_fwd_hook(2, MSG_DAS_steeringControl)) + self.assertFalse(self._tx(self._angle_cmd_msg(0, False))) + receive_lkas(False) + self.assertEqual(-1, self.safety.safety_fwd_hook(2, MSG_DAS_steeringControl)) + self.assertTrue(self._tx(self._angle_cmd_msg(0, False))) + + def test_autopark_forwards_stock_commands(self): + for state in self.active_autopark_states: + self.assertTrue(self._rx(self._pcm_status_msg(False))) + self.assertTrue(self._rx(self._pcm_status_msg(False, state))) + for addr in (MSG_DAS_steeringControl, MSG_APS_eacMonitor, MSG_DAS_Control): + self.assertEqual(0, self.safety.safety_fwd_hook(2, addr)) + self.assertTrue(self._rx(self._pcm_status_msg(False))) + self.assertEqual(-1, self.safety.safety_fwd_hook(2, MSG_DAS_steeringControl)) + self.assertEqual(-1, self.safety.safety_fwd_hook(2, MSG_APS_eacMonitor)) + self.assertEqual(-1 if self.LONGITUDINAL else 0, self.safety.safety_fwd_hook(2, MSG_DAS_Control)) + def test_angle_cmd_when_enabled(self): # We properly test lateral acceleration and jerk below pass diff --git a/opendbc/safety/tests/test_toyota.py b/opendbc/safety/tests/test_toyota.py index 64e57e5fd83..025343cc1f9 100755 --- a/opendbc/safety/tests/test_toyota.py +++ b/opendbc/safety/tests/test_toyota.py @@ -1,6 +1,5 @@ #!/usr/bin/env python3 import numpy as np -import random import unittest import itertools @@ -81,20 +80,22 @@ def _pcm_status_msg(self, enable): def test_diagnostics(self, stock_longitudinal: bool = False, ecu_disabled: bool = True): 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 + (False, b"\x0F\x02\x3E\x00\x01\x00\x00\x00"), # valid prefix, invalid trailing data (True, b"\x0F\x02\x3E\x00\x00\x00\x00\x00")): tester_present = libsafety_py.make_CANPacket(0x750, 0, msg) self.assertEqual(should_tx and ecu_disabled and not stock_longitudinal, self._tx(tester_present)) def test_block_aeb(self, stock_longitudinal: bool = False): - for controls_allowed in (True, False): - for bad in (True, False): - for _ in range(10): - self.safety.set_controls_allowed(controls_allowed) - dat = [random.randint(1, 255) for _ in range(7)] - if not bad: - dat = [0]*6 + dat[-1:] - msg = libsafety_py.make_CANPacket(0x283, 0, bytes(dat)) - self.assertEqual(not bad and not stock_longitudinal, self._tx(msg)) + # A single nonzero actuation byte must be rejected independently of the + # other fields. The final checksum byte is the only unrestricted byte. + for controls_allowed, byte, value in itertools.product((True, False), range(7), (0, 1, 255)): + with self.subTest(controls_allowed=controls_allowed, byte=byte, value=value): + self.safety.set_controls_allowed(controls_allowed) + dat = bytearray(7) + dat[byte] = value + msg = libsafety_py.make_CANPacket(0x283, 0, dat) + should_tx = (byte == 6 or value == 0) and not stock_longitudinal + self.assertEqual(should_tx, self._tx(msg)) # Only allow LTA msgs with no actuation def test_lta_steer_cmd(self): @@ -156,6 +157,28 @@ def setUp(self): self.safety.set_safety_hooks(CarParams.SafetyModel.toyota, self.EPS_SCALE) self.safety.init_tests() + def test_initializing_angle_does_not_update_sample(self): + # Torque mode accepts this packet without using its angle quality flag for + # RX validity, but an initializing angle must not enter the sample window. + for initializing in (True, False): + self.safety.set_angle_meas(0, 0) + for _ in range(6): + self.assertTrue(self._rx(self._angle_meas_msg(10, initializing))) + expected = 0 if initializing else round(10 / 0.0573) + self.assertEqual(self.safety.get_angle_meas_min(), expected) + self.assertEqual(self.safety.get_angle_meas_max(), expected) + + def test_unwind_toward_measurement_at_rate_limit(self): + # When the previous command exceeds the measurement allowance, require the + # full unwind rate until the command returns to that allowance. + previous = self.MAX_TORQUE_ERROR + self.MAX_RATE_DOWN + 10 + for sign, unwind in itertools.product((-1, 1), (self.MAX_RATE_DOWN - 1, self.MAX_RATE_DOWN)): + with self.subTest(sign=sign, unwind=unwind): + self.safety.set_controls_allowed(True) + self._set_prev_torque(sign * previous) + self.safety.set_torque_meas(0, 0) + self.assertEqual(self._tx(self._torque_cmd_msg(sign * (previous - unwind))), unwind == self.MAX_RATE_DOWN) + class TestToyotaSafetyAngle(TestToyotaSafetyBase, common.AngleSteeringSafetyTest): @@ -243,6 +266,25 @@ def test_lta_steer_cmd(self): self.assertEqual(should_tx, self._tx(self._lta_msg(1, 1, angle, 100))) self.assertTrue(self._tx(self._lta_msg(1, 1, angle, 0))) # should tx if we wind down torque + def test_driver_override_uses_nearest_sample_to_zero(self): + # A recent sample within the driver torque limit allows full LTA torque, + # even while older samples in the window exceed it, for either direction. + for sign in (-1, 1): + for low in (0, self.MAX_LTA_DRIVER_TORQUE): + with self.subTest(sign=sign, low=low): + self._reset_angle_measurement(0) + self._set_prev_desired_angle(0) + self.safety.set_controls_allowed(True) + high = self.MAX_LTA_DRIVER_TORQUE + 1 + for _ in range(6): + self.assertTrue(self._rx(self._torque_meas_msg(0, sign * high))) + self.assertFalse(self._tx(self._lta_msg(1, 1, 0, 100))) + self.assertTrue(self._rx(self._torque_meas_msg(0, sign * low))) + self.assertTrue(self._tx(self._lta_msg(1, 1, 0, 100))) + for _ in range(6): + self.assertTrue(self._rx(self._torque_meas_msg(0, sign * high))) + self.assertFalse(self._tx(self._lta_msg(1, 1, 0, 100))) + def test_angle_measurements(self): """ * Tests angle meas quality flag dictates whether angle measurement is parsed, and if rx is valid diff --git a/opendbc/safety/tests/test_volkswagen_meb.py b/opendbc/safety/tests/test_volkswagen_meb.py index 05a92693489..64b1c5dac69 100644 --- a/opendbc/safety/tests/test_volkswagen_meb.py +++ b/opendbc/safety/tests/test_volkswagen_meb.py @@ -95,11 +95,31 @@ def test_power_without_control(self): self.assertFalse(self._tx(self._curvature_cmd_msg(0, steer_req=True, power=max_power))) # steady power self.assertTrue(self._tx(self._curvature_cmd_msg(0, steer_req=True, power=max_power - 1))) # decreasing power - def _speed_msg(self, speed_mps: float): - spd_kph = speed_mps * 3.6 - values = {f"{s}_Radgeschw": spd_kph for s in ("VL", "VR", "HL", "HR")} + def _speed_msg(self, speed_mps: float, **wheel_speeds): + values = {f"{s}_Radgeschw": wheel_speeds.get(s, speed_mps) * 3.6 for s in ("VL", "VR", "HL", "HR")} return self.packer.make_can_msg_safety("ESC_51", 0, values) + def test_vehicle_moving_single_wheel(self): + for wheel in ("VL", "VR", "HL", "HR"): + with self.subTest(wheel=wheel): + self.assertTrue(self._rx(self._speed_msg(0))) + self.assertFalse(self.safety.get_vehicle_moving()) + self.assertTrue(self._rx(self._speed_msg(0, **{wheel: 1}))) + self.assertTrue(self.safety.get_vehicle_moving()) + self.assertTrue(self._rx(self._speed_msg(0))) + self.assertFalse(self.safety.get_vehicle_moving()) + + def test_cruise_main_states(self): + # Standby, active and override retain the main switch permission. A status + # frame alone must never enable openpilot longitudinal control. + for state in range(8): + for enabled in (False, True): + with self.subTest(state=state, enabled=enabled): + self.safety.set_controls_allowed(enabled) + msg = self.packer.make_can_msg_safety("Motor_51", 0, {"TSK_Status": state}) + self.assertTrue(self._rx(msg)) + self.assertEqual(enabled and state in (2, 3, 4, 5), self.safety.get_controls_allowed()) + def _speed_msg_2(self, speed_mps: float): values = {"ESP_v_Signal": speed_mps * 3.6} return self.packer.make_can_msg_safety("ESP_21", 0, values) @@ -297,6 +317,8 @@ def test_set_and_resume_buttons(self): self._rx(self._tsk_status_msg(False, main_switch=True)) self._rx(self._button_msg(_set=(button == "set"), resume=(button == "resume"), bus=0)) self.assertFalse(self.safety.get_controls_allowed(), f"controls allowed on {button} rising edge") + self._rx(self._button_msg(_set=(button == "set"), resume=(button == "resume"), bus=0)) + self.assertFalse(self.safety.get_controls_allowed(), f"controls allowed while holding {button}") self._rx(self._button_msg(bus=0)) self.assertTrue(self.safety.get_controls_allowed(), f"controls not allowed on {button} falling edge") diff --git a/opendbc/safety/tests/test_volkswagen_mlb.py b/opendbc/safety/tests/test_volkswagen_mlb.py index 57bb93a77de..007e7e72367 100755 --- a/opendbc/safety/tests/test_volkswagen_mlb.py +++ b/opendbc/safety/tests/test_volkswagen_mlb.py @@ -53,6 +53,15 @@ def _tsk_status_msg(self, enable): def _pcm_status_msg(self, enable): return self._tsk_status_msg(enable) + def test_cruise_states(self): + for state in range(4): + with self.subTest(state=state): + self.assertTrue(self._rx(self._pcm_status_msg(False))) + msg = self.packer.make_can_msg_safety("TSK_04", 1, {"TSK_Status_GRA_ACC_02": state}) + self.assertTrue(self._rx(msg)) + self.assertEqual(state in (1, 2), self.safety.get_controls_allowed()) + self.assertEqual(state in (1, 2), self.safety.get_cruise_engaged_prev()) + # Driver steering input torque def _torque_driver_msg(self, torque): values = {"EPS_Lenkmoment": abs(torque), "EPS_VZ_Lenkmoment": torque < 0} @@ -128,9 +137,20 @@ def test_cancel_button(self): # Disable on rising edge of cancel button self._rx(self._tsk_status_msg(False)) self.safety.set_controls_allowed(1) + self.assertTrue(self._rx(self._ls_01_msg(bus=0))) + self.assertTrue(self.safety.get_controls_allowed()) self._rx(self._ls_01_msg(cancel=True, bus=0)) self.assertFalse(self.safety.get_controls_allowed(), "controls allowed after cancel") + def test_steering_status(self): + for status in range(16): + with self.subTest(status=status): + self.safety.set_controls_allowed(True) + self._set_prev_torque(0) + values = {"HCA_01_LM_Offset": 1, "HCA_01_Status_HCA": status} + msg = self.packer.make_can_msg_safety("HCA_01", 0, values) + self.assertEqual(status in (5, 7), self._tx(msg)) + if __name__ == "__main__": unittest.main() diff --git a/opendbc/safety/tests/test_volkswagen_mqb.py b/opendbc/safety/tests/test_volkswagen_mqb.py index 9ef7e261f0c..5d24fdab816 100755 --- a/opendbc/safety/tests/test_volkswagen_mqb.py +++ b/opendbc/safety/tests/test_volkswagen_mqb.py @@ -127,6 +127,29 @@ class TestVolkswagenMqbStockSafety(TestVolkswagenMqbSafetyBase): 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]] FWD_BLACKLISTED_ADDRS = {0: [MSG_LH_EPS_03], 2: [MSG_HCA_01, MSG_LDW_02]} + def test_cruise_states(self): + for state in range(8): + with self.subTest(state=state): + self.assertTrue(self._rx(self._pcm_status_msg(False))) + msg = self.packer.make_can_msg_safety("TSK_06", 0, {"TSK_Status": state}) + self.assertTrue(self._rx(msg)) + self.assertEqual(state in (3, 4, 5), self.safety.get_controls_allowed()) + self.assertEqual(state in (3, 4, 5), self.safety.get_cruise_engaged_prev()) + + def test_stock_cruise_buttons_do_not_enable(self): + self.assertTrue(self._rx(self._tsk_status_msg(False))) + for button in ("GRA_Tip_Setzen", "GRA_Tip_Wiederaufnahme"): + for pressed in (True, False): + msg = self.packer.make_can_msg_safety("GRA_ACC_01", 0, {button: pressed}) + self.assertTrue(self._rx(msg)) + self.assertFalse(self.safety.get_controls_allowed()) + + self.assertTrue(self._rx(self._tsk_status_msg(True))) + self.assertTrue(self.safety.get_controls_allowed()) + msg = self.packer.make_can_msg_safety("GRA_ACC_01", 0, {"GRA_Abbrechen": True}) + self.assertTrue(self._rx(msg)) + self.assertFalse(self.safety.get_controls_allowed()) + def setUp(self): self.packer = CANPackerSafety("vw_mqb") self.safety = libsafety_py.libsafety @@ -175,6 +198,8 @@ def test_set_and_resume_buttons(self): self._rx(self._tsk_status_msg(False, main_switch=True)) self._rx(self._gra_acc_01_msg(_set=(button == "set"), resume=(button == "resume"), bus=0)) self.assertFalse(self.safety.get_controls_allowed(), f"controls allowed on {button} rising edge") + self._rx(self._gra_acc_01_msg(_set=(button == "set"), resume=(button == "resume"), bus=0)) + self.assertFalse(self.safety.get_controls_allowed(), f"controls allowed while holding {button}") self._rx(self._gra_acc_01_msg(bus=0)) self.assertTrue(self.safety.get_controls_allowed(), f"controls not allowed on {button} falling edge") diff --git a/opendbc/safety/tests/test_volkswagen_pq.py b/opendbc/safety/tests/test_volkswagen_pq.py index b6b134b7796..fb0712ec14c 100755 --- a/opendbc/safety/tests/test_volkswagen_pq.py +++ b/opendbc/safety/tests/test_volkswagen_pq.py @@ -109,6 +109,14 @@ class TestVolkswagenPqStockSafety(TestVolkswagenPqSafetyBase): TX_MSGS = [[MSG_HCA_1, 0], [MSG_GRA_NEU, 0], [MSG_GRA_NEU, 2], [MSG_LDW_1, 0]] FWD_BLACKLISTED_ADDRS = {2: [MSG_HCA_1, MSG_LDW_1]} + def test_cruise_states(self): + for state in range(4): + with self.subTest(state=state): + self.assertTrue(self._rx(self._pcm_status_msg(False))) + self.assertTrue(self._rx(self._motor_2_msg(cruise_engaged=state))) + self.assertEqual(state in (1, 2), self.safety.get_controls_allowed()) + self.assertEqual(state in (1, 2), self.safety.get_cruise_engaged_prev()) + def setUp(self): self.packer = CANPackerSafety("vw_pq") self.safety = libsafety_py.libsafety @@ -158,6 +166,8 @@ def test_set_and_resume_buttons(self): self._rx(self._motor_5_msg(main_switch=True)) self._rx(self._button_msg(_set=(button == "set"), resume=(button == "resume"), bus=0)) self.assertFalse(self.safety.get_controls_allowed(), f"controls allowed on {button} rising edge") + self._rx(self._button_msg(_set=(button == "set"), resume=(button == "resume"), bus=0)) + self.assertFalse(self.safety.get_controls_allowed(), f"controls allowed while holding {button}") self._rx(self._button_msg(bus=0)) self.assertTrue(self.safety.get_controls_allowed(), f"controls not allowed on {button} falling edge")