mawei/dragonpilot
1
0
Fork 0
mirror of https://github.com/dragonpilot/dragonpilot.git synced 2026-07-15 09:52:04 +08:00
Code Issues Packages Projects Releases Wiki Activity

Clean up controllers (#24340)

Browse Source 
* clean up lat controllers

* pass CP once

* sort
This commit is contained in:
Shane Smiskol
2022-04-28 00:42:52 -07:00
committed by GitHub
parent be0f27ac85
commit c9be2f02c3
7 changed files with 38 additions and 42 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/controlsd.py
+4 -5
View File
@@ -98,8 +98,7 @@ class Controls:
self.sm = messaging.SubMaster(['deviceState', 'pandaStates', 'peripheralState', 'modelV2', 'liveCalibration',
'driverMonitoringState', 'longitudinalPlan', 'lateralPlan', 'liveLocationKalman',
'managerState', 'liveParameters', 'radarState'] + self.camera_packets + joystick_packet,
ignore_alive=ignore, ignore_avg_freq=['radarState', 'longitudinalPlan'])
ignore_alive=ignore, ignore_avg_freq=['radarState', 'longitudinalPlan'])
# set alternative experiences from parameters
self.disengage_on_accelerator = params.get_bool("DisengageOnAccelerator")
@@ -557,15 +556,15 @@ class Controls:
# accel PID loop
pid_accel_limits = self.CI.get_pid_accel_limits(self.CP, CS.vEgo, self.v_cruise_kph * CV.KPH_TO_MS)
t_since_plan = (self.sm.frame - self.sm.rcv_frame['longitudinalPlan']) * DT_CTRL
actuators.accel = self.LoC.update(CC.longActive, CS, self.CP, long_plan, pid_accel_limits, t_since_plan)
actuators.accel = self.LoC.update(CC.longActive, CS, long_plan, pid_accel_limits, t_since_plan)
# Steering PID loop and lateral MPC
desired_curvature, desired_curvature_rate = get_lag_adjusted_curvature(self.CP, CS.vEgo,
lat_plan.psis,
lat_plan.curvatures,
lat_plan.curvatureRates)
actuators.steer, actuators.steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.CP, self.VM,
params, self.last_actuators, desired_curvature,
actuators.steer, actuators.steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.VM, params,
self.last_actuators, desired_curvature,
desired_curvature_rate, self.sm['liveLocationKalman'])
else:
lac_log = log.ControlsState.LateralDebugState.new_message()
selfdrive/controls/lib/latcontrol.py
+2 -2
View File
@@ -1,7 +1,7 @@
from abc import abstractmethod, ABC
from common.realtime import DT_CTRL
from common.numpy_fast import clip
from common.realtime import DT_CTRL
MIN_STEER_SPEED = 0.3
@@ -16,7 +16,7 @@ class LatControl(ABC):
self.steer_max = 1.0
@abstractmethod
def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate, llk):
def update(self, active, CS, VM, params, last_actuators, desired_curvature, desired_curvature_rate, llk):
pass
def reset(self):
selfdrive/controls/lib/latcontrol_angle.py
+1 -1
View File
@@ -7,7 +7,7 @@ STEER_ANGLE_SATURATION_THRESHOLD = 2.5 # Degrees
class LatControlAngle(LatControl):
def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate, llk):
def update(self, active, CS, VM, params, last_actuators, desired_curvature, desired_curvature_rate, llk):
angle_log = log.ControlsState.LateralAngleState.new_message()
if CS.vEgo < MIN_STEER_SPEED or not active:
selfdrive/controls/lib/latcontrol_indi.py
+1 -1
View File
@@ -63,7 +63,7 @@ class LatControlINDI(LatControl):
self.steer_filter.x = 0.
self.speed = 0.
def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate, llk):
def update(self, active, CS, VM, params, last_actuators, desired_curvature, desired_curvature_rate, llk):
self.speed = CS.vEgo
# Update Kalman filter
y = np.array([[math.radians(CS.steeringAngleDeg)], [math.radians(CS.steeringRateDeg)]])
selfdrive/controls/lib/latcontrol_pid.py
+5 -8
View File
@@ -1,23 +1,23 @@
import math
from selfdrive.controls.lib.pid import PIDController
from selfdrive.controls.lib.latcontrol import LatControl, MIN_STEER_SPEED
from cereal import log
from selfdrive.controls.lib.latcontrol import LatControl, MIN_STEER_SPEED
from selfdrive.controls.lib.pid import PIDController
class LatControlPID(LatControl):
def __init__(self, CP, CI):
super().__init__(CP, CI)
self.pid = PIDController((CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV),
(CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV),
k_f=CP.lateralTuning.pid.kf, pos_limit=1.0, neg_limit=-1.0)
(CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV),
k_f=CP.lateralTuning.pid.kf, pos_limit=self.steer_max, neg_limit=-self.steer_max)
self.get_steer_feedforward = CI.get_steer_feedforward_function()
def reset(self):
super().reset()
self.pid.reset()
def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate, llk):
def update(self, active, CS, VM, params, last_actuators, desired_curvature, desired_curvature_rate, llk):
pid_log = log.ControlsState.LateralPIDState.new_message()
pid_log.steeringAngleDeg = float(CS.steeringAngleDeg)
pid_log.steeringRateDeg = float(CS.steeringRateDeg)
@@ -33,9 +33,6 @@ class LatControlPID(LatControl):
pid_log.active = False
self.pid.reset()
else:
self.pid.pos_limit = self.steer_max
self.pid.neg_limit = -self.steer_max
# offset does not contribute to resistive torque
steer_feedforward = self.get_steer_feedforward(angle_steers_des_no_offset, CS.vEgo)
selfdrive/controls/lib/latcontrol_torque.py
+11 -12
View File
@@ -1,9 +1,10 @@
import math
from selfdrive.controls.lib.pid import PIDController
from cereal import log
from common.numpy_fast import interp
from selfdrive.controls.lib.latcontrol import LatControl, MIN_STEER_SPEED
from selfdrive.controls.lib.pid import PIDController
from selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_GRAVITY
from cereal import log
# At higher speeds (25+mph) we can assume:
# Lateral acceleration achieved by a specific car correlates to
@@ -12,7 +13,7 @@ from cereal import log
# This controller applies torque to achieve desired lateral
# accelerations. To compensate for the low speed effects we
# use a LOW_SPEED_FACTOR in the error. Additionally there is
# use a LOW_SPEED_FACTOR in the error. Additionally, there is
# friction in the steering wheel that needs to be overcome to
# move it at all, this is compensated for too.
@@ -24,12 +25,10 @@ JERK_THRESHOLD = 0.2
class LatControlTorque(LatControl):
def __init__(self, CP, CI):
super().__init__(CP, CI)
self.CP = CP
self.pid = PIDController(CP.lateralTuning.torque.kp, CP.lateralTuning.torque.ki,
k_f=CP.lateralTuning.torque.kf, pos_limit=1.0, neg_limit=-1.0)
k_f=CP.lateralTuning.torque.kf, pos_limit=self.steer_max, neg_limit=-self.steer_max)
self.get_steer_feedforward = CI.get_steer_feedforward_function()
self.steer_max = 1.0
self.pid.pos_limit = self.steer_max
self.pid.neg_limit = -self.steer_max
self.use_steering_angle = CP.lateralTuning.torque.useSteeringAngle
self.friction = CP.lateralTuning.torque.friction
@@ -37,7 +36,7 @@ class LatControlTorque(LatControl):
super().reset()
self.pid.reset()
def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate, llk):
def update(self, active, CS, VM, params, last_actuators, desired_curvature, desired_curvature_rate, llk):
pid_log = log.ControlsState.LateralTorqueState.new_message()
if CS.vEgo < MIN_STEER_SPEED or not active:
@@ -49,9 +48,9 @@ class LatControlTorque(LatControl):
actual_curvature = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll)
else:
actual_curvature = llk.angularVelocityCalibrated.value[2] / CS.vEgo
desired_lateral_accel = desired_curvature * CS.vEgo**2
desired_lateral_jerk = desired_curvature_rate * CS.vEgo**2
actual_lateral_accel = actual_curvature * CS.vEgo**2
desired_lateral_accel = desired_curvature * CS.vEgo ** 2
desired_lateral_jerk = desired_curvature_rate * CS.vEgo ** 2
actual_lateral_accel = actual_curvature * CS.vEgo ** 2
setpoint = desired_lateral_accel + LOW_SPEED_FACTOR * desired_curvature
measurement = actual_lateral_accel + LOW_SPEED_FACTOR * actual_curvature
@@ -61,7 +60,7 @@ class LatControlTorque(LatControl):
ff = desired_lateral_accel - params.roll * ACCELERATION_DUE_TO_GRAVITY
# convert friction into lateral accel units for feedforward
friction_compensation = interp(desired_lateral_jerk, [-JERK_THRESHOLD, JERK_THRESHOLD], [-self.friction, self.friction])
ff += friction_compensation / CP.lateralTuning.torque.kf
ff += friction_compensation / self.CP.lateralTuning.torque.kf
output_torque = self.pid.update(error,
override=CS.steeringPressed, feedforward=ff,
speed=CS.vEgo,
selfdrive/controls/lib/longcontrol.py
+14 -13
View File
@@ -1,8 +1,8 @@
from cereal import car
from common.numpy_fast import clip, interp
from common.realtime import DT_CTRL
from selfdrive.controls.lib.pid import PIDController
from selfdrive.controls.lib.drive_helpers import CONTROL_N
from selfdrive.controls.lib.pid import PIDController
from selfdrive.modeld.constants import T_IDXS
LongCtrlState = car.CarControl.Actuators.LongControlState
@@ -50,12 +50,13 @@ def long_control_state_trans(CP, active, long_control_state, v_ego, v_target,
return long_control_state
class LongControl():
class LongControl:
def __init__(self, CP):
self.CP = CP
self.long_control_state = LongCtrlState.off # initialized to off
self.pid = PIDController((CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV),
(CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV),
k_f = CP.longitudinalTuning.kf, rate=1 / DT_CTRL)
k_f=CP.longitudinalTuning.kf, rate=1 / DT_CTRL)
self.v_pid = 0.0
self.last_output_accel = 0.0
@@ -64,7 +65,7 @@ class LongControl():
self.pid.reset()
self.v_pid = v_pid
def update(self, active, CS, CP, long_plan, accel_limits, t_since_plan):
def update(self, active, CS, long_plan, accel_limits, t_since_plan):
"""Update longitudinal control. This updates the state machine and runs a PID loop"""
# Interp control trajectory
speeds = long_plan.speeds
@@ -72,11 +73,11 @@ class LongControl():
v_target = interp(t_since_plan, T_IDXS[:CONTROL_N], speeds)
a_target = interp(t_since_plan, T_IDXS[:CONTROL_N], long_plan.accels)
v_target_lower = interp(CP.longitudinalActuatorDelayLowerBound + t_since_plan, T_IDXS[:CONTROL_N], speeds)
a_target_lower = 2 * (v_target_lower - v_target) / CP.longitudinalActuatorDelayLowerBound - a_target
v_target_lower = interp(self.CP.longitudinalActuatorDelayLowerBound + t_since_plan, T_IDXS[:CONTROL_N], speeds)
a_target_lower = 2 * (v_target_lower - v_target) / self.CP.longitudinalActuatorDelayLowerBound - a_target
v_target_upper = interp(CP.longitudinalActuatorDelayUpperBound + t_since_plan, T_IDXS[:CONTROL_N], speeds)
a_target_upper = 2 * (v_target_upper - v_target) / CP.longitudinalActuatorDelayUpperBound - a_target
v_target_upper = interp(self.CP.longitudinalActuatorDelayUpperBound + t_since_plan, T_IDXS[:CONTROL_N], speeds)
a_target_upper = 2 * (v_target_upper - v_target) / self.CP.longitudinalActuatorDelayUpperBound - a_target
a_target = min(a_target_lower, a_target_upper)
v_target_future = speeds[-1]
@@ -93,7 +94,7 @@ class LongControl():
# Update state machine
output_accel = self.last_output_accel
self.long_control_state = long_control_state_trans(CP, active, self.long_control_state, CS.vEgo,
self.long_control_state = long_control_state_trans(self.CP, active, self.long_control_state, CS.vEgo,
v_target, v_target_future, CS.brakePressed,
CS.cruiseState.standstill)
@@ -107,8 +108,8 @@ 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 CS.vEgo < 1.5 and v_target_future < 0.7 and v_target_future < self.v_pid
deadzone = interp(CS.vEgo, CP.longitudinalTuning.deadzoneBP, CP.longitudinalTuning.deadzoneV)
prevent_overshoot = not self.CP.stoppingControl and CS.vEgo < 1.5 and v_target_future < 0.7 and v_target_future < self.v_pid
deadzone = interp(CS.vEgo, self.CP.longitudinalTuning.deadzoneBP, self.CP.longitudinalTuning.deadzoneV)
freeze_integrator = prevent_overshoot
error = self.v_pid - CS.vEgo
@@ -121,8 +122,8 @@ 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 CS.standstill or output_accel > CP.stopAccel:
output_accel -= CP.stoppingDecelRate * DT_CTRL
if not CS.standstill or output_accel > self.CP.stopAccel:
output_accel -= self.CP.stoppingDecelRate * DT_CTRL
output_accel = clip(output_accel, accel_limits[0], accel_limits[1])
self.reset(CS.vEgo)