From 5efe79fc696abdc7cf271459c962095922bc56a1 Mon Sep 17 00:00:00 2001 From: Michael Honan Date: Wed, 15 Dec 2021 08:39:05 +1100 Subject: [PATCH] Higher torque + new tuning Higher torque Update interface.py Try steerActuatorDelay Tuning lower Lower tune Update interface.py Update interface.py Update interface.py --- panda/board/safety/safety_subaru.h | 29 ++++++++++++++++++++++------- selfdrive/car/subaru/interface.py | 11 ++++++----- selfdrive/car/subaru/values.py | 2 ++ 3 files changed, 30 insertions(+), 12 deletions(-) diff --git a/panda/board/safety/safety_subaru.h b/panda/board/safety/safety_subaru.h index 6028034380fb77..abd96067682f38 100644 --- a/panda/board/safety/safety_subaru.h +++ b/panda/board/safety/safety_subaru.h @@ -1,4 +1,5 @@ const int SUBARU_MAX_STEER = 2047; // 1s +const int SUBARU_MAX_STEER_2018 = 3071; // Higher limit for some Impreza/Crosstrek // real time torque limit to prevent controls spamming // the real time limit is 1500/sec const int SUBARU_MAX_RT_DELTA = 940; // max delta torque allowed for real time checks @@ -37,6 +38,9 @@ AddrCheckStruct subaru_l_addr_checks[] = { #define SUBARU_L_ADDR_CHECK_LEN (sizeof(subaru_l_addr_checks) / sizeof(subaru_l_addr_checks[0])) addr_checks subaru_l_rx_checks = {subaru_l_addr_checks, SUBARU_L_ADDR_CHECK_LEN}; +const uint16_t SUBARU_PARAM_MAX_STEER_2018 = 2; +bool subaru_max_steer_2018 = false; + static uint8_t subaru_get_checksum(CANPacket_t *to_push) { return (uint8_t)GET_BYTE(to_push, 0); } @@ -164,13 +168,23 @@ static int subaru_tx_hook(CANPacket_t *to_send) { if (controls_allowed) { - // *** global torque limit check *** - violation |= max_limit_check(desired_torque, SUBARU_MAX_STEER, -SUBARU_MAX_STEER); - - // *** torque rate limit check *** - violation |= driver_limit_check(desired_torque, desired_torque_last, &torque_driver, - SUBARU_MAX_STEER, SUBARU_MAX_RATE_UP, SUBARU_MAX_RATE_DOWN, - SUBARU_DRIVER_TORQUE_ALLOWANCE, SUBARU_DRIVER_TORQUE_FACTOR); + if (subaru_max_steer_2018) { + // *** global torque limit check *** + violation |= max_limit_check(desired_torque, SUBARU_MAX_STEER_2018, -SUBARU_MAX_STEER_2018); + + // *** torque rate limit check *** + violation |= driver_limit_check(desired_torque, desired_torque_last, &torque_driver, + SUBARU_MAX_STEER_2018, SUBARU_MAX_RATE_UP, SUBARU_MAX_RATE_DOWN, + SUBARU_DRIVER_TORQUE_ALLOWANCE, SUBARU_DRIVER_TORQUE_FACTOR); + } else { + // *** global torque limit check *** + violation |= max_limit_check(desired_torque, SUBARU_MAX_STEER, -SUBARU_MAX_STEER); + + // *** torque rate limit check *** + violation |= driver_limit_check(desired_torque, desired_torque_last, &torque_driver, + SUBARU_MAX_STEER, SUBARU_MAX_RATE_UP, SUBARU_MAX_RATE_DOWN, + SUBARU_DRIVER_TORQUE_ALLOWANCE, SUBARU_DRIVER_TORQUE_FACTOR); + } // used next time desired_torque_last = desired_torque; @@ -313,6 +327,7 @@ static const addr_checks* subaru_init(int16_t param) { UNUSED(param); controls_allowed = false; relay_malfunction_reset(); + subaru_max_steer_2018 = GET_FLAG(param, SUBARU_PARAM_MAX_STEER_2018); return &subaru_rx_checks; } diff --git a/selfdrive/car/subaru/interface.py b/selfdrive/car/subaru/interface.py index fc6dec99df7010..2f349e1bbbe8cb 100644 --- a/selfdrive/car/subaru/interface.py +++ b/selfdrive/car/subaru/interface.py @@ -36,14 +36,15 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None): ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.0025, 0.1], [0.00025, 0.01]] if candidate == CAR.IMPREZA: + ret.safetyConfigs[0].safetyParam = 2 # increase limit on some crosstrek / impreza ret.mass = 1568. + STD_CARGO_KG ret.wheelbase = 2.67 ret.centerToFront = ret.wheelbase * 0.5 - ret.steerRatio = 15 - ret.steerActuatorDelay = 0.4 # end-to-end angle controller - ret.lateralTuning.pid.kf = 0.00007843996051470903 - ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0., 20], [0., 20]] - ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2, 0.3], [0.02, 0.025]] + ret.steerRatio = 13 + ret.steerActuatorDelay = 0.18 # end-to-end angle controller + ret.lateralTuning.pid.kf = 0.00005 + ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0., 20.], [0., 20.]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.1, 0.2], [0.01, 0.02]] if candidate == CAR.IMPREZA_2020: ret.mass = 1480. + STD_CARGO_KG diff --git a/selfdrive/car/subaru/values.py b/selfdrive/car/subaru/values.py index f33a5234e6d252..61fdad5a4b9b6e 100644 --- a/selfdrive/car/subaru/values.py +++ b/selfdrive/car/subaru/values.py @@ -6,6 +6,8 @@ class CarControllerParams: def __init__(self, CP): if CP.carFingerprint == CAR.IMPREZA_2020: self.STEER_MAX = 1439 + elif CP.carFingerprint == CAR.IMPREZA: + self.STEER_MAX = 3071 else: self.STEER_MAX = 2047 self.STEER_STEP = 2 # how often we update the steer cmd