From 5ebce679f81d075cfc0130a727bd1eb2bbbea469 Mon Sep 17 00:00:00 2001 From: MoreTore Date: Sat, 26 Apr 2025 14:59:42 -0500 Subject: [PATCH] Mazda panda --- panda/board/drivers/can_common.h | 7 +- panda/board/main.c | 13 ++ panda/board/safety/safety_mazda.h | 220 ++++++++++++++++++++++++++---- panda/python/__init__.py | 7 + 4 files changed, 218 insertions(+), 29 deletions(-) diff --git a/panda/board/drivers/can_common.h b/panda/board/drivers/can_common.h index 55d9985af..c54e587ce 100644 --- a/panda/board/drivers/can_common.h +++ b/panda/board/drivers/can_common.h @@ -200,7 +200,7 @@ void ignition_can_hook(CANPacket_t *to_push) { if (bus == 0) { int addr = GET_ADDR(to_push); int len = GET_LEN(to_push); - + // GM exception if ((addr == 0x1F1) && (len == 8)) { // SystemPowerMode (2=Run, 3=Crank Request) @@ -220,6 +220,11 @@ void ignition_can_hook(CANPacket_t *to_push) { ignition_can = (GET_BYTE(to_push, 0) >> 5) == 0x6U; ignition_can_cnt = 0U; } + // Mazda 2019 exception + if ((addr == 0x274) && (len == 8)) { + ignition_can = (GET_BYTE(to_push, 5) & 0x4U) != 0U; + ignition_can_cnt = 0U; + } } else if (bus == 2) { int addr = GET_ADDR(to_push); diff --git a/panda/board/main.c b/panda/board/main.c index b481c28b4..b2016ad80 100644 --- a/panda/board/main.c +++ b/panda/board/main.c @@ -108,6 +108,19 @@ void set_safety_mode(uint16_t mode, uint16_t param) { } can_silent = ALL_CAN_LIVE; break; + case SAFETY_MAZDA: + set_intercept_relay(true, false); + heartbeat_counter = 0U; + heartbeat_lost = false; + if (current_board->has_obd) { + if (GET_FLAG(param, 4) && GET_FLAG(param, (1 | 4))) { // TI Enabled for GEN 1 + current_board->set_can_mode(CAN_MODE_OBD_CAN2); + } else { + current_board->set_can_mode(CAN_MODE_NORMAL); + } + } + can_silent = ALL_CAN_LIVE; + break; default: set_intercept_relay(true, false); heartbeat_counter = 0U; diff --git a/panda/board/safety/safety_mazda.h b/panda/board/safety/safety_mazda.h index 19e2dd120..9ec1a6787 100644 --- a/panda/board/safety/safety_mazda.h +++ b/panda/board/safety/safety_mazda.h @@ -1,4 +1,5 @@ // CAN msgs we care about +/********** GEN1 msgs **********/ #define MAZDA_LKAS 0x243 #define MAZDA_LKAS_HUD 0x440 #define MAZDA_CRZ_CTRL 0x21c @@ -6,12 +7,50 @@ #define MAZDA_STEER_TORQUE 0x240 #define MAZDA_ENGINE_DATA 0x202 #define MAZDA_PEDALS 0x165 +#define TI_STEER_TORQUE 0x24A + +// Radar +#define MAZDA_CRZ_INFO 0x21B +#define MAZDA_CRZ_CTRL 0x21c +#define MAZDA_RADAR_361 0x361 +#define MAZDA_RADAR_362 0x362 +#define MAZDA_RADAR_363 0x363 +#define MAZDA_RADAR_364 0x364 +#define MAZDA_RADAR_365 0x365 +#define MAZDA_RADAR_366 0x366 +#define MAZDA_RADAR_499 0x499 + +/************* GEN2 msgs *************/ +#define MAZDA_2019_BRAKE 0x43F // main bus +#define MAZDA_2019_GAS 0x202 // camera bus DBC: ENGINE_DATA +#define MAZDA_2019_CRUISE 0x44A // main bus. DBC: CRUISE_STATE +#define MAZDA_2019_SPEED 0x217 // camera bus. DBC: SPEED +#define MAZDA_2019_STEER_TORQUE 0x24B // aux bus. DBC: EPS_FEEDBACK +#define MAZDA_2019_CRZ_BTNS 0x9d // rx on main tx on camera. DBC: CRZ_BTNS +#define MAZDA_2019_ACC 0x220 // main bus. DBC: ACC + +#define MAZDA_TI_LKAS 0x249 // CAN bus numbers #define MAZDA_MAIN 0 #define MAZDA_AUX 1 #define MAZDA_CAM 2 +// param flag masks +const int FLAG_GEN1 = 1; +const int FLAG_GEN2 = 2; +const int FLAG_TORQUE_INTERCEPTOR = 4; +const int FLAG_RADAR_INTERCEPTOR = 8; +const int FLAG_NO_FSC = 16; +const int FLAG_NO_MRCC = 32; + +bool gen1 = false; +bool gen2 = false; +bool torque_interceptor = false; +bool radar_interceptor = false; +bool no_fsc = false; +bool no_mrcc = false; + const SteeringLimits MAZDA_STEERING_LIMITS = { .max_steer = 800, .max_rate_up = 10, @@ -23,7 +62,29 @@ const SteeringLimits MAZDA_STEERING_LIMITS = { .type = TorqueDriverLimited, }; +const SteeringLimits MAZDA_2019_STEERING_LIMITS = { + .max_steer = 8000, + .max_rate_up = 45, + .max_rate_down = 80, + .max_rt_delta = 1688, // (45*100hz*250000/1000000)*1.5 + .max_rt_interval = 250000, + .driver_torque_factor = 1, + .driver_torque_allowance = 1400, + .type = TorqueDriverLimited, +}; + const CanMsg MAZDA_TX_MSGS[] = {{MAZDA_LKAS, 0, 8}, {MAZDA_CRZ_BTNS, 0, 8}, {MAZDA_LKAS_HUD, 0, 8}}; +const CanMsg MAZDA_TI_TX_MSGS[] = {{MAZDA_LKAS, 0, 8}, {MAZDA_TI_LKAS, 1, 8}, {MAZDA_CRZ_BTNS, 0, 8}, {MAZDA_LKAS_HUD, 0, 8}}; +const CanMsg MAZDA_RI_TX_MSGS[] = {{MAZDA_LKAS, 0, 8}, {MAZDA_CRZ_BTNS, 0, 8}, {MAZDA_LKAS_HUD, 0, 8}, + {MAZDA_CRZ_CTRL, 0, 8}, {MAZDA_CRZ_INFO, 0, 8}, {MAZDA_RADAR_361, 0, 8}, {MAZDA_RADAR_362, 0, 8}, + {MAZDA_RADAR_363, 0, 8}, {MAZDA_RADAR_364, 0, 8}, {MAZDA_RADAR_365, 0, 8}, {MAZDA_RADAR_366, 0, 8}, + {MAZDA_RADAR_499, 0, 8}}; +const CanMsg MAZDA_TI_RI_TX_MSGS[] = {{MAZDA_LKAS, 0, 8}, {MAZDA_TI_LKAS, 1, 8}, {MAZDA_CRZ_BTNS, 0, 8}, {MAZDA_LKAS_HUD, 0, 8}, + {MAZDA_CRZ_CTRL, 0, 8}, {MAZDA_CRZ_INFO, 0, 8}, {MAZDA_RADAR_361, 0, 8}, {MAZDA_RADAR_362, 0, 8}, + {MAZDA_RADAR_363, 0, 8}, {MAZDA_RADAR_364, 0, 8}, {MAZDA_RADAR_365, 0, 8}, {MAZDA_RADAR_366, 0, 8}, + {MAZDA_RADAR_499, 0, 8}}; + +const CanMsg MAZDA_2019_TX_MSGS[] = {{MAZDA_TI_LKAS, 1, 8}, {MAZDA_2019_ACC, 2, 8}}; RxCheck mazda_rx_checks[] = { {.msg = {{MAZDA_CRZ_CTRL, 0, 8, .frequency = 50U}, { 0 }, { 0 }}}, @@ -33,48 +94,110 @@ RxCheck mazda_rx_checks[] = { {.msg = {{MAZDA_PEDALS, 0, 8, .frequency = 50U}, { 0 }, { 0 }}}, }; +RxCheck mazda_ti_rx_checks[] = { + {.msg = {{MAZDA_CRZ_BTNS, 0, 8, .frequency = 10U}, { 0 }, { 0 }}}, + {.msg = {{MAZDA_STEER_TORQUE, 0, 8, .frequency = 83U}, { 0 }, { 0 }}}, + {.msg = {{MAZDA_ENGINE_DATA, 0, 8, .frequency = 100U}, { 0 }, { 0 }}}, + {.msg = {{MAZDA_PEDALS, 0, 8, .frequency = 50U}, { 0 }, { 0 }}}, + {.msg = {{TI_STEER_TORQUE, 1, 8, .frequency = 50U}, { 0 }, { 0 }}}, +}; + +RxCheck mazda_2019_rx_checks[] = { + {.msg = {{MAZDA_2019_BRAKE, 0, 8, .frequency = 20U}, { 0 }, { 0 }}}, + {.msg = {{MAZDA_2019_GAS, 2, 8, .frequency = 100U}, { 0 }, { 0 }}}, + {.msg = {{MAZDA_2019_CRUISE, 0, 8, .frequency = 10U}, { 0 }, { 0 }}}, + {.msg = {{MAZDA_2019_SPEED, 2, 8, .frequency = 30U}, { 0 }, { 0 }}}, + {.msg = {{MAZDA_2019_STEER_TORQUE, 1, 8, .frequency = 50U}, { 0 }, { 0 }}}, +}; + // 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 *to_push) { + int addr = GET_ADDR(to_push); if ((int)GET_BUS(to_push) == MAZDA_MAIN) { - int addr = GET_ADDR(to_push); + if (gen1){ + if (addr == MAZDA_ENGINE_DATA) { + // sample speed: scale by 0.01 to get kph + int speed = (GET_BYTE(to_push, 2) << 8) | GET_BYTE(to_push, 3); + vehicle_moving = speed > 10; // moving when speed > 0.1 kph + } - if (addr == MAZDA_ENGINE_DATA) { - // sample speed: scale by 0.01 to get kph - int speed = (GET_BYTE(to_push, 2) << 8) | GET_BYTE(to_push, 3); - vehicle_moving = speed > 10; // moving when speed > 0.1 kph + if (addr == MAZDA_STEER_TORQUE && !torque_interceptor) { + int torque_driver_new = GET_BYTE(to_push, 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 (addr == MAZDA_CRZ_CTRL && !no_mrcc && !radar_interceptor) { + acc_main_on = GET_BIT(to_push, 17U); + bool cruise_engaged = GET_BYTE(to_push, 0) & 0x8U; + pcm_cruise_check(cruise_engaged); + } + + if (addr == MAZDA_ENGINE_DATA) { + gas_pressed = (GET_BYTE(to_push, 4) || (GET_BYTE(to_push, 5) & 0xF0U)); + } + + if (addr == MAZDA_PEDALS) { + brake_pressed = (GET_BYTE(to_push, 0) & 0x10U); + if (radar_interceptor || no_mrcc) { + // acc_main_on = GET_BIT(to_push, 17U); TODO + bool cruise_engaged = GET_BYTE(to_push, 0) & 0x8U; + pcm_cruise_check(cruise_engaged); + } + } + + generic_rx_checks((addr == MAZDA_LKAS)); } + if (gen2) { + if (addr == MAZDA_2019_BRAKE) { + brake_pressed = (GET_BYTE(to_push, 5) & 0x4U); + } - if (addr == MAZDA_STEER_TORQUE) { - int torque_driver_new = GET_BYTE(to_push, 0) - 127U; - // update array of samples + if (addr == MAZDA_2019_CRUISE) { + acc_main_on = true; + bool cruise_engaged = GET_BYTE(to_push, 0) & 0x20U; + bool pre_enable = GET_BYTE(to_push, 0) & 0x40U; + pcm_cruise_check((cruise_engaged || pre_enable)); + } + + generic_rx_checks(addr == MAZDA_2019_SPEED); + } + } + + if ((GET_BUS(to_push) == MAZDA_AUX)) { + if (addr == TI_STEER_TORQUE && gen1 && torque_interceptor) { + int torque_driver_new = GET_BYTE(to_push, 0) - 127; update_sample(&torque_driver, torque_driver_new); } - // enter controls on rising edge of ACC, exit controls on ACC off - if (addr == MAZDA_CRZ_CTRL) { - acc_main_on = GET_BIT(to_push, 17U); - bool cruise_engaged = GET_BYTE(to_push, 0) & 0x8U; - pcm_cruise_check(cruise_engaged); + if (addr == MAZDA_2019_STEER_TORQUE && gen2) { + update_sample(&torque_driver, (int16_t)(GET_BYTE(to_push, 0) << 8 | GET_BYTE(to_push, 1))); } + } - if (addr == MAZDA_ENGINE_DATA) { - gas_pressed = (GET_BYTE(to_push, 4) || (GET_BYTE(to_push, 5) & 0xF0U)); + if ((GET_BUS(to_push) == MAZDA_CAM)) { + if (gen2) { + if (addr == MAZDA_2019_GAS) { + gas_pressed = (GET_BYTE(to_push, 4) || ((GET_BYTE(to_push, 5) & 0xC0U))); + } + + if (addr == MAZDA_2019_SPEED) { + // sample speed: scale by 0.01 to get kph + int speed = (GET_BYTE(to_push, 4) << 8) | (GET_BYTE(to_push, 5)); + vehicle_moving = (speed > 10); // moving when speed > 0.1 kph + } + generic_rx_checks(addr == MAZDA_2019_CRUISE); } - - if (addr == MAZDA_PEDALS) { - brake_pressed = (GET_BYTE(to_push, 0) & 0x10U); - } - - generic_rx_checks((addr == MAZDA_LKAS)); } } static bool mazda_tx_hook(const CANPacket_t *to_send) { bool tx = true; int bus = GET_BUS(to_send); + int addr = GET_ADDR(to_send); // Check if msg is sent on the main BUS - if (bus == MAZDA_MAIN) { - int addr = GET_ADDR(to_send); + if (gen1 && (bus == MAZDA_MAIN)) { // steer cmd checks if (addr == MAZDA_LKAS) { @@ -95,17 +218,34 @@ static bool mazda_tx_hook(const CANPacket_t *to_send) { } } } + if (gen2 && (bus == MAZDA_AUX) && (addr == MAZDA_TI_LKAS)) { + int desired_torque = (int16_t)((GET_BYTE(to_send, 0) << 8) | GET_BYTE(to_send, 1)); // signal is signed + if (steer_torque_cmd_checks(desired_torque, -1, MAZDA_2019_STEERING_LIMITS)) { + tx = false; + } + } return tx; } static int mazda_fwd_hook(int bus, int addr) { int bus_fwd = -1; - + bool block = (addr == MAZDA_TI_LKAS); if (bus == MAZDA_MAIN) { - bus_fwd = MAZDA_CAM; + block |= ((addr == MAZDA_2019_ACC) && gen2); + if (!block) { + bus_fwd = MAZDA_CAM; + } } else if (bus == MAZDA_CAM) { - bool block = (addr == MAZDA_LKAS) || (addr == MAZDA_LKAS_HUD); + if (gen1) { + block |= (addr == MAZDA_LKAS) || (addr == MAZDA_LKAS_HUD); + if (radar_interceptor) { + block |= (addr == MAZDA_CRZ_INFO) || (addr == MAZDA_CRZ_CTRL); + block |= (addr == MAZDA_RADAR_361) || (addr == MAZDA_RADAR_362); + block |= (addr == MAZDA_RADAR_363) || (addr == MAZDA_RADAR_364); + block |= (addr == MAZDA_RADAR_365) || (addr == MAZDA_RADAR_366); + } + } if (!block) { bus_fwd = MAZDA_MAIN; } @@ -117,8 +257,32 @@ static int mazda_fwd_hook(int bus, int addr) { } static safety_config mazda_init(uint16_t param) { - UNUSED(param); - return BUILD_SAFETY_CFG(mazda_rx_checks, MAZDA_TX_MSGS); + safety_config ret = BUILD_SAFETY_CFG(mazda_rx_checks, MAZDA_TX_MSGS); + gen1 = GET_FLAG(param, FLAG_GEN1); + gen2 = GET_FLAG(param, FLAG_GEN2); + radar_interceptor = GET_FLAG(param, FLAG_RADAR_INTERCEPTOR); + torque_interceptor = GET_FLAG(param, FLAG_TORQUE_INTERCEPTOR); + no_fsc = GET_FLAG(param, FLAG_NO_FSC); + no_mrcc = GET_FLAG(param, FLAG_NO_MRCC); + if (gen1) { + SET_RX_CHECKS(mazda_rx_checks, ret); + if (radar_interceptor && torque_interceptor) { + SET_RX_CHECKS(mazda_ti_rx_checks, ret); + SET_TX_MSGS(MAZDA_TI_RI_TX_MSGS, ret); + } else if (radar_interceptor) { + SET_TX_MSGS(MAZDA_RI_TX_MSGS, ret); + } else if (torque_interceptor) { + SET_RX_CHECKS(mazda_ti_rx_checks, ret); + SET_TX_MSGS(MAZDA_TI_TX_MSGS, ret); + } else { + SET_TX_MSGS(MAZDA_TX_MSGS, ret); + } + } + if (gen2) { + ret = BUILD_SAFETY_CFG(mazda_2019_rx_checks, MAZDA_2019_TX_MSGS); + } + + return ret; } const safety_hooks mazda_hooks = { diff --git a/panda/python/__init__.py b/panda/python/__init__.py index 9339bfcf8..6501dbea7 100644 --- a/panda/python/__init__.py +++ b/panda/python/__init__.py @@ -242,6 +242,13 @@ class Panda: FLAG_FORD_LONG_CONTROL = 1 FLAG_FORD_CANFD = 2 + FLAG_MAZDA_GEN1 = 1 + FLAG_MAZDA_GEN2 = 2 + FLAG_MAZDA_TORQUE_INTERCEPTOR = 4 + FLAG_MAZDA_RADAR_INTERCEPTOR = 8 + FLAG_MAZDA_NO_FSC = 16 + FLAG_MAZDA_NO_MRCC = 32 + def __init__(self, serial: str | None = None, claim: bool = True, disable_checks: bool = True, can_speed_kbps: int = 500): self._connect_serial = serial self._disable_checks = disable_checks