Skip to content

Commit

Permalink
Fix msg checks for non Honda and Toyota (commaai#426)
Browse files Browse the repository at this point in the history
  • Loading branch information
@pd0wm
pd0wm authored Jan 18, 2020
1 parent 3b35621 commit f67ec28
Show file tree
Hide file tree
Showing 6 changed files with 204 additions and 174 deletions.
24 changes: 12 additions & 12 deletions board/safety.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -148,20 +148,20 @@ bool addr_safety_check(CAN_FIFOMailBox_TypeDef *to_push,

if (index != -1) {
// checksum check
if ((get_checksum != NULL) && (compute_checksum != NULL)) {
if (rx_checks[index].check_checksum) {
uint8_t checksum = get_checksum(to_push);
uint8_t checksum_comp = compute_checksum(to_push);
rx_checks[index].valid_checksum = checksum_comp == checksum;
}
if ((get_checksum != NULL) && (compute_checksum != NULL) && rx_checks[index].check_checksum) {
uint8_t checksum = get_checksum(to_push);
uint8_t checksum_comp = compute_checksum(to_push);
rx_checks[index].valid_checksum = checksum_comp == checksum;
} else {
rx_checks[index].valid_checksum = true;
}

// counter check
if (get_counter != NULL) {
if (rx_checks[index].max_counter > 0U) {
uint8_t counter = get_counter(to_push);
update_counter(rx_checks, index, counter);
}
// counter check (max_counter == 0 means skip check)
if ((get_counter != NULL) && (rx_checks[index].max_counter > 0U)) {
uint8_t counter = get_counter(to_push);
update_counter(rx_checks, index, counter);
} else {
rx_checks[index].wrong_counters = 0U;
}
}
return is_msg_valid(rx_checks, index);
Expand Down
50 changes: 28 additions & 22 deletions board/safety/safety_chrysler.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,36 +20,42 @@ uint32_t chrysler_ts_last = 0;
struct sample_t chrysler_torque_meas; // last few torques measured

static int chrysler_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
int bus = GET_BUS(to_push);
int addr = GET_ADDR(to_push);

// Measured eps torque
if (addr == 544) {
int torque_meas_new = ((GET_BYTE(to_push, 4) & 0x7U) << 8) + GET_BYTE(to_push, 5) - 1024U;
bool valid = addr_safety_check(to_push, chrysler_rx_checks, CHRYSLER_RX_CHECK_LEN,
NULL, NULL, NULL);

// update array of samples
update_sample(&chrysler_torque_meas, torque_meas_new);
}
if (valid) {
int bus = GET_BUS(to_push);
int addr = GET_ADDR(to_push);

// Measured eps torque
if (addr == 544) {
int torque_meas_new = ((GET_BYTE(to_push, 4) & 0x7U) << 8) + GET_BYTE(to_push, 5) - 1024U;

// enter controls on rising edge of ACC, exit controls on ACC off
if (addr == 0x1F4) {
int cruise_engaged = ((GET_BYTE(to_push, 2) & 0x38) >> 3) == 7;
if (cruise_engaged && !chrysler_cruise_engaged_last) {
controls_allowed = 1;
// update array of samples
update_sample(&chrysler_torque_meas, torque_meas_new);
}
if (!cruise_engaged) {
controls_allowed = 0;

// enter controls on rising edge of ACC, exit controls on ACC off
if (addr == 0x1F4) {
int cruise_engaged = ((GET_BYTE(to_push, 2) & 0x38) >> 3) == 7;
if (cruise_engaged && !chrysler_cruise_engaged_last) {
controls_allowed = 1;
}
if (!cruise_engaged) {
controls_allowed = 0;
}
chrysler_cruise_engaged_last = cruise_engaged;
}
chrysler_cruise_engaged_last = cruise_engaged;
}

// TODO: add gas pressed check
// TODO: add gas pressed check

// check if stock camera ECU is on bus 0
if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (bus == 0) && (addr == 0x292)) {
relay_malfunction = true;
// check if stock camera ECU is on bus 0
if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (bus == 0) && (addr == 0x292)) {
relay_malfunction = true;
}
}
return 1;
return valid;
}

static int chrysler_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
Expand Down
126 changes: 66 additions & 60 deletions board/safety/safety_gm.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,78 +42,84 @@ uint32_t gm_ts_last = 0;
struct sample_t gm_torque_driver; // last few driver torques measured

static int gm_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
int bus = GET_BUS(to_push);
int addr = GET_ADDR(to_push);

if (addr == 388) {
int torque_driver_new = ((GET_BYTE(to_push, 6) & 0x7) << 8) | GET_BYTE(to_push, 7);
torque_driver_new = to_signed(torque_driver_new, 11);
// update array of samples
update_sample(&gm_torque_driver, torque_driver_new);
}

// sample speed, really only care if car is moving or not
// rear left wheel speed
if (addr == 842) {
gm_moving = GET_BYTE(to_push, 0) | GET_BYTE(to_push, 1);
}
bool valid = addr_safety_check(to_push, gm_rx_checks, GM_RX_CHECK_LEN,
NULL, NULL, NULL);

// ACC steering wheel buttons
if (addr == 481) {
int button = (GET_BYTE(to_push, 5) & 0x70) >> 4;
switch (button) {
case 2: // resume
case 3: // set
controls_allowed = 1;
break;
case 6: // cancel
controls_allowed = 0;
break;
default:
break; // any other button is irrelevant
if (valid) {
int bus = GET_BUS(to_push);
int addr = GET_ADDR(to_push);

if (addr == 388) {
int torque_driver_new = ((GET_BYTE(to_push, 6) & 0x7) << 8) | GET_BYTE(to_push, 7);
torque_driver_new = to_signed(torque_driver_new, 11);
// update array of samples
update_sample(&gm_torque_driver, torque_driver_new);
}
}

// exit controls on rising edge of brake press or on brake press when
// speed > 0
if (addr == 241) {
int brake = GET_BYTE(to_push, 1);
// Brake pedal's potentiometer returns near-zero reading
// even when pedal is not pressed
if (brake < 10) {
brake = 0;
// sample speed, really only care if car is moving or not
// rear left wheel speed
if (addr == 842) {
gm_moving = GET_BYTE(to_push, 0) | GET_BYTE(to_push, 1);
}
if (brake && (!gm_brake_prev || gm_moving)) {
controls_allowed = 0;

// ACC steering wheel buttons
if (addr == 481) {
int button = (GET_BYTE(to_push, 5) & 0x70) >> 4;
switch (button) {
case 2: // resume
case 3: // set
controls_allowed = 1;
break;
case 6: // cancel
controls_allowed = 0;
break;
default:
break; // any other button is irrelevant
}
}
gm_brake_prev = brake;
}

// exit controls on rising edge of gas press
if (addr == 417) {
int gas = GET_BYTE(to_push, 6);
if (gas && !gm_gas_prev) {
controls_allowed = 0;
// exit controls on rising edge of brake press or on brake press when
// speed > 0
if (addr == 241) {
int brake = GET_BYTE(to_push, 1);
// Brake pedal's potentiometer returns near-zero reading
// even when pedal is not pressed
if (brake < 10) {
brake = 0;
}
if (brake && (!gm_brake_prev || gm_moving)) {
controls_allowed = 0;
}
gm_brake_prev = brake;
}
gm_gas_prev = gas;
}

// exit controls on regen paddle
if (addr == 189) {
bool regen = GET_BYTE(to_push, 0) & 0x20;
if (regen) {
controls_allowed = 0;
// exit controls on rising edge of gas press
if (addr == 417) {
int gas = GET_BYTE(to_push, 6);
if (gas && !gm_gas_prev) {
controls_allowed = 0;
}
gm_gas_prev = gas;
}
}

// Check if ASCM or LKA camera are online
// on powertrain bus.
// 384 = ASCMLKASteeringCmd
// 715 = ASCMGasRegenCmd
if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (bus == 0) && ((addr == 384) || (addr == 715))) {
relay_malfunction = true;
// exit controls on regen paddle
if (addr == 189) {
bool regen = GET_BYTE(to_push, 0) & 0x20;
if (regen) {
controls_allowed = 0;
}
}

// Check if ASCM or LKA camera are online
// on powertrain bus.
// 384 = ASCMLKASteeringCmd
// 715 = ASCMGasRegenCmd
if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (bus == 0) && ((addr == 384) || (addr == 715))) {
relay_malfunction = true;
}
}
return 1;
return valid;
}

// all commands: gas/regen, friction brake and steering
Expand Down
50 changes: 28 additions & 22 deletions board/safety/safety_hyundai.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,35 +21,41 @@ uint32_t hyundai_ts_last = 0;
struct sample_t hyundai_torque_driver; // last few driver torques measured

static int hyundai_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
int bus = GET_BUS(to_push);
int addr = GET_ADDR(to_push);

if (addr == 897) {
int torque_driver_new = ((GET_BYTES_04(to_push) >> 11) & 0xfff) - 2048;
// update array of samples
update_sample(&hyundai_torque_driver, torque_driver_new);
}
bool valid = addr_safety_check(to_push, hyundai_rx_checks, HYUNDAI_RX_CHECK_LEN,
NULL, NULL, NULL);

if (valid) {
int bus = GET_BUS(to_push);
int addr = GET_ADDR(to_push);

// enter controls on rising edge of ACC, exit controls on ACC off
if (addr == 1057) {
// 2 bits: 13-14
int cruise_engaged = (GET_BYTES_04(to_push) >> 13) & 0x3;
if (cruise_engaged && !hyundai_cruise_engaged_last) {
controls_allowed = 1;
if (addr == 897) {
int torque_driver_new = ((GET_BYTES_04(to_push) >> 11) & 0xfff) - 2048;
// update array of samples
update_sample(&hyundai_torque_driver, torque_driver_new);
}
if (!cruise_engaged) {
controls_allowed = 0;

// enter controls on rising edge of ACC, exit controls on ACC off
if (addr == 1057) {
// 2 bits: 13-14
int cruise_engaged = (GET_BYTES_04(to_push) >> 13) & 0x3;
if (cruise_engaged && !hyundai_cruise_engaged_last) {
controls_allowed = 1;
}
if (!cruise_engaged) {
controls_allowed = 0;
}
hyundai_cruise_engaged_last = cruise_engaged;
}
hyundai_cruise_engaged_last = cruise_engaged;
}

// TODO: check gas pressed
// TODO: check gas pressed

// check if stock camera ECU is on bus 0
if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (bus == 0) && (addr == 832)) {
relay_malfunction = true;
// check if stock camera ECU is on bus 0
if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (bus == 0) && (addr == 832)) {
relay_malfunction = true;
}
}
return 1;
return valid;
}

static int hyundai_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
Expand Down
54 changes: 30 additions & 24 deletions board/safety/safety_subaru.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,36 +24,42 @@ uint32_t subaru_ts_last = 0;
struct sample_t subaru_torque_driver; // last few driver torques measured

static int subaru_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
int bus = GET_BUS(to_push);
int addr = GET_ADDR(to_push);

if (((addr == 0x119) || (addr == 0x371)) && (bus == 0)){
int bit_shift = (addr == 0x119) ? 16 : 29;
int torque_driver_new = ((GET_BYTES_04(to_push) >> bit_shift) & 0x7FF);
torque_driver_new = to_signed(torque_driver_new, 11);
// update array of samples
update_sample(&subaru_torque_driver, torque_driver_new);
}

// enter controls on rising edge of ACC, exit controls on ACC off
if (((addr == 0x240) || (addr == 0x144)) && (bus == 0)) {
int bit_shift = (addr == 0x240) ? 9 : 17;
int cruise_engaged = ((GET_BYTES_48(to_push) >> bit_shift) & 1);
if (cruise_engaged && !subaru_cruise_engaged_last) {
controls_allowed = 1;
bool valid = addr_safety_check(to_push, subaru_rx_checks, SUBARU_RX_CHECK_LEN,
NULL, NULL, NULL);

if (valid) {
int bus = GET_BUS(to_push);
int addr = GET_ADDR(to_push);

if (((addr == 0x119) || (addr == 0x371)) && (bus == 0)){
int bit_shift = (addr == 0x119) ? 16 : 29;
int torque_driver_new = ((GET_BYTES_04(to_push) >> bit_shift) & 0x7FF);
torque_driver_new = to_signed(torque_driver_new, 11);
// update array of samples
update_sample(&subaru_torque_driver, torque_driver_new);
}
if (!cruise_engaged) {
controls_allowed = 0;

// enter controls on rising edge of ACC, exit controls on ACC off
if (((addr == 0x240) || (addr == 0x144)) && (bus == 0)) {
int bit_shift = (addr == 0x240) ? 9 : 17;
int cruise_engaged = ((GET_BYTES_48(to_push) >> bit_shift) & 1);
if (cruise_engaged && !subaru_cruise_engaged_last) {
controls_allowed = 1;
}
if (!cruise_engaged) {
controls_allowed = 0;
}
subaru_cruise_engaged_last = cruise_engaged;
}
subaru_cruise_engaged_last = cruise_engaged;
}

// TODO: enforce cancellation on gas pressed
// TODO: enforce cancellation on gas pressed

if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (bus == 0) && ((addr == 0x122) || (addr == 0x164))) {
relay_malfunction = true;
if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (bus == 0) && ((addr == 0x122) || (addr == 0x164))) {
relay_malfunction = true;
}
}
return 1;
return valid;
}

static int subaru_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
Expand Down
Loading

0 comments on commit f67ec28

Please sign in to comment.