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 commentsalbatross
Adeeb
GitHub
parent
1256949be1
commit
08832ff29d
|
|
@ -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]))
|
||||
|
|
|
|||
File diff suppressed because it is too large
Load Diff
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue