Skip to content

Commit

Permalink
mazda master 0.9.7
Browse files Browse the repository at this point in the history
  • Loading branch information
MoreTore authored and MoreTore committed Jun 16, 2024
1 parent faa1802 commit 08e16ba
Show file tree
Hide file tree
Showing 5 changed files with 493 additions and 30 deletions.
13 changes: 13 additions & 0 deletions board/main.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
213 changes: 187 additions & 26 deletions board/safety/safety_mazda.h
View file
Original file line number Diff line number Diff line change
@@ -1,17 +1,56 @@
// CAN msgs we care about
/********** GEN1 msgs **********/
#define MAZDA_LKAS 0x243
#define MAZDA_LKAS_HUD 0x440
#define MAZDA_CRZ_CTRL 0x21c
#define MAZDA_CRZ_BTNS 0x09d
#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,
Expand All @@ -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 }}},
Expand All @@ -33,47 +94,106 @@ 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);
}

if (addr == MAZDA_STEER_TORQUE) {
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) {
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) {
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_2019_CRUISE) {
acc_main_on = (GET_BYTE(to_push, 0) & 0x20U) || GET_BYTE(to_push, 0) & 0x40U;
pcm_cruise_check(acc_main_on);
}

// enter controls on rising edge of ACC, exit controls on ACC off
if (addr == MAZDA_CRZ_CTRL) {
bool cruise_engaged = GET_BYTE(to_push, 0) & 0x8U;
pcm_cruise_check(cruise_engaged);
generic_rx_checks(addr == MAZDA_2019_SPEED);
}
}

if (addr == MAZDA_ENGINE_DATA) {
gas_pressed = (GET_BYTE(to_push, 4) || (GET_BYTE(to_push, 5) & 0xF0U));
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);
}

if (addr == MAZDA_PEDALS) {
brake_pressed = (GET_BYTE(to_push, 0) & 0x10U);
if (addr == MAZDA_2019_STEER_TORQUE && gen2) {
update_sample(&torque_driver, (int16_t)(GET_BYTE(to_push, 0) << 8 | GET_BYTE(to_push, 1)));
}
}

generic_rx_checks((addr == MAZDA_LKAS));
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);
}
}
}

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) {
Expand All @@ -94,17 +214,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;
}
Expand All @@ -116,8 +253,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 = {
Expand Down
7 changes: 7 additions & 0 deletions python/__init__.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -228,6 +228,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
Expand Down
6 changes: 5 additions & 1 deletion tests/safety/common.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -724,7 +724,7 @@ def test_spam_can_buses(self):
for bus in range(4):
for addr in self.SCANNED_ADDRS:
if [addr, bus] not in self.TX_MSGS:
self.assertFalse(self._tx(make_msg(bus, addr, 8)), f"allowed TX {addr=} {bus=}")
self.assertFalse(self._tx(make_msg(bus, addr, 8)), f"allowed TX {hex(addr)=} {bus=}")

def test_default_controls_not_allowed(self):
self.assertFalse(self.safety.get_controls_allowed())
Expand Down Expand Up @@ -804,6 +804,10 @@ def test_tx_hook_on_wrong_safety_mode(self):
# TODO: this should be blocked
if current_test in ["TestNissanSafety", "TestNissanSafetyAltEpsBus", "TestNissanLeafSafety"] and [addr, bus] in self.TX_MSGS:
continue
if current_test.startswith("TestMazdaSafetyGEN1") and [addr, bus] in self.TX_MSGS:
continue
if current_test.startswith("TestMazdaSafetyGEN2") and test_name.startswith("TestMazdaSafetyGEN1TI") and addr == 0x249 and bus == 1:
continue
self.assertFalse(self._tx(msg), f"transmit of {addr=:#x} {bus=} from {test_name} during {current_test} was allowed")


Expand Down
Loading

0 comments on commit 08e16ba

Please sign in to comment.