You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
110 lines
3.5 KiB
110 lines
3.5 KiB
#pragma once
|
|
|
|
#include "opendbc/safety/safety_declarations.h"
|
|
|
|
// CAN msgs we care about
|
|
#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
|
|
|
|
// CAN bus numbers
|
|
#define MAZDA_MAIN 0
|
|
#define MAZDA_CAM 2
|
|
|
|
// 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) {
|
|
if ((int)GET_BUS(to_push) == MAZDA_MAIN) {
|
|
int addr = GET_ADDR(to_push);
|
|
|
|
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) {
|
|
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) {
|
|
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);
|
|
}
|
|
}
|
|
}
|
|
|
|
static bool mazda_tx_hook(const CANPacket_t *to_send) {
|
|
const TorqueSteeringLimits MAZDA_STEERING_LIMITS = {
|
|
.max_torque = 800,
|
|
.max_rate_up = 10,
|
|
.max_rate_down = 25,
|
|
.max_rt_delta = 300,
|
|
.driver_torque_multiplier = 1,
|
|
.driver_torque_allowance = 15,
|
|
.type = TorqueDriverLimited,
|
|
};
|
|
|
|
bool tx = true;
|
|
int bus = GET_BUS(to_send);
|
|
// Check if msg is sent on the main BUS
|
|
if (bus == MAZDA_MAIN) {
|
|
int addr = GET_ADDR(to_send);
|
|
|
|
// steer cmd checks
|
|
if (addr == MAZDA_LKAS) {
|
|
int desired_torque = (((GET_BYTE(to_send, 0) & 0x0FU) << 8) | GET_BYTE(to_send, 1)) - 2048U;
|
|
|
|
if (steer_torque_cmd_checks(desired_torque, -1, MAZDA_STEERING_LIMITS)) {
|
|
tx = false;
|
|
}
|
|
}
|
|
|
|
// cruise buttons check
|
|
if (addr == MAZDA_CRZ_BTNS) {
|
|
// allow resume spamming while controls allowed, but
|
|
// only allow cancel while controls not allowed
|
|
bool cancel_cmd = (GET_BYTE(to_send, 0) == 0x1U);
|
|
if (!controls_allowed && !cancel_cmd) {
|
|
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 RxCheck mazda_rx_checks[] = {
|
|
{.msg = {{MAZDA_CRZ_CTRL, 0, 8, .ignore_checksum = true, .ignore_counter = true, .frequency = 50U}, { 0 }, { 0 }}},
|
|
{.msg = {{MAZDA_CRZ_BTNS, 0, 8, .ignore_checksum = true, .ignore_counter = true, .frequency = 10U}, { 0 }, { 0 }}},
|
|
{.msg = {{MAZDA_STEER_TORQUE, 0, 8, .ignore_checksum = true, .ignore_counter = true, .frequency = 83U}, { 0 }, { 0 }}},
|
|
{.msg = {{MAZDA_ENGINE_DATA, 0, 8, .ignore_checksum = true, .ignore_counter = true, .frequency = 100U}, { 0 }, { 0 }}},
|
|
{.msg = {{MAZDA_PEDALS, 0, 8, .ignore_checksum = true, .ignore_counter = true, .frequency = 50U}, { 0 }, { 0 }}},
|
|
};
|
|
|
|
UNUSED(param);
|
|
return BUILD_SAFETY_CFG(mazda_rx_checks, MAZDA_TX_MSGS);
|
|
}
|
|
|
|
const safety_hooks mazda_hooks = {
|
|
.init = mazda_init,
|
|
.rx = mazda_rx_hook,
|
|
.tx = mazda_tx_hook,
|
|
};
|
|
|