mirror of https://github.com/commaai/panda.git
Cleanup pedal nomenclature (#467)
* consolidate gas and brake nomenclature * fixes in code and tests
This commit is contained in:
parent
ceff91d3c5
commit
0f21b19bb3
|
@ -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_rt_torque_last = 0;
|
||||||
int chrysler_desired_torque_last = 0;
|
int chrysler_desired_torque_last = 0;
|
||||||
int chrysler_cruise_engaged_last = 0;
|
int chrysler_cruise_engaged_last = 0;
|
||||||
bool chrysler_gas_prev = false;
|
|
||||||
bool chrysler_brake_prev = false;
|
|
||||||
int chrysler_speed = 0;
|
int chrysler_speed = 0;
|
||||||
uint32_t chrysler_ts_last = 0;
|
uint32_t chrysler_ts_last = 0;
|
||||||
struct sample_t chrysler_torque_meas; // last few torques measured
|
struct sample_t chrysler_torque_meas; // last few torques measured
|
||||||
|
@ -108,20 +106,20 @@ static int chrysler_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
|
||||||
|
|
||||||
// exit controls on rising edge of gas press
|
// exit controls on rising edge of gas press
|
||||||
if (addr == 308) {
|
if (addr == 308) {
|
||||||
bool gas = (GET_BYTE(to_push, 5) & 0x7F) != 0;
|
bool gas_pressed = (GET_BYTE(to_push, 5) & 0x7F) != 0;
|
||||||
if (gas && !chrysler_gas_prev && (chrysler_speed > CHRYSLER_GAS_THRSLD)) {
|
if (gas_pressed && !gas_pressed_prev && (chrysler_speed > CHRYSLER_GAS_THRSLD)) {
|
||||||
controls_allowed = 0;
|
controls_allowed = 0;
|
||||||
}
|
}
|
||||||
chrysler_gas_prev = gas;
|
gas_pressed_prev = gas_pressed;
|
||||||
}
|
}
|
||||||
|
|
||||||
// exit controls on rising edge of brake press
|
// exit controls on rising edge of brake press
|
||||||
if (addr == 320) {
|
if (addr == 320) {
|
||||||
bool brake = (GET_BYTE(to_push, 0) & 0x7) == 5;
|
bool brake_pressed = (GET_BYTE(to_push, 0) & 0x7) == 5;
|
||||||
if (brake && (!chrysler_brake_prev || (chrysler_speed > CHRYSLER_STANDSTILL_THRSLD))) {
|
if (brake_pressed && (!brake_pressed_prev || (chrysler_speed > CHRYSLER_STANDSTILL_THRSLD))) {
|
||||||
controls_allowed = 0;
|
controls_allowed = 0;
|
||||||
}
|
}
|
||||||
chrysler_brake_prev = brake;
|
brake_pressed_prev = brake_pressed;
|
||||||
}
|
}
|
||||||
|
|
||||||
// check if stock camera ECU is on bus 0
|
// check if stock camera ECU is on bus 0
|
||||||
|
|
|
@ -7,8 +7,6 @@
|
||||||
// brake rising edge
|
// brake rising edge
|
||||||
// brake > 0mph
|
// brake > 0mph
|
||||||
|
|
||||||
int ford_brake_prev = 0;
|
|
||||||
int ford_gas_prev = 0;
|
|
||||||
bool ford_moving = false;
|
bool ford_moving = false;
|
||||||
|
|
||||||
static int ford_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
|
static int ford_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
|
||||||
|
@ -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
|
// exit controls on rising edge of brake press or on brake press when
|
||||||
// speed > 0
|
// speed > 0
|
||||||
if (addr == 0x165) {
|
if (addr == 0x165) {
|
||||||
int brake = GET_BYTE(to_push, 0) & 0x20;
|
int brake_pressed = GET_BYTE(to_push, 0) & 0x20;
|
||||||
if (brake && (!(ford_brake_prev) || ford_moving)) {
|
if (brake_pressed && (!brake_pressed_prev || ford_moving)) {
|
||||||
controls_allowed = 0;
|
controls_allowed = 0;
|
||||||
}
|
}
|
||||||
ford_brake_prev = brake;
|
brake_pressed_prev = brake_pressed;
|
||||||
}
|
}
|
||||||
|
|
||||||
// exit controls on rising edge of gas press
|
// exit controls on rising edge of gas press
|
||||||
if (addr == 0x204) {
|
if (addr == 0x204) {
|
||||||
int gas = (GET_BYTE(to_push, 0) & 0x03) | GET_BYTE(to_push, 1);
|
bool gas_pressed = ((GET_BYTE(to_push, 0) & 0x03) | GET_BYTE(to_push, 1)) != 0;
|
||||||
if (gas && !(ford_gas_prev)) {
|
if (gas_pressed && !gas_pressed_prev) {
|
||||||
controls_allowed = 0;
|
controls_allowed = 0;
|
||||||
}
|
}
|
||||||
ford_gas_prev = gas;
|
gas_pressed_prev = gas_pressed;
|
||||||
}
|
}
|
||||||
|
|
||||||
if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (bus == 0) && (addr == 0x3CA)) {
|
if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (bus == 0) && (addr == 0x3CA)) {
|
||||||
|
@ -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
|
// disallow actuator commands if gas or brake (with vehicle moving) are pressed
|
||||||
// and the the latching controls_allowed flag is True
|
// 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);
|
bool current_controls_allowed = controls_allowed && !(pedal_pressed);
|
||||||
|
|
||||||
if (relay_malfunction) {
|
if (relay_malfunction) {
|
||||||
|
|
|
@ -33,8 +33,6 @@ AddrCheckStruct gm_rx_checks[] = {
|
||||||
};
|
};
|
||||||
const int GM_RX_CHECK_LEN = sizeof(gm_rx_checks) / sizeof(gm_rx_checks[0]);
|
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;
|
bool gm_moving = false;
|
||||||
int gm_rt_torque_last = 0;
|
int gm_rt_torque_last = 0;
|
||||||
int gm_desired_torque_last = 0;
|
int gm_desired_torque_last = 0;
|
||||||
|
@ -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
|
// exit controls on rising edge of brake press or on brake press when
|
||||||
// speed > 0
|
// speed > 0
|
||||||
if (addr == 241) {
|
if (addr == 241) {
|
||||||
int brake = GET_BYTE(to_push, 1);
|
|
||||||
// Brake pedal's potentiometer returns near-zero reading
|
// Brake pedal's potentiometer returns near-zero reading
|
||||||
// even when pedal is not pressed
|
// even when pedal is not pressed
|
||||||
if (brake < 10) {
|
bool brake_pressed = GET_BYTE(to_push, 1) >= 10;
|
||||||
brake = 0;
|
if (brake_pressed && (!brake_pressed_prev || gm_moving)) {
|
||||||
}
|
|
||||||
if (brake && (!gm_brake_prev || gm_moving)) {
|
|
||||||
controls_allowed = 0;
|
controls_allowed = 0;
|
||||||
}
|
}
|
||||||
gm_brake_prev = brake;
|
brake_pressed_prev = brake_pressed;
|
||||||
}
|
}
|
||||||
|
|
||||||
// exit controls on rising edge of gas press
|
// exit controls on rising edge of gas press
|
||||||
if (addr == 417) {
|
if (addr == 417) {
|
||||||
int gas = GET_BYTE(to_push, 6);
|
bool gas_pressed = GET_BYTE(to_push, 6) != 0;
|
||||||
if (gas && !gm_gas_prev) {
|
if (gas_pressed && !gas_pressed_prev) {
|
||||||
controls_allowed = 0;
|
controls_allowed = 0;
|
||||||
}
|
}
|
||||||
gm_gas_prev = gas;
|
gas_pressed_prev = gas_pressed;
|
||||||
}
|
}
|
||||||
|
|
||||||
// exit controls on regen paddle
|
// exit controls on regen paddle
|
||||||
|
@ -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
|
// disallow actuator commands if gas or brake (with vehicle moving) are pressed
|
||||||
// and the the latching controls_allowed flag is True
|
// 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;
|
bool current_controls_allowed = controls_allowed && !pedal_pressed;
|
||||||
|
|
||||||
// BRAKE: safety check
|
// BRAKE: safety check
|
||||||
|
|
|
@ -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]);
|
const int HONDA_BH_RX_CHECKS_LEN = sizeof(honda_bh_rx_checks) / sizeof(honda_bh_rx_checks[0]);
|
||||||
|
|
||||||
int honda_brake = 0;
|
int honda_brake = 0;
|
||||||
int honda_gas_prev = 0;
|
|
||||||
bool honda_brake_pressed_prev = false;
|
|
||||||
bool honda_moving = false;
|
bool honda_moving = false;
|
||||||
bool honda_alt_brake_msg = false;
|
bool honda_alt_brake_msg = false;
|
||||||
bool honda_fwd_brake = false;
|
bool honda_fwd_brake = false;
|
||||||
|
@ -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);
|
bool is_user_brake_msg = honda_alt_brake_msg ? ((addr) == 0x1BE) : ((addr) == 0x17C);
|
||||||
if (is_user_brake_msg) {
|
if (is_user_brake_msg) {
|
||||||
bool brake_pressed = honda_alt_brake_msg ? (GET_BYTE((to_push), 0) & 0x10) : (GET_BYTE((to_push), 6) & 0x20);
|
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;
|
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)
|
// exit controls on rising edge of gas press if interceptor (0x201 w/ len = 6)
|
||||||
|
@ -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
|
// exit controls on rising edge of gas press if no interceptor
|
||||||
if (!gas_interceptor_detected) {
|
if (!gas_interceptor_detected) {
|
||||||
if (addr == 0x17C) {
|
if (addr == 0x17C) {
|
||||||
int gas = GET_BYTE(to_push, 0);
|
bool gas_pressed = GET_BYTE(to_push, 0) != 0;
|
||||||
if (gas && !honda_gas_prev) {
|
if (gas_pressed && !gas_pressed_prev) {
|
||||||
controls_allowed = 0;
|
controls_allowed = 0;
|
||||||
}
|
}
|
||||||
honda_gas_prev = gas;
|
gas_pressed_prev = gas_pressed;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
if ((bus == 2) && (addr == 0x1FA)) {
|
if ((bus == 2) && (addr == 0x1FA)) {
|
||||||
|
@ -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
|
// disallow actuator commands if gas or brake (with vehicle moving) are pressed
|
||||||
// and the the latching controls_allowed flag is True
|
// and the the latching controls_allowed flag is True
|
||||||
int pedal_pressed = honda_gas_prev || (gas_interceptor_prev > HONDA_GAS_INTERCEPTOR_THRESHOLD) ||
|
int pedal_pressed = gas_pressed_prev || (gas_interceptor_prev > HONDA_GAS_INTERCEPTOR_THRESHOLD) ||
|
||||||
(honda_brake_pressed_prev && honda_moving);
|
(brake_pressed_prev && honda_moving);
|
||||||
bool current_controls_allowed = controls_allowed && !(pedal_pressed);
|
bool current_controls_allowed = controls_allowed && !(pedal_pressed);
|
||||||
|
|
||||||
// BRAKE: safety check
|
// BRAKE: safety check
|
||||||
|
|
|
@ -22,8 +22,6 @@ int hyundai_rt_torque_last = 0;
|
||||||
int hyundai_desired_torque_last = 0;
|
int hyundai_desired_torque_last = 0;
|
||||||
int hyundai_cruise_engaged_last = 0;
|
int hyundai_cruise_engaged_last = 0;
|
||||||
int hyundai_speed = 0;
|
int hyundai_speed = 0;
|
||||||
bool hyundai_gas_last = false;
|
|
||||||
bool hyundai_brake_last = false;
|
|
||||||
uint32_t hyundai_ts_last = 0;
|
uint32_t hyundai_ts_last = 0;
|
||||||
struct sample_t hyundai_torque_driver; // last few driver torques measured
|
struct sample_t hyundai_torque_driver; // last few driver torques measured
|
||||||
|
|
||||||
|
@ -56,11 +54,11 @@ static int hyundai_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
|
||||||
|
|
||||||
// exit controls on rising edge of gas press
|
// exit controls on rising edge of gas press
|
||||||
if (addr == 608) {
|
if (addr == 608) {
|
||||||
bool gas = (GET_BYTE(to_push, 7) >> 6) != 0;
|
bool gas_pressed = (GET_BYTE(to_push, 7) >> 6) != 0;
|
||||||
if (gas && ! hyundai_gas_last) {
|
if (gas_pressed && !gas_pressed_prev) {
|
||||||
controls_allowed = 0;
|
controls_allowed = 0;
|
||||||
}
|
}
|
||||||
hyundai_gas_last = gas;
|
gas_pressed_prev = gas_pressed;
|
||||||
}
|
}
|
||||||
|
|
||||||
// sample subaru wheel speed, averaging opposite corners
|
// sample subaru wheel speed, averaging opposite corners
|
||||||
|
@ -72,11 +70,11 @@ static int hyundai_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
|
||||||
|
|
||||||
// exit controls on rising edge of brake press
|
// exit controls on rising edge of brake press
|
||||||
if (addr == 916) {
|
if (addr == 916) {
|
||||||
bool brake = (GET_BYTE(to_push, 6) >> 7) != 0;
|
bool brake_pressed = (GET_BYTE(to_push, 6) >> 7) != 0;
|
||||||
if (brake && (!hyundai_brake_last || (hyundai_speed > HYUNDAI_STANDSTILL_THRSLD))) {
|
if (brake_pressed && (!brake_pressed_prev || (hyundai_speed > HYUNDAI_STANDSTILL_THRSLD))) {
|
||||||
controls_allowed = 0;
|
controls_allowed = 0;
|
||||||
}
|
}
|
||||||
hyundai_brake_last = brake;
|
brake_pressed_prev = brake_pressed;
|
||||||
}
|
}
|
||||||
|
|
||||||
// check if stock camera ECU is on bus 0
|
// check if stock camera ECU is on bus 0
|
||||||
|
|
|
@ -29,8 +29,6 @@ float nissan_speed = 0;
|
||||||
uint32_t nissan_ts_angle_last = 0;
|
uint32_t nissan_ts_angle_last = 0;
|
||||||
int nissan_cruise_engaged_last = 0;
|
int nissan_cruise_engaged_last = 0;
|
||||||
int nissan_desired_angle_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
|
struct sample_t nissan_angle_meas; // last 3 steer angles
|
||||||
|
|
||||||
|
@ -64,11 +62,11 @@ static int nissan_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
|
||||||
|
|
||||||
// exit controls on rising edge of gas press
|
// exit controls on rising edge of gas press
|
||||||
if (addr == 0x15c) {
|
if (addr == 0x15c) {
|
||||||
int gas = ((GET_BYTE(to_push, 5) << 2) | ((GET_BYTE(to_push, 6) >> 6) & 0x3));
|
bool gas_pressed = ((GET_BYTE(to_push, 5) << 2) | ((GET_BYTE(to_push, 6) >> 6) & 0x3));
|
||||||
if ((gas > 0) && (nissan_gas_prev == 0)) {
|
if (gas_pressed && !gas_pressed_prev) {
|
||||||
controls_allowed = 0;
|
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
|
// 0x169 is lkas cmd. If it is on bus 0, then relay is unexpectedly closed
|
||||||
|
@ -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
|
// exit controls on rising edge of brake press, or if speed > 0 and brake
|
||||||
if (addr == 0x454) {
|
if (addr == 0x454) {
|
||||||
int brake = (GET_BYTE(to_push, 2) & 0x80);
|
bool brake_pressed = (GET_BYTE(to_push, 2) & 0x80) != 0;
|
||||||
if ((brake > 0) && ((nissan_brake_prev == 0) || (nissan_speed > 0.))) {
|
if (brake_pressed && (!brake_pressed_prev || (nissan_speed > 0.))) {
|
||||||
controls_allowed = 0;
|
controls_allowed = 0;
|
||||||
}
|
}
|
||||||
nissan_brake_prev = brake;
|
brake_pressed_prev = brake_pressed;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
@ -35,8 +35,6 @@ int subaru_rt_torque_last = 0;
|
||||||
int subaru_desired_torque_last = 0;
|
int subaru_desired_torque_last = 0;
|
||||||
int subaru_speed = 0;
|
int subaru_speed = 0;
|
||||||
uint32_t subaru_ts_last = 0;
|
uint32_t subaru_ts_last = 0;
|
||||||
bool subaru_gas_last = false;
|
|
||||||
bool subaru_brake_last = false;
|
|
||||||
bool subaru_global = false;
|
bool subaru_global = false;
|
||||||
struct sample_t subaru_torque_driver; // last few driver torques measured
|
struct sample_t subaru_torque_driver; // last few driver torques measured
|
||||||
|
|
||||||
|
@ -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)
|
// exit controls on rising edge of brake press (TODO: missing check for unsupported legacy models)
|
||||||
if ((addr == 0x139) && subaru_global) {
|
if ((addr == 0x139) && subaru_global) {
|
||||||
bool brake = (GET_BYTES_48(to_push) & 0xFFF0) > 0;
|
bool brake_pressed = (GET_BYTES_48(to_push) & 0xFFF0) > 0;
|
||||||
if (brake && (!subaru_brake_last || (subaru_speed > SUBARU_STANDSTILL_THRSLD))) {
|
if (brake_pressed && (!brake_pressed_prev || (subaru_speed > SUBARU_STANDSTILL_THRSLD))) {
|
||||||
controls_allowed = 0;
|
controls_allowed = 0;
|
||||||
}
|
}
|
||||||
subaru_brake_last = brake;
|
brake_pressed_prev = brake_pressed;
|
||||||
}
|
}
|
||||||
|
|
||||||
// exit controls on rising edge of gas press
|
// exit controls on rising edge of gas press
|
||||||
if (((addr == 0x40) && subaru_global) ||
|
if (((addr == 0x40) && subaru_global) ||
|
||||||
((addr == 0x140) && !subaru_global)) {
|
((addr == 0x140) && !subaru_global)) {
|
||||||
int byte = subaru_global ? 4 : 0;
|
int byte = subaru_global ? 4 : 0;
|
||||||
bool gas = GET_BYTE(to_push, byte) != 0;
|
bool gas_pressed = GET_BYTE(to_push, byte) != 0;
|
||||||
if (gas && !subaru_gas_last) {
|
if (gas_pressed && !gas_pressed_prev) {
|
||||||
controls_allowed = 0;
|
controls_allowed = 0;
|
||||||
}
|
}
|
||||||
subaru_gas_last = gas;
|
gas_pressed_prev = gas_pressed;
|
||||||
}
|
}
|
||||||
|
|
||||||
if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) &&
|
if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) &&
|
||||||
|
|
|
@ -34,8 +34,6 @@ float tesla_ts_angle_last = 0;
|
||||||
|
|
||||||
int tesla_controls_allowed_last = 0;
|
int tesla_controls_allowed_last = 0;
|
||||||
|
|
||||||
int tesla_brake_prev = 0;
|
|
||||||
int tesla_gas_prev = 0;
|
|
||||||
int tesla_speed = 0;
|
int tesla_speed = 0;
|
||||||
int eac_status = 0;
|
int eac_status = 0;
|
||||||
|
|
||||||
|
|
|
@ -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
|
int toyota_rt_torque_last = 0; // last desired torque for real time check
|
||||||
uint32_t toyota_ts_last = 0;
|
uint32_t toyota_ts_last = 0;
|
||||||
int toyota_cruise_engaged_last = 0; // cruise state
|
int toyota_cruise_engaged_last = 0; // cruise state
|
||||||
bool toyota_gas_prev = false;
|
|
||||||
bool toyota_brake_prev = false;
|
|
||||||
bool toyota_moving = false;
|
bool toyota_moving = false;
|
||||||
struct sample_t toyota_torque_meas; // last 3 motor torques produced by the eps
|
struct sample_t toyota_torque_meas; // last 3 motor torques produced by the eps
|
||||||
|
|
||||||
|
@ -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
|
// most cars have brake_pressed on 0x226, corolla and rav4 on 0x224
|
||||||
if ((addr == 0x224) || (addr == 0x226)) {
|
if ((addr == 0x224) || (addr == 0x226)) {
|
||||||
int byte = (addr == 0x224) ? 0 : 4;
|
int byte = (addr == 0x224) ? 0 : 4;
|
||||||
bool brake = ((GET_BYTE(to_push, byte) >> 5) & 1) != 0;
|
bool brake_pressed = ((GET_BYTE(to_push, byte) >> 5) & 1) != 0;
|
||||||
if (brake && (!toyota_brake_prev || toyota_moving)) {
|
if (brake_pressed && (!brake_pressed_prev || toyota_moving)) {
|
||||||
controls_allowed = 0;
|
controls_allowed = 0;
|
||||||
}
|
}
|
||||||
toyota_brake_prev = brake;
|
brake_pressed_prev = brake_pressed;
|
||||||
}
|
}
|
||||||
|
|
||||||
// exit controls on rising edge of interceptor gas press
|
// exit controls on rising edge of interceptor gas press
|
||||||
|
@ -132,11 +130,11 @@ static int toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
|
||||||
|
|
||||||
// exit controls on rising edge of gas press
|
// exit controls on rising edge of gas press
|
||||||
if (addr == 0x2C1) {
|
if (addr == 0x2C1) {
|
||||||
bool gas = GET_BYTE(to_push, 6) != 0;
|
bool gas_pressed = GET_BYTE(to_push, 6) != 0;
|
||||||
if (gas && !toyota_gas_prev && !gas_interceptor_detected) {
|
if (gas_pressed && !gas_pressed_prev && !gas_interceptor_detected) {
|
||||||
controls_allowed = 0;
|
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
|
// 0x2E4 is lkas cmd. If it is on bus 0, then relay is unexpectedly closed
|
||||||
|
|
|
@ -36,8 +36,6 @@ int volkswagen_rt_torque_last = 0;
|
||||||
int volkswagen_desired_torque_last = 0;
|
int volkswagen_desired_torque_last = 0;
|
||||||
uint32_t volkswagen_ts_last = 0;
|
uint32_t volkswagen_ts_last = 0;
|
||||||
bool volkswagen_moving = false;
|
bool volkswagen_moving = false;
|
||||||
bool volkswagen_brake_pressed_prev = false;
|
|
||||||
int volkswagen_gas_prev = 0;
|
|
||||||
int volkswagen_torque_msg = 0;
|
int volkswagen_torque_msg = 0;
|
||||||
int volkswagen_lane_msg = 0;
|
int volkswagen_lane_msg = 0;
|
||||||
uint8_t volkswagen_crc8_lut_8h2f[256]; // Static lookup table for CRC8 poly 0x2F, aka 8H2F/AUTOSAR
|
uint8_t volkswagen_crc8_lut_8h2f[256]; // Static lookup table for CRC8 poly 0x2F, aka 8H2F/AUTOSAR
|
||||||
|
@ -137,21 +135,21 @@ static int volkswagen_mqb_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
|
||||||
// Exit controls on rising edge of gas press
|
// Exit controls on rising edge of gas press
|
||||||
// Signal: Motor_20.MO_Fahrpedalrohwert_01
|
// Signal: Motor_20.MO_Fahrpedalrohwert_01
|
||||||
if (addr == MSG_MOTOR_20) {
|
if (addr == MSG_MOTOR_20) {
|
||||||
int gas = (GET_BYTES_04(to_push) >> 12) & 0xFF;
|
bool gas_pressed = ((GET_BYTES_04(to_push) >> 12) & 0xFF) != 0;
|
||||||
if ((gas > 0) && (volkswagen_gas_prev == 0)) {
|
if (gas_pressed && !gas_pressed_prev) {
|
||||||
controls_allowed = 0;
|
controls_allowed = 0;
|
||||||
}
|
}
|
||||||
volkswagen_gas_prev = gas;
|
gas_pressed_prev = gas_pressed;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Exit controls on rising edge of brake press
|
// Exit controls on rising edge of brake press
|
||||||
// Signal: ESP_05.ESP_Fahrer_bremst
|
// Signal: ESP_05.ESP_Fahrer_bremst
|
||||||
if (addr == MSG_ESP_05) {
|
if (addr == MSG_ESP_05) {
|
||||||
bool brake_pressed = (GET_BYTE(to_push, 3) & 0x4) >> 2;
|
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;
|
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
|
// If there are HCA messages on bus 0 not sent by OP, there's a relay problem
|
||||||
|
|
|
@ -85,6 +85,8 @@ bool controls_allowed = false;
|
||||||
bool relay_malfunction = false;
|
bool relay_malfunction = false;
|
||||||
bool gas_interceptor_detected = false;
|
bool gas_interceptor_detected = false;
|
||||||
int gas_interceptor_prev = 0;
|
int gas_interceptor_prev = 0;
|
||||||
|
bool gas_pressed_prev = false;
|
||||||
|
bool brake_pressed_prev = false;
|
||||||
|
|
||||||
// time since safety mode has been changed
|
// time since safety mode has been changed
|
||||||
uint32_t safety_mode_cnt = 0U;
|
uint32_t safety_mode_cnt = 0U;
|
||||||
|
|
|
@ -37,6 +37,8 @@ bool get_relay_malfunction(void);
|
||||||
void set_gas_interceptor_detected(bool c);
|
void set_gas_interceptor_detected(bool c);
|
||||||
bool get_gas_interceptor_detetcted(void);
|
bool get_gas_interceptor_detetcted(void);
|
||||||
int get_gas_interceptor_prev(void);
|
int get_gas_interceptor_prev(void);
|
||||||
|
bool get_gas_pressed_prev(void);
|
||||||
|
bool get_brake_pressed_prev(void);
|
||||||
int get_hw_type(void);
|
int get_hw_type(void);
|
||||||
void set_timer(uint32_t t);
|
void set_timer(uint32_t t);
|
||||||
|
|
||||||
|
@ -48,15 +50,12 @@ int set_safety_hooks(uint16_t mode, int16_t param);
|
||||||
void init_tests_toyota(void);
|
void init_tests_toyota(void);
|
||||||
int get_toyota_torque_meas_min(void);
|
int get_toyota_torque_meas_min(void);
|
||||||
int get_toyota_torque_meas_max(void);
|
int get_toyota_torque_meas_max(void);
|
||||||
bool get_toyota_gas_prev(void);
|
|
||||||
void set_toyota_torque_meas(int min, int max);
|
void set_toyota_torque_meas(int min, int max);
|
||||||
void set_toyota_desired_torque_last(int t);
|
void set_toyota_desired_torque_last(int t);
|
||||||
void set_toyota_rt_torque_last(int t);
|
void set_toyota_rt_torque_last(int t);
|
||||||
|
|
||||||
void init_tests_honda(void);
|
void init_tests_honda(void);
|
||||||
bool get_honda_moving(void);
|
bool get_honda_moving(void);
|
||||||
bool get_honda_brake_pressed_prev(void);
|
|
||||||
int get_honda_gas_prev(void);
|
|
||||||
void set_honda_fwd_brake(bool);
|
void set_honda_fwd_brake(bool);
|
||||||
void set_honda_alt_brake_msg(bool);
|
void set_honda_alt_brake_msg(bool);
|
||||||
int get_honda_hw(void);
|
int get_honda_hw(void);
|
||||||
|
@ -89,18 +88,15 @@ void set_subaru_rt_torque_last(int t);
|
||||||
bool get_subaru_global(void);
|
bool get_subaru_global(void);
|
||||||
|
|
||||||
void init_tests_volkswagen(void);
|
void init_tests_volkswagen(void);
|
||||||
int get_volkswagen_gas_prev(void);
|
|
||||||
int get_volkswagen_torque_driver_min(void);
|
int get_volkswagen_torque_driver_min(void);
|
||||||
int get_volkswagen_torque_driver_max(void);
|
int get_volkswagen_torque_driver_max(void);
|
||||||
bool get_volkswagen_moving(void);
|
bool get_volkswagen_moving(void);
|
||||||
bool get_volkswagen_brake_pressed_prev(void);
|
|
||||||
void set_volkswagen_desired_torque_last(int t);
|
void set_volkswagen_desired_torque_last(int t);
|
||||||
void set_volkswagen_rt_torque_last(int t);
|
void set_volkswagen_rt_torque_last(int t);
|
||||||
void set_volkswagen_torque_driver(int min, int max);
|
void set_volkswagen_torque_driver(int min, int max);
|
||||||
|
|
||||||
void init_tests_nissan(void);
|
void init_tests_nissan(void);
|
||||||
void set_nissan_desired_angle_last(int t);
|
void set_nissan_desired_angle_last(int t);
|
||||||
void set_nissan_brake_prev(int t);
|
|
||||||
|
|
||||||
""")
|
""")
|
||||||
|
|
||||||
|
|
|
@ -101,6 +101,14 @@ int get_gas_interceptor_prev(void){
|
||||||
return gas_interceptor_prev;
|
return gas_interceptor_prev;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
bool get_gas_pressed_prev(void){
|
||||||
|
return gas_pressed_prev;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool get_brake_pressed_prev(void){
|
||||||
|
return brake_pressed_prev;
|
||||||
|
}
|
||||||
|
|
||||||
int get_hw_type(void){
|
int get_hw_type(void){
|
||||||
return hw_type;
|
return hw_type;
|
||||||
}
|
}
|
||||||
|
@ -159,10 +167,6 @@ int get_chrysler_torque_meas_max(void){
|
||||||
return chrysler_torque_meas.max;
|
return chrysler_torque_meas.max;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool get_toyota_gas_prev(void){
|
|
||||||
return toyota_gas_prev;
|
|
||||||
}
|
|
||||||
|
|
||||||
int get_toyota_torque_meas_min(void){
|
int get_toyota_torque_meas_min(void){
|
||||||
return toyota_torque_meas.min;
|
return toyota_torque_meas.min;
|
||||||
}
|
}
|
||||||
|
@ -231,26 +235,10 @@ int get_volkswagen_moving(void){
|
||||||
return volkswagen_moving;
|
return volkswagen_moving;
|
||||||
}
|
}
|
||||||
|
|
||||||
int get_volkswagen_brake_pressed_prev(void){
|
|
||||||
return volkswagen_brake_pressed_prev;
|
|
||||||
}
|
|
||||||
|
|
||||||
int get_volkswagen_gas_prev(void){
|
|
||||||
return volkswagen_gas_prev;
|
|
||||||
}
|
|
||||||
|
|
||||||
bool get_honda_moving(void){
|
bool get_honda_moving(void){
|
||||||
return honda_moving;
|
return honda_moving;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool get_honda_brake_pressed_prev(void){
|
|
||||||
return honda_brake_pressed_prev;
|
|
||||||
}
|
|
||||||
|
|
||||||
int get_honda_gas_prev(void){
|
|
||||||
return honda_gas_prev;
|
|
||||||
}
|
|
||||||
|
|
||||||
void set_honda_alt_brake_msg(bool c){
|
void set_honda_alt_brake_msg(bool c){
|
||||||
honda_alt_brake_msg = c;
|
honda_alt_brake_msg = c;
|
||||||
}
|
}
|
||||||
|
@ -263,10 +251,6 @@ void set_honda_fwd_brake(bool c){
|
||||||
honda_fwd_brake = c;
|
honda_fwd_brake = c;
|
||||||
}
|
}
|
||||||
|
|
||||||
void set_nissan_brake_prev(bool c){
|
|
||||||
nissan_brake_prev = c;
|
|
||||||
}
|
|
||||||
|
|
||||||
void set_nissan_desired_angle_last(int t){
|
void set_nissan_desired_angle_last(int t){
|
||||||
nissan_desired_angle_last = t;
|
nissan_desired_angle_last = t;
|
||||||
}
|
}
|
||||||
|
@ -275,6 +259,8 @@ void init_tests(void){
|
||||||
// get HW_TYPE from env variable set in test.sh
|
// get HW_TYPE from env variable set in test.sh
|
||||||
hw_type = atoi(getenv("HW_TYPE"));
|
hw_type = atoi(getenv("HW_TYPE"));
|
||||||
safety_mode_cnt = 2U; // avoid ignoring relay_malfunction logic
|
safety_mode_cnt = 2U; // avoid ignoring relay_malfunction logic
|
||||||
|
gas_pressed_prev = false;
|
||||||
|
brake_pressed_prev = false;
|
||||||
}
|
}
|
||||||
|
|
||||||
void init_tests_toyota(void){
|
void init_tests_toyota(void){
|
||||||
|
@ -319,7 +305,6 @@ void init_tests_hyundai(void){
|
||||||
|
|
||||||
void init_tests_chrysler(void){
|
void init_tests_chrysler(void){
|
||||||
init_tests();
|
init_tests();
|
||||||
chrysler_gas_prev = false;
|
|
||||||
chrysler_speed = 0;
|
chrysler_speed = 0;
|
||||||
chrysler_torque_meas.min = 0;
|
chrysler_torque_meas.min = 0;
|
||||||
chrysler_torque_meas.max = 0;
|
chrysler_torque_meas.max = 0;
|
||||||
|
@ -342,8 +327,6 @@ void init_tests_subaru(void){
|
||||||
void init_tests_volkswagen(void){
|
void init_tests_volkswagen(void){
|
||||||
init_tests();
|
init_tests();
|
||||||
volkswagen_moving = false;
|
volkswagen_moving = false;
|
||||||
volkswagen_brake_pressed_prev = false;
|
|
||||||
volkswagen_gas_prev = 0;
|
|
||||||
volkswagen_torque_driver.min = 0;
|
volkswagen_torque_driver.min = 0;
|
||||||
volkswagen_torque_driver.max = 0;
|
volkswagen_torque_driver.max = 0;
|
||||||
volkswagen_desired_torque_last = 0;
|
volkswagen_desired_torque_last = 0;
|
||||||
|
@ -355,8 +338,6 @@ void init_tests_volkswagen(void){
|
||||||
void init_tests_honda(void){
|
void init_tests_honda(void){
|
||||||
init_tests();
|
init_tests();
|
||||||
honda_moving = false;
|
honda_moving = false;
|
||||||
honda_brake_pressed_prev = false;
|
|
||||||
honda_gas_prev = 0;
|
|
||||||
honda_fwd_brake = false;
|
honda_fwd_brake = false;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -365,8 +346,6 @@ void init_tests_nissan(void){
|
||||||
nissan_angle_meas.min = 0;
|
nissan_angle_meas.min = 0;
|
||||||
nissan_angle_meas.max = 0;
|
nissan_angle_meas.max = 0;
|
||||||
nissan_desired_angle_last = 0;
|
nissan_desired_angle_last = 0;
|
||||||
nissan_gas_prev = 0;
|
|
||||||
nissan_brake_prev = 0;
|
|
||||||
set_timer(0);
|
set_timer(0);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -145,9 +145,9 @@ class TestHondaSafety(unittest.TestCase):
|
||||||
self.assertEqual(1, self.safety.get_honda_moving())
|
self.assertEqual(1, self.safety.get_honda_moving())
|
||||||
|
|
||||||
def test_prev_brake(self):
|
def test_prev_brake(self):
|
||||||
self.assertFalse(self.safety.get_honda_brake_pressed_prev())
|
self.assertFalse(self.safety.get_brake_pressed_prev())
|
||||||
self.safety.safety_rx_hook(self._brake_msg(True))
|
self.safety.safety_rx_hook(self._brake_msg(True))
|
||||||
self.assertTrue(self.safety.get_honda_brake_pressed_prev())
|
self.assertTrue(self.safety.get_brake_pressed_prev())
|
||||||
|
|
||||||
def test_disengage_on_brake(self):
|
def test_disengage_on_brake(self):
|
||||||
self.safety.set_controls_allowed(1)
|
self.safety.set_controls_allowed(1)
|
||||||
|
@ -171,9 +171,9 @@ class TestHondaSafety(unittest.TestCase):
|
||||||
|
|
||||||
def test_prev_gas(self):
|
def test_prev_gas(self):
|
||||||
self.safety.safety_rx_hook(self._gas_msg(False))
|
self.safety.safety_rx_hook(self._gas_msg(False))
|
||||||
self.assertFalse(self.safety.get_honda_gas_prev())
|
self.assertFalse(self.safety.get_gas_pressed_prev())
|
||||||
self.safety.safety_rx_hook(self._gas_msg(True))
|
self.safety.safety_rx_hook(self._gas_msg(True))
|
||||||
self.assertTrue(self.safety.get_honda_gas_prev())
|
self.assertTrue(self.safety.get_gas_pressed_prev())
|
||||||
|
|
||||||
def test_prev_gas_interceptor(self):
|
def test_prev_gas_interceptor(self):
|
||||||
self.safety.safety_rx_hook(self._send_interceptor_msg(0x0, 0x201))
|
self.safety.safety_rx_hook(self._send_interceptor_msg(0x0, 0x201))
|
||||||
|
|
|
@ -43,10 +43,6 @@ class TestNissanSafety(unittest.TestCase):
|
||||||
t = int(t * -100)
|
t = int(t * -100)
|
||||||
self.safety.set_nissan_desired_angle_last(t)
|
self.safety.set_nissan_desired_angle_last(t)
|
||||||
|
|
||||||
def _set_brake_prev(self, state):
|
|
||||||
state = bool(state)
|
|
||||||
self.safety.set_nissan_brake_prev(state)
|
|
||||||
|
|
||||||
def _angle_meas_msg_array(self, angle):
|
def _angle_meas_msg_array(self, angle):
|
||||||
for i in range(6):
|
for i in range(6):
|
||||||
self.safety.safety_rx_hook(self._angle_meas_msg(angle))
|
self.safety.safety_rx_hook(self._angle_meas_msg(angle))
|
||||||
|
|
|
@ -137,7 +137,7 @@ class TestToyotaSafety(unittest.TestCase):
|
||||||
def test_prev_gas(self):
|
def test_prev_gas(self):
|
||||||
for g in range(0, 256):
|
for g in range(0, 256):
|
||||||
self.safety.safety_rx_hook(self._send_gas_msg(g))
|
self.safety.safety_rx_hook(self._send_gas_msg(g))
|
||||||
self.assertEqual(True if g > 0 else False, self.safety.get_toyota_gas_prev())
|
self.assertEqual(True if g > 0 else False, self.safety.get_gas_pressed_prev())
|
||||||
|
|
||||||
def test_prev_gas_interceptor(self):
|
def test_prev_gas_interceptor(self):
|
||||||
self.safety.safety_rx_hook(self._send_interceptor_msg(0x0, 0x201))
|
self.safety.safety_rx_hook(self._send_interceptor_msg(0x0, 0x201))
|
||||||
|
|
|
@ -154,7 +154,7 @@ class TestVolkswagenMqbSafety(unittest.TestCase):
|
||||||
def test_prev_gas(self):
|
def test_prev_gas(self):
|
||||||
for g in range(0, 256):
|
for g in range(0, 256):
|
||||||
self.safety.safety_rx_hook(self._motor_20_msg(g))
|
self.safety.safety_rx_hook(self._motor_20_msg(g))
|
||||||
self.assertEqual(g, self.safety.get_volkswagen_gas_prev())
|
self.assertEqual(True if g > 0 else False, self.safety.get_gas_pressed_prev())
|
||||||
|
|
||||||
def test_default_controls_not_allowed(self):
|
def test_default_controls_not_allowed(self):
|
||||||
self.assertFalse(self.safety.get_controls_allowed())
|
self.assertFalse(self.safety.get_controls_allowed())
|
||||||
|
@ -184,9 +184,9 @@ class TestVolkswagenMqbSafety(unittest.TestCase):
|
||||||
self.assertEqual(1, self.safety.get_volkswagen_moving())
|
self.assertEqual(1, self.safety.get_volkswagen_moving())
|
||||||
|
|
||||||
def test_prev_brake(self):
|
def test_prev_brake(self):
|
||||||
self.assertFalse(self.safety.get_volkswagen_brake_pressed_prev())
|
self.assertFalse(self.safety.get_brake_pressed_prev())
|
||||||
self.safety.safety_rx_hook(self._brake_msg(True))
|
self.safety.safety_rx_hook(self._brake_msg(True))
|
||||||
self.assertTrue(self.safety.get_volkswagen_brake_pressed_prev())
|
self.assertTrue(self.safety.get_brake_pressed_prev())
|
||||||
|
|
||||||
def test_brake_disengage(self):
|
def test_brake_disengage(self):
|
||||||
StdTest.test_allow_brake_at_zero_speed(self)
|
StdTest.test_allow_brake_at_zero_speed(self)
|
||||||
|
|
Loading…
Reference in New Issue