Higher torque + new tuning

Higher torque Update interface.py Try steerActuatorDelay Tuning lower Lower tune Update interface.py Update interface.py Update interface.py
budney · Mar 3, 2022 · 5efe79f · 5efe79f
1 parent b14a1df
commit 5efe79f
Show file tree

Hide file tree

Showing 3 changed files with 30 additions and 12 deletions.
diff --git 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
@@ -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
@@ -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