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

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

Browse Source 
* clean up honda/carcontroller

* indentation

* fixes

* clean up apply function

clean up apply function
pull/24010/head
Shane Smiskol 3 years ago 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
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. 121
      selfdrive/car/honda/carcontroller.py
  2. 14
      selfdrive/car/honda/interface.py

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

@ -1,18 +1,21 @@
from collections import namedtuple
from cereal import car
from common.realtime import DT_CTRL
from selfdrive.controls.lib.drive_helpers import rate_limit
from common.conversions import Conversions as CV
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.honda import hondacan
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
LongCtrlState = car.CarControl.Actuators.LongControlState
def compute_gb_honda_bosch(accel, speed):
#TODO returns 0s, is unused
# TODO returns 0s, is unused
return 0.0, 0.0
@ -33,14 +36,14 @@ def compute_gas_brake(accel, speed, fingerprint):
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):
# TODO not clear this does anything useful
def actuator_hysteresis(brake, braking, brake_steady, v_ego, car_fingerprint):
# hyst params
brake_hyst_on = 0.02 # to activate brakes exceed 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_on = 0.02 # to activate brakes exceed 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
#*** 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:
brake = 0.
braking = brake > 0.
@ -93,32 +96,34 @@ def process_hud_alert(hud_alert):
HUDData = namedtuple("HUDData",
["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):
self.CP = CP
self.packer = CANPacker(dbc_name)
self.params = CarControllerParams(CP)
self.frame = 0
self.braking = False
self.brake_steady = 0.
self.brake_last = 0.
self.apply_brake_last = 0
self.last_pump_ts = 0.
self.packer = CANPacker(dbc_name)
self.accel = 0
self.speed = 0
self.gas = 0
self.brake = 0
self.params = CarControllerParams(CP)
def update(self, c, CS, frame, actuators, pcm_cancel_cmd,
hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert):
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
P = self.params
if c.longActive:
if CC.longActive:
accel = actuators.accel
gas, brake = compute_gas_brake(actuators.accel, CS.out.vEgo, self.CP.carFingerprint)
else:
@ -126,57 +131,58 @@ class CarController():
gas, brake = 0.0, 0.0
# *** 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 ***
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:
if hud_control.lanesVisible:
hud_lanes = 1
else:
hud_lanes = 0
if c.enabled:
if hud_show_car:
if CC.enabled:
if hud_control.leadVisible:
hud_car = 2
else:
hud_car = 1
else:
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 ****
# 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 = []
# tester present - w/ no response (keeps radar disabled)
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))
# Send steering command.
idx = frame % 4
can_sends.append(hondacan.create_steering_control(self.packer, apply_steer,
c.latActive, self.CP.carFingerprint, idx, CS.CP.openpilotLongitudinalControl))
idx = self.frame % 4
can_sends.append(hondacan.create_steering_control(self.packer, apply_steer, CC.latActive, self.CP.carFingerprint,
idx, CS.CP.openpilotLongitudinalControl))
stopping = actuators.longControlState == LongCtrlState.stopping
# 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])
# 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
pcm_speed_BP = [-wind_brake,
-wind_brake*(3/4),
0.0,
0.5]
-wind_brake * (3 / 4),
0.0,
0.5]
# The Honda ODYSSEY seems to have different PCM_ACCEL
# msgs, is it other cars too?
if self.CP.enableGasInterceptor:
@ -187,19 +193,19 @@ class CarController():
clip(CS.out.vEgo - 3.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)]
pcm_speed = interp(gas-brake, pcm_speed_BP, pcm_speed_V)
pcm_accel = int((1.0) * 0xc6)
pcm_speed = interp(gas - brake, pcm_speed_BP, pcm_speed_V)
pcm_accel = int(1.0 * 0xc6)
else:
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 + 5.0, 0.0, 100.0)]
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_speed = interp(gas - brake, pcm_speed_BP, pcm_speed_V)
pcm_accel = int(clip((accel / 1.44) / max_accel, 0.0, 1.0) * 0xc6)
if not self.CP.openpilotLongitudinalControl:
if (frame % 2) == 0:
idx = frame // 2
if self.frame % 2 == 0:
idx = self.frame // 2
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 pcm_cancel_cmd:
@ -209,24 +215,26 @@ class CarController():
else:
# Send gas and brake commands.
if (frame % 2) == 0:
idx = frame // 2
ts = frame * DT_CTRL
if self.frame % 2 == 0:
idx = self.frame // 2
ts = self.frame * DT_CTRL
if self.CP.carFingerprint in HONDA_BOSCH:
self.accel = clip(accel, P.BOSCH_ACCEL_MIN, P.BOSCH_ACCEL_MAX)
self.gas = interp(accel, P.BOSCH_GAS_LOOKUP_BP, P.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))
self.accel = clip(accel, self.params.BOSCH_ACCEL_MIN, self.params.BOSCH_ACCEL_MAX)
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, CC.enabled, CC.longActive, accel, self.gas,
idx, stopping, self.CP.carFingerprint))
else:
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)
pcm_override = True
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.brake = apply_brake / P.NIDEC_BRAKE_MAX
self.brake = apply_brake / self.params.NIDEC_BRAKE_MAX
if self.CP.enableGasInterceptor:
# way too aggressive at low speed without this
@ -235,20 +243,20 @@ class CarController():
# 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 c.longActive:
self.gas = clip(gas_mult * (gas - brake + wind_brake*3/4), 0., 1.)
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, idx))
# Send dashboard UI commands.
if (frame % 10) == 0:
idx = (frame//10) % 4
if self.frame % 10 == 0:
idx = (self.frame // 10) % 4
hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), hud_car,
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))
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
if not self.CP.enableGasInterceptor:
@ -260,4 +268,5 @@ class CarController():
new_actuators.gas = self.gas
new_actuators.brake = self.brake
self.frame += 1
return new_actuators, can_sends

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

@ -418,17 +418,5 @@ class CarInterface(CarInterfaceBase):
# pass in a car.CarControl
# to be called @ 100hz
def apply(self, c):
hud_control = c.hudControl
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
ret = self.CC.update(c, self.CS)
return ret

Write Preview
Loading…
Cancel
Save