added bosch safety hooks and forwarding
parent
90c64b6c51
commit
5c7ef9ee43
|
@ -348,13 +348,14 @@ void can_rx(uint8_t can_number) {
|
|||
|
||||
// forwarding (panda only)
|
||||
#ifdef PANDA
|
||||
if (can_forwarding[bus_number] != -1) {
|
||||
int bus_fwd_num = can_forwarding[bus_number] != -1 ? can_forwarding[bus_number] : safety_fwd_hook(bus_number, &to_push);
|
||||
if (bus_fwd_num != -1) {
|
||||
CAN_FIFOMailBox_TypeDef to_send;
|
||||
to_send.RIR = to_push.RIR | 1; // TXRQ
|
||||
to_send.RDTR = to_push.RDTR;
|
||||
to_send.RDLR = to_push.RDLR;
|
||||
to_send.RDHR = to_push.RDHR;
|
||||
can_send(&to_send, can_forwarding[bus_number]);
|
||||
can_send(&to_send, bus_fwd_num);
|
||||
}
|
||||
#endif
|
||||
|
||||
|
|
|
@ -6,12 +6,14 @@ typedef void (*safety_hook_init)(int16_t param);
|
|||
typedef void (*rx_hook)(CAN_FIFOMailBox_TypeDef *to_push);
|
||||
typedef int (*tx_hook)(CAN_FIFOMailBox_TypeDef *to_send);
|
||||
typedef int (*tx_lin_hook)(int lin_num, uint8_t *data, int len);
|
||||
typedef int (*fwd_hook)(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd);
|
||||
|
||||
typedef struct {
|
||||
safety_hook_init init;
|
||||
rx_hook rx;
|
||||
tx_hook tx;
|
||||
tx_lin_hook tx_lin;
|
||||
fwd_hook fwd;
|
||||
} safety_hooks;
|
||||
|
||||
// This can be set by the safety hooks.
|
||||
|
@ -38,6 +40,10 @@ int safety_tx_lin_hook(int lin_num, uint8_t *data, int len){
|
|||
return current_hooks->tx_lin(lin_num, data, len);
|
||||
}
|
||||
|
||||
int safety_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
|
||||
return current_hooks->fwd(bus_num, to_fwd);
|
||||
}
|
||||
|
||||
typedef struct {
|
||||
uint16_t id;
|
||||
const safety_hooks *hooks;
|
||||
|
@ -48,12 +54,14 @@ typedef struct {
|
|||
#define SAFETY_TOYOTA 2
|
||||
#define SAFETY_TOYOTA_NOLIMITS 0x1336
|
||||
#define SAFETY_GM 3
|
||||
#define SAFETY_HONDA_BOSCH 4
|
||||
#define SAFETY_ALLOUTPUT 0x1337
|
||||
#define SAFETY_ELM327 0xE327
|
||||
|
||||
const safety_hook_config safety_hook_registry[] = {
|
||||
{SAFETY_NOOUTPUT, &nooutput_hooks},
|
||||
{SAFETY_HONDA, &honda_hooks},
|
||||
{SAFETY_HONDA_BOSCH, &honda_bosch_hooks},
|
||||
{SAFETY_TOYOTA, &toyota_hooks},
|
||||
{SAFETY_TOYOTA_NOLIMITS, &toyota_nolimits_hooks},
|
||||
{SAFETY_GM, &gm_hooks},
|
||||
|
|
|
@ -14,11 +14,16 @@ static int nooutput_tx_lin_hook(int lin_num, uint8_t *data, int len) {
|
|||
return false;
|
||||
}
|
||||
|
||||
static int nooutput_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
const safety_hooks nooutput_hooks = {
|
||||
.init = nooutput_init,
|
||||
.rx = default_rx_hook,
|
||||
.tx = nooutput_tx_hook,
|
||||
.tx_lin = nooutput_tx_lin_hook,
|
||||
.fwd = nooutput_fwd_hook,
|
||||
};
|
||||
|
||||
// *** all output safety mode ***
|
||||
|
@ -35,10 +40,15 @@ static int alloutput_tx_lin_hook(int lin_num, uint8_t *data, int len) {
|
|||
return true;
|
||||
}
|
||||
|
||||
static int alloutput_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
const safety_hooks alloutput_hooks = {
|
||||
.init = alloutput_init,
|
||||
.rx = default_rx_hook,
|
||||
.tx = alloutput_tx_hook,
|
||||
.tx_lin = alloutput_tx_lin_hook,
|
||||
.fwd = alloutput_fwd_hook,
|
||||
};
|
||||
|
||||
|
|
|
@ -31,9 +31,14 @@ static void elm327_init(int16_t param) {
|
|||
controls_allowed = 1;
|
||||
}
|
||||
|
||||
static int elm327_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
const safety_hooks elm327_hooks = {
|
||||
.init = elm327_init,
|
||||
.rx = elm327_rx_hook,
|
||||
.tx = elm327_tx_hook,
|
||||
.tx_lin = elm327_tx_lin_hook,
|
||||
.fwd = elm327_fwd_hook,
|
||||
};
|
||||
|
|
|
@ -172,10 +172,15 @@ static void gm_init(int16_t param) {
|
|||
controls_allowed = 0;
|
||||
}
|
||||
|
||||
static int gm_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
const safety_hooks gm_hooks = {
|
||||
.init = gm_init,
|
||||
.rx = gm_rx_hook,
|
||||
.tx = gm_tx_hook,
|
||||
.tx_lin = gm_tx_lin_hook,
|
||||
.fwd = gm_fwd_hook,
|
||||
};
|
||||
|
||||
|
|
|
@ -14,6 +14,9 @@ int gas_prev = 0;
|
|||
int gas_interceptor_prev = 0;
|
||||
int ego_speed = 0;
|
||||
|
||||
// TODO: auto-detect bosch hardware based on CAN messages?
|
||||
bool bosch_hardware = false;
|
||||
|
||||
static void honda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
|
||||
|
||||
// sample speed
|
||||
|
@ -35,7 +38,7 @@ static void honda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
|
|||
|
||||
// exit controls on rising edge of brake press or on brake press when
|
||||
// speed > 0
|
||||
if ((to_push->RIR>>21) == 0x17C) {
|
||||
if (!bosch_hardware && (to_push->RIR>>21) == 0x17C) {
|
||||
// bit 53
|
||||
int brake = to_push->RDHR & 0x200000;
|
||||
if (brake && (!(brake_prev) || ego_speed)) {
|
||||
|
@ -45,7 +48,7 @@ static void honda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
|
|||
}
|
||||
|
||||
// exit controls on rising edge of gas press if interceptor
|
||||
if ((to_push->RIR>>21) == 0x201) {
|
||||
if (!bosch_hardware && (to_push->RIR>>21) == 0x201) {
|
||||
gas_interceptor_detected = 1;
|
||||
int gas_interceptor = ((to_push->RDLR & 0xFF) << 8) | ((to_push->RDLR & 0xFF00) >> 8);
|
||||
if ((gas_interceptor > 328) && (gas_interceptor_prev <= 328)) {
|
||||
|
@ -55,7 +58,7 @@ static void honda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
|
|||
}
|
||||
|
||||
// exit controls on rising edge of gas press if no interceptor
|
||||
if (!gas_interceptor_detected) {
|
||||
if (!bosch_hardware && !gas_interceptor_detected) {
|
||||
if ((to_push->RIR>>21) == 0x17C) {
|
||||
int gas = to_push->RDLR & 0xFF;
|
||||
if (gas && !(gas_prev)) {
|
||||
|
@ -80,7 +83,7 @@ static int honda_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
|
|||
int current_controls_allowed = controls_allowed && !(pedal_pressed);
|
||||
|
||||
// BRAKE: safety check
|
||||
if ((to_send->RIR>>21) == 0x1FA) {
|
||||
if (!bosch_hardware && (to_send->RIR>>21) == 0x1FA) {
|
||||
if (current_controls_allowed) {
|
||||
if ((to_send->RDLR & 0xFFFFFF3F) != to_send->RDLR) return 0;
|
||||
} else {
|
||||
|
@ -89,7 +92,7 @@ static int honda_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
|
|||
}
|
||||
|
||||
// STEER: safety check
|
||||
if ((to_send->RIR>>21) == 0xE4 || (to_send->RIR>>21) == 0x194) {
|
||||
if ((to_send->RIR>>21) == 0xE4 || (!bosch_hardware && (to_send->RIR>>21) == 0x194)) {
|
||||
if (current_controls_allowed) {
|
||||
// all messages are fine here
|
||||
} else {
|
||||
|
@ -98,7 +101,7 @@ static int honda_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
|
|||
}
|
||||
|
||||
// GAS: safety check
|
||||
if ((to_send->RIR>>21) == 0x200) {
|
||||
if (!bosch_hardware && (to_send->RIR>>21) == 0x200) {
|
||||
if (current_controls_allowed) {
|
||||
// all messages are fine here
|
||||
} else {
|
||||
|
@ -119,10 +122,32 @@ static void honda_init(int16_t param) {
|
|||
controls_allowed = 0;
|
||||
}
|
||||
|
||||
static int honda_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
|
||||
int bus_fwd_num = -1;
|
||||
if (bosch_hardware && (bus_num == 1 || bus_num == 2)) {
|
||||
int addr = to_fwd->RIR>>21;
|
||||
bus_fwd_num = addr != 0xE4 && addr != 0x33D && addr < 0x1000 ? (uint8_t)(~bus_num & 0x3) : -1;
|
||||
}
|
||||
return bus_fwd_num;
|
||||
}
|
||||
|
||||
const safety_hooks honda_hooks = {
|
||||
.init = honda_init,
|
||||
.rx = honda_rx_hook,
|
||||
.tx = honda_tx_hook,
|
||||
.tx_lin = honda_tx_lin_hook,
|
||||
.fwd = honda_fwd_hook,
|
||||
};
|
||||
|
||||
static void honda_bosch_init(int16_t param) {
|
||||
controls_allowed = 0;
|
||||
bosch_hardware = true;
|
||||
}
|
||||
|
||||
const safety_hooks honda_bosch_hooks = {
|
||||
.init = honda_bosch_init,
|
||||
.rx = honda_rx_hook,
|
||||
.tx = honda_tx_hook,
|
||||
.tx_lin = honda_tx_lin_hook,
|
||||
.fwd = honda_fwd_hook,
|
||||
};
|
||||
|
|
|
@ -161,11 +161,16 @@ static void toyota_init(int16_t param) {
|
|||
dbc_eps_torque_factor = param;
|
||||
}
|
||||
|
||||
static int toyota_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
const safety_hooks toyota_hooks = {
|
||||
.init = toyota_init,
|
||||
.rx = toyota_rx_hook,
|
||||
.tx = toyota_tx_hook,
|
||||
.tx_lin = toyota_tx_lin_hook,
|
||||
.fwd = toyota_fwd_hook,
|
||||
};
|
||||
|
||||
static void toyota_nolimits_init(int16_t param) {
|
||||
|
@ -179,4 +184,5 @@ const safety_hooks toyota_nolimits_hooks = {
|
|||
.rx = toyota_rx_hook,
|
||||
.tx = toyota_tx_hook,
|
||||
.tx_lin = toyota_tx_lin_hook,
|
||||
.fwd = toyota_fwd_hook,
|
||||
};
|
||||
|
|
|
@ -104,6 +104,7 @@ class Panda(object):
|
|||
SAFETY_NOOUTPUT = 0
|
||||
SAFETY_HONDA = 1
|
||||
SAFETY_TOYOTA = 2
|
||||
SAFETY_HONDA_BOSCH = 4
|
||||
SAFETY_TOYOTA_NOLIMITS = 0x1336
|
||||
SAFETY_ALLOUTPUT = 0x1337
|
||||
SAFETY_ELM327 = 0xE327
|
||||
|
|
Loading…
Reference in New Issue