Controlsd refactor (#1487)

* it's a class

* more refactor

* remove that

* car interface should create that

* that too

* not a dict

* don't create permanent events every iteration

* break up long lines

* fix honda

* small optimization

* less long lines

* dict is faster

* latcontrol less args

* longcontrol less args

* update profiling script

* few optimizations

* create events together

* clean up

* more clean up

* remove comment

* clean up

* simplify state transition

* more clean up

* update comments
old-commit-hash: 08832ff29d

selfdrive/car/interfaces.py

@@ -4,6 +4,7 @@ from cereal import car
 from common.kalman.simple_kalman import KF1D
 from common.realtime import DT_CTRL
 from selfdrive.car import gen_empty_fingerprint
+from selfdrive.config import Conversions as CV
 from selfdrive.controls.lib.drive_helpers import EventTypes as ET, create_event
 from selfdrive.controls.lib.vehicle_model import VehicleModel
 
@@ -96,6 +97,10 @@ class CarInterfaceBase():
       events.append(create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
     if cs_out.gasPressed:
       events.append(create_event('pedalPressed', [ET.PRE_ENABLE]))
+    if cs_out.stockAeb:
+      events.append(create_event('stockAeb', []))
+    if cs_out.vEgo > 92 * CV.MPH_TO_MS:
+      events.append(create_event('speedTooHigh', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
 
     if cs_out.steerError:
       events.append(create_event('steerUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT]))

selfdrive/controls/lib/latcontrol_indi.py

@@ -62,9 +62,9 @@ class LatControlINDI():
 
     return self.sat_count > self.sat_limit
 
-  def update(self, active, v_ego, angle_steers, angle_steers_rate, eps_torque, steer_override, rate_limited, CP, path_plan):
+  def update(self, active, CS, CP, path_plan):
     # Update Kalman filter
-    y = np.matrix([[math.radians(angle_steers)], [math.radians(angle_steers_rate)]])
+    y = np.matrix([[math.radians(CS.steeringAngle)], [math.radians(CS.steeringRate)]])
     self.x = np.dot(self.A_K, self.x) + np.dot(self.K, y)
 
     indi_log = log.ControlsState.LateralINDIState.new_message()
@@ -72,7 +72,7 @@ class LatControlINDI():
     indi_log.steerRate = math.degrees(self.x[1])
     indi_log.steerAccel = math.degrees(self.x[2])
 
-    if v_ego < 0.3 or not active:
+    if CS.vEgo < 0.3 or not active:
       indi_log.active = False
       self.output_steer = 0.0
       self.delayed_output = 0.0
@@ -105,7 +105,7 @@ class LatControlINDI():
       else:
         self.output_steer = self.delayed_output + delta_u
 
-      steers_max = get_steer_max(CP, v_ego)
+      steers_max = get_steer_max(CP, CS.vEgo)
       self.output_steer = clip(self.output_steer, -steers_max, steers_max)
 
       indi_log.active = True
@@ -116,7 +116,7 @@ class LatControlINDI():
       indi_log.delta = float(delta_u)
       indi_log.output = float(self.output_steer)
 
-      check_saturation = (v_ego > 10.) and not rate_limited and not steer_override
+      check_saturation = (CS.vEgo> 10.) and not CS.steeringRateLimited and not CS.steeringPressed 
       indi_log.saturated = self._check_saturation(self.output_steer, check_saturation, steers_max)
 
     return float(self.output_steer), float(self.angle_steers_des), indi_log

selfdrive/controls/lib/latcontrol_lqr.py

@@ -43,22 +43,24 @@ class LatControlLQR():
 
     return self.sat_count > self.sat_limit
 
-  def update(self, active, v_ego, angle_steers, angle_steers_rate, eps_torque, steer_override, rate_limited, CP, path_plan):
+  def update(self, active, CS, CP, path_plan):
     lqr_log = log.ControlsState.LateralLQRState.new_message()
 
-    steers_max = get_steer_max(CP, v_ego)
-    torque_scale = (0.45 + v_ego / 60.0)**2  # Scale actuator model with speed
+    steers_max = get_steer_max(CP, CS.vEgo)
+    torque_scale = (0.45 + CS.vEgo / 60.0)**2  # Scale actuator model with speed
+
+    steering_angle = CS.steeringAngle
 
     # Subtract offset. Zero angle should correspond to zero torque
     self.angle_steers_des = path_plan.angleSteers - path_plan.angleOffset
-    angle_steers -= path_plan.angleOffset
+    steering_angle -= path_plan.angleOffset
 
     # Update Kalman filter
     angle_steers_k = float(self.C.dot(self.x_hat))
-    e = angle_steers - angle_steers_k
-    self.x_hat = self.A.dot(self.x_hat) + self.B.dot(eps_torque / torque_scale) + self.L.dot(e)
+    e = steering_angle - angle_steers_k
+    self.x_hat = self.A.dot(self.x_hat) + self.B.dot(CS.steeringTorqueEps / torque_scale) + self.L.dot(e)
 
-    if v_ego < 0.3 or not active:
+    if CS.vEgo < 0.3 or not active:
       lqr_log.active = False
       lqr_output = 0.
       self.reset()
@@ -70,7 +72,7 @@ class LatControlLQR():
       lqr_output = torque_scale * u_lqr / self.scale
 
       # Integrator
-      if steer_override:
+      if CS.steeringPressed:
         self.i_lqr -= self.i_unwind_rate * float(np.sign(self.i_lqr))
       else:
         error = self.angle_steers_des - angle_steers_k
@@ -84,7 +86,7 @@ class LatControlLQR():
       self.output_steer = lqr_output + self.i_lqr
       self.output_steer = clip(self.output_steer, -steers_max, steers_max)
 
-    check_saturation = (v_ego > 10) and not rate_limited and not steer_override
+    check_saturation = (CS.vEgo > 10) and not CS.steeringRateLimited and not CS.steeringPressed
     saturated = self._check_saturation(self.output_steer, check_saturation, steers_max)
 
     lqr_log.steerAngle = angle_steers_k + path_plan.angleOffset

selfdrive/controls/lib/latcontrol_pid.py

@@ -14,31 +14,31 @@ class LatControlPID():
   def reset(self):
     self.pid.reset()
 
-  def update(self, active, v_ego, angle_steers, angle_steers_rate, eps_torque, steer_override, rate_limited, CP, path_plan):
+  def update(self, active, CS, CP, path_plan):
     pid_log = log.ControlsState.LateralPIDState.new_message()
-    pid_log.steerAngle = float(angle_steers)
-    pid_log.steerRate = float(angle_steers_rate)
+    pid_log.steerAngle = float(CS.steeringAngle)
+    pid_log.steerRate = float(CS.steeringRate)
 
-    if v_ego < 0.3 or not active:
+    if CS.vEgo < 0.3 or not active:
       output_steer = 0.0
       pid_log.active = False
       self.pid.reset()
     else:
       self.angle_steers_des = path_plan.angleSteers  # get from MPC/PathPlanner
 
-      steers_max = get_steer_max(CP, v_ego)
+      steers_max = get_steer_max(CP, CS.vEgo)
       self.pid.pos_limit = steers_max
       self.pid.neg_limit = -steers_max
       steer_feedforward = self.angle_steers_des   # feedforward desired angle
       if CP.steerControlType == car.CarParams.SteerControlType.torque:
         # TODO: feedforward something based on path_plan.rateSteers
         steer_feedforward -= path_plan.angleOffset   # subtract the offset, since it does not contribute to resistive torque
-        steer_feedforward *= v_ego**2  # proportional to realigning tire momentum (~ lateral accel)
+        steer_feedforward *= CS.vEgo**2  # proportional to realigning tire momentum (~ lateral accel)
       deadzone = 0.0
 
-      check_saturation = (v_ego > 10) and not rate_limited and not steer_override
-      output_steer = self.pid.update(self.angle_steers_des, angle_steers, check_saturation=check_saturation, override=steer_override,
-                                     feedforward=steer_feedforward, speed=v_ego, deadzone=deadzone)
+      check_saturation = (CS.vEgo > 10) and not CS.steeringRateLimited and not CS.steeringPressed
+      output_steer = self.pid.update(self.angle_steers_des, CS.steeringAngle, check_saturation=check_saturation, override=CS.steeringPressed,
+                                     feedforward=steer_feedforward, speed=CS.vEgo, deadzone=deadzone)
       pid_log.active = True
       pid_log.p = self.pid.p
       pid_log.i = self.pid.i

selfdrive/controls/lib/longcontrol.py

@@ -71,19 +71,19 @@ class LongControl():
     self.pid.reset()
     self.v_pid = v_pid
 
-  def update(self, active, v_ego, brake_pressed, standstill, cruise_standstill, v_cruise, v_target, v_target_future, a_target, CP):
+  def update(self, active, CS, v_target, v_target_future, a_target, CP):
     """Update longitudinal control. This updates the state machine and runs a PID loop"""
     # Actuation limits
-    gas_max = interp(v_ego, CP.gasMaxBP, CP.gasMaxV)
-    brake_max = interp(v_ego, CP.brakeMaxBP, CP.brakeMaxV)
+    gas_max = interp(CS.vEgo, CP.gasMaxBP, CP.gasMaxV)
+    brake_max = interp(CS.vEgo, CP.brakeMaxBP, CP.brakeMaxV)
 
     # Update state machine
     output_gb = self.last_output_gb
-    self.long_control_state = long_control_state_trans(active, self.long_control_state, v_ego,
+    self.long_control_state = long_control_state_trans(active, self.long_control_state, CS.vEgo,
                                                        v_target_future, self.v_pid, output_gb,
-                                                       brake_pressed, cruise_standstill)
+                                                       CS.brakePressed, CS.cruiseState.standstill)
 
-    v_ego_pid = max(v_ego, MIN_CAN_SPEED)  # Without this we get jumps, CAN bus reports 0 when speed < 0.3
+    v_ego_pid = max(CS.vEgo, MIN_CAN_SPEED)  # Without this we get jumps, CAN bus reports 0 when speed < 0.3
 
     if self.long_control_state == LongCtrlState.off:
       self.v_pid = v_ego_pid
@@ -98,7 +98,7 @@ class LongControl():
 
       # Toyota starts braking more when it thinks you want to stop
       # Freeze the integrator so we don't accelerate to compensate, and don't allow positive acceleration
-      prevent_overshoot = not CP.stoppingControl and v_ego < 1.5 and v_target_future < 0.7
+      prevent_overshoot = not CP.stoppingControl and CS.vEgo < 1.5 and v_target_future < 0.7
       deadzone = interp(v_ego_pid, CP.longitudinalTuning.deadzoneBP, CP.longitudinalTuning.deadzoneV)
 
       output_gb = self.pid.update(self.v_pid, v_ego_pid, speed=v_ego_pid, deadzone=deadzone, feedforward=a_target, freeze_integrator=prevent_overshoot)
@@ -109,18 +109,18 @@ class LongControl():
     # Intention is to stop, switch to a different brake control until we stop
     elif self.long_control_state == LongCtrlState.stopping:
       # Keep applying brakes until the car is stopped
-      if not standstill or output_gb > -BRAKE_STOPPING_TARGET:
+      if not CS.standstill or output_gb > -BRAKE_STOPPING_TARGET:
         output_gb -= STOPPING_BRAKE_RATE / RATE
       output_gb = clip(output_gb, -brake_max, gas_max)
 
-      self.v_pid = v_ego
+      self.v_pid = CS.vEgo
       self.pid.reset()
 
     # Intention is to move again, release brake fast before handing control to PID
     elif self.long_control_state == LongCtrlState.starting:
       if output_gb < -0.2:
         output_gb += STARTING_BRAKE_RATE / RATE
-      self.v_pid = v_ego
+      self.v_pid = CS.vEgo
       self.pid.reset()
 
     self.last_output_gb = output_gb

selfdrive/test/profiling/controlsd.py

@@ -7,7 +7,7 @@ import pprofile
 import pyprof2calltree
 
 from tools.lib.logreader import LogReader
-from selfdrive.controls.controlsd import controlsd_thread
+from selfdrive.controls.controlsd import main as controlsd_thread
 from selfdrive.test.profiling.lib import SubMaster, PubMaster, SubSocket, ReplayDone
 from selfdrive.test.process_replay.process_replay import CONFIGS