From 6fa8d389f3c9b4deb108c36e719615946f5659a2 Mon Sep 17 00:00:00 2001 From: HaraldSchafer Date: Wed, 11 Aug 2021 18:32:37 -0700 Subject: [PATCH] Change long control honda pilot, ridgeline, honda fit, civic, ilx (#21886) * not used * just use gas * allow gas * try 0 * typo * set to default * set default * makes more sense * downtune and cleanup * max accel always * that is needed to smooth gas to brake * try downtuning low speed * try from default * nidec is not bosch * air resistance exists * more smoothing * wind brake sooner * Civic experiments2 (#21862) * control on throttle * throttle modifier * get acura in line * try set speed control again (#21870) * new acura function * put back * put back * make linter happy * define wind_brake once * Civic experiments (#21908) * remove civic * start from scratch * data drivenm * the data does not lie * data collection * data driven * limit * final candidate * fix windbrake * Civic experiments (#21910) * remove civic * start from scratch * data drivenm * the data does not lie * data collection * data driven * limit * final candidate * fix windbrake * add acura control * more cleanup * cleanup * update ref * clip * seems to fault * always continuous * new ref old-commit-hash: a8f3e2b3c26f2b617a1c9c94250b44adce861d04 --- selfdrive/car/honda/carcontroller.py | 71 +++++++++++++++++------- selfdrive/car/honda/interface.py | 63 ++++----------------- selfdrive/car/honda/values.py | 8 +++ selfdrive/test/process_replay/ref_commit | 2 +- 4 files changed, 69 insertions(+), 75 deletions(-) diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py index 7204006e11..dfd0bfdc0a 100644 --- a/selfdrive/car/honda/carcontroller.py +++ b/selfdrive/car/honda/carcontroller.py @@ -5,12 +5,13 @@ from selfdrive.controls.lib.drive_helpers import rate_limit from common.numpy_fast import clip, interp from selfdrive.car import create_gas_command from selfdrive.car.honda import hondacan -from selfdrive.car.honda.values import CruiseButtons, CAR, VISUAL_HUD, HONDA_BOSCH, CarControllerParams +from selfdrive.car.honda.values import OLD_NIDEC_LONG_CONTROL, CruiseButtons, CAR, VISUAL_HUD, HONDA_BOSCH, CarControllerParams from opendbc.can.packer import CANPacker VisualAlert = car.CarControl.HUDControl.VisualAlert +#TODO not clear this does anything useful def actuator_hystereses(brake, braking, brake_steady, v_ego, car_fingerprint): # hyst params brake_hyst_on = 0.02 # to activate brakes exceed this value @@ -72,7 +73,7 @@ def process_hud_alert(hud_alert): HUDData = namedtuple("HUDData", - ["pcm_accel", "v_cruise", "car", + ["pcm_accel", "v_cruise", "car", "lanes", "fcw", "acc_alert", "steer_required"]) @@ -125,8 +126,6 @@ class CarController(): fcw_display, steer_required, acc_alert = process_hud_alert(hud_alert) - hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), hud_car, - hud_lanes, fcw_display, acc_alert, steer_required) # **** process the car messages **** @@ -148,10 +147,40 @@ class CarController(): can_sends.append(hondacan.create_steering_control(self.packer, apply_steer, lkas_active, CS.CP.carFingerprint, idx, CS.CP.openpilotLongitudinalControl)) - # Send dashboard UI commands. - if (frame % 10) == 0: - idx = (frame//10) % 4 - can_sends.extend(hondacan.create_ui_commands(self.packer, pcm_speed, hud, CS.CP.carFingerprint, CS.is_metric, idx, CS.CP.openpilotLongitudinalControl, CS.stock_hud)) + + accel = actuators.gas - actuators.brake + + # TODO: pass in LoC.long_control_state and use that to decide starting/stoppping + stopping = accel < 0 and CS.out.vEgo < 0.3 + starting = accel > 0 and CS.out.vEgo < 0.3 + + # Prevent rolling backwards + + accel = -1.0 if stopping else accel + if CS.CP.carFingerprint in HONDA_BOSCH: + apply_accel = interp(accel, P.BOSCH_ACCEL_LOOKUP_BP, P.BOSCH_ACCEL_LOOKUP_V) + else: + apply_accel = interp(accel, P.NIDEC_ACCEL_LOOKUP_BP, P.NIDEC_ACCEL_LOOKUP_V) + + # wind brake from air resistance decel at highspeed + wind_brake = interp(CS.out.vEgo, [0.0, 2.3, 20.0], [0.0, 0.0, 0.1]) + if CS.CP.carFingerprint in OLD_NIDEC_LONG_CONTROL: + #pcm_speed = pcm_speed + pcm_accel = int(clip(pcm_accel, 0, 1) * 0xc6) + elif CS.CP.carFingerprint == CAR.ACURA_ILX: + pcm_speed = CS.out.vEgo + apply_accel + pcm_accel = int(1.0 * 0xc6) + else: + if apply_accel > 0: + pcm_speed = 100 + max_accel = interp(CS.out.vEgo, P.NIDEC_MAX_ACCEL_BP, P.NIDEC_MAX_ACCEL_V) + pcm_accel = int(clip(apply_accel/max_accel, 0.0, 1.0) * 0xc6) + else: + decel = max(0.0, -accel) + pcm_speed = CS.out.vEgo * clip((wind_brake - decel)/max(wind_brake, .01), 0.0, 1.0) + pcm_accel = int(0) + + if not CS.CP.openpilotLongitudinalControl: if (frame % 2) == 0: @@ -168,31 +197,31 @@ class CarController(): if (frame % 2) == 0: idx = frame // 2 ts = frame * DT_CTRL - if CS.CP.carFingerprint in HONDA_BOSCH: - accel = actuators.gas - actuators.brake - - # TODO: pass in LoC.long_control_state and use that to decide starting/stopping - stopping = accel < 0 and CS.out.vEgo < 0.3 - starting = accel > 0 and CS.out.vEgo < 0.3 - - # Prevent rolling backwards - accel = -1.0 if stopping else accel - apply_accel = interp(accel, P.BOSCH_ACCEL_LOOKUP_BP, P.BOSCH_ACCEL_LOOKUP_V) + if CS.CP.carFingerprint in HONDA_BOSCH: apply_gas = interp(accel, P.BOSCH_GAS_LOOKUP_BP, P.BOSCH_GAS_LOOKUP_V) can_sends.extend(hondacan.create_acc_commands(self.packer, enabled, apply_accel, apply_gas, idx, stopping, starting, CS.CP.carFingerprint)) else: - apply_gas = clip(actuators.gas, 0., 1.) - apply_brake = int(clip(self.brake_last * P.BRAKE_MAX, 0, P.BRAKE_MAX - 1)) + apply_brake = clip(self.brake_last - wind_brake, 0.0, 1.0) + apply_brake = int(clip(apply_brake * P.BRAKE_MAX, 0, P.BRAKE_MAX - 1)) pump_on, self.last_pump_ts = brake_pump_hysteresis(apply_brake, self.apply_brake_last, self.last_pump_ts, ts) can_sends.append(hondacan.create_brake_command(self.packer, apply_brake, pump_on, - pcm_override, pcm_cancel_cmd, hud.fcw, idx, CS.CP.carFingerprint, CS.stock_brake)) + pcm_override, pcm_cancel_cmd, fcw_display, idx, CS.CP.carFingerprint, CS.stock_brake)) self.apply_brake_last = apply_brake if CS.CP.enableGasInterceptor: # 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 + apply_gas = clip(actuators.gas, 0., 1.) can_sends.append(create_gas_command(self.packer, apply_gas, idx)) + hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), hud_car, + hud_lanes, fcw_display, acc_alert, steer_required) + + # Send dashboard UI commands. + if (frame % 10) == 0: + idx = (frame//10) % 4 + can_sends.extend(hondacan.create_ui_commands(self.packer, pcm_speed, hud, CS.CP.carFingerprint, CS.is_metric, idx, CS.CP.openpilotLongitudinalControl, CS.stock_hud)) + return can_sends diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index 15446623c9..032eb11e98 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -1,8 +1,7 @@ #!/usr/bin/env python3 -import numpy as np from cereal import car from panda import Panda -from common.numpy_fast import clip, interp +from common.numpy_fast import interp from common.params import Params from selfdrive.config import Conversions as CV from selfdrive.car.honda.values import CarControllerParams, CruiseButtons, CAR, HONDA_BOSCH, HONDA_BOSCH_ALT_BRAKE_SIGNAL @@ -29,49 +28,10 @@ def compute_gb_honda_nidec(accel, speed): return float(accel) / 4.8 - creep_brake -def get_compute_gb_acura(): - # generate a function that takes in [desired_accel, current_speed] -> [-1.0, 1.0] - # where -1.0 is max brake and 1.0 is max gas - # see debug/dump_accel_from_fiber.py to see how those parameters were generated - w0 = np.array([[ 1.22056961, -0.39625418, 0.67952657], - [ 1.03691769, 0.78210306, -0.41343188]]) - b0 = np.array([ 0.01536703, -0.14335321, -0.26932889]) - w2 = np.array([[-0.59124422, 0.42899439, 0.38660881], - [ 0.79973811, 0.13178682, 0.08550351], - [-0.15651935, -0.44360259, 0.76910877]]) - b2 = np.array([ 0.15624429, 0.02294923, -0.0341086 ]) - w4 = np.array([[-0.31521443], - [-0.38626176], - [ 0.52667892]]) - b4 = np.array([-0.02922216]) - - def compute_output(dat, w0, b0, w2, b2, w4, b4): - m0 = np.dot(dat, w0) + b0 - m0 = leakyrelu(m0, 0.1) - m2 = np.dot(m0, w2) + b2 - m2 = leakyrelu(m2, 0.1) - m4 = np.dot(m2, w4) + b4 - return m4 - - def leakyrelu(x, alpha): - return np.maximum(x, alpha * x) - - def _compute_gb_acura(accel, speed): - # linearly extrap below v1 using v1 and v2 data - v1 = 5. - v2 = 10. - dat = np.array([accel, speed]) - if speed > 5.: - m4 = compute_output(dat, w0, b0, w2, b2, w4, b4) - else: - dat[1] = v1 - m4v1 = compute_output(dat, w0, b0, w2, b2, w4, b4) - dat[1] = v2 - m4v2 = compute_output(dat, w0, b0, w2, b2, w4, b4) - m4 = (speed - v1) * (m4v2 - m4v1) / (v2 - v1) + m4v1 - return float(m4) - - return _compute_gb_acura +def compute_gb_acura(accel, speed): + GB_VALUES = [-2., 0.0, 0.8] + GB_BP = [-5., 0.0, 4.0] + return interp(accel, GB_BP, GB_VALUES) class CarInterface(CarInterfaceBase): @@ -79,7 +39,7 @@ class CarInterface(CarInterfaceBase): super().__init__(CP, CarController, CarState) if self.CS.CP.carFingerprint == CAR.ACURA_ILX: - self.compute_gb = get_compute_gb_acura() + self.compute_gb = compute_gb_acura elif self.CS.CP.carFingerprint in HONDA_BOSCH: self.compute_gb = compute_gb_honda_bosch else: @@ -183,11 +143,10 @@ class CarInterface(CarInterfaceBase): ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 2560], [0, 2560]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[1.1], [0.33]] tire_stiffness_factor = 1. - ret.longitudinalTuning.kpBP = [0., 5., 35.] - ret.longitudinalTuning.kpV = [3.6, 2.4, 1.5] + ret.longitudinalTuning.kpV = [1.2, 0.8, 0.5] ret.longitudinalTuning.kiBP = [0., 35.] - ret.longitudinalTuning.kiV = [0.54, 0.36] + ret.longitudinalTuning.kiV = [0.18, 0.12] elif candidate in (CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL): stop_and_go = True @@ -441,7 +400,7 @@ class CarInterface(CarInterfaceBase): ret.brakeMaxV = [1.] # max brake allowed, 3.5m/s^2 else: ret.gasMaxBP = [0.] # m/s - ret.gasMaxV = [0.6] if ret.enableGasInterceptor else [0.] # max gas allowed + ret.gasMaxV = [0.6] # max gas allowed ret.brakeMaxBP = [5., 20.] # m/s ret.brakeMaxV = [1., 0.8] # max brake allowed @@ -559,14 +518,12 @@ class CarInterface(CarInterfaceBase): else: hud_v_cruise = 255 - pcm_accel = int(clip(c.cruiseControl.accelOverride, 0, 1) * 0xc6) - can_sends = self.CC.update(c.enabled, self.CS, self.frame, c.actuators, c.cruiseControl.speedOverride, c.cruiseControl.override, c.cruiseControl.cancel, - pcm_accel, + c.cruiseControl.accelOverride, hud_v_cruise, c.hudControl.lanesVisible, hud_show_car=c.hudControl.leadVisible, diff --git a/selfdrive/car/honda/values.py b/selfdrive/car/honda/values.py index 0d82ff947e..26704f3f84 100644 --- a/selfdrive/car/honda/values.py +++ b/selfdrive/car/honda/values.py @@ -18,6 +18,13 @@ class CarControllerParams(): self.STEER_LOOKUP_BP = [v * -1 for v in CP.lateralParams.torqueBP][1:][::-1] + list(CP.lateralParams.torqueBP) self.STEER_LOOKUP_V = [v * -1 for v in CP.lateralParams.torqueV][1:][::-1] + list(CP.lateralParams.torqueV) + self.NIDEC_ACCEL_LOOKUP_BP = [-1., 0., .6] + self.NIDEC_ACCEL_LOOKUP_V = [-4.8, 0., 2.0] + + self.NIDEC_MAX_ACCEL_V = [0.5, 2.4, 1.4, 0.6] + self.NIDEC_MAX_ACCEL_BP = [0.0, 4.0, 10., 20.] + + self.BOSCH_ACCEL_LOOKUP_BP = [-1., 0., 0.6] self.BOSCH_ACCEL_LOOKUP_V = [-3.5, 0., 2.] self.BOSCH_GAS_LOOKUP_BP = [0., 0.6] @@ -1297,5 +1304,6 @@ SPEED_FACTOR = { CAR.INSIGHT: 1., } +OLD_NIDEC_LONG_CONTROL = set([CAR.ODYSSEY, CAR.ACURA_RDX, CAR.CRV, CAR.HRV]) HONDA_BOSCH = set([CAR.ACCORD, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_5G, CAR.CRV_HYBRID, CAR.INSIGHT, CAR.ACURA_RDX_3G]) HONDA_BOSCH_ALT_BRAKE_SIGNAL = set([CAR.ACCORD, CAR.CRV_5G, CAR.ACURA_RDX_3G]) diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 99919c6254..2d3229e0b2 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -8a2cac15ef8decb0881eb7e67e47de8a81e592aa \ No newline at end of file +eedd8e26cfdbf1bb3cdc566c3ed30c4054efb8cf \ No newline at end of file