retropilot
/
panda
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Misra 15 5 (#210) 

Remove 15.5 violations
master
rbiasini 2019-06-12 18:18:07 -07:00 committed by GitHub
parent 3b496eefd3
commit 67b831ef45
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
14 changed files with 226 additions and 151 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
board/safety.h
View File

@ -81,16 +81,18 @@ const safety_hook_config safety_hook_registry[] = {
#define HOOK_CONFIG_COUNT (sizeof(safety_hook_registry)/sizeof(safety_hook_config))
int safety_set_mode(uint16_t mode, int16_t param) {
int set_status = -1; // not set
for (int i = 0; i < HOOK_CONFIG_COUNT; i++) {
if (safety_hook_registry[i].id == mode) {
current_hooks = safety_hook_registry[i].hooks;
if (current_hooks->init) {
current_hooks->init(param);
}
return 0;
set_status = 0; // set
break;
}
}
return -1;
if ((set_status == 0) && (current_hooks->init != NULL)) {
current_hooks->init(param);
}
return set_status;
}
// compute the time elapsed (in microseconds) from 2 counter samples
@ -118,7 +120,7 @@ void update_sample(struct sample_t *sample, int sample_new) {
// get the minimum and maximum measured samples
sample->min = sample->values[0];
sample->max = sample->values[0];
for (int i = 1; i < sizeof(sample->values)/sizeof(sample->values[0]); i++) {
for (int i = 1; i < sizeof(sample->values) / sizeof(sample->values[0]); i++) {
if (sample->values[i] < sample->min) {
sample->min = sample->values[i];
}
@ -184,10 +186,13 @@ int rt_rate_limit_check(int val, int val_last, const int MAX_RT_DELTA) {
// interp function that holds extreme values
float interpolate(struct lookup_t xy, float x) {
int size = sizeof(xy.x) / sizeof(xy.x[0]);
float ret = xy.y[size - 1]; // default output is last point
// x is lower than the first point in the x array. Return the first point
if (x <= xy.x[0]) {
return xy.y[0];
ret = xy.y[0];
} else {
// find the index such that (xy.x[i] <= x < xy.x[i+1]) and linearly interp
@ -201,10 +206,10 @@ float interpolate(struct lookup_t xy, float x) {
if (dx <= 0.) {
dx = 0.0001;
}
return (dy * (x - x0) / dx) + y0;
ret = (dy * (x - x0) / dx) + y0;
break;
}
}
// if no such point is found, then x > xy.x[size-1]. Return last point
return xy.y[size - 1];
}
return ret;
}

5
board/safety/safety_cadillac.h
View File

@ -54,6 +54,7 @@ static void cadillac_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
}
static int cadillac_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
int tx = 1;
uint32_t addr = to_send->RIR >> 21;
// steer cmd checks
@ -102,11 +103,11 @@ static int cadillac_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
}
if (violation || cadillac_supercruise_on) {
return false;
tx = 0;
}
}
return true;
return tx;
}
static void cadillac_init(int16_t param) {

18
board/safety/safety_chrysler.h
View File

@ -54,9 +54,11 @@ static void chrysler_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
static int chrysler_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
// There can be only one! (camera)
int tx = 1;
// If camera is on bus 0, then nothing can be sent
if (chrysler_camera_detected) {
return 0;
tx = 0;
}
uint32_t addr;
@ -112,7 +114,7 @@ static int chrysler_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
}
if (violation) {
return false;
tx = 0;
}
}
@ -122,7 +124,7 @@ static int chrysler_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
// TODO: fix bug preventing the button msg to be fwd'd on bus 2
// 1 allows the message through
return true;
return tx;
}
static void chrysler_init(int16_t param) {
@ -131,16 +133,18 @@ static void chrysler_init(int16_t param) {
}
static int chrysler_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
int bus_fwd = -1;
int32_t addr = to_fwd->RIR >> 21;
// forward CAN 0 -> 2 so stock LKAS camera sees messages
if ((bus_num == 0) && !chrysler_camera_detected) {
return 2;
bus_fwd = 2;
}
// forward all messages from camera except LKAS_COMMAND and LKAS_HUD
if ((bus_num == 2) && !chrysler_camera_detected && (addr != 658) && (addr != 678)) {
return 0;
bus_fwd = 0;
}
return -1; // do not forward
return bus_fwd;
}

46
board/safety/safety_elm327.h
View File

