sadmen
/
jyoung
mirror of https://github.com/jyoung8607/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

long control: new API (#32706)

Browse Source 
* Simplify long control

* Seperate

* Rename

* Try new api for toyota

* rm v_pid everywhere

* No speed in reset

* 0 is better default

* unassigned variable

* Update other cars

* Update gm

* SIMPLIFY

* simplify more

* fix API boundry

* Fix stopping bug

* Small fixes

* Update ref
old-commit-hash: bc303df6a0
vw-mqb-aeb
Harald Schäfer 1 year ago committed by GitHub
parent a854878499
commit 47d5601d8a
18 changed files with 74 additions and 136 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 4
      cereal/car.capnp
  2. 8
      cereal/log.capnp
  3. 4
      selfdrive/car/ford/interface.py
  4. 12
      selfdrive/car/gm/interface.py
  5. 8
      selfdrive/car/honda/interface.py
  6. 4
      selfdrive/car/hyundai/interface.py
  7. 6
      selfdrive/car/interfaces.py
  8. 5
      selfdrive/car/subaru/interface.py
  9. 5
      selfdrive/car/tesla/interface.py
  10. 1
      selfdrive/car/tests/test_car_interfaces.py
  11. 21
      selfdrive/car/toyota/interface.py
  12. 2
      selfdrive/car/volkswagen/interface.py
  13. 6
      selfdrive/controls/controlsd.py
  14. 10
      selfdrive/controls/lib/drive_helpers.py
  15. 78
      selfdrive/controls/lib/longcontrol.py
  16. 33
      selfdrive/controls/lib/longitudinal_planner.py
  17. 2
      selfdrive/test/process_replay/ref_commit
  18. 1
      tools/replay/ui.py

4
cereal/car.capnp
Unescape View File

@ -541,8 +541,8 @@ struct CarParams {
kiBP @2 :List(Float32);
kiV @3 :List(Float32);
kf @6 :Float32;
deadzoneBP @4 :List(Float32);
deadzoneV @5 :List(Float32);
deadzoneBPDEPRECATED @4 :List(Float32);
deadzoneVDEPRECATED @5 :List(Float32);
}
struct LateralINDITuning {

8
cereal/log.capnp
Unescape View File

@ -698,7 +698,6 @@ struct ControlsState @0x97ff69c53601abf1 {
personality @66 :LongitudinalPersonality;
longControlState @30 :Car.CarControl.Actuators.LongControlState;
vPid @2 :Float32;
vTargetLead @3 :Float32;
vCruise @22 :Float32; # actual set speed
vCruiseCluster @63 :Float32; # set speed to display in the UI
@ -866,6 +865,7 @@ struct ControlsState @0x97ff69c53601abf1 {
canMonoTimesDEPRECATED @21 :List(UInt64);
desiredCurvatureRateDEPRECATED @62 :Float32;
canErrorCounterDEPRECATED @57 :UInt32;
vPidDEPRECATED @2 :Float32;
}
# All SI units and in device frame
@ -1060,6 +1060,11 @@ struct LongitudinalPlan @0xe00b5b3eba12876c {
accels @32 :List(Float32);
speeds @33 :List(Float32);
jerks @34 :List(Float32);
aTarget @18 :Float32;
shouldStop @37: Bool;
allowThrottle @38: Bool;
allowBrake @39: Bool;
solverExecutionTime @35 :Float32;
@ -1076,7 +1081,6 @@ struct LongitudinalPlan @0xe00b5b3eba12876c {
aCruiseDEPRECATED @17 :Float32;
vTargetDEPRECATED @3 :Float32;
vTargetFutureDEPRECATED @14 :Float32;
aTargetDEPRECATED @18 :Float32;
vStartDEPRECATED @26 :Float32;
aStartDEPRECATED @27 :Float32;
vMaxDEPRECATED @20 :Float32;

4
selfdrive/car/ford/interface.py
Unescape View File

@ -22,10 +22,6 @@ class CarInterface(CarInterfaceBase):
ret.steerActuatorDelay = 0.2
ret.steerLimitTimer = 1.0
ret.longitudinalTuning.kpBP = [0.]
ret.longitudinalTuning.kpV = [0.5]
ret.longitudinalTuning.kiV = [0.]
CAN = CanBus(fingerprint=fingerprint)
cfgs = [get_safety_config(car.CarParams.SafetyModel.ford)]
if CAN.main >= 4:

12
selfdrive/car/gm/interface.py
Unescape View File

@ -94,11 +94,7 @@ class CarInterface(CarInterfaceBase):
else:
ret.transmissionType = TransmissionType.automatic
ret.longitudinalTuning.deadzoneBP = [0.]
ret.longitudinalTuning.deadzoneV = [0.15]
ret.longitudinalTuning.kpBP = [5., 35.]
ret.longitudinalTuning.kiBP = [0.]
ret.longitudinalTuning.kiBP = [5., 35.]
if candidate in CAMERA_ACC_CAR:
ret.experimentalLongitudinalAvailable = True
@ -110,8 +106,7 @@ class CarInterface(CarInterfaceBase):
ret.minSteerSpeed = 10 * CV.KPH_TO_MS
# Tuning for experimental long
ret.longitudinalTuning.kpV = [2.0, 1.5]
ret.longitudinalTuning.kiV = [0.72]
ret.longitudinalTuning.kiV = [2.0, 1.5]
ret.stoppingDecelRate = 2.0 # reach brake quickly after enabling
ret.vEgoStopping = 0.25
ret.vEgoStarting = 0.25
@ -131,8 +126,7 @@ class CarInterface(CarInterfaceBase):
ret.minSteerSpeed = 7 * CV.MPH_TO_MS
# Tuning
ret.longitudinalTuning.kpV = [2.4, 1.5]
ret.longitudinalTuning.kiV = [0.36]
ret.longitudinalTuning.kiV = [2.4, 1.5]
# These cars have been put into dashcam only due to both a lack of users and test coverage.
# These cars likely still work fine. Once a user confirms each car works and a test route is

8
selfdrive/car/honda/interface.py
Unescape View File

@ -72,17 +72,13 @@ class CarInterface(CarInterfaceBase):
ret.lateralTuning.pid.kf = 0.00006 # conservative feed-forward
if candidate in HONDA_BOSCH:
ret.longitudinalTuning.kpV = [0.25]
ret.longitudinalTuning.kiV = [0.05]
ret.longitudinalActuatorDelay = 0.5 # s
if candidate in HONDA_BOSCH_RADARLESS:
ret.stopAccel = CarControllerParams.BOSCH_ACCEL_MIN # stock uses -4.0 m/s^2 once stopped but limited by safety model
else:
# default longitudinal tuning for all hondas
ret.longitudinalTuning.kpBP = [0., 5., 35.]
ret.longitudinalTuning.kpV = [1.2, 0.8, 0.5]
ret.longitudinalTuning.kiBP = [0., 35.]
ret.longitudinalTuning.kiV = [0.18, 0.12]
ret.longitudinalTuning.kiBP = [0., 5., 35.]
ret.longitudinalTuning.kiV = [1.2, 0.8, 0.5]
eps_modified = False
for fw in car_fw:

4
selfdrive/car/hyundai/interface.py
Unescape View File

@ -80,12 +80,8 @@ class CarInterface(CarInterfaceBase):
# *** longitudinal control ***
if candidate in CANFD_CAR:
ret.longitudinalTuning.kpV = [0.1]
ret.longitudinalTuning.kiV = [0.0]
ret.experimentalLongitudinalAvailable = candidate not in (CANFD_UNSUPPORTED_LONGITUDINAL_CAR | CANFD_RADAR_SCC_CAR)
else:
ret.longitudinalTuning.kpV = [0.5]
ret.longitudinalTuning.kiV = [0.0]
ret.experimentalLongitudinalAvailable = candidate not in (UNSUPPORTED_LONGITUDINAL_CAR | CAMERA_SCC_CAR)
ret.openpilotLongitudinalControl = experimental_long and ret.experimentalLongitudinalAvailable
ret.pcmCruise = not ret.openpilotLongitudinalControl

6
selfdrive/car/interfaces.py
Unescape View File

@ -202,13 +202,11 @@ class CarInterfaceBase(ABC):
ret.vEgoStopping = 0.5
ret.vEgoStarting = 0.5
ret.stoppingControl = True
ret.longitudinalTuning.deadzoneBP = [0.]
ret.longitudinalTuning.deadzoneV = [0.]
ret.longitudinalTuning.kf = 1.
ret.longitudinalTuning.kpBP = [0.]
ret.longitudinalTuning.kpV = [1.]
ret.longitudinalTuning.kpV = [0.]
ret.longitudinalTuning.kiBP = [0.]
ret.longitudinalTuning.kiV = [1.]
ret.longitudinalTuning.kiV = [0.]
# TODO estimate car specific lag, use .15s for now
ret.longitudinalActuatorDelay = 0.15
ret.steerLimitTimer = 1.0

5
selfdrive/car/subaru/interface.py
Unescape View File

@ -91,11 +91,6 @@ class CarInterface(CarInterfaceBase):
ret.flags |= SubaruFlags.DISABLE_EYESIGHT.value
if ret.openpilotLongitudinalControl:
ret.longitudinalTuning.kpBP = [0., 5., 35.]
ret.longitudinalTuning.kpV = [0.8, 1.0, 1.5]
ret.longitudinalTuning.kiBP = [0., 35.]
ret.longitudinalTuning.kiV = [0.54, 0.36]
ret.stoppingControl = True
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_SUBARU_LONG

5
selfdrive/car/tesla/interface.py
Unescape View File

@ -18,11 +18,6 @@ class CarInterface(CarInterfaceBase):
ret.steerControlType = car.CarParams.SteerControlType.angle
# Set kP and kI to 0 over the whole speed range to have the planner accel as actuator command
ret.longitudinalTuning.kpBP = [0]
ret.longitudinalTuning.kpV = [0]
ret.longitudinalTuning.kiBP = [0]
ret.longitudinalTuning.kiV = [0]
ret.longitudinalActuatorDelay = 0.5 # s
ret.radarTimeStep = (1.0 / 8) # 8Hz

1
selfdrive/car/tests/test_car_interfaces.py
Unescape View File

@ -75,7 +75,6 @@ class TestCarInterfaces:
# Longitudinal sanity checks
assert len(car_params.longitudinalTuning.kpV) == len(car_params.longitudinalTuning.kpBP)
assert len(car_params.longitudinalTuning.kiV) == len(car_params.longitudinalTuning.kiBP)
assert len(car_params.longitudinalTuning.deadzoneV) == len(car_params.longitudinalTuning.deadzoneBP)
# Lateral sanity checks
if car_params.steerControlType != car.CarParams.SteerControlType.angle:

21
selfdrive/car/toyota/interface.py
Unescape View File

@ -142,22 +142,15 @@ class CarInterface(CarInterfaceBase):
ret.minEnableSpeed = -1. if stop_and_go else MIN_ACC_SPEED
tune = ret.longitudinalTuning
tune.deadzoneBP = [0., 9.]
tune.deadzoneV = [.0, .15]
if candidate in TSS2_CAR:
tune.kpBP = [0., 5., 20.]
tune.kpV = [1.3, 1.0, 0.7]
tune.kiBP = [0., 5., 12., 20., 27.]
tune.kiV = [.35, .23, .20, .17, .1]
if candidate in TSS2_CAR:
ret.vEgoStopping = 0.25
ret.vEgoStarting = 0.25
ret.stoppingDecelRate = 0.3 # reach stopping target smoothly
tune.kpV = [0.0]
tune.kiV = [0.5]
ret.vEgoStopping = 0.25
ret.vEgoStarting = 0.25
ret.stoppingDecelRate = 0.3 # reach stopping target smoothly
else:
tune.kpBP = [0., 5., 35.]
tune.kiBP = [0., 35.]
tune.kpV = [3.6, 2.4, 1.5]
tune.kiV = [0.54, 0.36]
tune.kiBP = [0., 5., 35.]
tune.kiV = [3.6, 2.4, 1.5]
return ret

2
selfdrive/car/volkswagen/interface.py
Unescape View File

@ -96,8 +96,6 @@ class CarInterface(CarInterfaceBase):
ret.stopAccel = -0.55
ret.vEgoStarting = 0.1
ret.vEgoStopping = 0.5
ret.longitudinalTuning.kpV = [0.1]
ret.longitudinalTuning.kiV = [0.0]
ret.autoResumeSng = ret.minEnableSpeed == -1
return ret

6
selfdrive/controls/controlsd.py
Unescape View File

@ -563,13 +563,12 @@ class Controls:
if not CC.latActive:
self.LaC.reset()
if not CC.longActive:
self.LoC.reset(v_pid=CS.vEgo)
self.LoC.reset()
if not self.joystick_mode:
# accel PID loop
pid_accel_limits = self.CI.get_pid_accel_limits(self.CP, CS.vEgo, self.v_cruise_helper.v_cruise_kph * CV.KPH_TO_MS)
t_since_plan = (self.sm.frame - self.sm.recv_frame['longitudinalPlan']) * DT_CTRL
actuators.accel = self.LoC.update(CC.longActive, CS, long_plan, pid_accel_limits, t_since_plan)
actuators.accel = self.LoC.update(CC.longActive, CS, long_plan.aTarget, long_plan.shouldStop, pid_accel_limits)
# Steering PID loop and lateral MPC
self.desired_curvature = clip_curvature(CS.vEgo, self.desired_curvature, model_v2.action.desiredCurvature)
@ -752,7 +751,6 @@ class Controls:
controlsState.state = self.state
controlsState.engageable = not self.events.contains(ET.NO_ENTRY)
controlsState.longControlState = self.LoC.long_control_state
controlsState.vPid = float(self.LoC.v_pid)
controlsState.vCruise = float(self.v_cruise_helper.v_cruise_kph)
controlsState.vCruiseCluster = float(self.v_cruise_helper.v_cruise_cluster_kph)
controlsState.upAccelCmd = float(self.LoC.pid.p)

10
selfdrive/controls/lib/drive_helpers.py
Unescape View File

@ -141,16 +141,6 @@ class VCruiseHelper:
self.v_cruise_cluster_kph = self.v_cruise_kph
def apply_deadzone(error, deadzone):
if error > deadzone:
error -= deadzone
elif error < - deadzone:
error += deadzone
else:
error = 0.
return error
def apply_center_deadzone(error, deadzone):
if (error > - deadzone) and (error < deadzone):
error = 0.

78
selfdrive/controls/lib/longcontrol.py
Unescape View File

@ -1,7 +1,7 @@
from cereal import car
from openpilot.common.numpy_fast import clip, interp
from openpilot.common.numpy_fast import clip
from openpilot.common.realtime import DT_CTRL
from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N, apply_deadzone
from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N
from openpilot.selfdrive.controls.lib.pid import PIDController
from openpilot.selfdrive.modeld.constants import ModelConstants
@ -10,18 +10,10 @@ CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N]
LongCtrlState = car.CarControl.Actuators.LongControlState
def long_control_state_trans(CP, active, long_control_state, v_ego, v_target,
v_target_1sec, brake_pressed, cruise_standstill):
accelerating = v_target_1sec > v_target
planned_stop = (v_target < CP.vEgoStopping and
v_target_1sec < CP.vEgoStopping and
not accelerating)
stay_stopped = (v_ego < CP.vEgoStopping and
(brake_pressed or cruise_standstill))
stopping_condition = planned_stop or stay_stopped
starting_condition = (v_target_1sec > CP.vEgoStarting and
accelerating and
def long_control_state_trans(CP, active, long_control_state, v_ego,
should_stop, brake_pressed, cruise_standstill):
stopping_condition = should_stop
starting_condition = (not should_stop and
not cruise_standstill and
not brake_pressed)
started_condition = v_ego > CP.vEgoStarting
@ -34,7 +26,6 @@ def long_control_state_trans(CP, active, long_control_state, v_ego, v_target,
long_control_state = LongCtrlState.pid
if stopping_condition:
long_control_state = LongCtrlState.stopping
elif long_control_state == LongCtrlState.stopping:
if starting_condition and CP.startingState:
long_control_state = LongCtrlState.starting
@ -49,78 +40,45 @@ def long_control_state_trans(CP, active, long_control_state, v_ego, v_target,
return long_control_state
class LongControl:
def __init__(self, CP):
self.CP = CP
self.long_control_state = LongCtrlState.off # initialized to off
self.long_control_state = LongCtrlState.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)
self.v_pid = 0.0
self.last_output_accel = 0.0
def reset(self, v_pid):
"""Reset PID controller and change setpoint"""
def reset(self):
self.pid.reset()
self.v_pid = v_pid
def update(self, active, CS, long_plan, accel_limits, t_since_plan):
def update(self, active, CS, a_target, should_stop, accel_limits):
"""Update longitudinal control. This updates the state machine and runs a PID loop"""
# Interp control trajectory
speeds = long_plan.speeds
if len(speeds) == CONTROL_N:
v_target_now = interp(t_since_plan, CONTROL_N_T_IDX, speeds)
a_target_now = interp(t_since_plan, CONTROL_N_T_IDX, long_plan.accels)
v_target = interp(self.CP.longitudinalActuatorDelay + t_since_plan, CONTROL_N_T_IDX, speeds)
a_target = 2 * (v_target - v_target_now) / self.CP.longitudinalActuatorDelay - a_target_now
v_target_1sec = interp(self.CP.longitudinalActuatorDelay + t_since_plan + 1.0, CONTROL_N_T_IDX, speeds)
else:
v_target = 0.0
v_target_now = 0.0
v_target_1sec = 0.0
a_target = 0.0
self.pid.neg_limit = accel_limits[0]
self.pid.pos_limit = accel_limits[1]
output_accel = self.last_output_accel
self.long_control_state = long_control_state_trans(self.CP, active, self.long_control_state, CS.vEgo,
v_target, v_target_1sec, CS.brakePressed,
should_stop, CS.brakePressed,
CS.cruiseState.standstill)
if self.long_control_state == LongCtrlState.off:
self.reset(CS.vEgo)
self.reset()
output_accel = 0.
elif self.long_control_state == LongCtrlState.stopping:
output_accel = self.last_output_accel
if output_accel > self.CP.stopAccel:
output_accel = min(output_accel, 0.0)
output_accel -= self.CP.stoppingDecelRate * DT_CTRL
self.reset(CS.vEgo)
self.reset()
elif self.long_control_state == LongCtrlState.starting:
output_accel = self.CP.startAccel
self.reset(CS.vEgo)
elif self.long_control_state == LongCtrlState.pid:
self.v_pid = v_target_now
self.reset()
# 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
# TODO too complex, needs to be simplified and tested on toyotas
prevent_overshoot = not self.CP.stoppingControl and CS.vEgo < 1.5 and v_target_1sec < 0.7 and v_target_1sec < 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
error_deadzone = apply_deadzone(error, deadzone)
output_accel = self.pid.update(error_deadzone, speed=CS.vEgo,
feedforward=a_target,
freeze_integrator=freeze_integrator)
else: # LongCtrlState.pid
error = a_target - CS.aEgo
output_accel = self.pid.update(error, speed=CS.vEgo,
feedforward=a_target)
self.last_output_accel = clip(output_accel, accel_limits[0], accel_limits[1])
return self.last_output_accel

33
selfdrive/controls/lib/longitudinal_planner.py
Unescape View File

@ -19,6 +19,7 @@ LON_MPC_STEP = 0.2 # first step is 0.2s
A_CRUISE_MIN = -1.2
A_CRUISE_MAX_VALS = [1.6, 1.2, 0.8, 0.6]
A_CRUISE_MAX_BP = [0., 10.0, 25., 40.]
CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N]
# Lookup table for turns
_A_TOTAL_MAX_V = [1.7, 3.2]
@ -34,7 +35,6 @@ def limit_accel_in_turns(v_ego, angle_steers, a_target, CP):
This function returns a limited long acceleration allowed, depending on the existing lateral acceleration
this should avoid accelerating when losing the target in turns
"""
# FIXME: This function to calculate lateral accel is incorrect and should use the VehicleModel
# The lookup table for turns should also be updated if we do this
a_total_max = interp(v_ego, _A_TOTAL_MAX_BP, _A_TOTAL_MAX_V)
@ -44,6 +44,26 @@ def limit_accel_in_turns(v_ego, angle_steers, a_target, CP):
return [a_target[0], min(a_target[1], a_x_allowed)]
def get_accel_from_plan(CP, long_plan):
speeds = long_plan.speeds
if len(speeds) == CONTROL_N:
v_target_now = interp(DT_MDL, CONTROL_N_T_IDX, speeds)
a_target_now = interp(DT_MDL, CONTROL_N_T_IDX, long_plan.accels)
v_target = interp(CP.longitudinalActuatorDelay + DT_MDL, CONTROL_N_T_IDX, speeds)
a_target = 2 * (v_target - v_target_now) / CP.longitudinalActuatorDelay - a_target_now
v_target_1sec = interp(CP.longitudinalActuatorDelay + DT_MDL + 1.0, CONTROL_N_T_IDX, speeds)
else:
v_target = 0.0
v_target_now = 0.0
v_target_1sec = 0.0
a_target = 0.0
should_stop = (v_target < CP.vEgoStopping and
v_target_1sec < CP.vEgoStopping)
return a_target, should_stop
class LongitudinalPlanner:
def __init__(self, CP, init_v=0.0, init_a=0.0, dt=DT_MDL):
self.CP = CP
@ -142,9 +162,14 @@ class LongitudinalPlanner:
plan_send.valid = sm.all_checks(service_list=['carState', 'controlsState'])
longitudinalPlan = plan_send.longitudinalPlan
longitudinalPlan.modelMonoTime = sm.logMonoTime['modelV2']
longitudinalPlan.processingDelay = (plan_send.logMonoTime / 1e9) - sm.logMonoTime['modelV2']
longitudinalPlan.solverExecutionTime = self.mpc.solve_time
longitudinalPlan.allowBrake = True
longitudinalPlan.allowThrottle = True
longitudinalPlan.speeds = self.v_desired_trajectory.tolist()
longitudinalPlan.accels = self.a_desired_trajectory.tolist()
@ -154,6 +179,10 @@ class LongitudinalPlanner:
longitudinalPlan.longitudinalPlanSource = self.mpc.source
longitudinalPlan.fcw = self.fcw
longitudinalPlan.solverExecutionTime = self.mpc.solve_time
a_target, should_stop = get_accel_from_plan(self.CP, longitudinalPlan)
longitudinalPlan.aTarget = a_target
longitudinalPlan.shouldStop = should_stop
longitudinalPlan.allowBrake = True
longitudinalPlan.allowThrottle = True
pm.send('longitudinalPlan', plan_send)

2
selfdrive/test/process_replay/ref_commit
Unescape View File

@ -1 +1 @@
6438bd5edad674c2de3c7e2d126271cb2576383d
8737e368e17f859291164286feb4246e00c0b4a5

1
tools/replay/ui.py
Unescape View File

@ -158,7 +158,6 @@ def ui_thread(addr):
# TODO brake is deprecated
plot_arr[-1, name_to_arr_idx['computer_brake']] = clip(-sm['carControl'].actuators.accel/4.0, 0.0, 1.0)
plot_arr[-1, name_to_arr_idx['v_ego']] = sm['carState'].vEgo
plot_arr[-1, name_to_arr_idx['v_pid']] = sm['controlsState'].vPid
plot_arr[-1, name_to_arr_idx['v_cruise']] = sm['carState'].cruiseState.speed
plot_arr[-1, name_to_arr_idx['a_ego']] = sm['carState'].aEgo

Write Preview
Loading…
Cancel
Save