2018-05-02 23:45:23 -06:00
|
|
|
// 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;
|
2019-07-10 21:56:03 -06:00
|
|
|
bool ford_moving = false;
|
2018-05-02 23:45:23 -06:00
|
|
|
|
|
|
|
static void ford_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
|
|
|
|
|
2019-06-14 20:13:05 -06:00
|
|
|
int addr = GET_ADDR(to_push);
|
2019-11-15 01:32:45 -07:00
|
|
|
int bus = GET_BUS(to_push);
|
2019-06-14 20:13:05 -06:00
|
|
|
|
|
|
|
if (addr == 0x217) {
|
2018-05-02 23:45:23 -06:00
|
|
|
// wheel speeds are 14 bits every 16
|
2019-07-10 21:56:03 -06:00
|
|
|
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);
|
|
|
|
}
|
2018-05-02 23:45:23 -06:00
|
|
|
}
|
|
|
|
|
|
|
|
// state machine to enter and exit controls
|
2019-06-14 20:13:05 -06:00
|
|
|
if (addr == 0x83) {
|
2019-07-10 21:56:03 -06:00
|
|
|
bool cancel = GET_BYTE(to_push, 1) & 0x1;
|
|
|
|
bool set_or_resume = GET_BYTE(to_push, 3) & 0x30;
|
2018-05-02 23:45:23 -06:00
|
|
|
if (cancel) {
|
|
|
|
controls_allowed = 0;
|
2019-06-17 15:13:50 -06:00
|
|
|
}
|
|
|
|
if (set_or_resume) {
|
2018-05-02 23:45:23 -06:00
|
|
|
controls_allowed = 1;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
// exit controls on rising edge of brake press or on brake press when
|
|
|
|
// speed > 0
|
2019-06-14 20:13:05 -06:00
|
|
|
if (addr == 0x165) {
|
2019-07-10 21:56:03 -06:00
|
|
|
int brake = GET_BYTE(to_push, 0) & 0x20;
|
|
|
|
if (brake && (!(ford_brake_prev) || ford_moving)) {
|
2018-05-02 23:45:23 -06:00
|
|
|
controls_allowed = 0;
|
|
|
|
}
|
|
|
|
ford_brake_prev = brake;
|
|
|
|
}
|
|
|
|
|
|
|
|
// exit controls on rising edge of gas press
|
2019-06-14 20:13:05 -06:00
|
|
|
if (addr == 0x204) {
|
2019-07-10 21:56:03 -06:00
|
|
|
int gas = (GET_BYTE(to_push, 0) & 0x03) | GET_BYTE(to_push, 1);
|
2018-05-02 23:45:23 -06:00
|
|
|
if (gas && !(ford_gas_prev)) {
|
|
|
|
controls_allowed = 0;
|
|
|
|
}
|
|
|
|
ford_gas_prev = gas;
|
|
|
|
}
|
2019-11-15 01:32:45 -07:00
|
|
|
|
2019-11-26 22:37:01 -07:00
|
|
|
if ((safety_mode_cnt > 1) && (bus == 0) && (addr == 0x3CA)) {
|
2019-11-15 01:32:45 -07:00
|
|
|
relay_malfunction = true;
|
|
|
|
}
|
2018-05-02 23:45:23 -06:00
|
|
|
}
|
|
|
|
|
|
|
|
// 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) {
|
|
|
|
|
2019-06-12 19:18:07 -06:00
|
|
|
int tx = 1;
|
2019-11-15 01:32:45 -07:00
|
|
|
int addr = GET_ADDR(to_send);
|
|
|
|
|
2018-05-02 23:45:23 -06:00
|
|
|
// disallow actuator commands if gas or brake (with vehicle moving) are pressed
|
|
|
|
// and the the latching controls_allowed flag is True
|
2019-07-10 21:56:03 -06:00
|
|
|
int pedal_pressed = ford_gas_prev || (ford_brake_prev && ford_moving);
|
2019-06-12 21:12:48 -06:00
|
|
|
bool current_controls_allowed = controls_allowed && !(pedal_pressed);
|
2019-11-15 01:32:45 -07:00
|
|
|
|
|
|
|
if (relay_malfunction) {
|
|
|
|
tx = 0;
|
|
|
|
}
|
2018-05-02 23:45:23 -06:00
|
|
|
|
|
|
|
// STEER: safety check
|
2019-06-12 19:18:07 -06:00
|
|
|
if (addr == 0x3CA) {
|
2019-06-17 15:13:50 -06:00
|
|
|
if (!current_controls_allowed) {
|
2018-05-02 23:45:23 -06:00
|
|
|
// bits 7-4 need to be 0xF to disallow lkas commands
|
2019-07-10 21:56:03 -06:00
|
|
|
if ((GET_BYTE(to_send, 0) & 0xF0) != 0xF0) {
|
2019-06-12 19:18:07 -06:00
|
|
|
tx = 0;
|
|
|
|
}
|
2018-05-02 23:45:23 -06:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
// FORCE CANCEL: safety check only relevant when spamming the cancel button
|
|
|
|
// ensuring that set and resume aren't sent
|
2019-06-12 19:18:07 -06:00
|
|
|
if (addr == 0x83) {
|
2019-07-10 21:56:03 -06:00
|
|
|
if ((GET_BYTE(to_send, 3) & 0x30) != 0) {
|
2019-06-12 19:18:07 -06:00
|
|
|
tx = 0;
|
|
|
|
}
|
2018-05-02 23:45:23 -06:00
|
|
|
}
|
|
|
|
|
|
|
|
// 1 allows the message through
|
2019-06-12 19:18:07 -06:00
|
|
|
return tx;
|
2018-05-02 23:45:23 -06:00
|
|
|
}
|
|
|
|
|
2019-11-15 01:32:45 -07:00
|
|
|
// TODO: keep camera on bus 2 and make a fwd_hook
|
|
|
|
|
2018-05-02 23:45:23 -06:00
|
|
|
const safety_hooks ford_hooks = {
|
2018-08-28 17:01:40 -06:00
|
|
|
.init = nooutput_init,
|
2018-05-02 23:45:23 -06:00
|
|
|
.rx = ford_rx_hook,
|
|
|
|
.tx = ford_tx_hook,
|
2018-08-28 17:01:40 -06:00
|
|
|
.tx_lin = nooutput_tx_lin_hook,
|
2019-06-11 20:50:12 -06:00
|
|
|
.fwd = default_fwd_hook,
|
2018-05-02 23:45:23 -06:00
|
|
|
};
|