mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-02-18 23:33:58 +08:00
Code Issues Packages Projects Releases Wiki Activity

Change car controller interface from gas/brake to acceleration (#21911)

Browse Source 
* retune civic

* seems smooth

* back to normal

* new ref

* fix conflict

* runs

* rm

* accel scale is 4

* toyota should be good

* more cleanup

* fixup

* better naming

* update ref

* deprecated

* sending brake when not enable causes a fault

* rm gas and brake

* unused

* update ref

* acura logic is no more

* wrong before

* revert tuning cleanup

* adress comments

* update ref

* already on master

Co-authored-by: Willem Melching <willem.melching@gmail.com>
This commit is contained in:
HaraldSchafer
2021-08-30 18:32:52 +02:00
committed by GitHub
parent 2764e59362
commit 1ac89f14aa
22 changed files with 103 additions and 181 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
selfdrive/car/chrysler/interface.py
View File

@@ -6,10 +6,6 @@ from selfdrive.car.interfaces import CarInterfaceBase
class CarInterface(CarInterfaceBase):
@staticmethod
def compute_gb(accel, speed):
return float(accel) / 3.0
@staticmethod
def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None):
ret = CarInterfaceBase.get_std_params(candidate, fingerprint)

5
selfdrive/car/ford/interface.py
View File

@@ -7,11 +7,6 @@ from selfdrive.car.interfaces import CarInterfaceBase
class CarInterface(CarInterfaceBase):
@staticmethod
def compute_gb(accel, speed):
return float(accel) / 3.0
@staticmethod
def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None):
ret = CarInterfaceBase.get_std_params(candidate, fingerprint)

9
selfdrive/car/gm/carcontroller.py
View File

@@ -46,18 +46,13 @@ class CarController():
can_sends.append(gmcan.create_steering_control(self.packer_pt, CanBus.POWERTRAIN, apply_steer, idx, lkas_enabled))
# GAS/BRAKE
# no output if not enabled, but keep sending keepalive messages
# treat pedals as one
final_pedal = actuators.gas - actuators.brake
if not enabled:
# Stock ECU sends max regen when not enabled.
apply_gas = P.MAX_ACC_REGEN
apply_brake = 0
else:
apply_gas = int(round(interp(final_pedal, P.GAS_LOOKUP_BP, P.GAS_LOOKUP_V)))
apply_brake = int(round(interp(final_pedal, P.BRAKE_LOOKUP_BP, P.BRAKE_LOOKUP_V)))
apply_gas = int(round(interp(actuators.accel, P.GAS_LOOKUP_BP, P.GAS_LOOKUP_V)))
apply_brake = int(round(interp(actuators.accel, P.BRAKE_LOOKUP_BP, P.BRAKE_LOOKUP_V)))
# Gas/regen and brakes - all at 25Hz
if (frame % 4) == 0:

4
selfdrive/car/gm/interface.py
View File

@@ -11,10 +11,6 @@ EventName = car.CarEvent.EventName
class CarInterface(CarInterfaceBase):
@staticmethod
def compute_gb(accel, speed):
return float(accel) / 4.0
@staticmethod
def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None):
ret = CarInterfaceBase.get_std_params(candidate, fingerprint)

4
selfdrive/car/gm/values.py
View File

@@ -26,9 +26,9 @@ class CarControllerParams():
ZERO_GAS = 2048
MAX_BRAKE = 350 # Should be around 3.5m/s^2, including regen
self.MAX_ACC_REGEN = 1404 # ACC Regen braking is slightly less powerful than max regen paddle
self.GAS_LOOKUP_BP = [-0.25, 0., 0.5]
self.GAS_LOOKUP_BP = [-1.0, 0., 2.0]
self.GAS_LOOKUP_V = [self.MAX_ACC_REGEN, ZERO_GAS, MAX_GAS]
self.BRAKE_LOOKUP_BP = [-1., -0.25]
self.BRAKE_LOOKUP_BP = [-4., -1.0]
self.BRAKE_LOOKUP_V = [MAX_BRAKE, 0]
class CAR:

57
selfdrive/car/honda/carcontroller.py
View File

