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.
102 lines
2.6 KiB
102 lines
2.6 KiB
// board enforces
|
|
// in-state
|
|
// accel set/resume
|
|
// out-state
|
|
// cancel button
|
|
// accel rising edge
|
|
// brake rising edge
|
|
// brake > 0mph
|
|
|
|
int ford_brake_prev = 0;
|
|
int ford_gas_prev = 0;
|
|
bool ford_moving = false;
|
|
|
|
static void ford_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
|
|
|
|
int addr = GET_ADDR(to_push);
|
|
|
|
if (addr == 0x217) {
|
|
// wheel speeds are 14 bits every 16
|
|
ford_moving = false;
|
|
for (int i = 0; i < 8; i += 2) {
|
|
ford_moving |= GET_BYTE(to_push, i) | (GET_BYTE(to_push, (int)(i + 1)) & 0xFCU);
|
|
}
|
|
}
|
|
|
|
// state machine to enter and exit controls
|
|
if (addr == 0x83) {
|
|
bool cancel = GET_BYTE(to_push, 1) & 0x1;
|
|
bool set_or_resume = GET_BYTE(to_push, 3) & 0x30;
|
|
if (cancel) {
|
|
controls_allowed = 0;
|
|
}
|
|
if (set_or_resume) {
|
|
controls_allowed = 1;
|
|
}
|
|
}
|
|
|
|
// exit controls on rising edge of brake press or on brake press when
|
|
// speed > 0
|
|
if (addr == 0x165) {
|
|
int brake = GET_BYTE(to_push, 0) & 0x20;
|
|
if (brake && (!(ford_brake_prev) || ford_moving)) {
|
|
controls_allowed = 0;
|
|
}
|
|
ford_brake_prev = brake;
|
|
}
|
|
|
|
// exit controls on rising edge of gas press
|
|
if (addr == 0x204) {
|
|
int gas = (GET_BYTE(to_push, 0) & 0x03) | GET_BYTE(to_push, 1);
|
|
if (gas && !(ford_gas_prev)) {
|
|
controls_allowed = 0;
|
|
}
|
|
ford_gas_prev = gas;
|
|
}
|
|
}
|
|
|
|
// all commands: just steering
|
|
// if controls_allowed and no pedals pressed
|
|
// allow all commands up to limit
|
|
// else
|
|
// block all commands that produce actuation
|
|
|
|
static int ford_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
|
|
|
|
int tx = 1;
|
|
// disallow actuator commands if gas or brake (with vehicle moving) are pressed
|
|
// and the the latching controls_allowed flag is True
|
|
int pedal_pressed = ford_gas_prev || (ford_brake_prev && ford_moving);
|
|
bool current_controls_allowed = controls_allowed && !(pedal_pressed);
|
|
int addr = GET_ADDR(to_send);
|
|
|
|
// STEER: safety check
|
|
if (addr == 0x3CA) {
|
|
if (!current_controls_allowed) {
|
|
// bits 7-4 need to be 0xF to disallow lkas commands
|
|
if ((GET_BYTE(to_send, 0) & 0xF0) != 0xF0) {
|
|
tx = 0;
|
|
}
|
|
}
|
|
}
|
|
|
|
// FORCE CANCEL: safety check only relevant when spamming the cancel button
|
|
// ensuring that set and resume aren't sent
|
|
if (addr == 0x83) {
|
|
if ((GET_BYTE(to_send, 3) & 0x30) != 0) {
|
|
tx = 0;
|
|
}
|
|
}
|
|
|
|
// 1 allows the message through
|
|
return tx;
|
|
}
|
|
|
|
const safety_hooks ford_hooks = {
|
|
.init = nooutput_init,
|
|
.rx = ford_rx_hook,
|
|
.tx = ford_tx_hook,
|
|
.tx_lin = nooutput_tx_lin_hook,
|
|
.ignition = default_ign_hook,
|
|
.fwd = default_fwd_hook,
|
|
};
|
|
|