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

Make macros to get msg attributes (#218) 

* made GET_ADDR, GET_BUS and GET_LEN macros

* for simplicity, addr is int instead of uint32_t as it can only use up to 29 bits
master
rbiasini 2019-06-14 19:13:05 -07:00 committed by GitHub
parent 19d1e41eed
commit bacfcd0fd0
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
13 changed files with 105 additions and 158 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
board/safety/safety_cadillac.h
View File

@ -19,15 +19,15 @@ uint32_t cadillac_ts_last = 0;
int cadillac_supercruise_on = 0;
struct sample_t cadillac_torque_driver; // last few driver torques measured
int cadillac_get_torque_idx(uint32_t addr, int array_size) {
return min(max(addr - 0x151U, 0), array_size); // 0x151 is id 0, 0x152 is id 1 and so on...
int cadillac_get_torque_idx(int addr, int array_size) {
return min(max(addr - 0x151, 0), array_size); // 0x151 is id 0, 0x152 is id 1 and so on...
}
static void cadillac_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
int bus_number = (to_push->RDTR >> 4) & 0xFF;
uint32_t addr = to_push->RIR >> 21;
int bus = GET_BUS(to_push);
int addr = GET_ADDR(to_push);
if (addr == 356U) {
if (addr == 356) {
int torque_driver_new = ((to_push->RDLR & 0x7) << 8) | ((to_push->RDLR >> 8) & 0xFF);
torque_driver_new = to_signed(torque_driver_new, 11);
// update array of samples
@ -35,12 +35,12 @@ static void cadillac_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
}
// this message isn't all zeros when ignition is on
if ((addr == 0x160U) && (bus_number == 0)) {
if ((addr == 0x160) && (bus == 0)) {
cadillac_ign = to_push->RDLR > 0;
}
// enter controls on rising edge of ACC, exit controls on ACC off
if ((addr == 0x370U) && (bus_number == 0)) {
if ((addr == 0x370) && (bus == 0)) {
int cruise_engaged = to_push->RDLR & 0x800000; // bit 23
if (cruise_engaged && !cadillac_cruise_engaged_last) {
controls_allowed = 1;
@ -51,17 +51,17 @@ static void cadillac_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
}
// know supercruise mode and block openpilot msgs if on
if ((addr == 0x152U) || (addr == 0x154U)) {
if ((addr == 0x152) || (addr == 0x154)) {
cadillac_supercruise_on = (to_push->RDHR>>4) & 0x1;
}
}
static int cadillac_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
int tx = 1;
uint32_t addr = to_send->RIR >> 21;
int addr = GET_ADDR(to_send);
// steer cmd checks
if ((addr == 0x151U) || (addr == 0x152U) || (addr == 0x153U) || (addr == 0x154U)) {
if ((addr == 0x151) || (addr == 0x152) || (addr == 0x153) || (addr == 0x154)) {
int desired_torque = ((to_send->RDLR & 0x3f) << 8) + ((to_send->RDLR & 0xff00) >> 8);
int violation = 0;
uint32_t ts = TIM2->CNT;

33
board/safety/safety_chrysler.h
View File

@ -13,20 +13,11 @@ uint32_t chrysler_ts_last = 0;
struct sample_t chrysler_torque_meas; // last few torques measured
static void chrysler_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
int bus = (to_push->RDTR >> 4) & 0xFF;
uint32_t addr;
if ((to_push->RIR & 4) != 0) {
// Extended
// Not looked at, but have to be separated
// to avoid address collision
addr = to_push->RIR >> 3;
} else {
// Normal
addr = to_push->RIR >> 21;
}
int bus = GET_BUS(to_push);
int addr = GET_ADDR(to_push);
// Measured eps torque
if (addr == 544U) {
if (addr == 544) {
uint32_t rdhr = to_push->RDHR;
int torque_meas_new = ((rdhr & 0x7U) << 8) + ((rdhr & 0xFF00U) >> 8) - 1024U;
@ -35,7 +26,7 @@ static void chrysler_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
}
// enter controls on rising edge of ACC, exit controls on ACC off
if (addr == 0x1F4U) {
if (addr == 0x1F4) {
int cruise_engaged = ((to_push->RDLR & 0x380000) >> 19) == 7;
if (cruise_engaged && !chrysler_cruise_engaged_last) {
controls_allowed = 1;
@ -46,7 +37,7 @@ static void chrysler_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
}
// check if stock camera ECU is still online
if ((bus == 0) && (addr == 0x292U)) {
if ((bus == 0) && (addr == 0x292)) {
chrysler_camera_detected = 1;
controls_allowed = 0;
}
@ -61,18 +52,10 @@ static int chrysler_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
tx = 0;
}
uint32_t addr;
if ((to_send->RIR & 4) != 0) {
// Extended
addr = to_send->RIR >> 3;
} else {
// Normal
addr = to_send->RIR >> 21;
}
int addr = GET_ADDR(to_send);
// LKA STEER
if (addr == 0x292U) {
if (addr == 0x292) {
uint32_t rdlr = to_send->RDLR;
int desired_torque = ((rdlr & 0x7U) << 8) + ((rdlr & 0xFF00U) >> 8) - 1024U;
uint32_t ts = TIM2->CNT;
@ -135,7 +118,7 @@ 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;
int addr = GET_ADDR(to_fwd);
// forward CAN 0 -> 2 so stock LKAS camera sees messages
if ((bus_num == 0) && !chrysler_camera_detected) {
bus_fwd = 2;

24
board/safety/safety_elm327.h
View File

@ -1,29 +1,25 @@
static int elm327_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
int tx = 1;
int bus = GET_BUS(to_send);
int addr = GET_ADDR(to_send);
int len = GET_LEN(to_send);
//All ELM traffic must appear on CAN0
if (((to_send->RDTR >> 4) & 0xf) != 0) {
if (bus != 0) {
tx = 0;
}
//All ISO 15765-4 messages must be 8 bytes long
if ((to_send->RDTR & 0xf) != 8) {
if (len != 8) {
tx = 0;
}
if ((to_send->RIR & 4) != 0) {
uint32_t addr = to_send->RIR >> 3;
//Check valid 29 bit send addresses for ISO 15765-4
if (!((addr == 0x18DB33F1U) || ((addr & 0x1FFF00FFU) == 0x18DA00F1U))) {
tx = 0;
}
} else {
uint32_t addr = to_send->RIR >> 21;
//Check valid 11 bit send addresses for ISO 15765-4
if (!((addr == 0x7DFU) || ((addr & 0x7F8U) == 0x7E0U))) {
tx = 0;
}
//Check valid 29 bit send addresses for ISO 15765-4
//Check valid 11 bit send addresses for ISO 15765-4
if ((addr != 0x18DB33F1) && ((addr & 0x1FFF00FF) != 0x18DA00F1) &&
((addr != 0x7DF) && ((addr & 0x7F8) != 0x7E0))) {
tx = 0;
}
return tx;
}

12
board/safety/safety_ford.h
View File

@ -13,14 +13,16 @@ int ford_is_moving = 0;
static void ford_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
if ((to_push->RIR>>21) == 0x217) {
int addr = GET_ADDR(to_push);
if (addr == 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) {
if (addr == 0x83) {
bool cancel = (to_push->RDLR >> 8) & 0x1;
bool set_or_resume = (to_push->RDLR >> 28) & 0x3;
if (cancel) {
@ -32,7 +34,7 @@ static void ford_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) == 0x165) {
if (addr == 0x165) {
int brake = to_push->RDLR & 0x20;
if (brake && (!(ford_brake_prev) || ford_is_moving)) {
controls_allowed = 0;
@ -41,7 +43,7 @@ static void ford_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
}
// exit controls on rising edge of gas press
if ((to_push->RIR>>21) == 0x204) {
if (addr == 0x204) {
int gas = to_push->RDLR & 0xFF03;
if (gas && !(ford_gas_prev)) {
controls_allowed = 0;
@ -63,7 +65,7 @@ static int ford_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
// and the the latching controls_allowed flag is True
int pedal_pressed = ford_gas_prev || (ford_brake_prev && ford_is_moving);
bool current_controls_allowed = controls_allowed && !(pedal_pressed);
int addr = to_send->RIR >> 21;
int addr = GET_ADDR(to_send);
// STEER: safety check
if (addr == 0x3CA) {

46
board/safety/safety_gm.h
View File

@ -31,26 +31,17 @@ uint32_t gm_ts_last = 0;
struct sample_t gm_torque_driver; // last few driver torques measured
static void gm_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
int bus_number = (to_push->RDTR >> 4) & 0xFF;
uint32_t addr;
if ((to_push->RIR & 4) != 0) {
// Extended
// Not looked at, but have to be separated
// to avoid address collision
addr = to_push->RIR >> 3;
} else {
// Normal
addr = to_push->RIR >> 21;
}
int bus_number = GET_BUS(to_push);
int addr = GET_ADDR(to_push);
if (addr == 388U) {
if (addr == 388) {
int torque_driver_new = (((to_push->RDHR >> 16) & 0x7) << 8) | ((to_push->RDHR >> 24) & 0xFF);
torque_driver_new = to_signed(torque_driver_new, 11);
// update array of samples
update_sample(&gm_torque_driver, torque_driver_new);
}
if ((addr == 0x1F1U) && (bus_number == 0)) {
if ((addr == 0x1F1) && (bus_number == 0)) {
//Bit 5 should be ignition "on"
//Backup plan is Bit 2 (accessory power)
bool ign = ((to_push->RDLR) & 0x20) != 0;
@ -59,7 +50,7 @@ static void gm_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
// sample speed, really only care if car is moving or not
// rear left wheel speed
if (addr == 842U) {
if (addr == 842) {
gm_speed = to_push->RDLR & 0xFFFF;
}
@ -67,13 +58,13 @@ static void gm_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
// on powertrain bus.
// 384 = ASCMLKASteeringCmd
// 715 = ASCMGasRegenCmd
if ((bus_number == 0) && ((addr == 384U) || (addr == 715U))) {
if ((bus_number == 0) && ((addr == 384) || (addr == 715))) {
gm_ascm_detected = 1;
controls_allowed = 0;
}
// ACC steering wheel buttons
if (addr == 481U) {
if (addr == 481) {
int buttons = (to_push->RDHR >> 12) & 0x7;
// res/set - enable, cancel button - disable
if ((buttons == 2) || (buttons == 3)) {
@ -85,7 +76,7 @@ static void gm_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
// exit controls on rising edge of brake press or on brake press when
// speed > 0
if (addr == 241U) {
if (addr == 241) {
int brake = (to_push->RDLR & 0xFF00) >> 8;
// Brake pedal's potentiometer returns near-zero reading
// even when pedal is not pressed
@ -99,7 +90,7 @@ static void gm_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
}
// exit controls on rising edge of gas press
if (addr == 417U) {
if (addr == 417) {
int gas = to_push->RDHR & 0xFF0000;
if (gas && !gm_gas_prev && long_controls_allowed) {
controls_allowed = 0;
@ -108,7 +99,7 @@ static void gm_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
}
// exit controls on regen paddle
if (addr == 189U) {
if (addr == 189) {
bool regen = to_push->RDLR & 0x20;
if (regen) {
controls_allowed = 0;
@ -136,17 +127,10 @@ static int gm_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
int pedal_pressed = gm_gas_prev || (gm_brake_prev && gm_speed);
bool current_controls_allowed = controls_allowed && !pedal_pressed;
uint32_t addr;
if ((to_send->RIR & 4) != 0) {
// Extended
addr = to_send->RIR >> 3;
} else {
// Normal
addr = to_send->RIR >> 21;
}
int addr = GET_ADDR(to_send);
// BRAKE: safety check
if (addr == 789U) {
if (addr == 789) {
uint32_t rdlr = to_send->RDLR;
int brake = ((rdlr & 0xFU) << 8) + ((rdlr & 0xFF00U) >> 8);
brake = (0x1000 - brake) & 0xFFF;
@ -162,7 +146,7 @@ static int gm_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
}
// LKA STEER: safety check
if (addr == 384U) {
if (addr == 384) {
uint32_t rdlr = to_send->RDLR;
int desired_torque = ((rdlr & 0x7U) << 8) + ((rdlr & 0xFF00U) >> 8);
uint32_t ts = TIM2->CNT;
@ -211,12 +195,12 @@ static int gm_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
}
// PARK ASSIST STEER: unlimited torque, no thanks
if (addr == 823U) {
if (addr == 823) {
tx = 0;
}
// GAS/REGEN: safety check
if (addr == 715U) {
if (addr == 715) {
uint32_t rdlr = to_send->RDLR;
int gas_regen = ((rdlr & 0x7F0000U) >> 11) + ((rdlr & 0xF8000000U) >> 27);
bool apply = (rdlr & 1U) != 0U;

11
board/safety/safety_gm_ascm.h
View File

@ -6,22 +6,23 @@ static int gm_ascm_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
int bus_fwd = -1;
if (bus_num == 0) {
uint32_t addr = to_fwd->RIR >> 21;
int addr = GET_ADDR(to_fwd);
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 == 0x152U) || (addr == 0x154U)) {
int supercruise_on = (to_fwd->RDHR>>4) & 0x1; // bit 36
if ((addr == 0x152) || (addr == 0x154)) {
int supercruise_on = (to_fwd->RDHR >> 4) & 0x1; // bit 36
if (!supercruise_on) {
bus_fwd = -1;
}
} else if ((addr == 0x151U) || (addr == 0x153U) || (addr == 0x314U)) {
} 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 + 1U) << 21) | (to_fwd->RIR & 0x1fffff);
uint32_t fwd_addr = addr + 1;
to_fwd->RIR = (fwd_addr << 21) | (to_fwd->RIR & 0x1fffff);
}
}

14
board/safety/safety_honda.h
View File

@ -16,7 +16,8 @@ bool honda_alt_brake_msg = false;
static void honda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
int addr = to_push->RIR >> 21;
int addr = GET_ADDR(to_push);
int len = GET_LEN(to_push);
// sample speed
if (addr == 0x158) {
@ -56,7 +57,7 @@ static void honda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
// exit controls on rising edge of gas press if interceptor (0x201 w/ len = 6)
// length check because bosch hardware also uses this id (0x201 w/ len = 8)
if ((addr == 0x201) && ((to_push->RDTR & 0xf) == 6)) {
if ((addr == 0x201) && (len == 6)) {
gas_interceptor_detected = 1;
int gas_interceptor = ((to_push->RDLR & 0xFF) << 8) | ((to_push->RDLR & 0xFF00) >> 8);
if ((gas_interceptor > HONDA_GAS_INTERCEPTOR_THRESHOLD) &&
@ -87,8 +88,9 @@ 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;
int addr = GET_ADDR(to_send);
int bus = GET_BUS(to_send);
// disallow actuator commands if gas or brake (with vehicle moving) are pressed
// and the the latching controls_allowed flag is True
@ -135,7 +137,7 @@ static int honda_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
// ensuring that only the cancel button press is sent (VAL 2) when controls are off.
// This avoids unintended engagements while still allowing resume spam
if ((addr == 0x296) && honda_bosch_hardware &&
!current_controls_allowed && ((to_send->RDTR >> 4) & 0xFF) == 0) {
!current_controls_allowed && (bus == 0)) {
if (((to_send->RDLR >> 5) & 0x7) != 2) {
tx = 0;
}
@ -169,7 +171,7 @@ static int honda_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
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 addr = GET_ADDR(to_fwd);
int is_lkas_msg = (addr == 0xE4) || (addr == 0x194) || (addr == 0x33D);
int is_acc_msg = (addr == 0x1FA) || (addr == 0x30C) || (addr == 0x39F);
int block_fwd = is_lkas_msg || (is_acc_msg && long_controls_allowed);
@ -186,7 +188,7 @@ static int honda_bosch_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
if (bus_num == 2) {
bus_fwd = 1;
} else if (bus_num == 1) {
int addr = to_fwd->RIR >> 21;
int addr = GET_ADDR(to_fwd);
int is_lkas_msg = (addr == 0xE4) || (addr == 0x33D);
if (!is_lkas_msg) {
bus_fwd = 2;

40
board/safety/safety_hyundai.h
View File

@ -16,37 +16,28 @@ uint32_t hyundai_ts_last = 0;
struct sample_t hyundai_torque_driver; // last few driver torques measured
static void hyundai_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
int bus = (to_push->RDTR >> 4) & 0xFF;
uint32_t addr;
if ((to_push->RIR & 4) != 0) {
// Extended
// Not looked at, but have to be separated
// to avoid address collision
addr = to_push->RIR >> 3;
} else {
// Normal
addr = to_push->RIR >> 21;
}
int bus = GET_BUS(to_push);
int addr = GET_ADDR(to_push);
if (addr == 897U) {
if (addr == 897) {
int torque_driver_new = ((to_push->RDLR >> 11) & 0xfff) - 2048;
// update array of samples
update_sample(&hyundai_torque_driver, torque_driver_new);
}
// check if stock camera ECU is still online
if ((bus == 0) && (addr == 832U)) {
if ((bus == 0) && (addr == 832)) {
hyundai_camera_detected = 1;
controls_allowed = 0;
}
// Find out which bus the camera is on
if (addr == 832U) {
if (addr == 832) {
hyundai_camera_bus = bus;
}
// enter controls on rising edge of ACC, exit controls on ACC off
if (addr == 1057U) {
if (addr == 1057) {
// 2 bits: 13-14
int cruise_engaged = (to_push->RDLR >> 13) & 0x3;
if (cruise_engaged && !hyundai_cruise_engaged_last) {
@ -58,7 +49,7 @@ static void hyundai_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
}
// 832 is lkas cmd. If it is on camera bus, then giraffe switch 2 is high
if ((addr == 832U) && (bus == hyundai_camera_bus) && (hyundai_camera_bus != 0)) {
if ((addr == 832) && (bus == hyundai_camera_bus) && (hyundai_camera_bus != 0)) {
hyundai_giraffe_switch_2 = 1;
}
}
@ -66,23 +57,15 @@ static void hyundai_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
static int hyundai_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
int tx = 1;
int addr = GET_ADDR(to_send);
// There can be only one! (camera)
if (hyundai_camera_detected) {
tx = 0;
}
uint32_t addr;
if ((to_send->RIR & 4) != 0) {
// Extended
addr = to_send->RIR >> 3;
} else {
// Normal
addr = to_send->RIR >> 21;
}
// LKA STEER: safety check
if (addr == 832U) {
if (addr == 832) {
int desired_torque = ((to_send->RDLR >> 16) & 0x7ff) - 1024;
uint32_t ts = TIM2->CNT;
bool violation = 0;
@ -132,7 +115,7 @@ static int hyundai_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
// ensuring that only the cancel button press is sent (VAL 4) when controls are off.
// 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 ((addr == 1265) && !controls_allowed && (bus == 0) {
// if ((to_send->RDLR & 0x7) != 4) {
// tx = 0;
// }
@ -150,7 +133,8 @@ static int hyundai_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
if (bus_num == 0) {
bus_fwd = hyundai_camera_bus;
} else if (bus_num == hyundai_camera_bus) {
if ((to_fwd->RIR>>21) != 832) {
int addr = GET_ADDR(to_fwd);
if (addr != 832) {
bus_fwd = 0;
}
}

14
board/safety/safety_subaru.h
View File

@ -16,10 +16,10 @@ struct sample_t subaru_torque_driver; // last few driver torques measure
static void subaru_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
int bus_number = (to_push->RDTR >> 4) & 0xFF;
uint32_t addr = to_push->RIR >> 21;
int bus = GET_BUS(to_push);
int addr = GET_ADDR(to_push);
if ((addr == 0x119U) && (bus_number == 0)){
if ((addr == 0x119) && (bus == 0)){
int torque_driver_new = ((to_push->RDLR >> 16) & 0x7FF);
torque_driver_new = to_signed(torque_driver_new, 11);
// update array of samples
@ -27,7 +27,7 @@ static void subaru_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
}
// enter controls on rising edge of ACC, exit controls on ACC off
if ((addr == 0x240U) && (bus_number == 0)) {
if ((addr == 0x240) && (bus == 0)) {
int cruise_engaged = (to_push->RDHR >> 9) & 1;
if (cruise_engaged && !subaru_cruise_engaged_last) {
controls_allowed = 1;
@ -40,10 +40,10 @@ 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;
int addr = GET_ADDR(to_send);
// steer cmd checks
if (addr == 0x122U) {
if (addr == 0x122) {
int desired_torque = ((to_send->RDLR >> 16) & 0x1FFF);
bool violation = 0;
uint32_t ts = TIM2->CNT;
@ -104,7 +104,7 @@ static int subaru_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
// 356 is LKAS for Global Platform
// 545 is ES_Distance
// 802 is ES_LKAS
int32_t addr = to_fwd->RIR >> 21;
int addr = GET_ADDR(to_fwd);
int block_msg = (addr == 290) || (addr == 356) || (addr == 545) || (addr == 802);
if (!block_msg) {
bus_fwd = 0; // Main CAN

28
board/safety/safety_tesla.h
View File

@ -51,19 +51,9 @@ static void tesla_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
set_gmlan_digital_output(0); // #define GMLAN_HIGH 0
reset_gmlan_switch_timeout(); //we're still in tesla safety mode, reset the timeout counter and make sure our output is enabled
//int bus_number = (to_push->RDTR >> 4) & 0xFF;
uint32_t addr;
if ((to_push->RIR & 4) != 0) {
// Extended
// Not looked at, but have to be separated
// to avoid address collision
addr = to_push->RIR >> 3;
} else {
// Normal
addr = to_push->RIR >> 21;
}
int addr = GET_ADDR(to_push);
if (addr == 0x45U) {
if (addr == 0x45) {
// 6 bits starting at position 0
int lever_position = (to_push->RDLR & 0x3F);
if (lever_position == 2) { // pull forward
@ -76,7 +66,7 @@ static void tesla_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
}
// Detect drive rail on (ignition) (start recording)
if (addr == 0x348U) {
if (addr == 0x348) {
// GTW_status
int drive_rail_on = (to_push->RDLR & 0x0001);
tesla_ignition_started = drive_rail_on == 1;
@ -84,7 +74,7 @@ static void tesla_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
// exit controls on brake press
// DI_torque2::DI_brakePedal 0x118
if (addr == 0x118U) {
if (addr == 0x118) {
// 1 bit at position 16
if ((((to_push->RDLR & 0x8000)) >> 15) == 1) {
// disable break cancel by commenting line below
@ -99,7 +89,7 @@ static void tesla_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
// exit controls on EPAS error
// EPAS_sysStatus::EPAS_eacStatus 0x370
if (addr == 0x370U) {
if (addr == 0x370) {
// if EPAS_eacStatus is not 1 or 2, disable control
eac_status = ((to_push->RDHR >> 21)) & 0x7;
// For human steering override we must not disable controls when eac_status == 0
@ -110,7 +100,7 @@ static void tesla_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
}
}
//get latest steering wheel angle
if (addr == 0x00EU) {
if (addr == 0x00E) {
float angle_meas_now = (int)(((((to_push->RDLR & 0x3F) << 8) + ((to_push->RDLR >> 8) & 0xFF)) * 0.1) - 819.2);
uint32_t ts = TIM2->CNT;
uint32_t ts_elapsed = get_ts_elapsed(ts, tesla_ts_angle_last);
@ -150,11 +140,11 @@ 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 = to_send->RIR >> 21;
int addr = GET_ADDR(to_send);
// do not transmit CAN message if steering angle too high
// DAS_steeringControl::DAS_steeringAngleRequest
if (addr == 0x488U) {
if (addr == 0x488) {
float angle_raw = ((to_send->RDLR & 0x7F) << 8) + ((to_send->RDLR & 0xFF00) >> 8);
float desired_angle = (angle_raw * 0.1) - 1638.35;
bool violation = 0;
@ -202,7 +192,7 @@ 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;
int addr = GET_ADDR(to_fwd);
if (bus_num == 0) {
// change inhibit of GTW_epasControl

10
board/safety/safety_toyota.h
View File

@ -34,7 +34,8 @@ struct sample_t toyota_torque_meas; // last 3 motor torques produced by th
static void toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
int addr = to_push->RIR >> 21;
int bus = GET_BUS(to_push);
int addr = GET_ADDR(to_push);
// get eps motor torque (0.66 factor in dbc)
if (addr == 0x260) {
@ -85,7 +86,6 @@ static void toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
toyota_gas_prev = gas;
}
int bus = (to_push->RDTR >> 4) & 0xF;
// msgs are only on bus 2 if panda is connected to frc
if (bus == 2) {
toyota_camera_forwarded = 1;
@ -100,8 +100,8 @@ 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;
int addr = GET_ADDR(to_send);
int bus = GET_BUS(to_send);
// Check if msg is sent on BUS 0
if (bus == 0) {
@ -203,7 +203,7 @@ static int toyota_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
if (bus_num == 0) {
bus_fwd = 2;
} else if (bus_num == 2) {
int addr = to_fwd->RIR>>21;
int addr = GET_ADDR(to_fwd);
// 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

7
board/safety/safety_toyota_ipas.h
View File

@ -35,7 +35,7 @@ static void toyota_ipas_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
// check standard toyota stuff as well
toyota_rx_hook(to_push);
int addr = to_push->RIR >> 21;
int addr = GET_ADDR(to_push);
if (addr == 0x260) {
// get driver steering torque
@ -101,10 +101,11 @@ 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;
int bus = GET_BUS(to_send);
int addr = GET_ADDR(to_send);
// Check if msg is sent on BUS 0
if (((to_send->RDTR >> 4) & 0xF) == 0) {
if (bus == 0) {
// STEER ANGLE
if ((addr == 0x266) || (addr == 0x167)) {

4
board/safety_declarations.h
View File

@ -1,3 +1,7 @@
#define GET_BUS(msg) (((msg)->RDTR >> 4) & 0xFF)
#define GET_LEN(msg) ((msg)->RDTR & 0xf)
#define GET_ADDR(msg) ((((msg)->RIR & 4) != 0) ? ((msg)->RIR >> 3) : ((msg)->RIR >> 21))
// sample struct that keeps 3 samples in memory
struct sample_t {
int values[6];