@@ -11,7 +11,29 @@ from opendbc.can.packer import CANPacker
VisualAlert = car.CarControl.HUDControl.VisualAlert
# TODO: not clear this does anything useful
def compute_gb_honda_bosch(accel, speed):
#TODO returns 0s, is unused
return 0.0, 0.0
def compute_gb_honda_nidec(accel, speed):
creep_brake = 0.0
creep_speed = 2.3
creep_brake_value = 0.15
if speed < creep_speed:
creep_brake = (creep_speed - speed) / creep_speed * creep_brake_value
gb = float(accel) / 4.8 - creep_brake
return clip(gb, 0.0, 1.0), clip(-gb, 0.0, 1.0)
def compute_gas_brake(accel, speed, fingerprint):
if fingerprint in HONDA_BOSCH:
return compute_gb_honda_bosch(accel, speed)
else:
return compute_gb_honda_nidec(accel, speed)
#TODO not clear this does anything useful
def actuator_hystereses(brake, braking, brake_steady, v_ego, car_fingerprint):
# hyst params
brake_hyst_on = 0.02 # to activate brakes exceed this value
@@ -94,8 +116,15 @@ class CarController():
P = self.params
if enabled:
accel = actuators.accel
gas, brake = compute_gas_brake(actuators.accel, CS.out.vEgo, CS.CP.carFingerprint)
else:
accel = 0.0
gas, brake = 0.0, 0.0
# *** apply brake hysteresis ***
brake, self.braking, self.brake_steady = actuator_hystereses(actuators.brake, self.braking, self.brake_steady, CS.out.vEgo, CS.CP.carFingerprint)
pre_limit_brake, self.braking, self.brake_steady = actuator_hystereses(brake, self.braking, self.brake_steady, CS.out.vEgo, CS.CP.carFingerprint)
# *** no output if not enabled ***
if not enabled and CS.out.cruiseState.enabled:
@@ -107,7 +136,7 @@ class CarController():
pcm_cancel_cmd = pcm_cancel_cmd and CS.CP.pcmCruise
# *** rate limit after the enable check ***
self.brake_last = rate_limit(brake, self.brake_last, -2., DT_CTRL)
self.brake_last = rate_limit(pre_limit_brake, self.brake_last, -2., DT_CTRL)
# vehicle hud display, wait for one update from 10Hz 0x304 msg
if hud_show_lanes:
@@ -147,18 +176,13 @@ class CarController():
lkas_active, CS.CP.carFingerprint, idx, CS.CP.openpilotLongitudinalControl))
accel = actuators.gas - actuators.brake
# TODO: pass in LoC.long_control_state and use that to decide starting/stoppping
stopping = accel < 0 and CS.out.vEgo < 0.3
starting = accel > 0 and CS.out.vEgo < 0.3
# Prevent rolling backwards
accel = -1.0 if stopping else accel
if CS.CP.carFingerprint in HONDA_BOSCH:
apply_accel = interp(accel, P.BOSCH_ACCEL_LOOKUP_BP, P.BOSCH_ACCEL_LOOKUP_V)
else:
apply_accel = interp(accel, P.NIDEC_ACCEL_LOOKUP_BP, P.NIDEC_ACCEL_LOOKUP_V)
accel = -4.0 if stopping else accel
# wind brake from air resistance decel at high speed
wind_brake = interp(CS.out.vEgo, [0.0, 2.3, 35.0], [0.001, 0.002, 0.15])
@@ -167,14 +191,15 @@ class CarController():
pcm_accel = int(clip(pcm_accel, 0, 1) * 0xc6)
else:
max_accel = interp(CS.out.vEgo, P.NIDEC_MAX_ACCEL_BP, P.NIDEC_MAX_ACCEL_V)
pcm_accel = int(clip(apply_accel/max_accel, 0.0, 1.0) * 0xc6)
# TODO this 1.44 is just to maintain previous behavior
pcm_accel = int(clip((accel/1.44)/max_accel, 0.0, 1.0) * 0xc6)
pcm_speed_BP = [-wind_brake,
-wind_brake*(3/4),
0.0]
pcm_speed_V = [0.0,
clip(CS.out.vEgo + apply_accel/2.0 - 2.0, 0.0, 100.0),
clip(CS.out.vEgo + apply_accel/2.0 + 2.0, 0.0, 100.0)]
pcm_speed = interp(accel, pcm_speed_BP, pcm_speed_V)
clip(CS.out.vEgo + accel/2.0 - 2.0, 0.0, 100.0),
clip(CS.out.vEgo + accel/2.0 + 2.0, 0.0, 100.0)]
pcm_speed = interp(-brake, pcm_speed_BP, pcm_speed_V)
if not CS.CP.openpilotLongitudinalControl:
if (frame % 2) == 0:
@@ -193,8 +218,8 @@ class CarController():
ts = frame * DT_CTRL
if CS.CP.carFingerprint in HONDA_BOSCH:
apply_gas = interp(accel, P.BOSCH_GAS_LOOKUP_BP, P.BOSCH_GAS_LOOKUP_V)
can_sends.extend(hondacan.create_acc_commands(self.packer, enabled, apply_accel, apply_gas, idx, stopping, starting, CS.CP.carFingerprint))
bosch_gas = interp(accel, P.BOSCH_GAS_LOOKUP_BP, P.BOSCH_GAS_LOOKUP_V)
can_sends.extend(hondacan.create_acc_commands(self.packer, enabled, accel, bosch_gas, idx, stopping, starting, CS.CP.carFingerprint))
else:
apply_brake = clip(self.brake_last - wind_brake, 0.0, 1.0)
@@ -209,7 +234,7 @@ class CarController():
gas_mult = interp(CS.out.vEgo, [0., 10.], [0.4, 1.0])
# send exactly zero if apply_gas is zero. Interceptor will send the max between read value and apply_gas.
# This prevents unexpected pedal range rescaling
apply_gas = clip(gas_mult * actuators.gas, 0., 1.)
apply_gas = clip(gas_mult * gas, 0., 1.)
can_sends.append(create_gas_command(self.packer, apply_gas, idx))
hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), hud_car,

