mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-07-06 14:16:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Change long control honda pilot, ridgeline, honda fit, civic, ilx (#21886)

Browse Source 
* 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
This commit is contained in:
HaraldSchafer
2021-08-11 18:32:37 -07:00
committed by GitHub
parent 3c9ce1ab2a
commit 6fa8d389f3
4 changed files with 69 additions and 75 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/car/honda/carcontroller.py
+50 -21
View File
@@ -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)
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
selfdrive/car/honda/interface.py
+10 -53
View File
@@ -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,
selfdrive/car/honda/values.py
+8
View File
@@ -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])
selfdrive/test/process_replay/ref_commit
+1 -1
View File
@@ -1 +1 @@
8a2cac15ef8decb0881eb7e67e47de8a81e592aa
eedd8e26cfdbf1bb3cdc566c3ed30c4054efb8cf