@ -1,28 +1,46 @@
static int elm327_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
//All ELM traffic must appear on CAN0
if(((to_send->RDTR >> 4) & 0xf) != 0) return 0;
//All ISO 15765-4 messages must be 8 bytes long
if((to_send->RDTR & 0xf) != 8) return 0;
if(to_send->RIR & 4){
int tx = 1;
//All ELM traffic must appear on CAN0
if (((to_send->RDTR >> 4) & 0xf) != 0) {
tx = 0;
}
//All ISO 15765-4 messages must be 8 bytes long
if ((to_send->RDTR & 0xf) != 8) {
tx = 0;
}
if (to_send->RIR & 4) {
uint32_t addr = to_send->RIR >> 3;
//Check valid 29 bit send addresses for ISO 15765-4
if(!((addr == 0x18DB33F1) || ((addr & 0x1FFF00FF) == 0x18DA00F1))) return 0;
if (!((addr == 0x18DB33F1) || ((addr & 0x1FFF00FF) == 0x18DA00F1))) {
tx = 0;
}
} else {
uint32_t addr = to_send->RIR >> 21;
//Check valid 11 bit send addresses for ISO 15765-4
if(!((addr == 0x7DF) || ((addr & 0x7F8) == 0x7E0))) return 0;
if (!((addr == 0x7DF) || ((addr & 0x7F8) == 0x7E0))) {
tx = 0;
}
}
return true;
return tx;
}
static int elm327_tx_lin_hook(int lin_num, uint8_t *data, int len) {
if(lin_num != 0) return false; //Only operate on LIN 0, aka serial 2
if((len < 5) || (len > 11)) return false; //Valid KWP size
if(!(((data[0] & 0xF8) == 0xC0) && ((data[0] & 0x07) > 0) &&
(data[1] == 0x33) && (data[2] == 0xF1))) return false; //Bad msg
return true;
int tx = 1;
if (lin_num != 0) {
tx = 0; //Only operate on LIN 0, aka serial 2
}
if ((len < 5) || (len > 11)) {
tx = 0; //Valid KWP size
}
if (!(((data[0] & 0xF8) == 0xC0) && ((data[0] & 0x07) > 0) &&
(data[1] == 0x33) && (data[2] == 0xF1))) {
tx = 0; //Bad msg
}
return tx;
}
const safety_hooks elm327_hooks = {

16
board/safety/safety_ford.h
View File

@ -58,29 +58,35 @@ static void ford_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
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_is_moving);
int current_controls_allowed = controls_allowed && !(pedal_pressed);
int addr = to_send->RIR >> 21;
// STEER: safety check
if ((to_send->RIR>>21) == 0x3CA) {
if (addr == 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;
if (((to_send->RDLR >> 4) & 0xF) != 0xF) {
tx = 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;
if (addr == 0x83) {
if ((to_send->RDLR >> 28) & 0x3) {
tx = 0;
}
}
// 1 allows the message through
return true;
return tx;
}
const safety_hooks ford_hooks = {

28
board/safety/safety_gm.h
View File

@ -124,9 +124,11 @@ static void gm_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
static int gm_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
int tx = 1;
// There can be only one! (ASCM)
if (gm_ascm_detected) {
return 0;
tx = 0;
}
// disallow actuator commands if gas or brake (with vehicle moving) are pressed
@ -149,9 +151,13 @@ static int gm_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
int brake = ((rdlr & 0xF) << 8) + ((rdlr & 0xFF00) >> 8);
brake = (0x1000 - brake) & 0xFFF;
if (current_controls_allowed && long_controls_allowed) {
if (brake > GM_MAX_BRAKE) return 0;
if (brake > GM_MAX_BRAKE) {
tx = 0;
}
} else {
if (brake != 0) return 0;
if (brake != 0) {
tx = 0;
}
}
}
@ -200,12 +206,14 @@ static int gm_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
}
if (violation) {
return false;
tx = 0;
}
}
// PARK ASSIST STEER: unlimited torque, no thanks
if (addr == 823) return 0;
if (addr == 823) {
tx = 0;
}
// GAS/REGEN: safety check
if (addr == 715) {
@ -213,16 +221,20 @@ static int gm_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
int gas_regen = ((rdlr & 0x7F0000) >> 11) + ((rdlr & 0xF8000000) >> 27);
int apply = rdlr & 1;
if (current_controls_allowed && long_controls_allowed) {
if (gas_regen > GM_MAX_GAS) return 0;
if (gas_regen > GM_MAX_GAS) {
tx = 0;
}
} else {
// Disabled message is !engaed with gas
// value that corresponds to max regen.
if (apply || (gas_regen != GM_MAX_REGEN)) return 0;
if (apply || (gas_regen != GM_MAX_REGEN)) {
tx = 0;
}
}
}
// 1 allows the message through
return true;
return tx;
}
static void gm_init(int16_t param) {

37
board/safety/safety_gm_ascm.h
View File

@ -3,42 +3,33 @@
static int gm_ascm_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
uint32_t addr = to_fwd->RIR>>21;
int bus_fwd = -1;
if (bus_num == 0) {
// do not propagate lkas messages from ascm to actuators
uint32_t addr = to_fwd->RIR >> 21;
bus_fwd = 2;
// do not propagate lkas messages from ascm to actuators, unless supercruise is on
// block 0x152 and 0x154, which are the lkas command from ASCM1 and ASCM2
// block 0x315 and 0x2cb, which are the brake and accel commands from ASCM1
//if ((addr == 0x152) || (addr == 0x154) || (addr == 0x315) || (addr == 0x2cb)) {
if ((addr == 0x152) || (addr == 0x154)) {
int supercruise_on = (to_fwd->RDHR>>4) & 0x1; // bit 36
if (!supercruise_on) return -1;
if (!supercruise_on) {
bus_fwd = -1;
}
} else if ((addr == 0x151) || (addr == 0x153) || (addr == 0x314)) {
// on the chassis bus, the OBDII port is on the module side, so we need to read
// the lkas messages sent by openpilot (put on unused 0x151 ane 0x153 addrs) and send it to
// the actuator as 0x152 and 0x154
to_fwd->RIR = ((addr + 1) << 21) | (to_fwd->RIR & 0x1fffff);
}
// on the chassis bus, the OBDII port is on the module side, so we need to read
// the lkas messages sent by openpilot (put on unused 0x151 ane 0x153 addrs) and send it to
// the actuator as 0x152 and 0x154
if (addr == 0x151) {
to_fwd->RIR = (0x152 << 21) | (to_fwd->RIR & 0x1fffff);
}
if (addr == 0x153) {
to_fwd->RIR = (0x154 << 21) | (to_fwd->RIR & 0x1fffff);
}
// brake
if (addr == 0x314) {
to_fwd->RIR = (0x315 << 21) | (to_fwd->RIR & 0x1fffff);
}
return 2;
}
if (bus_num == 2) {
return 0;
bus_fwd = 0;
}
return -1;
return bus_fwd;
}
const safety_hooks gm_ascm_hooks = {

52
board/safety/safety_honda.h
View File

@ -87,6 +87,7 @@ static void honda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
static int honda_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
int addr = to_send->RIR >> 21;
int tx = 1;
// disallow actuator commands if gas or brake (with vehicle moving) are pressed
// and the the latching controls_allowed flag is True
@ -97,9 +98,13 @@ static int honda_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
// BRAKE: safety check
if (addr == 0x1FA) {
if (current_controls_allowed && long_controls_allowed) {
if ((to_send->RDLR & 0xFFFFFF3F) != to_send->RDLR) return 0;
if ((to_send->RDLR & 0xFFFFFF3F) != to_send->RDLR) {
tx = 0;
}
} else {
if ((to_send->RDLR & 0xFFFF0000) != to_send->RDLR) return 0;
if ((to_send->RDLR & 0xFFFF0000) != to_send->RDLR) {
tx = 0;
}
}
}
@ -108,7 +113,9 @@ static int honda_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
if (current_controls_allowed) {
// all messages are fine here
} else {
if ((to_send->RDLR & 0xFFFF0000) != to_send->RDLR) return 0;
if ((to_send->RDLR & 0xFFFF0000) != to_send->RDLR) {
tx = 0;
}
}
}
@ -117,7 +124,9 @@ static int honda_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
if (current_controls_allowed && long_controls_allowed) {
// all messages are fine here
} else {
if ((to_send->RDLR & 0xFFFF0000) != to_send->RDLR) return 0;
if ((to_send->RDLR & 0xFFFF0000) != to_send->RDLR) {
tx = 0;
}
}
}
@ -126,11 +135,13 @@ static int honda_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
// This avoids unintended engagements while still allowing resume spam
if ((addr == 0x296) && honda_bosch_hardware &&
!current_controls_allowed && ((to_send->RDTR >> 4) & 0xFF) == 0) {
if (((to_send->RDLR >> 5) & 0x7) != 2) return 0;
if (((to_send->RDLR >> 5) & 0x7) != 2) {
tx = 0;
}
}
// 1 allows the message through
return true;
return tx;
}
static void honda_init(int16_t param) {
@ -151,31 +162,36 @@ static int honda_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
// 0xE4 is steering on all cars except CRV and RDX, 0x194 for CRV and RDX,
// 0x1FA is brake control, 0x30C is acc hud, 0x33D is lkas hud,
// 0x39f is radar hud
int addr = to_fwd->RIR>>21;
int bus_fwd = -1;
if (bus_num == 0) {
return 2;
bus_fwd = 2;
} else if (bus_num == 2) {
// block stock lkas messages and stock acc messages (if OP is doing ACC)
int addr = to_fwd->RIR>>21;
int is_lkas_msg = (addr == 0xE4) || (addr == 0x194) || (addr == 0x33D);
int is_acc_msg = (addr == 0x1FA) || (addr == 0x30C) || (addr == 0x39F);
if (is_lkas_msg || (is_acc_msg && long_controls_allowed)) {
return -1;
int block_fwd = is_lkas_msg || (is_acc_msg && long_controls_allowed);
if (!block_fwd) {
bus_fwd = 0;
}
return 0;
}
return -1;
return bus_fwd;
}
static int honda_bosch_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
int bus_fwd = -1;
int addr = to_fwd->RIR >> 21;
int is_lkas_msg = (addr == 0xE4) || (addr == 0x33D);
if (bus_num == 2) {
return 1;
} else if ((bus_num == 1) && !is_lkas_msg) {
return 2;
bus_fwd = 1;
} else if (bus_num == 1) {
int addr = to_fwd->RIR >> 21;
int is_lkas_msg = (addr == 0xE4) || (addr == 0x33D);
if (!is_lkas_msg) {
bus_fwd = 2;
}
}
return -1;
return bus_fwd;
}
const safety_hooks honda_hooks = {

28
board/safety/safety_hyundai.h
View File

@ -65,9 +65,11 @@ static void hyundai_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
static int hyundai_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
int tx = 1;
// There can be only one! (camera)
if (hyundai_camera_detected) {
return 0;
tx = 0;
}
uint32_t addr;
@ -122,7 +124,7 @@ static int hyundai_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
}
if (violation) {
return false;
tx = 0;
}
}
@ -131,21 +133,29 @@ static int hyundai_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
// This avoids unintended engagements while still allowing resume spam
// TODO: fix bug preventing the button msg to be fwd'd on bus 2
//if (((to_send->RIR>>21) == 1265) && !controls_allowed && ((to_send->RDTR >> 4) & 0xFF) == 0) {
// if ((to_send->RDLR & 0x7) != 4) return 0;
// if ((to_send->RDLR & 0x7) != 4) {
// tx = 0;
// }
//}
// 1 allows the message through
return true;
return tx;
}
static int hyundai_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
int bus_fwd = -1;
// forward cam to ccan and viceversa, except lkas cmd
if (((bus_num == 0) || (bus_num == hyundai_camera_bus)) && hyundai_giraffe_switch_2) {
if (((to_fwd->RIR>>21) == 832) && (bus_num == hyundai_camera_bus)) return -1;
if (bus_num == 0) return hyundai_camera_bus;
if (bus_num == hyundai_camera_bus) return 0;
if (hyundai_giraffe_switch_2) {
if (bus_num == 0) {
bus_fwd = hyundai_camera_bus;
} else if (bus_num == hyundai_camera_bus) {
if ((to_fwd->RIR>>21) != 832) {
bus_fwd = 0;
}
}
}
return -1;
return bus_fwd;
}
static void hyundai_init(int16_t param) {

18
board/safety/safety_subaru.h
View File

@ -39,6 +39,7 @@ static void subaru_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
}
static int subaru_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
int tx = 1;
uint32_t addr = to_send->RIR >> 21;
// steer cmd checks
@ -86,31 +87,32 @@ static int subaru_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
}
if (violation) {
return false;
tx = 0;
}
}
return true;
return tx;
}
static int subaru_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
int32_t addr = to_fwd->RIR >> 21;
int bus_fwd = -1;
if (bus_num == 0) {
return 2; // Camera CAN
bus_fwd = 2; // Camera CAN
} else if (bus_num == 2) {
// 356 is LKAS for outback 2015
// 356 is LKAS for Global Platform
// 545 is ES_Distance
// 802 is ES_LKAS
if ((addr == 290) || (addr == 356) || (addr == 545) || (addr == 802)){
return -1;
int32_t addr = to_fwd->RIR >> 21;
int block_msg = (addr == 290) || (addr == 356) || (addr == 545) || (addr == 802);
if (!block_msg) {
bus_fwd = 0; // Main CAN
}
return 0; // Main CAN
}
// fallback to do not forward
return -1;
return bus_fwd;
}
const safety_hooks subaru_hooks = {

39
board/safety/safety_tesla.h
View File

@ -69,7 +69,6 @@ static void tesla_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
if (lever_position == 2) { // pull forward
// activate openpilot
controls_allowed = 1;
//}
} else if (lever_position == 1) { // push towards the back
// deactivate openpilot
controls_allowed = 0;
@ -150,6 +149,7 @@ static void tesla_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
static int tesla_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
int tx = 1;
uint32_t addr;
float angle_raw;
float desired_angle;
@ -167,10 +167,7 @@ static int tesla_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
if (st_enabled == 0) {
//steering is not enabled, do not check angles and do send
tesla_desired_angle_last = desired_angle;
return true;
}
if (controls_allowed) {
} else if (controls_allowed) {
// add 1 to not false trigger the violation
float delta_angle_up = interpolate(TESLA_LOOKUP_ANGLE_RATE_UP, tesla_speed) + 1.;
float delta_angle_down = interpolate(TESLA_LOOKUP_ANGLE_RATE_DOWN, tesla_speed) + 1.;
@ -186,14 +183,14 @@ static int tesla_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
if (violation) {
controls_allowed = 0;
return false;
tx = 0;
}
tesla_desired_angle_last = desired_angle;
return true;
} else {
tx = 0;
}
return false;
}
return true;
return tx;
}
static void tesla_init(int16_t param) {
@ -208,36 +205,30 @@ static int tesla_ign_hook() {
static int tesla_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
int bus_fwd = -1;
int32_t addr = to_fwd->RIR >> 21;
if (bus_num == 0) {
// change inhibit of GTW_epasControl
if (addr != 0x214) {
// remove EPB_epasControl
bus_fwd = 2; // Custom EPAS bus
}
if (addr == 0x101) {
to_fwd->RDLR = to_fwd->RDLR | 0x4000; // 0x4000: WITH_ANGLE, 0xC000: WITH_BOTH (angle and torque)
int checksum = (((to_fwd->RDLR & 0xFF00) >> 8) + (to_fwd->RDLR & 0xFF) + 2) & 0xFF;
to_fwd->RDLR = to_fwd->RDLR & 0xFFFF;
to_fwd->RDLR = to_fwd->RDLR + (checksum << 16);
return 2;
}
// remove EPB_epasControl
if (addr == 0x214) {
return -1;
}
return 2; // Custom EPAS bus
}
if (bus_num == 2) {
// remove GTW_epasControl in forwards
if (addr == 0x101) {
return -1;
if (addr != 0x101) {
bus_fwd = 0; // Chassis CAN
}
return 0; // Chassis CAN
}
return -1;
return bus_fwd;
}
const safety_hooks tesla_hooks = {

35
board/safety/safety_toyota.h
View File

@ -99,20 +99,26 @@ static void toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
static int toyota_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
int tx = 1;
int addr = to_send->RIR >> 21;
int bus = (to_send->RDTR >> 4) & 0xF;
// Check if msg is sent on BUS 0
if (((to_send->RDTR >> 4) & 0xF) == 0) {
if (bus == 0) {
// no IPAS in non IPAS mode
if ((addr == 0x266) || (addr == 0x167)) return false;
if ((addr == 0x266) || (addr == 0x167)) {
tx = 0;
}
// GAS PEDAL: safety check
if (addr == 0x200) {
if (controls_allowed && long_controls_allowed) {
// all messages are fine here
} else {
if ((to_send->RDLR & 0xFFFF0000) != to_send->RDLR) return 0;
if ((to_send->RDLR & 0xFFFF0000) != to_send->RDLR) {
tx = 0;
}
}
}
@ -122,9 +128,11 @@ static int toyota_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
desired_accel = to_signed(desired_accel, 16);
if (controls_allowed && long_controls_allowed) {
int violation = max_limit_check(desired_accel, TOYOTA_MAX_ACCEL, TOYOTA_MIN_ACCEL);
if (violation) return 0;
if (violation) {
tx = 0;
}
} else if (desired_accel != 0) {
return 0;
tx = 0;
}
}
@ -172,13 +180,13 @@ static int toyota_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
}
if (violation) {
return false;
tx = 0;
}
}
}
// 1 allows the message through
return true;
return tx;
}
static void toyota_init(int16_t param) {
@ -190,22 +198,23 @@ static void toyota_init(int16_t param) {
static int toyota_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
int bus_fwd = -1;
if (toyota_camera_forwarded && !toyota_giraffe_switch_1) {
int addr = to_fwd->RIR>>21;
if (bus_num == 0) {
return 2;
bus_fwd = 2;
} else if (bus_num == 2) {
int addr = to_fwd->RIR>>21;
// block stock lkas messages and stock acc messages (if OP is doing ACC)
int is_lkas_msg = ((addr == 0x2E4) || (addr == 0x412));
// in TSSP 2.0 the camera does ACC as well, so filter 0x343
int is_acc_msg = (addr == 0x343);
if (is_lkas_msg || (is_acc_msg && long_controls_allowed)) {
return -1;
int block_msg = is_lkas_msg || (is_acc_msg && long_controls_allowed);
if (!block_msg) {
bus_fwd = 0;
}
return 0;
}
}
return -1;
return bus_fwd;
}
const safety_hooks toyota_hooks = {

29
board/safety/safety_toyota_ipas.h
View File

@ -35,7 +35,9 @@ static void toyota_ipas_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
// check standard toyota stuff as well
toyota_rx_hook(to_push);
if ((to_push->RIR>>21) == 0x260) {
int addr = to_push->RIR >> 21;
if (addr == 0x260) {
// get driver steering torque
int16_t torque_driver_new = (((to_push->RDLR) & 0xFF00) | ((to_push->RDLR >> 16) & 0xFF));
@ -44,7 +46,7 @@ static void toyota_ipas_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
}
// get steer angle
if ((to_push->RIR>>21) == 0x25) {
if (addr == 0x25) {
int angle_meas_new = ((to_push->RDLR & 0xf) << 8) + ((to_push->RDLR & 0xff00) >> 8);
uint32_t ts = TIM2->CNT;
@ -78,12 +80,12 @@ static void toyota_ipas_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
}
// get speed
if ((to_push->RIR>>21) == 0xb4) {
if (addr == 0xb4) {
speed = ((float) (((to_push->RDHR) & 0xFF00) | ((to_push->RDHR >> 16) & 0xFF))) * 0.01 / 3.6;
}
// get ipas state
if ((to_push->RIR>>21) == 0x262) {
if (addr == 0x262) {
ipas_state = (to_push->RDLR & 0xf);
}
@ -97,11 +99,15 @@ static void toyota_ipas_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
static int toyota_ipas_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
int tx = 1;
int bypass_standard_tx_hook = 0;
int addr = to_send->RIR >> 21;
// Check if msg is sent on BUS 0
if (((to_send->RDTR >> 4) & 0xF) == 0) {
// STEER ANGLE
if (((to_send->RIR>>21) == 0x266) || ((to_send->RIR>>21) == 0x167)) {
if ((addr == 0x266) || (addr == 0x167)) {
angle_control = 1; // we are in angle control mode
int desired_angle = ((to_send->RDLR & 0xf) << 8) + ((to_send->RDLR & 0xff00) >> 8);
@ -138,15 +144,18 @@ static int toyota_ipas_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
desired_angle_last = desired_angle;
if (violation) {
return false;
tx = 0;
}
return true;
bypass_standard_tx_hook = 1;
}
}
// check standard toyota stuff as well
return toyota_tx_hook(to_send);
// check standard toyota stuff as well if addr isn't IPAS related
if (!bypass_standard_tx_hook) {
tx &= toyota_tx_hook(to_send);
}
return tx;
}
const safety_hooks toyota_ipas_hooks = {

1
tests/safety/test.c
View File

@ -44,6 +44,7 @@ TIM_TypeDef *TIM2 = &timer;
#define PANDA
#define NULL ((void*)0)
#define static
#include "safety.h"