Merged GlockeNessMnster's changes. There's been a change in the

signature of the lateral-control update() methods, and I added
the actuators argument while keeping the CI argument, even though
CI is effectively ignored. Just hoping this works; removing the
unused argument can come later.

Squashed commit of the following:

f010902 Discrete PIDFF + BackFeed
12bb1b4 Reorg and PID_Tune 3071
38c5c4e Bump submodules
f0a33d8 update Uploader to move immediates as high instead
c8d309d subauru-community
101b3ce subaru-community submodules

RELEASES_SUBARU.md

@@ -1,6 +1,7 @@
 2021-12-16
 ==========
 * Merge upstream (0.8.12) / @martinl
+* Merge upstream (master) / @budney
 
 2021-12-01
 ==========

release/files_common

@@ -588,6 +588,7 @@ opendbc/subaru_global_2020_hybrid_generated.dbc
 opendbc/subaru_outback_2015_generated.dbc
 opendbc/subaru_outback_2019_generated.dbc
 opendbc/subaru_forester_2017_generated.dbc
+opendbc/subaru_wrx_2018_generated.dbc
 
 opendbc/toyota_rav4_hybrid_2017_pt_generated.dbc
 opendbc/toyota_rav4_2017_pt_generated.dbc

selfdrive/car/chrysler/carcontroller.py

@@ -17,6 +17,9 @@ def __init__(self, dbc_name, CP, VM):
 
     self.packer = CANPacker(dbc_name)
 
+  def get_last_output(self):
+    return self.apply_steer_last / CarControllerParams.STEER_MAX
+
   def update(self, enabled, CS, actuators, pcm_cancel_cmd, hud_alert):
     # this seems needed to avoid steering faults and to force the sync with the EPS counter
     frame = CS.lkas_counter

selfdrive/car/ford/carcontroller.py

@@ -11,6 +11,7 @@
 
 class CarController():
   def __init__(self, dbc_name, CP, VM):
+    self.apply_steer_last = 0.0
     self.packer = CANPacker(dbc_name)
     self.enabled_last = False
     self.main_on_last = False
@@ -25,6 +26,7 @@ def update(self, enabled, CS, frame, actuators, visual_alert, pcm_cancel):
     steer_alert = visual_alert in [VisualAlert.steerRequired, VisualAlert.ldw]
 
     apply_steer = actuators.steer
+    self.apply_steer_last = apply_steer
 
     if pcm_cancel:
       #print "CANCELING!!!!"

selfdrive/car/gm/carcontroller.py

@@ -27,6 +27,9 @@ def __init__(self, dbc_name, CP, VM):
     self.packer_obj = CANPacker(DBC[CP.carFingerprint]['radar'])
     self.packer_ch = CANPacker(DBC[CP.carFingerprint]['chassis'])
 
+  def get_last_output(self):
+    return self.apply_steer_last / self.params.STEER_MAX
+
   def update(self, enabled, CS, frame, actuators,
              hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert):
 

selfdrive/car/honda/carcontroller.py

@@ -98,6 +98,7 @@ def process_hud_alert(hud_alert):
 
 class CarController():
   def __init__(self, dbc_name, CP, VM):
+    self.apply_steer_last = 0.
     self.braking = False
     self.brake_steady = 0.
     self.brake_last = 0.
@@ -111,7 +112,10 @@ def __init__(self, dbc_name, CP, VM):
     self.brake = 0
 
     self.params = CarControllerParams(CP)
-
+
+  def get_last_output(self):
+    return -self.apply_steer_last / self.params.STEER_MAX
+
   def update(self, enabled, active, CS, frame, actuators, pcm_cancel_cmd,
              hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert):
 
@@ -154,6 +158,8 @@ def update(self, enabled, active, CS, frame, actuators, pcm_cancel_cmd,
 
     lkas_active = enabled and not CS.steer_not_allowed
 
+    self.appy_steer_last = apply_steer
+
     # Send CAN commands.
     can_sends = []
 

selfdrive/car/hyundai/carcontroller.py

@@ -46,6 +46,9 @@ def __init__(self, dbc_name, CP, VM):
     self.last_resume_frame = 0
     self.accel = 0
 
+  def get_last_output(self):
+    return self.apply_steer_last / self.p.STEER_MAX
+
   def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, hud_speed,
              left_lane, right_lane, left_lane_depart, right_lane_depart):
     # Steering Torque

selfdrive/car/interfaces.py

@@ -84,6 +84,9 @@ def get_std_params(candidate, fingerprint):
     ret.minSteerSpeed = 0.
     ret.wheelSpeedFactor = 1.0
 
