9b1ff06bba
Maintain openpilot lateral control when the brake or gas pedals are used. Deactivation occurs only through the 'Cruise Control' button. Lots of credit goes to "pfeiferj"! Couldn't of done it without him! https: //github.com/pfeiferj/openpilot Co-Authored-By: Jacob Pfeifer <jacob@pfeifer.dev>
131 lines
3.6 KiB
C
131 lines
3.6 KiB
C
// 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_AUX 1
|
|
#define MAZDA_CAM 2
|
|
|
|
const SteeringLimits MAZDA_STEERING_LIMITS = {
|
|
.max_steer = 800,
|
|
.max_rate_up = 10,
|
|
.max_rate_down = 25,
|
|
.max_rt_delta = 300,
|
|
.max_rt_interval = 250000,
|
|
.driver_torque_factor = 1,
|
|
.driver_torque_allowance = 15,
|
|
.type = TorqueDriverLimited,
|
|
};
|
|
|
|
const CanMsg MAZDA_TX_MSGS[] = {{MAZDA_LKAS, 0, 8}, {MAZDA_CRZ_BTNS, 0, 8}, {MAZDA_LKAS_HUD, 0, 8}};
|
|
|
|
RxCheck mazda_rx_checks[] = {
|
|
{.msg = {{MAZDA_CRZ_CTRL, 0, 8, .frequency = 50U}, { 0 }, { 0 }}},
|
|
{.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 }}},
|
|
};
|
|
|
|
// 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) {
|
|
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);
|
|
}
|
|
|
|
generic_rx_checks((addr == MAZDA_LKAS));
|
|
}
|
|
}
|
|
|
|
static bool mazda_tx_hook(const CANPacket_t *to_send) {
|
|
bool tx = true;
|
|
int addr = GET_ADDR(to_send);
|
|
int bus = GET_BUS(to_send);
|
|
|
|
// Check if msg is sent on the main BUS
|
|
if (bus == MAZDA_MAIN) {
|
|
|
|
// 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 contrls not allowed
|
|
bool cancel_cmd = (GET_BYTE(to_send, 0) == 0x1U);
|
|
if (!controls_allowed && !cancel_cmd) {
|
|
tx = false;
|
|
}
|
|
}
|
|
}
|
|
|
|
return tx;
|
|
}
|
|
|
|
static int mazda_fwd_hook(int bus, int addr) {
|
|
int bus_fwd = -1;
|
|
|
|
if (bus == MAZDA_MAIN) {
|
|
bus_fwd = MAZDA_CAM;
|
|
} else if (bus == MAZDA_CAM) {
|
|
bool block = (addr == MAZDA_LKAS) || (addr == MAZDA_LKAS_HUD);
|
|
if (!block) {
|
|
bus_fwd = MAZDA_MAIN;
|
|
}
|
|
} else {
|
|
// don't fwd
|
|
}
|
|
|
|
return bus_fwd;
|
|
}
|
|
|
|
static safety_config mazda_init(uint16_t param) {
|
|
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,
|
|
.fwd = mazda_fwd_hook,
|
|
};
|