sadmen
/
dragonpilot
mirror of https://github.com/dragonpilot-community/dragonpilot.git
1
0
Fork 0
Code Issues 67 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/214/head
Shane Smiskol 4 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
Unified 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 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
def update(self, c, CS, frame, actuators, pcm_cancel_cmd, hud_control = CC.hudControl
hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert): 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 CC.longActive:
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
Unescape 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

Write Preview
Loading…
Cancel
Save