Skip to content

Commit

Permalink
Cleanup pedal nomenclature (commaai#467)
Browse files Browse the repository at this point in the history
* consolidate gas and brake nomenclature

* fixes in code and tests
  • Loading branch information
rbiasini authored Mar 9, 2020
1 parent ceff91d commit 0f21b19
Show file tree
Hide file tree
Showing 17 changed files with 78 additions and 128 deletions.
14 changes: 6 additions & 8 deletions board/safety/safety_chrysler.h
Original file line number Diff line number Diff line change
Expand Up @@ -21,8 +21,6 @@ const int CHRYSLER_RX_CHECK_LEN = sizeof(chrysler_rx_checks) / sizeof(chrysler_r
int chrysler_rt_torque_last = 0;
int chrysler_desired_torque_last = 0;
int chrysler_cruise_engaged_last = 0;
bool chrysler_gas_prev = false;
bool chrysler_brake_prev = false;
int chrysler_speed = 0;
uint32_t chrysler_ts_last = 0;
struct sample_t chrysler_torque_meas; // last few torques measured
Expand Down Expand Up @@ -108,20 +106,20 @@ static int chrysler_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {

// exit controls on rising edge of gas press
if (addr == 308) {
bool gas = (GET_BYTE(to_push, 5) & 0x7F) != 0;
if (gas && !chrysler_gas_prev && (chrysler_speed > CHRYSLER_GAS_THRSLD)) {
bool gas_pressed = (GET_BYTE(to_push, 5) & 0x7F) != 0;
if (gas_pressed && !gas_pressed_prev && (chrysler_speed > CHRYSLER_GAS_THRSLD)) {
controls_allowed = 0;
}
chrysler_gas_prev = gas;
gas_pressed_prev = gas_pressed;
}

// exit controls on rising edge of brake press
if (addr == 320) {
bool brake = (GET_BYTE(to_push, 0) & 0x7) == 5;
if (brake && (!chrysler_brake_prev || (chrysler_speed > CHRYSLER_STANDSTILL_THRSLD))) {
bool brake_pressed = (GET_BYTE(to_push, 0) & 0x7) == 5;
if (brake_pressed && (!brake_pressed_prev || (chrysler_speed > CHRYSLER_STANDSTILL_THRSLD))) {
controls_allowed = 0;
}
chrysler_brake_prev = brake;
brake_pressed_prev = brake_pressed;
}

// check if stock camera ECU is on bus 0
Expand Down
16 changes: 7 additions & 9 deletions board/safety/safety_ford.h
Original file line number Diff line number Diff line change
Expand Up @@ -7,8 +7,6 @@
// brake rising edge
// brake > 0mph

int ford_brake_prev = 0;
int ford_gas_prev = 0;
bool ford_moving = false;

static int ford_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
Expand Down Expand Up @@ -39,20 +37,20 @@ static int ford_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
// exit controls on rising edge of brake press or on brake press when
// speed > 0
if (addr == 0x165) {
int brake = GET_BYTE(to_push, 0) & 0x20;
if (brake && (!(ford_brake_prev) || ford_moving)) {
int brake_pressed = GET_BYTE(to_push, 0) & 0x20;
if (brake_pressed && (!brake_pressed_prev || ford_moving)) {
controls_allowed = 0;
}
ford_brake_prev = brake;
brake_pressed_prev = brake_pressed;
}

// exit controls on rising edge of gas press
if (addr == 0x204) {
int gas = (GET_BYTE(to_push, 0) & 0x03) | GET_BYTE(to_push, 1);
if (gas && !(ford_gas_prev)) {
bool gas_pressed = ((GET_BYTE(to_push, 0) & 0x03) | GET_BYTE(to_push, 1)) != 0;
if (gas_pressed && !gas_pressed_prev) {
controls_allowed = 0;
}
ford_gas_prev = gas;
gas_pressed_prev = gas_pressed;
}

if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (bus == 0) && (addr == 0x3CA)) {
Expand All @@ -74,7 +72,7 @@ static int ford_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {

// 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_moving);
int pedal_pressed = gas_pressed_prev || (brake_pressed_prev && ford_moving);
bool current_controls_allowed = controls_allowed && !(pedal_pressed);

if (relay_malfunction) {
Expand Down
19 changes: 7 additions & 12 deletions board/safety/safety_gm.h
Original file line number Diff line number Diff line change
Expand Up @@ -33,8 +33,6 @@ AddrCheckStruct gm_rx_checks[] = {
};
const int GM_RX_CHECK_LEN = sizeof(gm_rx_checks) / sizeof(gm_rx_checks[0]);

int gm_brake_prev = 0;
int gm_gas_prev = 0;
bool gm_moving = false;
int gm_rt_torque_last = 0;
int gm_desired_torque_last = 0;
Expand Down Expand Up @@ -81,25 +79,22 @@ static int 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 == 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)) {
bool brake_pressed = GET_BYTE(to_push, 1) >= 10;
if (brake_pressed && (!brake_pressed_prev || gm_moving)) {
controls_allowed = 0;
}
gm_brake_prev = brake;
brake_pressed_prev = brake_pressed;
}

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

// exit controls on regen paddle
Expand Down Expand Up @@ -143,7 +138,7 @@ static int gm_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {

// disallow actuator commands if gas or brake (with vehicle moving) are pressed
// and the the latching controls_allowed flag is True
int pedal_pressed = gm_gas_prev || (gm_brake_prev && gm_moving);
int pedal_pressed = gas_pressed_prev || (brake_pressed_prev && gm_moving);
bool current_controls_allowed = controls_allowed && !pedal_pressed;

// BRAKE: safety check
Expand Down
16 changes: 7 additions & 9 deletions board/safety/safety_honda.h
Original file line number Diff line number Diff line change
Expand Up @@ -28,8 +28,6 @@ AddrCheckStruct honda_bh_rx_checks[] = {
const int HONDA_BH_RX_CHECKS_LEN = sizeof(honda_bh_rx_checks) / sizeof(honda_bh_rx_checks[0]);

int honda_brake = 0;
int honda_gas_prev = 0;
bool honda_brake_pressed_prev = false;
bool honda_moving = false;
bool honda_alt_brake_msg = false;
bool honda_fwd_brake = false;
Expand Down Expand Up @@ -112,10 +110,10 @@ static int honda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
bool is_user_brake_msg = honda_alt_brake_msg ? ((addr) == 0x1BE) : ((addr) == 0x17C);
if (is_user_brake_msg) {
bool brake_pressed = honda_alt_brake_msg ? (GET_BYTE((to_push), 0) & 0x10) : (GET_BYTE((to_push), 6) & 0x20);
if (brake_pressed && (!(honda_brake_pressed_prev) || honda_moving)) {
if (brake_pressed && (!brake_pressed_prev || honda_moving)) {
controls_allowed = 0;
}
honda_brake_pressed_prev = brake_pressed;
brake_pressed_prev = brake_pressed;
}

// exit controls on rising edge of gas press if interceptor (0x201 w/ len = 6)
Expand All @@ -133,11 +131,11 @@ static int honda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
// exit controls on rising edge of gas press if no interceptor
if (!gas_interceptor_detected) {
if (addr == 0x17C) {
int gas = GET_BYTE(to_push, 0);
if (gas && !honda_gas_prev) {
bool gas_pressed = GET_BYTE(to_push, 0) != 0;
if (gas_pressed && !gas_pressed_prev) {
controls_allowed = 0;
}
honda_gas_prev = gas;
gas_pressed_prev = gas_pressed;
}
}
if ((bus == 2) && (addr == 0x1FA)) {
Expand Down Expand Up @@ -194,8 +192,8 @@ static int honda_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {

// disallow actuator commands if gas or brake (with vehicle moving) are pressed
// and the the latching controls_allowed flag is True
int pedal_pressed = honda_gas_prev || (gas_interceptor_prev > HONDA_GAS_INTERCEPTOR_THRESHOLD) ||
(honda_brake_pressed_prev && honda_moving);
int pedal_pressed = gas_pressed_prev || (gas_interceptor_prev > HONDA_GAS_INTERCEPTOR_THRESHOLD) ||
(brake_pressed_prev && honda_moving);
bool current_controls_allowed = controls_allowed && !(pedal_pressed);

// BRAKE: safety check
Expand Down
14 changes: 6 additions & 8 deletions board/safety/safety_hyundai.h
Original file line number Diff line number Diff line change
Expand Up @@ -22,8 +22,6 @@ int hyundai_rt_torque_last = 0;
int hyundai_desired_torque_last = 0;
int hyundai_cruise_engaged_last = 0;
int hyundai_speed = 0;
bool hyundai_gas_last = false;
bool hyundai_brake_last = false;
uint32_t hyundai_ts_last = 0;
struct sample_t hyundai_torque_driver; // last few driver torques measured

Expand Down Expand Up @@ -56,11 +54,11 @@ static int hyundai_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {

// exit controls on rising edge of gas press
if (addr == 608) {
bool gas = (GET_BYTE(to_push, 7) >> 6) != 0;
if (gas && ! hyundai_gas_last) {
bool gas_pressed = (GET_BYTE(to_push, 7) >> 6) != 0;
if (gas_pressed && !gas_pressed_prev) {
controls_allowed = 0;
}
hyundai_gas_last = gas;
gas_pressed_prev = gas_pressed;
}

// sample subaru wheel speed, averaging opposite corners
Expand All @@ -72,11 +70,11 @@ static int hyundai_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {

// exit controls on rising edge of brake press
if (addr == 916) {
bool brake = (GET_BYTE(to_push, 6) >> 7) != 0;
if (brake && (!hyundai_brake_last || (hyundai_speed > HYUNDAI_STANDSTILL_THRSLD))) {
bool brake_pressed = (GET_BYTE(to_push, 6) >> 7) != 0;
if (brake_pressed && (!brake_pressed_prev || (hyundai_speed > HYUNDAI_STANDSTILL_THRSLD))) {
controls_allowed = 0;
}
hyundai_brake_last = brake;
brake_pressed_prev = brake_pressed;
}

// check if stock camera ECU is on bus 0
Expand Down
14 changes: 6 additions & 8 deletions board/safety/safety_nissan.h
Original file line number Diff line number Diff line change
Expand Up @@ -29,8 +29,6 @@ float nissan_speed = 0;
uint32_t nissan_ts_angle_last = 0;
int nissan_cruise_engaged_last = 0;
int nissan_desired_angle_last = 0;
int nissan_gas_prev = 0;
int nissan_brake_prev = 0;

struct sample_t nissan_angle_meas; // last 3 steer angles

Expand Down Expand Up @@ -64,11 +62,11 @@ static int nissan_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {

// exit controls on rising edge of gas press
if (addr == 0x15c) {
int gas = ((GET_BYTE(to_push, 5) << 2) | ((GET_BYTE(to_push, 6) >> 6) & 0x3));
if ((gas > 0) && (nissan_gas_prev == 0)) {
bool gas_pressed = ((GET_BYTE(to_push, 5) << 2) | ((GET_BYTE(to_push, 6) >> 6) & 0x3));
if (gas_pressed && !gas_pressed_prev) {
controls_allowed = 0;
}
nissan_gas_prev = gas;
gas_pressed_prev = gas_pressed;
}

// 0x169 is lkas cmd. If it is on bus 0, then relay is unexpectedly closed
Expand All @@ -91,11 +89,11 @@ static int nissan_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {

// exit controls on rising edge of brake press, or if speed > 0 and brake
if (addr == 0x454) {
int brake = (GET_BYTE(to_push, 2) & 0x80);
if ((brake > 0) && ((nissan_brake_prev == 0) || (nissan_speed > 0.))) {
bool brake_pressed = (GET_BYTE(to_push, 2) & 0x80) != 0;
if (brake_pressed && (!brake_pressed_prev || (nissan_speed > 0.))) {
controls_allowed = 0;
}
nissan_brake_prev = brake;
brake_pressed_prev = brake_pressed;
}
}
}
Expand Down
14 changes: 6 additions & 8 deletions board/safety/safety_subaru.h
Original file line number Diff line number Diff line change
Expand Up @@ -35,8 +35,6 @@ int subaru_rt_torque_last = 0;
int subaru_desired_torque_last = 0;
int subaru_speed = 0;
uint32_t subaru_ts_last = 0;
bool subaru_gas_last = false;
bool subaru_brake_last = false;
bool subaru_global = false;
struct sample_t subaru_torque_driver; // last few driver torques measured

Expand Down Expand Up @@ -106,22 +104,22 @@ static int subaru_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {

// exit controls on rising edge of brake press (TODO: missing check for unsupported legacy models)
if ((addr == 0x139) && subaru_global) {
bool brake = (GET_BYTES_48(to_push) & 0xFFF0) > 0;
if (brake && (!subaru_brake_last || (subaru_speed > SUBARU_STANDSTILL_THRSLD))) {
bool brake_pressed = (GET_BYTES_48(to_push) & 0xFFF0) > 0;
if (brake_pressed && (!brake_pressed_prev || (subaru_speed > SUBARU_STANDSTILL_THRSLD))) {
controls_allowed = 0;
}
subaru_brake_last = brake;
brake_pressed_prev = brake_pressed;
}

// exit controls on rising edge of gas press
if (((addr == 0x40) && subaru_global) ||
((addr == 0x140) && !subaru_global)) {
int byte = subaru_global ? 4 : 0;
bool gas = GET_BYTE(to_push, byte) != 0;
if (gas && !subaru_gas_last) {
bool gas_pressed = GET_BYTE(to_push, byte) != 0;
if (gas_pressed && !gas_pressed_prev) {
controls_allowed = 0;
}
subaru_gas_last = gas;
gas_pressed_prev = gas_pressed;
}

if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) &&
Expand Down
2 changes: 0 additions & 2 deletions board/safety/safety_tesla.h
Original file line number Diff line number Diff line change
Expand Up @@ -34,8 +34,6 @@ float tesla_ts_angle_last = 0;

int tesla_controls_allowed_last = 0;

int tesla_brake_prev = 0;
int tesla_gas_prev = 0;
int tesla_speed = 0;
int eac_status = 0;

Expand Down
14 changes: 6 additions & 8 deletions board/safety/safety_toyota.h
Original file line number Diff line number Diff line change
Expand Up @@ -40,8 +40,6 @@ int toyota_desired_torque_last = 0; // last desired steer torque
int toyota_rt_torque_last = 0; // last desired torque for real time check
uint32_t toyota_ts_last = 0;
int toyota_cruise_engaged_last = 0; // cruise state
bool toyota_gas_prev = false;
bool toyota_brake_prev = false;
bool toyota_moving = false;
struct sample_t toyota_torque_meas; // last 3 motor torques produced by the eps

Expand Down Expand Up @@ -112,11 +110,11 @@ static int toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
// most cars have brake_pressed on 0x226, corolla and rav4 on 0x224
if ((addr == 0x224) || (addr == 0x226)) {
int byte = (addr == 0x224) ? 0 : 4;
bool brake = ((GET_BYTE(to_push, byte) >> 5) & 1) != 0;
if (brake && (!toyota_brake_prev || toyota_moving)) {
bool brake_pressed = ((GET_BYTE(to_push, byte) >> 5) & 1) != 0;
if (brake_pressed && (!brake_pressed_prev || toyota_moving)) {
controls_allowed = 0;
}
toyota_brake_prev = brake;
brake_pressed_prev = brake_pressed;
}

// exit controls on rising edge of interceptor gas press
Expand All @@ -132,11 +130,11 @@ static int toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {

// exit controls on rising edge of gas press
if (addr == 0x2C1) {
bool gas = GET_BYTE(to_push, 6) != 0;
if (gas && !toyota_gas_prev && !gas_interceptor_detected) {
bool gas_pressed = GET_BYTE(to_push, 6) != 0;
if (gas_pressed && !gas_pressed_prev && !gas_interceptor_detected) {
controls_allowed = 0;
}
toyota_gas_prev = gas;
gas_pressed_prev = gas_pressed;
}

// 0x2E4 is lkas cmd. If it is on bus 0, then relay is unexpectedly closed
Expand Down
12 changes: 5 additions & 7 deletions board/safety/safety_volkswagen.h
Original file line number Diff line number Diff line change
Expand Up @@ -36,8 +36,6 @@ int volkswagen_rt_torque_last = 0;
int volkswagen_desired_torque_last = 0;
uint32_t volkswagen_ts_last = 0;
bool volkswagen_moving = false;
bool volkswagen_brake_pressed_prev = false;
int volkswagen_gas_prev = 0;
int volkswagen_torque_msg = 0;
int volkswagen_lane_msg = 0;
uint8_t volkswagen_crc8_lut_8h2f[256]; // Static lookup table for CRC8 poly 0x2F, aka 8H2F/AUTOSAR
Expand Down Expand Up @@ -137,21 +135,21 @@ static int volkswagen_mqb_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
// Exit controls on rising edge of gas press
// Signal: Motor_20.MO_Fahrpedalrohwert_01
if (addr == MSG_MOTOR_20) {
int gas = (GET_BYTES_04(to_push) >> 12) & 0xFF;
if ((gas > 0) && (volkswagen_gas_prev == 0)) {
bool gas_pressed = ((GET_BYTES_04(to_push) >> 12) & 0xFF) != 0;
if (gas_pressed && !gas_pressed_prev) {
controls_allowed = 0;
}
volkswagen_gas_prev = gas;
gas_pressed_prev = gas_pressed;
}

// Exit controls on rising edge of brake press
// Signal: ESP_05.ESP_Fahrer_bremst
if (addr == MSG_ESP_05) {
bool brake_pressed = (GET_BYTE(to_push, 3) & 0x4) >> 2;
if (brake_pressed && (!(volkswagen_brake_pressed_prev) || volkswagen_moving)) {
if (brake_pressed && (!brake_pressed_prev || volkswagen_moving)) {
controls_allowed = 0;
}
volkswagen_brake_pressed_prev = brake_pressed;
brake_pressed_prev = brake_pressed;
}

// If there are HCA messages on bus 0 not sent by OP, there's a relay problem
Expand Down
2 changes: 2 additions & 0 deletions board/safety_declarations.h
Original file line number Diff line number Diff line change
Expand Up @@ -85,6 +85,8 @@ bool controls_allowed = false;
bool relay_malfunction = false;
bool gas_interceptor_detected = false;
int gas_interceptor_prev = 0;
bool gas_pressed_prev = false;
bool brake_pressed_prev = false;

// time since safety mode has been changed
uint32_t safety_mode_cnt = 0U;
Expand Down
Loading

0 comments on commit 0f21b19

Please sign in to comment.