+    ret.lateralTuning.pid.kdBP = [0.]
+    ret.lateralTuning.pid.kdV = [0.]
+
     ret.pcmCruise = True     # openpilot's state is tied to the PCM's cruise state on most cars
     ret.minEnableSpeed = -1. # enable is done by stock ACC, so ignore this
     ret.steerRatioRear = 0.  # no rear steering, at least on the listed cars aboveA
@@ -105,6 +108,12 @@ def get_std_params(candidate, fingerprint):
     ret.longitudinalActuatorDelayUpperBound = 0.15
     return ret
 
+  def calc_last_outputs(self, request):
+    lat_out = request
+    if hasattr(self.CC, 'get_last_output'):
+      lat_out = self.CC.get_last_output()
+    return lat_out
+
   @abstractmethod
   def update(self, c: car.CarControl, can_strings: List[bytes]) -> car.CarState:
     pass

selfdrive/car/mazda/carcontroller.py

@@ -13,6 +13,9 @@ def __init__(self, dbc_name, CP, VM):
     self.steer_rate_limited = False
     self.brake_counter = 0
 
+  def get_last_output(self):
+    return self.apply_steer_last / CarControllerParams.STEER_MAX
+
   def update(self, c, CS, frame):
     can_sends = []
 

selfdrive/car/subaru/carcontroller.py

@@ -26,6 +26,9 @@ def __init__(self, dbc_name, CP, VM):
     self.p = CarControllerParams(CP)
     self.packer = CANPacker(DBC[CP.carFingerprint]['pt'])
 
+  def get_last_output(self):
+    return self.apply_steer_last / self.p.STEER_MAX
+
   def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, left_line, right_line, left_lane_depart, right_lane_depart):
 
     can_sends = []

selfdrive/car/subaru/interface.py

@@ -52,7 +52,20 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None):
       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.03]]
 
+    if candidate == CAR.IMPREZA_3071:
+      ret.safetyConfigs[0].safetyParam = 2 # increase limit on some crosstrek / impreza
+      ret.mass = 1410. + STD_CARGO_KG
+      ret.wheelbase = 2.67
+      ret.centerToFront = ret.wheelbase * 0.5
+      ret.steerRatio = 13 # learns 15-16 13-17 stock?
+      ret.steerActuatorDelay = 0.0  # torque apply delay (.2 + .2 offset)
+      ret.lateralTuning.pid.kf = 0.00018
+      ret.lateralTuning.pid.kpBP, ret.lateralTuning.pid.kpV = [[9., 32.], [0.2, 0.6]]
+      ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kiV = [[9., 32.], [0.1, 0.3]]
+      ret.lateralTuning.pid.kdBP, ret.lateralTuning.pid.kdV = [[9., 32.], [0.04 , 0.12]]
+
     if candidate == CAR.IMPREZA_2020:
+      ret.safetyConfigs[0].safetyParam = 1 # lower max_steer for 2020
       ret.mass = 1480. + STD_CARGO_KG
       ret.wheelbase = 2.67
       ret.centerToFront = ret.wheelbase * 0.5
@@ -74,6 +87,19 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None):
       ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2, 0.3], [0.02, 0.03]]
 
     if candidate == CAR.FORESTER:
+      ret.mass = 1568. + STD_CARGO_KG
+      ret.wheelbase = 2.67
+      ret.centerToFront = ret.wheelbase * 0.5
+      ret.steerRatio = 17           # learns 17, 15-19 stock?
+      ret.steerActuatorDelay = 0.0
+      ret.lateralTuning.pid.kf = 0.000045
+      ret.lateralTuning.pid.kpBP, ret.lateralTuning.pid.kpV = [[9., 32.], [0.1, 0.3]]
+      ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kiV = [[9., 32.], [0.1, 0.3]]
+      ret.lateralTuning.pid.kdBP, ret.lateralTuning.pid.kdV = [[9., 32.], [0.02, 0.06]]
+      #ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0., 14., 23.], [0., 14., 23.]]
+      #ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.01, 0.065, 0.2], [0.001, 0.015, 0.025]]
+
+    if candidate == CAR.OUTBACK:
       ret.mass = 1568. + STD_CARGO_KG
       ret.wheelbase = 2.67
       ret.centerToFront = ret.wheelbase * 0.5

selfdrive/car/subaru/values.py

@@ -6,22 +6,26 @@
 
 class CarControllerParams:
   def __init__(self, CP):
-    if CP.carFingerprint == CAR.IMPREZA_2020:
-      self.STEER_MAX = 1439
-    else:
-      self.STEER_MAX = 2047
     self.STEER_STEP = 2                # how often we update the steer cmd