44
selfdrive/car/honda/interface.py
View File

@@ -15,38 +15,7 @@ EventName = car.CarEvent.EventName
TransmissionType = car.CarParams.TransmissionType
def compute_gb_honda_bosch(accel, speed):
return float(accel) / 3.5
def compute_gb_honda_nidec(accel, speed):
creep_brake = 0.0
creep_speed = 2.3
creep_brake_value = 0.15
if speed < creep_speed:
creep_brake = (creep_speed - speed) / creep_speed * creep_brake_value
return float(accel) / 4.8 - creep_brake
def compute_gb_acura(accel, speed):
GB_VALUES = [-2., 0.0, 0.8]
GB_BP = [-5., 0.0, 4.0]
return interp(accel, GB_BP, GB_VALUES)
class CarInterface(CarInterfaceBase):
def __init__(self, CP, CarController, CarState):
super().__init__(CP, CarController, CarState)
if self.CS.CP.carFingerprint in HONDA_BOSCH:
self.compute_gb = compute_gb_honda_bosch
else:
self.compute_gb = compute_gb_honda_nidec
@staticmethod
def compute_gb(accel, speed): # pylint: disable=method-hidden
raise NotImplementedError
@staticmethod
def calc_accel_override(a_ego, a_target, v_ego, v_target):
@@ -333,17 +302,6 @@ class CarInterface(CarInterfaceBase):
ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront,
tire_stiffness_factor=tire_stiffness_factor)
if candidate in HONDA_BOSCH:
ret.gasMaxBP = [0.] # m/s
ret.gasMaxV = [0.6]
ret.brakeMaxBP = [0.] # m/s
ret.brakeMaxV = [1.] # max brake allowed, 3.5m/s^2
else:
ret.gasMaxBP = [0.] # m/s
ret.gasMaxV = [0.6] # max gas allowed
ret.brakeMaxBP = [5., 20.] # m/s
ret.brakeMaxV = [1., 0.8] # max brake allowed
ret.startAccel = 0.5
ret.steerActuatorDelay = 0.1
@@ -422,7 +380,7 @@ class CarInterface(CarInterfaceBase):
# we engage when pcm is active (rising edge)
if ret.cruiseState.enabled and not self.CS.out.cruiseState.enabled:
events.add(EventName.pcmEnable)
elif not ret.cruiseState.enabled and (c.actuators.brake <= 0. or not self.CP.openpilotLongitudinalControl):
elif not ret.cruiseState.enabled and (c.actuators.accel >= 0. or not self.CP.openpilotLongitudinalControl):
# it can happen that car cruise disables while comma system is enabled: need to
# keep braking if needed or if the speed is very low
if ret.vEgo < self.CP.minEnableSpeed + 2.:

