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

Revert "remove pedal (#31903)" 

This reverts commit fa12a672
This commit is contained in:
Jason Wen 2024-05-04 23:54:13 -04:00
parent a48a909997
commit 415acad40c
No known key found for this signature in database
GPG Key ID: EF8EA444C1E7B69C
11 changed files with 110 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
selfdrive/car/__init__.py
View File

@ -179,6 +179,27 @@ def crc8_pedal(data):
return crc
def create_gas_interceptor_command(packer, gas_amount, idx):
# Common gas pedal msg generator
enable = gas_amount > 0.001
values = {
"ENABLE": enable,
"COUNTER_PEDAL": idx & 0xF,
}
if enable:
values["GAS_COMMAND"] = gas_amount * 255.
values["GAS_COMMAND2"] = gas_amount * 255.
dat = packer.make_can_msg("GAS_COMMAND", 0, values)[2]
checksum = crc8_pedal(dat[:-1])
values["CHECKSUM_PEDAL"] = checksum
return packer.make_can_msg("GAS_COMMAND", 0, values)
def make_can_msg(addr, dat, bus):
return [addr, 0, dat, bus]

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

@ -4,6 +4,7 @@ from cereal import car
from openpilot.common.numpy_fast import clip, interp
from openpilot.common.realtime import DT_CTRL
from opendbc.can.packer import CANPacker
from openpilot.selfdrive.car import create_gas_interceptor_command
from openpilot.selfdrive.car.honda import hondacan
from openpilot.selfdrive.car.honda.values import CruiseButtons, VISUAL_HUD, HONDA_BOSCH, HONDA_BOSCH_RADARLESS, HONDA_NIDEC_ALT_PCM_ACCEL, CarControllerParams
from openpilot.selfdrive.car.interfaces import CarControllerBase
@ -182,7 +183,7 @@ class CarController(CarControllerBase):
0.5]
# The Honda ODYSSEY seems to have different PCM_ACCEL
# msgs, is it other cars too?
if not CC.longActive:
if self.CP.enableGasInterceptorDEPRECATED or not CC.longActive:
pcm_speed = 0.0
pcm_accel = int(0.0)
elif self.CP.carFingerprint in HONDA_NIDEC_ALT_PCM_ACCEL:
@ -234,6 +235,19 @@ class CarController(CarControllerBase):
self.apply_brake_last = apply_brake
self.brake = apply_brake / self.params.NIDEC_BRAKE_MAX
if self.CP.enableGasInterceptorDEPRECATED:
# way too aggressive at low speed without this
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
# Sending non-zero gas when OP is not enabled will cause the PCM not to respond to throttle as expected
# when you do enable.
if CC.longActive:
self.gas = clip(gas_mult * (gas - brake + wind_brake * 3 / 4), 0., 1.)
else:
self.gas = 0.0
can_sends.append(create_gas_interceptor_command(self.packer, self.gas, self.frame // 2))
# Send dashboard UI commands.
if self.frame % 10 == 0:
hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), hud_control.leadVisible,
@ -242,7 +256,9 @@ class CarController(CarControllerBase):
if self.CP.openpilotLongitudinalControl and self.CP.carFingerprint not in HONDA_BOSCH:
self.speed = pcm_speed
self.gas = pcm_accel / self.params.NIDEC_GAS_MAX
if not self.CP.enableGasInterceptorDEPRECATED:
self.gas = pcm_accel / self.params.NIDEC_GAS_MAX
new_actuators = actuators.copy()
new_actuators.speed = self.speed

13
selfdrive/car/honda/carstate.py
View File

@ -72,6 +72,10 @@ def get_can_messages(CP, gearbox_msg):
else:
messages.append(("DOORS_STATUS", 3))
# add gas interceptor reading if we are using it
if CP.enableGasInterceptorDEPRECATED:
messages.append(("GAS_SENSOR", 50))
if CP.carFingerprint in HONDA_BOSCH_RADARLESS:
messages.append(("CRUISE_FAULT_STATUS", 50))
elif CP.carFingerprint == CAR.CLARITY:
@ -191,8 +195,13 @@ class CarState(CarStateBase):
gear = int(cp.vl[self.gearbox_msg]["GEAR_SHIFTER"])
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(gear, None))
ret.gas = cp.vl["POWERTRAIN_DATA"]["PEDAL_GAS"]
ret.gasPressed = ret.gas > 1e-5
if self.CP.enableGasInterceptorDEPRECATED:
# Same threshold as panda, equivalent to 1e-5 with previous DBC scaling
ret.gas = (cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS"] + cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS2"]) // 2
ret.gasPressed = ret.gas > 492
else:
ret.gas = cp.vl["POWERTRAIN_DATA"]["PEDAL_GAS"]
ret.gasPressed = ret.gas > 1e-5
ret.steeringTorque = cp.vl["STEER_STATUS"]["STEER_TORQUE_SENSOR"]
ret.steeringTorqueEps = cp.vl["STEER_MOTOR_TORQUE"]["MOTOR_TORQUE"]

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

