diff --git a/opendbc_repo/opendbc/safety/modes/mazda.h b/opendbc_repo/opendbc/safety/modes/mazda.h index a36d75a40..e77160030 100644 --- a/opendbc_repo/opendbc/safety/modes/mazda.h +++ b/opendbc_repo/opendbc/safety/modes/mazda.h @@ -3,6 +3,7 @@ #include "opendbc/safety/declarations.h" // CAN msgs we care about +/********** GEN1 msgs **********/ #define MAZDA_LKAS 0x243U #define MAZDA_LKAS_HUD 0x440U #define MAZDA_CRZ_CTRL 0x21cU @@ -10,23 +11,118 @@ #define MAZDA_STEER_TORQUE 0x240U #define MAZDA_ENGINE_DATA 0x202U #define MAZDA_PEDALS 0x165U +#define TI_STEER_TORQUE 0x24AU +// Radar +#define MAZDA_CRZ_INFO 0x21BU +#define MAZDA_RADAR_361 0x361U +#define MAZDA_RADAR_362 0x362U +#define MAZDA_RADAR_363 0x363U +#define MAZDA_RADAR_364 0x364U +#define MAZDA_RADAR_365 0x365U +#define MAZDA_RADAR_366 0x366U +#define MAZDA_RADAR_499 0x499U +/************* GEN2 msgs *************/ +#define MAZDA_2019_BRAKE 0x43FU // main bus +#define MAZDA_2019_GAS 0x202U // camera bus DBC: ENGINE_DATA +#define MAZDA_2019_CRUISE 0x44AU // main bus. DBC: CRUISE_STATE +#define MAZDA_2019_SPEED 0x217U // camera bus. DBC: SPEED +#define MAZDA_2019_STEER_TORQUE 0x24BU // aux bus. DBC: TI_FEEDBACK +#define MAZDA_2019_CRZ_BTNS 0x9DU // rx on main tx on camera. DBC: CRZ_BTNS +#define MAZDA_2019_ACC 0x220U // main bus. DBC: ACC +#define MAZDA_TI_LKAS 0x249U +/************* GEN3 msgs *************/ +#define MAZDA_2023_SPEED 0x215 // main bus. DBC: WHEEL_SPEEDS +#define MAZDA_2023_BRAKE 0x9F // main bus. DBC: BRAKE_PEDAL +#define MAZDA_2023_ACC 0x21e // main bus. DBC: ACC // 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_GEN3 = 4; +const int FLAG_TORQUE_INTERCEPTOR = 8; +const int FLAG_RADAR_INTERCEPTOR = 16; +const int FLAG_NO_FSC = 32; +const int FLAG_NO_MRCC = 64; + +bool gen1 = false; +bool gen2 = false; +bool gen3 = false; +bool torque_interceptor = false; +bool radar_interceptor = false; +bool no_fsc = false; +bool no_mrcc = false; + + // 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 + if (gen1) { + 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 && !torque_interceptor) { + 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 && !no_mrcc && !radar_interceptor) { + acc_main_on = GET_BIT(msg, 17U); + 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_PEDALS) { + brake_pressed = (msg->data[0] & 0x10U); + if (radar_interceptor || no_mrcc) { + // acc_main_on = GET_BIT(to_push, 17U); TODO + bool cruise_engaged = msg->data[0] & 0x8U; + pcm_cruise_check(cruise_engaged); + } + } } - if (msg->addr == MAZDA_STEER_TORQUE) { - int torque_driver_new = msg->data[0] - 127U; - // update array of samples + if (gen2) { + if (msg->addr == MAZDA_2019_BRAKE) { + brake_pressed = (msg->data[5] & 0x4U); + } + + if (msg->addr == MAZDA_2019_CRUISE) { + acc_main_on = true; + bool cruise_engaged = msg->data[0] & 0x20U; + bool pre_enable = msg->data[0] & 0x40U; + pcm_cruise_check((cruise_engaged || pre_enable)); + } + } + + if (gen3) { + if (msg->addr == MAZDA_2023_BRAKE) { + brake_pressed = (msg->data[7] & 0x8U); + } + if (msg->addr == MAZDA_2023_SPEED) { + int speed = ( (msg->data[0] << 8) | (msg->data[1]) ) - 10000; // Front Left Wheel Speed + vehicle_moving = speed != 0; + } + } + } + + if ((msg->bus == MAZDA_AUX)) { + + if (msg->addr == TI_STEER_TORQUE && gen1 && torque_interceptor) { + int torque_driver_new = msg->data[0] - 127; update_sample(&torque_driver, torque_driver_new); } @@ -38,12 +134,38 @@ static void mazda_rx_hook(const CANPacket_t *msg) { acc_main_on = GET_BIT(msg, 17U); } - if (msg->addr == MAZDA_ENGINE_DATA) { - gas_pressed = (msg->data[4] || (msg->data[5] & 0xF0U)); + if (msg->addr == MAZDA_2019_STEER_TORQUE && (gen2 || gen3)) { + update_sample(&torque_driver, (int16_t)(msg->data[0] << 8 | msg->data[1])); + } } - if (msg->addr == MAZDA_PEDALS) { - brake_pressed = (msg->data[0] & 0x10U); + if (msg->addr == MAZDA_2019_CRUISE && gen3) { + uint8_t state = msg->data[0] & 0x70U; + bool cruise_engaged = (0x30U == state); + bool cruise_override = (0x40U == state); + //bool cruise_disable = (state == 0x10U); + acc_main_on = true; + pcm_cruise_check(cruise_engaged || cruise_override); + } + } + + if ((msg->bus == MAZDA_CAM)) { + if (gen2) { + + if (msg->addr == MAZDA_2019_GAS) { + gas_pressed = (msg->data[4] || ((msg->data[5] & 0xC0U))); + } + + if (msg->addr == MAZDA_2019_SPEED) { + // sample speed: scale by 0.01 to get kph + int speed = (msg->data[4] << 8) | (msg->data[5]); + vehicle_moving = (speed > 10); // moving when speed > 0.1 kph + } + } + if (gen3) { + if (msg->addr == MAZDA_2019_GAS) { + gas_pressed = (msg->data[4] || (msg->data[5] & 0xC0U)); + } } } } @@ -59,6 +181,16 @@ static bool mazda_tx_hook(const CANPacket_t *msg) { .type = TorqueDriverLimited, }; + const TorqueSteeringLimits MAZDA_2019_STEERING_LIMITS = { + .max_torque = 8000, + .max_rate_up = 45, + .max_rate_down = 80, + .max_rt_delta = 1688, // (45*100hz*250000/1000000)*1.5 + .driver_torque_multiplier = 1, + .driver_torque_allowance = 1400, + .type = TorqueDriverLimited, + }; + bool tx = true; // Check if msg is sent on the main BUS if (msg->bus == (unsigned char)MAZDA_MAIN) { @@ -81,12 +213,29 @@ static bool mazda_tx_hook(const CANPacket_t *msg) { } } } + if ((gen2 || gen3) && (msg->bus == MAZDA_AUX) && (msg->addr == MAZDA_TI_LKAS)) { + int desired_torque = (int16_t)((msg->data[0] << 8) | msg->data[1]); // signal is signed + if (steer_torque_cmd_checks(desired_torque, -1, MAZDA_2019_STEERING_LIMITS)) { + tx = false; + } + } return tx; } static safety_config mazda_init(uint16_t param) { static const CanMsg MAZDA_TX_MSGS[] = {{MAZDA_LKAS, 0, 8, .check_relay = true}, {MAZDA_CRZ_BTNS, 0, 8, .check_relay = false}, {MAZDA_LKAS_HUD, 0, 8, .check_relay = true}}; + static const CanMsg MAZDA_TI_TX_MSGS[] = {{MAZDA_LKAS, 0, 8, .check_relay = true}, {MAZDA_TI_LKAS, 1, 8, .check_relay = false}, {MAZDA_CRZ_BTNS, 0, 8, .check_relay = false}, {MAZDA_LKAS_HUD, 0, 8, .check_relay = true}}; + static const CanMsg MAZDA_RI_TX_MSGS[] = {{MAZDA_LKAS, 0, 8, .check_relay = true}, {MAZDA_CRZ_BTNS, 0, 8, .check_relay = false}, {MAZDA_LKAS_HUD, 0, 8, .check_relay = true}, + {MAZDA_CRZ_CTRL, 0, 8, .check_relay = true}, {MAZDA_CRZ_INFO, 0, 8, .check_relay = true}, {MAZDA_RADAR_361, 0, 8, .check_relay = true}, {MAZDA_RADAR_362, 0, 8, .check_relay = true}, + {MAZDA_RADAR_363, 0, 8, .check_relay = true}, {MAZDA_RADAR_364, 0, 8, .check_relay = true}, {MAZDA_RADAR_365, 0, 8, .check_relay = true}, {MAZDA_RADAR_366, 0, 8, .check_relay = true}, + {MAZDA_RADAR_499, 0, 8, .check_relay = true}}; + static const CanMsg MAZDA_TI_RI_TX_MSGS[] = {{MAZDA_LKAS, 0, 8, .check_relay = true}, {MAZDA_TI_LKAS, 1, 8, .check_relay = false}, {MAZDA_CRZ_BTNS, 0, 8, .check_relay = false}, {MAZDA_LKAS_HUD, 0, 8, .check_relay = true}, + {MAZDA_CRZ_CTRL, 0, 8, .check_relay = true}, {MAZDA_CRZ_INFO, 0, 8, .check_relay = true}, {MAZDA_RADAR_361, 0, 8, .check_relay = true}, {MAZDA_RADAR_362, 0, 8, .check_relay = true}, + {MAZDA_RADAR_363, 0, 8, .check_relay = true}, {MAZDA_RADAR_364, 0, 8, .check_relay = true}, {MAZDA_RADAR_365, 0, 8, .check_relay = true}, {MAZDA_RADAR_366, 0, 8, .check_relay = true}, + {MAZDA_RADAR_499, 0, 8, .check_relay = true}}; + + static const CanMsg MAZDA_2019_TX_MSGS[] = {{MAZDA_TI_LKAS, 1, 8, .check_relay = false}, {MAZDA_2019_ACC, 2, 8, .check_relay = true}}; static RxCheck mazda_rx_checks[] = { {.msg = {{MAZDA_CRZ_CTRL, 0, 8, 50U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, @@ -95,13 +244,67 @@ static safety_config mazda_init(uint16_t param) { {.msg = {{MAZDA_ENGINE_DATA, 0, 8, 100U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, {.msg = {{MAZDA_PEDALS, 0, 8, 50U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, }; + static RxCheck mazda_ti_rx_checks[] = { + {.msg = {{MAZDA_CRZ_BTNS, 0, 8, 10U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, + {.msg = {{MAZDA_STEER_TORQUE, 0, 8, 83U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, + {.msg = {{MAZDA_ENGINE_DATA, 0, 8, 100U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, + {.msg = {{MAZDA_PEDALS, 0, 8, 50U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, + {.msg = {{TI_STEER_TORQUE, 1, 8, 50U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, + }; + static RxCheck mazda_2019_rx_checks[] = { + {.msg = {{MAZDA_2019_BRAKE, 0, 8, 20U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, + {.msg = {{MAZDA_2019_GAS, 2, 8, 100U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, + {.msg = {{MAZDA_2019_CRUISE, 0, 8, 10U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, + {.msg = {{MAZDA_2019_SPEED, 2, 8, 30U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, + {.msg = {{MAZDA_2019_STEER_TORQUE, 1, 8, 50U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, + }; - SAFETY_UNUSED(param); - return BUILD_SAFETY_CFG(mazda_rx_checks, MAZDA_TX_MSGS); + static RxCheck mazda_2023_rx_checks[] = { + {.msg = {{MAZDA_2023_BRAKE, 0, 8, 5U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, + {.msg = {{MAZDA_2019_GAS, 2, 8, 100U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, + {.msg = {{MAZDA_2019_CRUISE, 1, 8, 10U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, + {.msg = {{MAZDA_2023_SPEED, 2, 8, 30U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, + {.msg = {{MAZDA_2019_STEER_TORQUE, 1, 8, 50U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, +}; + + // Check flags + gen1 = GET_FLAG(param, FLAG_GEN1); + gen2 = GET_FLAG(param, FLAG_GEN2); + gen3 = GET_FLAG(param, FLAG_GEN3); + 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); + + safety_config ret = BUILD_SAFETY_CFG(mazda_rx_checks, MAZDA_TX_MSGS); + + 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); + } + if (gen3) { + ret = BUILD_SAFETY_CFG(mazda_2023_rx_checks, MAZDA_2019_TX_MSGS); + } + + return ret; } const safety_hooks mazda_hooks = { .init = mazda_init, .rx = mazda_rx_hook, .tx = mazda_tx_hook, -}; +}; \ No newline at end of file diff --git a/panda/board/drivers/can_common.h b/panda/board/drivers/can_common.h index 7ec89f7ba..abb2c6c81 100644 --- a/panda/board/drivers/can_common.h +++ b/panda/board/drivers/can_common.h @@ -237,6 +237,11 @@ void ignition_can_hook(CANPacket_t *msg) { ignition_can = (msg->data[0] >> 5) == 0x6U; ignition_can_cnt = 0U; } + // Mazda 2019 exception + if ((msg->addr == 0x274) && (len == 8)) { + ignition_can = (msg->data[5] & 0x4U) != 0U; + ignition_can_cnt = 0U; + } } } diff --git a/panda/board/main.c b/panda/board/main.c index a1bb0c508..19ba6af84 100644 --- a/panda/board/main.c +++ b/panda/board/main.c @@ -81,6 +81,17 @@ void set_safety_mode(uint16_t mode, uint16_t param) { } can_silent = false; break; + case SAFETY_MAZDA: + set_intercept_relay(true, false); + heartbeat_counter = 0U; + heartbeat_lost = false; + 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 = false; + break; default: set_intercept_relay(true, false); heartbeat_counter = 0U;