5
selfdrive/car/honda/values.py
View File

@@ -22,10 +22,7 @@ class CarControllerParams():
self.NIDEC_MAX_ACCEL_V = [0.5, 2.4, 1.4, 0.6]
self.NIDEC_MAX_ACCEL_BP = [0.0, 4.0, 10., 20.]
self.BOSCH_ACCEL_LOOKUP_BP = [-1., 0., 0.6]
self.BOSCH_ACCEL_LOOKUP_V = [-3.5, 0., 2.]
self.BOSCH_GAS_LOOKUP_BP = [0., 0.6]
self.BOSCH_GAS_LOOKUP_BP = [0., 2.0] # 2m/s^2
self.BOSCH_GAS_LOOKUP_V = [0, 2000]

5
selfdrive/car/hyundai/interface.py
View File

@@ -6,11 +6,6 @@ from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness,
from selfdrive.car.interfaces import CarInterfaceBase
class CarInterface(CarInterfaceBase):
@staticmethod
def compute_gb(accel, speed):
return float(accel) / 3.0
@staticmethod
def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[]): # pylint: disable=dangerous-default-value
ret = CarInterfaceBase.get_std_params(candidate, fingerprint)

12
selfdrive/car/interfaces.py
View File

@@ -45,10 +45,6 @@ class CarInterfaceBase():
def calc_accel_override(a_ego, a_target, v_ego, v_target):
return 1.
@staticmethod
def compute_gb(accel, speed):
raise NotImplementedError
@staticmethod
def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None):
raise NotImplementedError
@@ -72,15 +68,11 @@ class CarInterfaceBase():
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
ret.gasMaxBP = [0.]
ret.gasMaxV = [.5] # half max brake
ret.brakeMaxBP = [0.]
ret.brakeMaxV = [1.]
ret.openpilotLongitudinalControl = False
ret.startAccel = 0.0
ret.minSpeedCan = 0.3
ret.stoppingBrakeRate = 0.2 # brake_travel/s while trying to stop
ret.startingBrakeRate = 0.8 # brake_travel/s while releasing on restart
ret.stoppingDecelRate = 0.8 # brake_travel/s while trying to stop
ret.startingAccelRate = 3.2 # brake_travel/s while releasing on restart
ret.stoppingControl = True
ret.longitudinalTuning.deadzoneBP = [0.]
ret.longitudinalTuning.deadzoneV = [0.]

4
selfdrive/car/nissan/interface.py
View File

@@ -9,10 +9,6 @@ class CarInterface(CarInterfaceBase):
super().__init__(CP, CarController, CarState)
self.cp_adas = self.CS.get_adas_can_parser(CP)
@staticmethod
def compute_gb(accel, speed):
return float(accel) / 4.0
@staticmethod
def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None):

4
selfdrive/car/subaru/interface.py
View File

@@ -6,10 +6,6 @@ from selfdrive.car.interfaces import CarInterfaceBase
class CarInterface(CarInterfaceBase):
@staticmethod
def compute_gb(accel, speed):
return float(accel) / 4.0
@staticmethod
def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None):
ret = CarInterfaceBase.get_std_params(candidate, fingerprint)

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

@@ -6,11 +6,6 @@ from selfdrive.car.interfaces import CarInterfaceBase
class CarInterface(CarInterfaceBase):
@staticmethod
def compute_gb(accel, speed):
# TODO: is this correct?
return accel
@staticmethod
def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None):
ret = CarInterfaceBase.get_std_params(candidate, fingerprint)

12
selfdrive/car/toyota/carcontroller.py
View File

