spacecruft
/
nopenpilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Honda: clean up CarInterface.apply and CarController (#23876) 

* clean up honda/carcontroller

* indentation

* fixes

* clean up apply function

clean up apply function
pull/24010/head
Shane Smiskol 2022-03-18 19:10:59 -07:00 committed by GitHub
parent 99c02bdd29
commit 70286766da
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 66 additions and 69 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -1,18 +1,21 @@
from collections import namedtuple from collections import namedtuple
from cereal import car from cereal import car
from common.realtime import DT_CTRL from common.conversions import Conversions as CV
from selfdrive.controls.lib.drive_helpers import rate_limit
from common.numpy_fast import clip, interp from common.numpy_fast import clip, interp
from common.realtime import DT_CTRL
from opendbc.can.packer import CANPacker
from selfdrive.car import create_gas_interceptor_command from selfdrive.car import create_gas_interceptor_command
from selfdrive.car.honda import hondacan from selfdrive.car.honda import hondacan
from selfdrive.car.honda.values import CruiseButtons, VISUAL_HUD, HONDA_BOSCH, HONDA_NIDEC_ALT_PCM_ACCEL, CarControllerParams from selfdrive.car.honda.values import CruiseButtons, VISUAL_HUD, HONDA_BOSCH, HONDA_NIDEC_ALT_PCM_ACCEL, CarControllerParams
from opendbc.can.packer import CANPacker from selfdrive.controls.lib.drive_helpers import rate_limit
VisualAlert = car.CarControl.HUDControl.VisualAlert VisualAlert = car.CarControl.HUDControl.VisualAlert
LongCtrlState = car.CarControl.Actuators.LongControlState LongCtrlState = car.CarControl.Actuators.LongControlState
def compute_gb_honda_bosch(accel, speed): def compute_gb_honda_bosch(accel, speed):
#TODO returns 0s, is unused # TODO returns 0s, is unused
return 0.0, 0.0 return 0.0, 0.0
@ -33,14 +36,14 @@ def compute_gas_brake(accel, speed, fingerprint):
return compute_gb_honda_nidec(accel, speed) return compute_gb_honda_nidec(accel, speed)
#TODO not clear this does anything useful # TODO not clear this does anything useful
def actuator_hystereses(brake, braking, brake_steady, v_ego, car_fingerprint): def actuator_hysteresis(brake, braking, brake_steady, v_ego, car_fingerprint):
# hyst params # hyst params
brake_hyst_on = 0.02 # to activate brakes exceed this value brake_hyst_on = 0.02 # to activate brakes exceed this value
brake_hyst_off = 0.005 # to deactivate brakes below this value brake_hyst_off = 0.005 # to deactivate brakes below this value
brake_hyst_gap = 0.01 # don't change brake command for small oscillations within this value brake_hyst_gap = 0.01 # don't change brake command for small oscillations within this value
#*** hysteresis logic to avoid brake blinking. go above 0.1 to trigger # *** hysteresis logic to avoid brake blinking. go above 0.1 to trigger
if (brake < brake_hyst_on and not braking) or brake < brake_hyst_off: if (brake < brake_hyst_on and not braking) or brake < brake_hyst_off:
brake = 0. brake = 0.
braking = brake > 0. braking = brake > 0.
@ -93,32 +96,34 @@ def process_hud_alert(hud_alert):
HUDData = namedtuple("HUDData", HUDData = namedtuple("HUDData",
["pcm_accel", "v_cruise", "car", ["pcm_accel", "v_cruise", "car",
"lanes", "fcw", "acc_alert", "steer_required"]) "lanes", "fcw", "acc_alert", "steer_required"])
class CarController(): class CarController:
def __init__(self, dbc_name, CP, VM): def __init__(self, dbc_name, CP, VM):
self.CP = CP self.CP = CP
self.packer = CANPacker(dbc_name)
self.params = CarControllerParams(CP)
self.frame = 0
self.braking = False self.braking = False
self.brake_steady = 0. self.brake_steady = 0.
self.brake_last = 0. self.brake_last = 0.
self.apply_brake_last = 0 self.apply_brake_last = 0
self.last_pump_ts = 0. self.last_pump_ts = 0.
self.packer = CANPacker(dbc_name)
self.accel = 0 self.accel = 0
self.speed = 0 self.speed = 0
self.gas = 0 self.gas = 0
self.brake = 0 self.brake = 0
self.params = CarControllerParams(CP) def update(self, CC, CS):
actuators = CC.actuators
hud_control = CC.hudControl
hud_v_cruise = hud_control.setSpeed * CV.MS_TO_KPH if hud_control.speedVisible else 255
pcm_cancel_cmd = CC.cruiseControl.cancel
def update(self, c, CS, frame, actuators, pcm_cancel_cmd, if CC.longActive:
hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert):
P = self.params
if c.longActive:
accel = actuators.accel accel = actuators.accel
gas, brake = compute_gas_brake(actuators.accel, CS.out.vEgo, self.CP.carFingerprint) gas, brake = compute_gas_brake(actuators.accel, CS.out.vEgo, self.CP.carFingerprint)
else: else:
@ -126,57 +131,58 @@ class CarController():
gas, brake = 0.0, 0.0 gas, brake = 0.0, 0.0
# *** apply brake hysteresis *** # *** apply brake hysteresis ***
pre_limit_brake, self.braking, self.brake_steady = actuator_hystereses(brake, self.braking, self.brake_steady, CS.out.vEgo, self.CP.carFingerprint) pre_limit_brake, self.braking, self.brake_steady = actuator_hysteresis(brake, self.braking, self.brake_steady,
CS.out.vEgo, self.CP.carFingerprint)
# *** rate limit after the enable check *** # *** rate limit after the enable check ***
self.brake_last = rate_limit(pre_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 # vehicle hud display, wait for one update from 10Hz 0x304 msg
if hud_show_lanes: if hud_control.lanesVisible:
hud_lanes = 1 hud_lanes = 1
else: else:
hud_lanes = 0 hud_lanes = 0
if c.enabled: if CC.enabled:
if hud_show_car: if hud_control.leadVisible:
hud_car = 2 hud_car = 2
else: else:
hud_car = 1 hud_car = 1
else: else:
hud_car = 0 hud_car = 0
fcw_display, steer_required, acc_alert = process_hud_alert(hud_alert) fcw_display, steer_required, acc_alert = process_hud_alert(hud_control.visualAlert)
# **** process the car messages **** # **** process the car messages ****
# steer torque is converted back to CAN reference (positive when steering right) # steer torque is converted back to CAN reference (positive when steering right)
apply_steer = int(interp(-actuators.steer * P.STEER_MAX, P.STEER_LOOKUP_BP, P.STEER_LOOKUP_V)) apply_steer = int(interp(-actuators.steer * self.params.STEER_MAX,
self.params.STEER_LOOKUP_BP, self.params.STEER_LOOKUP_V))
# Send CAN commands. # Send CAN commands
can_sends = [] can_sends = []
# tester present - w/ no response (keeps radar disabled) # tester present - w/ no response (keeps radar disabled)
if self.CP.carFingerprint in HONDA_BOSCH and self.CP.openpilotLongitudinalControl: if self.CP.carFingerprint in HONDA_BOSCH and self.CP.openpilotLongitudinalControl:
if (frame % 10) == 0: if self.frame % 10 == 0:
can_sends.append((0x18DAB0F1, 0, b"\x02\x3E\x80\x00\x00\x00\x00\x00", 1)) can_sends.append((0x18DAB0F1, 0, b"\x02\x3E\x80\x00\x00\x00\x00\x00", 1))
# Send steering command. # Send steering command.
idx = frame % 4 idx = self.frame % 4
can_sends.append(hondacan.create_steering_control(self.packer, apply_steer, can_sends.append(hondacan.create_steering_control(self.packer, apply_steer, CC.latActive, self.CP.carFingerprint,
c.latActive, self.CP.carFingerprint, idx, CS.CP.openpilotLongitudinalControl)) idx, CS.CP.openpilotLongitudinalControl))
stopping = actuators.longControlState == LongCtrlState.stopping stopping = actuators.longControlState == LongCtrlState.stopping
# wind brake from air resistance decel at high speed # 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]) wind_brake = interp(CS.out.vEgo, [0.0, 2.3, 35.0], [0.001, 0.002, 0.15])
# all of this is only relevant for HONDA NIDEC # all of this is only relevant for HONDA NIDEC
max_accel = interp(CS.out.vEgo, P.NIDEC_MAX_ACCEL_BP, P.NIDEC_MAX_ACCEL_V) max_accel = interp(CS.out.vEgo, self.params.NIDEC_MAX_ACCEL_BP, self.params.NIDEC_MAX_ACCEL_V)
# TODO this 1.44 is just to maintain previous behavior # TODO this 1.44 is just to maintain previous behavior
pcm_speed_BP = [-wind_brake, pcm_speed_BP = [-wind_brake,
-wind_brake*(3/4), -wind_brake * (3 / 4),
0.0, 0.0,
0.5] 0.5]
# The Honda ODYSSEY seems to have different PCM_ACCEL # The Honda ODYSSEY seems to have different PCM_ACCEL
# msgs, is it other cars too? # msgs, is it other cars too?
if self.CP.enableGasInterceptor: if self.CP.enableGasInterceptor:
@ -187,19 +193,19 @@ class CarController():
clip(CS.out.vEgo - 3.0, 0.0, 100.0), clip(CS.out.vEgo - 3.0, 0.0, 100.0),
clip(CS.out.vEgo + 0.0, 0.0, 100.0), clip(CS.out.vEgo + 0.0, 0.0, 100.0),
clip(CS.out.vEgo + 5.0, 0.0, 100.0)] clip(CS.out.vEgo + 5.0, 0.0, 100.0)]
pcm_speed = interp(gas-brake, pcm_speed_BP, pcm_speed_V) pcm_speed = interp(gas - brake, pcm_speed_BP, pcm_speed_V)
pcm_accel = int((1.0) * 0xc6) pcm_accel = int(1.0 * 0xc6)
else: else:
pcm_speed_V = [0.0, pcm_speed_V = [0.0,
clip(CS.out.vEgo - 2.0, 0.0, 100.0), clip(CS.out.vEgo - 2.0, 0.0, 100.0),
clip(CS.out.vEgo + 2.0, 0.0, 100.0), clip(CS.out.vEgo + 2.0, 0.0, 100.0),
clip(CS.out.vEgo + 5.0, 0.0, 100.0)] clip(CS.out.vEgo + 5.0, 0.0, 100.0)]
pcm_speed = interp(gas-brake, pcm_speed_BP, pcm_speed_V) pcm_speed = interp(gas - brake, pcm_speed_BP, pcm_speed_V)
pcm_accel = int(clip((accel/1.44)/max_accel, 0.0, 1.0) * 0xc6) pcm_accel = int(clip((accel / 1.44) / max_accel, 0.0, 1.0) * 0xc6)
if not self.CP.openpilotLongitudinalControl: if not self.CP.openpilotLongitudinalControl:
if (frame % 2) == 0: if self.frame % 2 == 0:
idx = frame // 2 idx = self.frame // 2
can_sends.append(hondacan.create_bosch_supplemental_1(self.packer, self.CP.carFingerprint, idx)) can_sends.append(hondacan.create_bosch_supplemental_1(self.packer, self.CP.carFingerprint, idx))
# If using stock ACC, spam cancel command to kill gas when OP disengages. # If using stock ACC, spam cancel command to kill gas when OP disengages.
if pcm_cancel_cmd: if pcm_cancel_cmd:
@ -209,24 +215,26 @@ class CarController():
else: else:
# Send gas and brake commands. # Send gas and brake commands.
if (frame % 2) == 0: if self.frame % 2 == 0:
idx = frame // 2 idx = self.frame // 2
ts = frame * DT_CTRL ts = self.frame * DT_CTRL
if self.CP.carFingerprint in HONDA_BOSCH: if self.CP.carFingerprint in HONDA_BOSCH:
self.accel = clip(accel, P.BOSCH_ACCEL_MIN, P.BOSCH_ACCEL_MAX) self.accel = clip(accel, self.params.BOSCH_ACCEL_MIN, self.params.BOSCH_ACCEL_MAX)
self.gas = interp(accel, P.BOSCH_GAS_LOOKUP_BP, P.BOSCH_GAS_LOOKUP_V) self.gas = interp(accel, self.params.BOSCH_GAS_LOOKUP_BP, self.params.BOSCH_GAS_LOOKUP_V)
can_sends.extend(hondacan.create_acc_commands(self.packer, c.enabled, c.longActive, accel, self.gas, idx, stopping, self.CP.carFingerprint)) can_sends.extend(hondacan.create_acc_commands(self.packer, CC.enabled, CC.longActive, accel, self.gas,
idx, stopping, self.CP.carFingerprint))
else: else:
apply_brake = clip(self.brake_last - wind_brake, 0.0, 1.0) apply_brake = clip(self.brake_last - wind_brake, 0.0, 1.0)
apply_brake = int(clip(apply_brake * P.NIDEC_BRAKE_MAX, 0, P.NIDEC_BRAKE_MAX - 1)) apply_brake = int(clip(apply_brake * self.params.NIDEC_BRAKE_MAX, 0, self.params.NIDEC_BRAKE_MAX - 1))
pump_on, self.last_pump_ts = brake_pump_hysteresis(apply_brake, self.apply_brake_last, self.last_pump_ts, ts) pump_on, self.last_pump_ts = brake_pump_hysteresis(apply_brake, self.apply_brake_last, self.last_pump_ts, ts)
pcm_override = True pcm_override = True
can_sends.append(hondacan.create_brake_command(self.packer, apply_brake, pump_on, can_sends.append(hondacan.create_brake_command(self.packer, apply_brake, pump_on,
pcm_override, pcm_cancel_cmd, fcw_display, idx, self.CP.carFingerprint, CS.stock_brake)) pcm_override, pcm_cancel_cmd, fcw_display, idx,
self.CP.carFingerprint, CS.stock_brake))
self.apply_brake_last = apply_brake self.apply_brake_last = apply_brake
self.brake = apply_brake / P.NIDEC_BRAKE_MAX self.brake = apply_brake / self.params.NIDEC_BRAKE_MAX
if self.CP.enableGasInterceptor: if self.CP.enableGasInterceptor:
# way too aggressive at low speed without this # way too aggressive at low speed without this
@ -235,20 +243,20 @@ class CarController():
# This prevents unexpected pedal range rescaling # 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 # Sending non-zero gas when OP is not enabled will cause the PCM not to respond to throttle as expected
# when you do enable. # when you do enable.
if c.longActive: if CC.longActive:
self.gas = clip(gas_mult * (gas - brake + wind_brake*3/4), 0., 1.) self.gas = clip(gas_mult * (gas - brake + wind_brake * 3 / 4), 0., 1.)
else: else:
self.gas = 0.0 self.gas = 0.0
can_sends.append(create_gas_interceptor_command(self.packer, self.gas, idx)) can_sends.append(create_gas_interceptor_command(self.packer, self.gas, idx))
# Send dashboard UI commands. # Send dashboard UI commands.
if (frame % 10) == 0: if self.frame % 10 == 0:
idx = (frame//10) % 4 idx = (self.frame // 10) % 4
hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), hud_car, hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), hud_car,
hud_lanes, fcw_display, acc_alert, steer_required) hud_lanes, fcw_display, acc_alert, steer_required)
can_sends.extend(hondacan.create_ui_commands(self.packer, self.CP, pcm_speed, hud, CS.is_metric, idx, CS.stock_hud)) can_sends.extend(hondacan.create_ui_commands(self.packer, self.CP, pcm_speed, hud, CS.is_metric, idx, CS.stock_hud))
if (self.CP.openpilotLongitudinalControl) and (self.CP.carFingerprint not in HONDA_BOSCH): if self.CP.openpilotLongitudinalControl and self.CP.carFingerprint not in HONDA_BOSCH:
self.speed = pcm_speed self.speed = pcm_speed
if not self.CP.enableGasInterceptor: if not self.CP.enableGasInterceptor:
@ -260,4 +268,5 @@ class CarController():
new_actuators.gas = self.gas new_actuators.gas = self.gas
new_actuators.brake = self.brake new_actuators.brake = self.brake
self.frame += 1
return new_actuators, can_sends return new_actuators, can_sends

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

@ -418,17 +418,5 @@ class CarInterface(CarInterfaceBase):
# pass in a car.CarControl # pass in a car.CarControl
# to be called @ 100hz # to be called @ 100hz
def apply(self, c): def apply(self, c):
hud_control = c.hudControl ret = self.CC.update(c, self.CS)
if hud_control.speedVisible:
hud_v_cruise = hud_control.setSpeed * CV.MS_TO_KPH
else:
hud_v_cruise = 255
ret = self.CC.update(c, self.CS, self.frame,
c.actuators, c.cruiseControl.cancel,
hud_v_cruise, hud_control.lanesVisible,
hud_show_car=hud_control.leadVisible,
hud_alert=hud_control.visualAlert)
self.frame += 1
return ret return ret