From 70286766dabb129b9303bbf5099a13df72a83d15 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Fri, 18 Mar 2022 19:10:59 -0700 Subject: [PATCH] Honda: clean up CarInterface.apply and CarController (#23876) * clean up honda/carcontroller * indentation * fixes * clean up apply function clean up apply function --- selfdrive/car/honda/carcontroller.py | 121 ++++++++++++++------------- selfdrive/car/honda/interface.py | 14 +--- 2 files changed, 66 insertions(+), 69 deletions(-) diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py index ae179e663c..62aecd71a9 100644 --- a/selfdrive/car/honda/carcontroller.py +++ b/selfdrive/car/honda/carcontroller.py @@ -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 diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index 352fb2eb44..5e0c428c3e 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -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