-    self.STEER_DELTA_UP = 50           # torque increase per refresh, 0.8s to max
-    self.STEER_DELTA_DOWN = 70         # torque decrease per refresh
+    self.STEER_MAX = 2047
+    self.STEER_DELTA_UP = 30           # torque increase per refresh, 0.8s to max
+    self.STEER_DELTA_DOWN = 30         # torque decrease per refresh
     self.STEER_DRIVER_ALLOWANCE = 60   # allowed driver torque before start limiting
     self.STEER_DRIVER_MULTIPLIER = 10  # weight driver torque heavily
     self.STEER_DRIVER_FACTOR = 1       # from dbc
+    if CP.carFingerprint == CAR.IMPREZA_2020:
+      self.STEER_MAX = 1439
+    if CP.carFingerprint == CAR.IMPREZA_3071:
+      self.STEER_MAX = 3047
+      self.STEER_DELTA_UP = 40
+      self.STEER_DELTA_DOWN = 60
 
 class CAR:
   ASCENT = "SUBARU ASCENT LIMITED 2019"
   IMPREZA = "SUBARU IMPREZA LIMITED 2019"
   IMPREZA_2020 = "SUBARU IMPREZA SPORT 2020"
   CROSSTREK_2020H = "SUBARU CROSSTREK LIMITED 2020 HYBRID"
+  IMPREZA_3071 = "SUBARU IMPREZA/CROSSTREK"
   FORESTER = "SUBARU FORESTER 2019"
   FORESTER_PREGLOBAL = "SUBARU FORESTER 2017 - 2018"
   LEGACY_PREGLOBAL = "SUBARU LEGACY 2015 - 2017"
@@ -104,7 +108,6 @@ class CAR:
       b'\x7a\xc0\x0c\x00',
       b'z\xc0\b\x00',
       b'\x8a\xc0\x00\x00',
-      b'z\xc0\x04\x00',
       b'z\xc0\x00\x00',
       b'\x8a\xc0\x10\x00',
     ],
@@ -243,6 +246,59 @@ class CAR:
       b'\032\xf6B`\000'
     ],
   },
+  CAR.IMPREZA_3071: {
+    (Ecu.esp, 0x7b0, None): [
+      b'\x7a\x94\x3f\x90\x00',
+      b'\xa2 \x185\x00',
+      b'\xa2 \x193\x00',
+      b'z\x94.\x90\x00',
+      b'z\x94\b\x90\x01',
+      b'\xa2 \x19`\x00',
+      b'z\x94\f\x90\001',
+    ],
+    (Ecu.eps, 0x746, None): [
+      b'z\xc0\x04\x00',
+    ],
+    (Ecu.fwdCamera, 0x787, None): [
+      b'\x00\x00d\xb5\x1f@ \x0e',
+      b'\x00\x00d\xdc\x1f@ \x0e',
+      b'\x00\x00e\x1c\x1f@ \x14',
+      b'\x00\x00d)\x1f@ \a',
+      b'\x00\x00e+\x1f@ \x14',
+      b'\000\000e+\000\000\000\000',
+      b'\000\000dd\037@ \016',
+      b'\000\000e\002\037@ \024',
+    ],
+    (Ecu.engine, 0x7e0, None): [
+      b'\xaa\x61\x66\x73\x07',
+      b'\xbeacr\a',
+      b'\xc5!`r\a',
+      b'\xaa!ds\a',
+      b'\xaa!`u\a',
+      b'\xaa!dq\a',
+      b'\xaa!dt\a',
+      b'\xc5!dr\a',
+      b'\xc5!ar\a',
+      b'\xbe!as\a',
+      b'\xc5!ds\a',
+      b'\xc5!`s\a',
+      b'\xaa!au\a',
+      b'\xbe!at\a',
+    ],
+    (Ecu.transmission, 0x7e1, None): [
+      b'\xe3\xe5\x46\x31\x00',
+      b'\xe4\xe5\x061\x00',
+      b'\xe5\xf5\x04\x00\x00',
+      b'\xe3\xf5G\x00\x00',
+      b'\xe3\xf5\a\x00\x00',
+      b'\xe3\xf5C\x00\x00',
+      b'\xe5\xf5B\x00\x00',
+      b'\xe5\xf5$\000\000',
+      b'\xe4\xf5\a\000\000',
+      b'\xe3\xf5F\000\000',
+      b'\xe4\xf5\002\000\000',
+    ],
+  },
   CAR.FORESTER_PREGLOBAL: {
     # 2018 Forester 2.5i Touring - UDM / @Oreo
     # 2018 Forester 2.5 Limited - Canada / @litobro
@@ -543,6 +599,7 @@ class CAR:
   CAR.IMPREZA: 80,
   CAR.IMPREZA_2020: 80,
   CAR.CROSSTREK_2020H: 80,
+  CAR.IMPREZA_3071: 80,
   CAR.FORESTER: 80,
   CAR.FORESTER_PREGLOBAL: 75,
   CAR.LEGACY_PREGLOBAL: 75,
@@ -559,6 +616,7 @@ class CAR:
   CAR.IMPREZA: dbc_dict('subaru_global_2017_generated', None),
   CAR.IMPREZA_2020: dbc_dict('subaru_global_2017_generated', None),
   CAR.CROSSTREK_2020H: dbc_dict('subaru_global_2020_hybrid_generated', None),
+  CAR.IMPREZA_3071: dbc_dict('subaru_global_2017_generated', None),
   CAR.FORESTER: dbc_dict('subaru_global_2017_generated', None),
   CAR.FORESTER_PREGLOBAL: dbc_dict('subaru_forester_2017_generated', None),
   CAR.LEGACY_PREGLOBAL: dbc_dict('subaru_outback_2015_generated', None),
@@ -571,4 +629,4 @@ class CAR:
 }
 
 PREGLOBAL_CARS = [CAR.FORESTER_PREGLOBAL, CAR.LEGACY_PREGLOBAL, CAR.LEGACY_PREGLOBAL_2018, CAR.LEVORG_PREGLOBAL, CAR.OUTBACK_PREGLOBAL, CAR.OUTBACK_PREGLOBAL_2018, CAR.WRX_PREGLOBAL]