@@ -1,5 +1,5 @@
from cereal import car
from common.numpy_fast import clip
from common.numpy_fast import clip, interp
from selfdrive.car import apply_toyota_steer_torque_limits, create_gas_command, make_can_msg
from selfdrive.car.toyota.toyotacan import create_steer_command, create_ui_command, \
create_accel_command, create_acc_cancel_command, \
@@ -7,7 +7,6 @@ from selfdrive.car.toyota.toyotacan import create_steer_command, create_ui_comma
from selfdrive.car.toyota.values import CAR, STATIC_DSU_MSGS, NO_STOP_TIMER_CAR, TSS2_CAR, \
MIN_ACC_SPEED, PEDAL_HYST_GAP, CarControllerParams
from opendbc.can.packer import CANPacker
VisualAlert = car.CarControl.HUDControl.VisualAlert
@@ -45,7 +44,9 @@ class CarController():
# gas and brake
interceptor_gas_cmd = 0.
pcm_accel_cmd = actuators.gas - actuators.brake
# TODO this is needed to preserve behavior, but doesn't make sense
# This can all be cleaned up
pcm_accel_cmd = actuators.accel / CarControllerParams.ACCEL_SCALE
if CS.CP.enableGasInterceptor:
# handle hysteresis when around the minimum acc speed
@@ -57,8 +58,9 @@ class CarController():
if self.use_interceptor and enabled:
# only send negative accel when using interceptor. gas handles acceleration
# +0.06 offset to reduce ABS pump usage when OP is engaged
interceptor_gas_cmd = clip(actuators.gas, 0., 1.)
pcm_accel_cmd = 0.06 - actuators.brake
MAX_INTERCEPTOR_GAS = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED], [0.2, 0.5])
interceptor_gas_cmd = clip(actuators.accel/CarControllerParams.ACCEL_SCALE, 0., MAX_INTERCEPTOR_GAS)
pcm_accel_cmd = 0.06 - clip(-actuators.accel/CarControllerParams.ACCEL_SCALE, 0., 1.)
pcm_accel_cmd, self.accel_steady = accel_hysteresis(pcm_accel_cmd, self.accel_steady, enabled)
pcm_accel_cmd = clip(pcm_accel_cmd * CarControllerParams.ACCEL_SCALE, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX)

15
selfdrive/car/toyota/interface.py
View File

@@ -1,7 +1,7 @@
#!/usr/bin/env python3
from cereal import car
from selfdrive.config import Conversions as CV
from selfdrive.car.toyota.values import Ecu, CAR, TSS2_CAR, NO_DSU_CAR, MIN_ACC_SPEED, PEDAL_HYST_GAP, CarControllerParams
from selfdrive.car.toyota.values import Ecu, CAR, TSS2_CAR, NO_DSU_CAR, MIN_ACC_SPEED, PEDAL_HYST_GAP
from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint
from selfdrive.car.interfaces import CarInterfaceBase
@@ -9,10 +9,6 @@ EventName = car.CarEvent.EventName
class CarInterface(CarInterfaceBase):
@staticmethod
def compute_gb(accel, speed):
return float(accel) / CarControllerParams.ACCEL_SCALE
@staticmethod
def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[]): # pylint: disable=dangerous-default-value
ret = CarInterfaceBase.get_std_params(candidate, fingerprint)
@@ -316,9 +312,6 @@ class CarInterface(CarInterfaceBase):
ret.communityFeature = ret.enableGasInterceptor or ret.enableDsu or smartDsu
if ret.enableGasInterceptor:
# Transitions from original pedal tuning at MIN_ACC_SPEED to default tuning at MIN_ACC_SPEED + hysteresis gap
ret.gasMaxBP = [0., MIN_ACC_SPEED]
ret.gasMaxV = [0.2, 0.5]
ret.longitudinalTuning.kpBP = [0., 5., MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_HYST_GAP, 35.]
ret.longitudinalTuning.kpV = [1.2, 0.8, 0.765, 2.255, 1.5]
ret.longitudinalTuning.kiBP = [0., MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_HYST_GAP, 35.]
@@ -331,8 +324,8 @@ class CarInterface(CarInterfaceBase):
ret.longitudinalTuning.kpV = [1.3, 1.0, 0.7]
ret.longitudinalTuning.kiBP = [0., 5., 12., 20., 27.]
ret.longitudinalTuning.kiV = [.35, .23, .20, .17, .1]
ret.stoppingBrakeRate = 0.1 # reach stopping target smoothly
ret.startingBrakeRate = 2.0 # release brakes fast
ret.stoppingDecelRate = 0.3 # reach stopping target smoothly
ret.startingAccelRate = 6.0 # release brakes fast
ret.startAccel = 1.2 # Accelerate from 0 faster
else:
# Default longitudinal tune
@@ -363,7 +356,7 @@ class CarInterface(CarInterfaceBase):
events.add(EventName.lowSpeedLockout)
if ret.vEgo < self.CP.minEnableSpeed and self.CP.openpilotLongitudinalControl:
events.add(EventName.belowEngageSpeed)
if c.actuators.gas > 0.1:
if c.actuators.accel > 0.3:
# some margin on the actuator to not false trigger cancellation while stopping
events.add(EventName.speedTooLow)
if ret.vEgo < 0.001:

