Fix msg checks for non Honda and Toyota (#426)
parent
3b35621671
commit
f67ec28e3f
|
@ -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);
|
||||
|
|
|
@ -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);
|
||||
|
||||
// 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;
|
||||
// Measured eps torque
|
||||
if (addr == 544) {
|
||||
int torque_meas_new = ((GET_BYTE(to_push, 4) & 0x7U) << 8) + GET_BYTE(to_push, 5) - 1024U;
|
||||
|
||||
// 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) {
|
||||
|
|
|
@ -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);
|
||||
}
|
||||
bool valid = addr_safety_check(to_push, gm_rx_checks, GM_RX_CHECK_LEN,
|
||||
NULL, NULL, NULL);
|
||||
|
||||
// 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 (valid) {
|
||||
int bus = GET_BUS(to_push);
|
||||
int addr = GET_ADDR(to_push);
|
||||
|
||||
// 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
|
||||
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);
|
||||
}
|
||||
|
||||
// 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
|
||||
}
|
||||
}
|
||||
|
||||
// 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;
|
||||
}
|
||||
|
||||
// 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;
|
||||
break;
|
||||
default:
|
||||
break; // any other button is irrelevant
|
||||
}
|
||||
gm_gas_prev = gas;
|
||||
}
|
||||
}
|
||||
|
||||
// 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;
|
||||
// exit controls on regen paddle
|
||||
if (addr == 189) {
|
||||
bool regen = GET_BYTE(to_push, 0) & 0x20;
|
||||
if (regen) {
|
||||
controls_allowed = 0;
|
||||
}
|
||||
}
|
||||
if (brake && (!gm_brake_prev || gm_moving)) {
|
||||
controls_allowed = 0;
|
||||
}
|
||||
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;
|
||||
}
|
||||
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;
|
||||
// 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;
|
||||
}
|
||||
}
|
||||
|
||||
// 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
|
||||
|
|
|
@ -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);
|
||||
|
||||
// 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 (valid) {
|
||||
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);
|
||||
}
|
||||
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) {
|
||||
|
|
|
@ -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);
|
||||
}
|
||||
bool valid = addr_safety_check(to_push, subaru_rx_checks, SUBARU_RX_CHECK_LEN,
|
||||
NULL, NULL, NULL);
|
||||
|
||||
// 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 (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) {
|
||||
|
|
|
@ -33,42 +33,48 @@ uint32_t volkswagen_ts_last = 0;
|
|||
int volkswagen_gas_prev = 0;
|
||||
|
||||
static int volkswagen_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
|
||||
int bus = GET_BUS(to_push);
|
||||
int addr = GET_ADDR(to_push);
|
||||
|
||||
// Update driver input torque samples from EPS_01.Driver_Strain for absolute torque, and EPS_01.Driver_Strain_VZ
|
||||
// for the direction.
|
||||
if ((bus == 0) && (addr == MSG_EPS_01)) {
|
||||
int torque_driver_new = GET_BYTE(to_push, 5) | ((GET_BYTE(to_push, 6) & 0x1F) << 8);
|
||||
int sign = (GET_BYTE(to_push, 6) & 0x80) >> 7;
|
||||
if (sign == 1) {
|
||||
torque_driver_new *= -1;
|
||||
}
|
||||
bool valid = addr_safety_check(to_push, volkswagen_rx_checks, VOLKSWAGEN_RX_CHECK_LEN,
|
||||
NULL, NULL, NULL);
|
||||
|
||||
update_sample(&volkswagen_torque_driver, torque_driver_new);
|
||||
if (valid) {
|
||||
int bus = GET_BUS(to_push);
|
||||
int addr = GET_ADDR(to_push);
|
||||
|
||||
// Update driver input torque samples from EPS_01.Driver_Strain for absolute torque, and EPS_01.Driver_Strain_VZ
|
||||
// for the direction.
|
||||
if ((bus == 0) && (addr == MSG_EPS_01)) {
|
||||
int torque_driver_new = GET_BYTE(to_push, 5) | ((GET_BYTE(to_push, 6) & 0x1F) << 8);
|
||||
int sign = (GET_BYTE(to_push, 6) & 0x80) >> 7;
|
||||
if (sign == 1) {
|
||||
torque_driver_new *= -1;
|
||||
}
|
||||
|
||||
update_sample(&volkswagen_torque_driver, torque_driver_new);
|
||||
}
|
||||
|
||||
// Monitor ACC_06.ACC_Status_ACC for stock ACC status. Because the current MQB port is lateral-only, OP's control
|
||||
// allowed state is directly driven by stock ACC engagement. Permit the ACC message to come from either bus, in
|
||||
// order to accommodate future camera-side integrations if needed.
|
||||
if (addr == MSG_ACC_06) {
|
||||
int acc_status = (GET_BYTE(to_push, 7) & 0x70) >> 4;
|
||||
controls_allowed = ((acc_status == 3) || (acc_status == 4) || (acc_status == 5)) ? 1 : 0;
|
||||
}
|
||||
|
||||
// exit controls on rising edge of gas press. Bits [12-20)
|
||||
if (addr == MSG_MOTOR_20) {
|
||||
int gas = (GET_BYTES_04(to_push) >> 12) & 0xFF;
|
||||
if ((gas > 0) && (volkswagen_gas_prev == 0)) {
|
||||
controls_allowed = 0;
|
||||
}
|
||||
volkswagen_gas_prev = gas;
|
||||
}
|
||||
|
||||
if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (bus == 0) && (addr == MSG_HCA_01)) {
|
||||
relay_malfunction = true;
|
||||
}
|
||||
}
|
||||
|
||||
// Monitor ACC_06.ACC_Status_ACC for stock ACC status. Because the current MQB port is lateral-only, OP's control
|
||||
// allowed state is directly driven by stock ACC engagement. Permit the ACC message to come from either bus, in
|
||||
// order to accommodate future camera-side integrations if needed.
|
||||
if (addr == MSG_ACC_06) {
|
||||
int acc_status = (GET_BYTE(to_push, 7) & 0x70) >> 4;
|
||||
controls_allowed = ((acc_status == 3) || (acc_status == 4) || (acc_status == 5)) ? 1 : 0;
|
||||
}
|
||||
|
||||
// exit controls on rising edge of gas press. Bits [12-20)
|
||||
if (addr == MSG_MOTOR_20) {
|
||||
int gas = (GET_BYTES_04(to_push) >> 12) & 0xFF;
|
||||
if ((gas > 0) && (volkswagen_gas_prev == 0)) {
|
||||
controls_allowed = 0;
|
||||
}
|
||||
volkswagen_gas_prev = gas;
|
||||
}
|
||||
|
||||
if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (bus == 0) && (addr == MSG_HCA_01)) {
|
||||
relay_malfunction = true;
|
||||
}
|
||||
return 1;
|
||||
return valid;
|
||||
}
|
||||
|
||||
static int volkswagen_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
|
||||
|
|
Loading…
Reference in New Issue