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.
136 lines
3.4 KiB
136 lines
3.4 KiB
// board enforces
|
|
// in-state
|
|
// ACC is active (green)
|
|
// out-state
|
|
// brake pressed
|
|
// stock LKAS ECU is online
|
|
// ACC is not active (white or disabled)
|
|
|
|
// chrysler_: namespacing
|
|
int chrysler_speed = 0;
|
|
|
|
// silence everything if stock ECUs are still online
|
|
int chrysler_lkas_detected = 0;
|
|
int chrysler_desired_torque_last = 0;
|
|
|
|
static void chrysler_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
|
|
int bus_number = (to_push->RDTR >> 4) & 0xFF;
|
|
uint32_t addr;
|
|
if (to_push->RIR & 4) {
|
|
// Extended
|
|
// Not looked at, but have to be separated
|
|
// to avoid address collision
|
|
addr = to_push->RIR >> 3;
|
|
} else {
|
|
// Normal
|
|
addr = to_push->RIR >> 21;
|
|
}
|
|
|
|
if (addr == 0x144 && bus_number == 0) {
|
|
chrysler_speed = ((to_push->RDLR & 0xFF000000) >> 16) | (to_push->RDHR & 0xFF);
|
|
}
|
|
|
|
// check if stock LKAS ECU is still online
|
|
if (addr == 0x292 && bus_number == 0) {
|
|
chrysler_lkas_detected = 1;
|
|
controls_allowed = 0;
|
|
}
|
|
|
|
// ["ACC_2"]['ACC_STATUS_2'] == 7 for active (green) Adaptive Cruise Control
|
|
if (addr == 0x1f4 && bus_number == 0) {
|
|
if (((to_push->RDLR & 0x380000) >> 19) == 7) {
|
|
controls_allowed = 1;
|
|
} else {
|
|
controls_allowed = 0;
|
|
}
|
|
}
|
|
|
|
// exit controls on brake press by human
|
|
if (addr == 0x140) {
|
|
if (to_push->RDLR & 0x4) {
|
|
controls_allowed = 0;
|
|
}
|
|
}
|
|
}
|
|
|
|
// if controls_allowed
|
|
// allow steering up to limit
|
|
// else
|
|
// block all commands that produce actuation
|
|
static int chrysler_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
|
|
// There can be only one! (LKAS)
|
|
if (chrysler_lkas_detected) {
|
|
return 0;
|
|
}
|
|
|
|
uint32_t addr;
|
|
if (to_send->RIR & 4) {
|
|
// Extended
|
|
addr = to_send->RIR >> 3;
|
|
} else {
|
|
// Normal
|
|
addr = to_send->RIR >> 21;
|
|
}
|
|
|
|
// LKA STEER: Too large of values cause the steering actuator ECU to silently
|
|
// fault and no longer actuate the wheel until the car is rebooted.
|
|
if (addr == 0x292) {
|
|
int rdlr = to_send->RDLR;
|
|
int straight = 1024;
|
|
int steer = ((rdlr & 0x7) << 8) + ((rdlr & 0xFF00) >> 8) - straight;
|
|
int max_steer = 230;
|
|
int max_rate = 50; // ECU is fine with 100, but 3 is typical.
|
|
if (steer > max_steer) {
|
|
return false;
|
|
}
|
|
if (steer < -max_steer) {
|
|
return false;
|
|
}
|
|
if (!controls_allowed && steer != 0) {
|
|
// If controls not allowed, only allow steering to move closer to 0.
|
|
if (chrysler_desired_torque_last == 0) {
|
|
return false;
|
|
}
|
|
if ((chrysler_desired_torque_last > 0) && (steer >= chrysler_desired_torque_last)) {
|
|
return false;
|
|
}
|
|
if ((chrysler_desired_torque_last < 0) && (steer <= chrysler_desired_torque_last)) {
|
|
return false;
|
|
}
|
|
}
|
|
if (steer < (chrysler_desired_torque_last - max_rate)) {
|
|
return false;
|
|
}
|
|
if (steer > (chrysler_desired_torque_last + max_rate)) {
|
|
return false;
|
|
}
|
|
|
|
chrysler_desired_torque_last = steer;
|
|
}
|
|
|
|
// 1 allows the message through
|
|
return true;
|
|
}
|
|
|
|
static int chrysler_tx_lin_hook(int lin_num, uint8_t *data, int len) {
|
|
// LIN is not used.
|
|
return false;
|
|
}
|
|
|
|
static void chrysler_init(int16_t param) {
|
|
controls_allowed = 0;
|
|
}
|
|
|
|
static int chrysler_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
|
|
return -1;
|
|
}
|
|
|
|
const safety_hooks chrysler_hooks = {
|
|
.init = chrysler_init,
|
|
.rx = chrysler_rx_hook,
|
|
.tx = chrysler_tx_hook,
|
|
.tx_lin = chrysler_tx_lin_hook,
|
|
.ignition = default_ign_hook,
|
|
.fwd = chrysler_fwd_hook,
|
|
};
|
|
|
|
|