4
selfdrive/car/volkswagen/interface.py
View File

@@ -20,10 +20,6 @@ class CarInterface(CarInterfaceBase):
self.ext_bus = CANBUS.cam
self.cp_ext = self.cp_cam
@staticmethod
def compute_gb(accel, speed):
return float(accel) / 4.0
@staticmethod
def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None):
ret = CarInterfaceBase.get_std_params(candidate, fingerprint)

11
selfdrive/controls/controlsd.py
View File

@@ -118,7 +118,7 @@ class Controls:
self.AM = AlertManager()
self.events = Events()
self.LoC = LongControl(self.CP, self.CI.compute_gb)
self.LoC = LongControl(self.CP)
self.VM = VehicleModel(self.CP)
if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
@@ -459,8 +459,8 @@ class Controls:
self.LoC.reset(v_pid=CS.vEgo)
if not self.joystick_mode:
# Gas/Brake PID loop
actuators.gas, actuators.brake, self.v_target, self.a_target = self.LoC.update(self.active, CS, self.CP, long_plan)
# accel PID loop
actuators.accel, self.v_target, self.a_target = self.LoC.update(self.active, CS, self.CP, long_plan)
# Steering PID loop and lateral MPC
desired_curvature, desired_curvature_rate = get_lag_adjusted_curvature(self.CP, CS.vEgo,
@@ -472,8 +472,7 @@ class Controls:
else:
lac_log = log.ControlsState.LateralDebugState.new_message()
if self.sm.rcv_frame['testJoystick'] > 0 and self.active:
gb = clip(self.sm['testJoystick'].axes[0], -1, 1)
actuators.gas, actuators.brake = max(gb, 0), max(-gb, 0)
actuators.accel = 4.0*clip(self.sm['testJoystick'].axes[0], -1, 1)
steer = clip(self.sm['testJoystick'].axes[1], -1, 1)
# max angle is 45 for angle-based cars
@@ -528,7 +527,7 @@ class Controls:
# TODO remove car specific stuff in controls
# Some override values for Honda
# brake discount removes a sharp nonlinearity
brake_discount = (1.0 - clip(actuators.brake * 3., 0.0, 1.0))
brake_discount = (1.0 - clip(-actuators.accel * (3.0/4.0), 0.0, 1.0))
speed_override = max(0.0, (self.LoC.v_pid + CS.cruiseState.speedOffset) * brake_discount)
CC.cruiseControl.speedOverride = float(speed_override if self.CP.pcmCruise else 0.0)
CC.cruiseControl.accelOverride = float(self.CI.calc_accel_override(CS.aEgo, self.a_target,

54
selfdrive/controls/lib/longcontrol.py
View File

@@ -6,19 +6,23 @@ from selfdrive.modeld.constants import T_IDXS
LongCtrlState = log.ControlsState.LongControlState
ACCEL_MAX = 2.0
ACCEL_MIN = -4.0
ACCEL_SCALE = 4.0
STOPPING_EGO_SPEED = 0.5
STOPPING_TARGET_SPEED_OFFSET = 0.01
STARTING_TARGET_SPEED = 0.5
BRAKE_THRESHOLD_TO_PID = 0.2
DECEL_THRESHOLD_TO_PID = 0.8
BRAKE_STOPPING_TARGET = 0.5 # apply at least this amount of brake to maintain the vehicle stationary
DECEL_STOPPING_TARGET = 2.0 # apply at least this amount of brake to maintain the vehicle stationary
RATE = 100.0
DEFAULT_LONG_LAG = 0.15
# TODO this logic isn't really car independent, does not belong here
def long_control_state_trans(active, long_control_state, v_ego, v_target, v_pid,
output_gb, brake_pressed, cruise_standstill, min_speed_can):
output_accel, brake_pressed, cruise_standstill, min_speed_can):
"""Update longitudinal control state machine"""
stopping_target_speed = min_speed_can + STOPPING_TARGET_SPEED_OFFSET
stopping_condition = (v_ego < 2.0 and cruise_standstill) or \
@@ -47,22 +51,23 @@ def long_control_state_trans(active, long_control_state, v_ego, v_target, v_pid,
elif long_control_state == LongCtrlState.starting:
if stopping_condition:
long_control_state = LongCtrlState.stopping
elif output_gb >= -BRAKE_THRESHOLD_TO_PID:
elif output_accel >= -DECEL_THRESHOLD_TO_PID:
long_control_state = LongCtrlState.pid
return long_control_state
class LongControl():
def __init__(self, CP, compute_gb):
def __init__(self, CP):
self.long_control_state = LongCtrlState.off # initialized to off
self.pid = PIController((CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV),
(CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV),
rate=RATE,
sat_limit=0.8,
convert=compute_gb)
sat_limit=0.8)
self.pid.pos_limit = ACCEL_MAX
self.pid.neg_limit = ACCEL_MIN
self.v_pid = 0.0
self.last_output_gb = 0.0
self.last_output_accel = 0.0
def reset(self, v_pid):
"""Reset PID controller and change setpoint"""
@@ -83,55 +88,48 @@ class LongControl():
a_target = 0.0
# Actuation limits
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
output_accel = self.last_output_accel
self.long_control_state = long_control_state_trans(active, self.long_control_state, CS.vEgo,
v_target_future, self.v_pid, output_gb,
v_target_future, self.v_pid, output_accel,
CS.brakePressed, CS.cruiseState.standstill, CP.minSpeedCan)
v_ego_pid = max(CS.vEgo, CP.minSpeedCan) # Without this we get jumps, CAN bus reports 0 when speed < 0.3
if self.long_control_state == LongCtrlState.off or CS.gasPressed:
self.reset(v_ego_pid)
output_gb = 0.
output_accel = 0.
# tracking objects and driving
elif self.long_control_state == LongCtrlState.pid:
self.v_pid = v_target
self.pid.pos_limit = gas_max
self.pid.neg_limit = - brake_max
# 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
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)
output_accel = self.pid.update(self.v_pid, v_ego_pid, speed=v_ego_pid, deadzone=deadzone, feedforward=a_target, freeze_integrator=prevent_overshoot)
if prevent_overshoot:
output_gb = min(output_gb, 0.0)
output_accel = min(output_accel, 0.0)
# 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_gb > -BRAKE_STOPPING_TARGET:
output_gb -= CP.stoppingBrakeRate / RATE
output_gb = clip(output_gb, -brake_max, gas_max)
if not CS.standstill or output_accel > -DECEL_STOPPING_TARGET:
output_accel -= CP.stoppingDecelRate / RATE
output_accel = clip(output_accel, ACCEL_MIN, ACCEL_MAX)
self.reset(CS.vEgo)
# 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 += CP.startingBrakeRate / RATE
if output_accel < -DECEL_THRESHOLD_TO_PID:
output_accel += CP.startingAccelRate / RATE
self.reset(CS.vEgo)
self.last_output_gb = output_gb
final_gas = clip(output_gb, 0., gas_max)
final_brake = -clip(output_gb, -brake_max, 0.)
self.last_output_accel = output_accel
final_accel = clip(output_accel, ACCEL_MIN, ACCEL_MAX)
return final_gas, final_brake, v_target, a_target
return final_accel, v_target, a_target

9
selfdrive/controls/lib/pid.py
View File

@@ -11,7 +11,7 @@ def apply_deadzone(error, deadzone):
return error
class PIController():
def __init__(self, k_p, k_i, k_f=1., pos_limit=None, neg_limit=None, rate=100, sat_limit=0.8, convert=None):
def __init__(self, k_p, k_i, k_f=1., pos_limit=None, neg_limit=None, rate=100, sat_limit=0.8):
self._k_p = k_p # proportional gain
self._k_i = k_i # integral gain
self.k_f = k_f # feedforward gain
@@ -23,7 +23,6 @@ class PIController():
self.i_unwind_rate = 0.3 / rate
self.i_rate = 1.0 / rate
self.sat_limit = sat_limit
self.convert = convert
self.reset()
@@ -68,9 +67,6 @@ class PIController():
i = self.i + error * self.k_i * self.i_rate
control = self.p + self.f + i
if self.convert is not None:
control = self.convert(control, speed=self.speed)
# Update when changing i will move the control away from the limits
# or when i will move towards the sign of the error
if ((error >= 0 and (control <= self.pos_limit or i < 0.0)) or
@@ -79,9 +75,6 @@ class PIController():
self.i = i
control = self.p + self.f + self.i
if self.convert is not None:
control = self.convert(control, speed=self.speed)
self.saturated = self._check_saturation(control, check_saturation, error)
self.control = clip(control, self.neg_limit, self.pos_limit)

2
selfdrive/test/process_replay/ref_commit
View File

@@ -1 +1 @@
b9d80fdb3942a1c43621349dafe0763b03717c10
b845a62e02d23f734dad2811b6a96d17447a933f

7
tools/replay/ui.py
View File

@@ -10,6 +10,7 @@ import numpy as np
import pygame # pylint: disable=import-error
import cereal.messaging as messaging
from common.numpy_fast import clip
from common.basedir import BASEDIR
from selfdrive.config import UIParams as UP
from tools.replay.lib.ui_helpers import (_BB_TO_FULL_FRAME, _FULL_FRAME_SIZE,
@@ -146,10 +147,12 @@ def ui_thread(addr, frame_address):
plot_arr[-1, name_to_arr_idx['angle_steers_des']] = sm['carControl'].actuators.steeringAngleDeg
plot_arr[-1, name_to_arr_idx['angle_steers_k']] = angle_steers_k
plot_arr[-1, name_to_arr_idx['gas']] = sm['carState'].gas
plot_arr[-1, name_to_arr_idx['computer_gas']] = sm['carControl'].actuators.gas
# TODO gas is deprecated
plot_arr[-1, name_to_arr_idx['computer_gas']] = clip(sm['carControl'].actuators.accel/4.0, 0.0, 1.0)
plot_arr[-1, name_to_arr_idx['user_brake']] = sm['carState'].brake
plot_arr[-1, name_to_arr_idx['steer_torque']] = sm['carControl'].actuators.steer * ANGLE_SCALE
plot_arr[-1, name_to_arr_idx['computer_brake']] = sm['carControl'].actuators.brake
# 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_override']] = sm['carControl'].cruiseControl.speedOverride

8
tools/sim/bridge.py
View File

@@ -11,6 +11,7 @@ from typing import Any
import cereal.messaging as messaging
from common.params import Params
from common.numpy_fast import clip
from common.realtime import Ratekeeper, DT_DMON
from lib.can import can_function
from selfdrive.car.honda.values import CruiseButtons
@@ -295,9 +296,10 @@ def bridge(q):
if is_openpilot_engaged:
sm.update(0)
throttle_op = sm['carControl'].actuators.gas #[0,1]
brake_op = sm['carControl'].actuators.brake #[0,1]
steer_op = sm['carControl'].actuators.steeringAngleDeg
# TODO gas and brake is deprecated
throttle_op = clip(sm['carControl'].actuators.accel/4.0, 0.0, 1.0)
brake_op = clip(-sm['carControl'].actuators.accel/4.0, 0.0, 1.0)
steer_op = sm['carControl'].actuators.steeringAngleDeg
throttle_out = throttle_op
steer_out = steer_op