@ -24,6 +24,8 @@ class CarInterface(CarInterfaceBase):
def get_pid_accel_limits(CP, current_speed, cruise_speed):
if CP.carFingerprint in HONDA_BOSCH:
return CarControllerParams.BOSCH_ACCEL_MIN, CarControllerParams.BOSCH_ACCEL_MAX
elif CP.enableGasInterceptorDEPRECATED:
return CarControllerParams.NIDEC_ACCEL_MIN, CarControllerParams.NIDEC_ACCEL_MAX
else:
# NIDECs don't allow acceleration near cruise_speed,
# so limit limits of pid to prevent windup
@ -49,9 +51,10 @@ class CarInterface(CarInterfaceBase):
ret.customStockLongAvailable = True
else:
ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.hondaNidec)]
ret.enableGasInterceptorDEPRECATED = 0x201 in fingerprint[CAN.pt]
ret.openpilotLongitudinalControl = True
ret.pcmCruise = True
ret.pcmCruise = not ret.enableGasInterceptorDEPRECATED
if candidate == CAR.HONDA_CRV_5G:
ret.enableBsm = 0x12f8bfa7 in fingerprint[CAN.radar]
@ -237,13 +240,16 @@ class CarInterface(CarInterfaceBase):
if ret.openpilotLongitudinalControl and candidate in HONDA_BOSCH:
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_HONDA_BOSCH_LONG
if ret.enableGasInterceptorDEPRECATED and candidate not in HONDA_BOSCH:
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_HONDA_GAS_INTERCEPTOR
if candidate in HONDA_BOSCH_RADARLESS:
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_HONDA_RADARLESS
# min speed to enable ACC. if car can do stop and go, then set enabling speed
# to a negative value, so it won't matter. Otherwise, add 0.5 mph margin to not
# conflict with PCM acc
ret.autoResumeSng = candidate in (HONDA_BOSCH | {CAR.HONDA_CIVIC, CAR.HONDA_CLARITY})
ret.autoResumeSng = candidate in (HONDA_BOSCH | {CAR.HONDA_CIVIC, CAR.HONDA_CLARITY}) or ret.enableGasInterceptorDEPRECATED
ret.minEnableSpeed = -1. if ret.autoResumeSng else 25.5 * CV.MPH_TO_MS
ret.steerActuatorDelay = 0.1

2
selfdrive/car/tests/routes.py
View File

@ -295,6 +295,8 @@ routes = [
CarTestRoute("66c1699b7697267d/2024-03-03--13-09-53", TESLA.TESLA_MODELS_RAVEN),
# Segments that test specific issues
# Controls mismatch due to interceptor threshold
CarTestRoute("cfb32f0fb91b173b|2022-04-06--14-54-45", HONDA.CIVIC, segment=21),
# Controls mismatch due to standstill threshold
CarTestRoute("bec2dcfde6a64235|2022-04-08--14-21-32", HONDA.HONDA_CRV_HYBRID, segment=22),
]

4
selfdrive/car/tests/test_car_interfaces.py
View File

@ -74,6 +74,10 @@ class TestCarInterfaces(unittest.TestCase):
self.assertEqual(len(car_params.longitudinalTuning.kiV), len(car_params.longitudinalTuning.kiBP))
self.assertEqual(len(car_params.longitudinalTuning.deadzoneV), len(car_params.longitudinalTuning.deadzoneBP))
# If we're using the interceptor for gasPressed, we should be commanding gas with it
if car_params.enableGasInterceptorDEPRECATED:
self.assertTrue(car_params.openpilotLongitudinalControl)
# Lateral sanity checks
if car_params.steerControlType != car.CarParams.SteerControlType.angle:
tune = car_params.lateralTuning

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

