Ford safety (#115)
* added initial ford safety header * added ford ok safety. Missing brake check * added force cancel check in ford safety * added brake check to fusion * fixed nomenclaturemaster
parent
9cffa74e04
commit
ace4a224c8
|
@ -30,6 +30,7 @@ int controls_allowed = 0;
|
|||
#include "safety/safety_toyota_ipas.h"
|
||||
#endif
|
||||
#include "safety/safety_gm.h"
|
||||
#include "safety/safety_ford.h"
|
||||
#include "safety/safety_elm327.h"
|
||||
|
||||
const safety_hooks *current_hooks = &nooutput_hooks;
|
||||
|
@ -64,10 +65,11 @@ typedef struct {
|
|||
#define SAFETY_NOOUTPUT 0
|
||||
#define SAFETY_HONDA 1
|
||||
#define SAFETY_TOYOTA 2
|
||||
#define SAFETY_TOYOTA_IPAS 0x1335
|
||||
#define SAFETY_TOYOTA_NOLIMITS 0x1336
|
||||
#define SAFETY_GM 3
|
||||
#define SAFETY_HONDA_BOSCH 4
|
||||
#define SAFETY_FORD 5
|
||||
#define SAFETY_TOYOTA_IPAS 0x1335
|
||||
#define SAFETY_TOYOTA_NOLIMITS 0x1336
|
||||
#define SAFETY_ALLOUTPUT 0x1337
|
||||
#define SAFETY_ELM327 0xE327
|
||||
|
||||
|
@ -76,11 +78,12 @@ const safety_hook_config safety_hook_registry[] = {
|
|||
{SAFETY_HONDA, &honda_hooks},
|
||||
{SAFETY_HONDA_BOSCH, &honda_bosch_hooks},
|
||||
{SAFETY_TOYOTA, &toyota_hooks},
|
||||
{SAFETY_GM, &gm_hooks},
|
||||
{SAFETY_FORD, &ford_hooks},
|
||||
{SAFETY_TOYOTA_NOLIMITS, &toyota_nolimits_hooks},
|
||||
#ifdef PANDA
|
||||
{SAFETY_TOYOTA_IPAS, &toyota_ipas_hooks},
|
||||
#endif
|
||||
{SAFETY_GM, &gm_hooks},
|
||||
{SAFETY_ALLOUTPUT, &alloutput_hooks},
|
||||
{SAFETY_ELM327, &elm327_hooks},
|
||||
};
|
||||
|
|
|
@ -0,0 +1,110 @@
|
|||
// 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;
|
||||
int ford_is_moving = 0;
|
||||
|
||||
static void ford_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
|
||||
|
||||
if ((to_push->RIR>>21) == 0x217) {
|
||||
// wheel speeds are 14 bits every 16
|
||||
ford_is_moving = 0xFCFF & (to_push->RDLR | (to_push->RDLR >> 16) |
|
||||
to_push->RDHR | (to_push->RDHR >> 16));
|
||||
}
|
||||
|
||||
// state machine to enter and exit controls
|
||||
if ((to_push->RIR>>21) == 0x83) {
|
||||
int cancel = ((to_push->RDLR >> 8) & 0x1);
|
||||
int set_or_resume = (to_push->RDLR >> 28) & 0x3;
|
||||
if (cancel) {
|
||||
controls_allowed = 0;
|
||||
} else if (set_or_resume) {
|
||||
controls_allowed = 1;
|
||||
}
|
||||
}
|
||||
|
||||
// exit controls on rising edge of brake press or on brake press when
|
||||
// speed > 0
|
||||
if ((to_push->RIR>>21) == 0x165) {
|
||||
int brake = to_push->RDLR & 0x20;
|
||||
if (brake && (!(ford_brake_prev) || ford_is_moving)) {
|
||||
controls_allowed = 0;
|
||||
}
|
||||
ford_brake_prev = brake;
|
||||
}
|
||||
|
||||
// exit controls on rising edge of gas press
|
||||
if ((to_push->RIR>>21) == 0x204) {
|
||||
int gas = to_push->RDLR & 0xFF03;
|
||||
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) {
|
||||
|
||||
// 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_is_moving);
|
||||
int current_controls_allowed = controls_allowed && !(pedal_pressed);
|
||||
|
||||
// STEER: safety check
|
||||
if ((to_send->RIR>>21) == 0x3CA) {
|
||||
if (current_controls_allowed) {
|
||||
// all messages are fine here
|
||||
} else {
|
||||
// bits 7-4 need to be 0xF to disallow lkas commands
|
||||
if (((to_send->RDLR >> 4) & 0xF) != 0xF) return 0;
|
||||
}
|
||||
}
|
||||
|
||||
// FORCE CANCEL: safety check only relevant when spamming the cancel button
|
||||
// ensuring that set and resume aren't sent
|
||||
if ((to_send->RIR>>21) == 0x83) {
|
||||
if ((to_send->RDLR >> 28) & 0x3) return 0;
|
||||
}
|
||||
|
||||
// 1 allows the message through
|
||||
return true;
|
||||
}
|
||||
|
||||
static int ford_tx_lin_hook(int lin_num, uint8_t *data, int len) {
|
||||
// TODO: add safety if using LIN
|
||||
return true;
|
||||
}
|
||||
|
||||
static void ford_init(int16_t param) {
|
||||
controls_allowed = 0;
|
||||
}
|
||||
|
||||
static int ford_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
static int ford_ign_hook() {
|
||||
return -1;
|
||||
}
|
||||
|
||||
const safety_hooks ford_hooks = {
|
||||
.init = ford_init,
|
||||
.rx = ford_rx_hook,
|
||||
.tx = ford_tx_hook,
|
||||
.tx_lin = ford_tx_lin_hook,
|
||||
.ignition = ford_ign_hook,
|
||||
.fwd = ford_fwd_hook,
|
||||
};
|
Loading…
Reference in New Issue