-GLOBAL_CARS_SNG = [CAR.ASCENT, CAR.IMPREZA, CAR.IMPREZA_2020, CAR.FORESTER]
+GLOBAL_CARS_SNG = [CAR.ASCENT, CAR.IMPREZA, CAR.IMPREZA_2020, CAR.IMPREZA_3071, CAR.FORESTER]

selfdrive/car/toyota/carcontroller.py

@@ -22,6 +22,9 @@ def __init__(self, dbc_name, CP, VM):
     self.gas = 0
     self.accel = 0
 
+  def get_last_output(self):
+    return self.last_steer / CarControllerParams.STEER_MAX
+
   def update(self, enabled, active, CS, frame, actuators, pcm_cancel_cmd, hud_alert,
              left_line, right_line, lead, left_lane_depart, right_lane_depart):
 

selfdrive/car/volkswagen/carcontroller.py

@@ -21,6 +21,9 @@ def __init__(self, dbc_name, CP, VM):
 
     self.steer_rate_limited = False
 
+  def get_last_output(self):
+    return self.apply_steer_last / P.STEER_MAX
+
   def update(self, enabled, CS, frame, ext_bus, actuators, visual_alert, left_lane_visible, right_lane_visible, left_lane_depart, right_lane_depart):
     """ Controls thread """
 

selfdrive/controls/controlsd.py

@@ -511,7 +511,7 @@ def state_control(self, CS):
                                                                              lat_plan.psis,
                                                                              lat_plan.curvatures,
                                                                              lat_plan.curvatureRates)
-      actuators.steer, actuators.steeringAngleDeg, lac_log = self.LaC.update(lat_active, CS, self.CP, self.VM, params, self.last_actuators,
+      actuators.steer, actuators.steeringAngleDeg, lac_log = self.LaC.update(lat_active, CS, self.CP, self.CI, self.VM, params, self.last_actuators,
                                                                              desired_curvature, desired_curvature_rate)
     else:
       lac_log = log.ControlsState.LateralDebugState.new_message()

selfdrive/controls/lib/latcontrol_angle.py

@@ -10,7 +10,7 @@ def __init__(self, CP):
   def reset(self):
     pass
 
-  def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate):
+  def update(self, active, CS, CP, CI, VM, params, last_actuators, desired_curvature, desired_curvature_rate):
     angle_log = log.ControlsState.LateralAngleState.new_message()
 
     if CS.vEgo < 0.3 or not active:

selfdrive/controls/lib/latcontrol_indi.py

@@ -82,7 +82,7 @@ def _check_saturation(self, control, check_saturation, limit):
 
     return self.sat_count > self.sat_limit
 
-  def update(self, active, CS, CP, VM, params, last_actuators, curvature, curvature_rate):
+  def update(self, active, CS, CP, CI, VM, params, last_actuators, curvature, curvature_rate):
     self.speed = CS.vEgo
     # Update Kalman filter
     y = np.array([[math.radians(CS.steeringAngleDeg)], [math.radians(CS.steeringRateDeg)]])

selfdrive/controls/lib/latcontrol_lqr.py

@@ -44,7 +44,7 @@ def _check_saturation(self, control, check_saturation, limit):
 
     return self.sat_count > self.sat_limit
 
-  def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate):
+  def update(self, active, CS, CP, CI, VM, params, last_actuators, desired_curvature, desired_curvature_rate):
     lqr_log = log.ControlsState.LateralLQRState.new_message()
 
     steers_max = get_steer_max(CP, CS.vEgo)