@ -1,11 +1,12 @@
from cereal import car
from openpilot.common.numpy_fast import clip
from openpilot.common.numpy_fast import clip, interp
from openpilot.common.params import Params
from openpilot.selfdrive.car import apply_meas_steer_torque_limits, apply_std_steer_angle_limits, common_fault_avoidance, make_can_msg
from openpilot.selfdrive.car import apply_meas_steer_torque_limits, apply_std_steer_angle_limits, common_fault_avoidance, \
create_gas_interceptor_command, make_can_msg
from openpilot.selfdrive.car.interfaces import CarControllerBase
from openpilot.selfdrive.car.toyota import toyotacan
from openpilot.selfdrive.car.toyota.values import CAR, STATIC_DSU_MSGS, NO_STOP_TIMER_CAR, TSS2_CAR, \
CarControllerParams, ToyotaFlags, \
MIN_ACC_SPEED, PEDAL_TRANSITION, CarControllerParams, ToyotaFlags, \
UNSUPPORTED_DSU_CAR
from opendbc.can.packer import CANPacker
@ -104,6 +105,21 @@ class CarController(CarControllerBase):
lta_active, self.frame // 2, torque_wind_down))
# *** gas and brake ***
if self.CP.enableGasInterceptorDEPRECATED and CC.longActive:
MAX_INTERCEPTOR_GAS = 0.5
# RAV4 has very sensitive gas pedal
if self.CP.carFingerprint in (CAR.RAV4, CAR.RAV4H, CAR.HIGHLANDER):
PEDAL_SCALE = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.15, 0.3, 0.0])
elif self.CP.carFingerprint in (CAR.COROLLA,):
PEDAL_SCALE = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.3, 0.4, 0.0])
else:
PEDAL_SCALE = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.4, 0.5, 0.0])
# offset for creep and windbrake
pedal_offset = interp(CS.out.vEgo, [0.0, 2.3, MIN_ACC_SPEED + PEDAL_TRANSITION], [-.4, 0.0, 0.2])
pedal_command = PEDAL_SCALE * (actuators.accel + pedal_offset)
interceptor_gas_cmd = clip(pedal_command, 0., MAX_INTERCEPTOR_GAS)
else:
interceptor_gas_cmd = 0.
pcm_accel_cmd = clip(actuators.accel, self.params.ACCEL_MIN, self.params.ACCEL_MAX)
# TODO: probably can delete this. CS.pcm_acc_status uses a different signal
@ -112,7 +128,7 @@ class CarController(CarControllerBase):
pcm_cancel_cmd = 1
# on entering standstill, send standstill request
if CS.out.standstill and not self.last_standstill and (self.CP.carFingerprint not in NO_STOP_TIMER_CAR):
if CS.out.standstill and not self.last_standstill and (self.CP.carFingerprint not in NO_STOP_TIMER_CAR or self.CP.enableGasInterceptorDEPRECATED):
self.standstill_req = True
if CS.pcm_acc_status != 8:
# pcm entered standstill or it's disabled
@ -147,6 +163,12 @@ class CarController(CarControllerBase):
else:
can_sends.append(toyotacan.create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, CS.acc_type, False, self.distance_button, reverse_acc))
if self.frame % 2 == 0 and self.CP.enableGasInterceptorDEPRECATED and self.CP.openpilotLongitudinalControl:
# send exactly zero if gas cmd is zero. Interceptor will send the max between read value and gas cmd.
# This prevents unexpected pedal range rescaling
can_sends.append(create_gas_interceptor_command(self.packer, interceptor_gas_cmd, self.frame // 2))
self.gas = interceptor_gas_cmd
# *** hud ui ***
if self.CP.carFingerprint != CAR.TOYOTA_PRIUS_V:
# ui mesg is at 1Hz but we send asap if:

11
selfdrive/car/toyota/carstate.py
View File

@ -69,7 +69,12 @@ class CarState(CarStateBase):
ret.brakeHoldActive = cp.vl["ESP_CONTROL"]["BRAKE_HOLD_ACTIVE"] == 1
ret.brakeLightsDEPRECATED = bool(cp.vl["ESP_CONTROL"]["BRAKE_LIGHTS_ACC"])
ret.gasPressed = cp.vl["PCM_CRUISE"]["GAS_RELEASED"] == 0
if self.CP.enableGasInterceptorDEPRECATED:
ret.gas = (cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS"] + cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS2"]) // 2
ret.gasPressed = ret.gas > 805
else:
# TODO: find a common gas pedal percentage signal
ret.gasPressed = cp.vl["PCM_CRUISE"]["GAS_RELEASED"] == 0
ret.wheelSpeeds = self.get_wheel_speeds(
cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_FL"],
@ -298,6 +303,10 @@ class CarState(CarStateBase):
else:
messages.append(("PCM_CRUISE_2", 33))
# add gas interceptor reading if we are using it
if CP.enableGasInterceptorDEPRECATED:
messages.append(("GAS_SENSOR", 50))
if CP.enableBsm:
messages.append(("BSM", 1))

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

@ -134,20 +134,24 @@ class CarInterface(CarInterfaceBase):
# - TSS-P DSU-less cars w/ CAN filter installed (no radar parser yet)
ret.openpilotLongitudinalControl = use_sdsu or ret.enableDsu or candidate in (TSS2_CAR - RADAR_ACC_CAR) or bool(ret.flags & ToyotaFlags.DISABLE_RADAR.value)
ret.autoResumeSng = ret.openpilotLongitudinalControl and candidate in NO_STOP_TIMER_CAR
ret.enableGasInterceptorDEPRECATED = 0x201 in fingerprint[0] and ret.openpilotLongitudinalControl
if not ret.openpilotLongitudinalControl:
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_TOYOTA_STOCK_LONGITUDINAL
if ret.enableGasInterceptorDEPRECATED:
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_TOYOTA_GAS_INTERCEPTOR
# min speed to enable ACC. if car can do stop and go, then set enabling speed
# to a negative value, so it won't matter.
ret.minEnableSpeed = -1. if stop_and_go else MIN_ACC_SPEED
ret.minEnableSpeed = -1. if (stop_and_go or ret.enableGasInterceptorDEPRECATED) else MIN_ACC_SPEED
sp_tss2_long_tune = Params().get_bool("ToyotaTSS2Long")
tune = ret.longitudinalTuning
tune.deadzoneBP = [0., 9.]
tune.deadzoneV = [.0, .15]
if candidate in TSS2_CAR:
if candidate in TSS2_CAR or ret.enableGasInterceptorDEPRECATED:
tune.kpBP = [0., 5., 20., 30.] if sp_tss2_long_tune else [0., 5., 20.]
tune.kpV = [1.3, 1.0, 0.7, 0.1] if sp_tss2_long_tune else [1.3, 1.0, 0.7]
tune.kiBP = [0., 1., 2., 3., 4., 5., 12., 20., 27., 40.] if sp_tss2_long_tune else [0., 5., 12., 20., 27.]
@ -188,7 +192,7 @@ class CarInterface(CarInterfaceBase):
events.add(EventName.vehicleSensorsInvalid)
if self.CP.openpilotLongitudinalControl:
if ret.cruiseState.standstill and not ret.brakePressed:
if ret.cruiseState.standstill and not ret.brakePressed and not self.CP.enableGasInterceptorDEPRECATED:
events.add(EventName.resumeRequired)
if self.CS.low_speed_lockout:
events.add(EventName.lowSpeedLockout)

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

@ -10,6 +10,8 @@ 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):
# Ignore cruise standstill if car has a gas interceptor
cruise_standstill = cruise_standstill and not CP.enableGasInterceptorDEPRECATED
accelerating = v_target_1sec > v_target
planned_stop = (v_target < CP.vEgoStopping and
v_target_1sec < CP.vEgoStopping and

2
selfdrive/test/process_replay/migration.py
View File

@ -74,7 +74,7 @@ def migrate_pandaStates(lr):
# TODO: safety param migration should be handled automatically
safety_param_migration = {
"TOYOTA_PRIUS": EPS_SCALE["TOYOTA_PRIUS"] | Panda.FLAG_TOYOTA_STOCK_LONGITUDINAL,
"TOYOTA_RAV4": EPS_SCALE["TOYOTA_RAV4"] | Panda.FLAG_TOYOTA_ALT_BRAKE,
"TOYOTA_RAV4": EPS_SCALE["TOYOTA_RAV4"] | Panda.FLAG_TOYOTA_ALT_BRAKE | Panda.FLAG_TOYOTA_GAS_INTERCEPTOR,
"KIA_EV6": Panda.FLAG_HYUNDAI_EV_GAS | Panda.FLAG_HYUNDAI_CANFD_HDA2,
}